1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
|
#include "cethcan.h"
#include <net/if.h>
#include <net/ethernet.h>
#include <netpacket/packet.h>
#define ETHER_PROTO 0x88b7
#define ETHER_MCADDR 0xff, 0x3a, 0xf6, 'C', 'A', 'N'
struct ether {
struct can_user *u;
struct event *ev;
char *ifname;
int ifindex;
int sock;
};
struct frame {
uint8_t daddr[ETH_ALEN];
uint8_t saddr[ETH_ALEN];
uint16_t proto;
uint8_t oui[3];
uint16_t protoid;
uint8_t protover;
uint32_t ts1, ts2;
uint32_t can_daddr;
uint8_t can_dlc;
uint8_t can_data[8];
} __attribute__((packed));
static void ether_can_handler(void *arg, struct can_message *msg)
{
struct ether *e = arg;
lprintf("%s: TX not implemented", e->u->name);
}
static void ether_rx_dataframe(struct ether *e, struct frame *f)
{
struct can_message msg;
msg.daddr = ntohl(f->can_daddr);
msg.dlc = f->can_dlc;
if (msg.dlc > 8)
msg.dlc = 8;
memcpy(msg.bytes, f->can_data, msg.dlc);
can_broadcast(e->u, &msg);
}
static void ether_sock_handler(int sock, short event, void *arg)
{
struct ether *e = arg;
union {
struct sockaddr_ll ll;
struct sockaddr_storage ss;
struct sockaddr su;
} lladdr;
socklen_t addrlen = sizeof(lladdr);
union {
struct frame frame;
uint8_t raw[1536];
} buf;
if (event & EV_READ) {
ssize_t rlen = recvfrom(e->sock, &buf, sizeof(buf),
MSG_TRUNC, &lladdr.su, &addrlen);
if (rlen > 0) {
if (0) {
lprintf("got %zd bytes", rlen);
char pbuffer[16 * 3 + 1];
for (size_t i = 0; i < (size_t)rlen; i++) {
size_t j = i % 16;
sprintf(pbuffer + (3 * j), " %02x", buf.raw[i]);
if (j == 15)
lprintf(">>%s", pbuffer);
}
if ((rlen % 16) != 15)
lprintf(">>%s", pbuffer);
}
if (ntohs(buf.frame.proto) == ETHER_PROTO
&& buf.frame.oui[0] == 0x00
&& buf.frame.oui[1] == 0x80
&& buf.frame.oui[2] == 0x41
&& ntohs(buf.frame.protoid) == 0xaaaa) {
switch (buf.frame.protover) {
case 3:
ether_rx_dataframe(e, &buf.frame);
break;
default:
lprintf("%s: unsupported CAN protocol version %d",
e->u->name, buf.frame.protover);
}
} else {
lprintf("%s: non-CAN frame (%zd bytes)",
e->u->name, rlen);
}
}
}
}
int ether_init(json_t *config)
{
struct ether *e;
const char *iface;
int ifindex;
struct packet_mreq pmr = {
.mr_type = PACKET_MR_MULTICAST,
.mr_alen = ETH_ALEN,
.mr_address = { ETHER_MCADDR },
};
if (!json_is_object(config)) {
lprintf("ethernet config must be an object/dictionary");
return 1;
}
if (!json_is_string(json_object_get(config, "interface"))) {
lprintf("ethernet config must have an 'interface' key");
return 1;
}
iface = json_string_value(json_object_get(config, "interface"));
ifindex = if_nametoindex(iface);
if (ifindex == 0) {
lprintf("ethernet interface '%s' error: %s",
iface, strerror(errno));
return 1;
}
e = calloc(sizeof(*e), 1);
e->ifname = strdup(iface);
e->ifindex = ifindex;
e->sock = socket(AF_PACKET, SOCK_RAW, htons(ETHER_PROTO));
if (e->sock == -1) {
lprintf("ethernet interface '%s' socket() error: %s",
iface, strerror(errno));
return 1;
}
pmr.mr_ifindex = ifindex;
if (setsockopt(e->sock, SOL_PACKET, PACKET_ADD_MEMBERSHIP,
&pmr, sizeof(pmr))) {
lprintf("ethernet interface '%s' multicast join error: %s",
iface, strerror(errno));
return 1;
}
e->u = can_register_alloc(e, ether_can_handler, "ether[%s]", iface);
e->ev = event_new(ev_base, e->sock, EV_READ | EV_PERSIST, ether_sock_handler, e);
event_add(e->ev, NULL);
return 0;
}
|
