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
|
#include "cethcan.h"
#include <sys/types.h>
#include <sys/socket.h>
#include <sys/ioctl.h>
#include <net/if.h>
#include <linux/can.h>
#include <linux/can/raw.h>
struct socan {
struct can_user *u;
struct event *ev;
char *ifname;
int ifindex;
int sock;
};
static void socan_handler(void *arg, struct can_message *msg)
{
struct socan *sc = arg;
lprintf("%s: TX not implemented", sc->u->name);
}
static void socan_event(int sock, short event, void *arg)
{
struct socan *sc = arg;
struct can_message msg;
union {
struct sockaddr_can can;
struct sockaddr_storage ss;
struct sockaddr su;
} canaddr;
socklen_t addrlen = sizeof(canaddr);
struct canfd_frame frame;
if (event & EV_READ) {
ssize_t rlen = recvfrom(sc->sock, &frame, sizeof(frame),
MSG_TRUNC, &canaddr.su, &addrlen);
if (rlen > 0) {
#if 0
if (1) {
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);
}
#endif
if (frame.can_id & CAN_EFF_FLAG)
msg.daddr = 0x00080000
| (frame.can_id & 0x0003ffff)
| ((frame.can_id & 0x1ffc0000) << 3);
else
msg.daddr = (frame.can_id & 0x7ff) << 21;
msg.dlc = frame.len;
if (msg.dlc > 8)
msg.dlc = 8;
memcpy(msg.bytes, frame.data, msg.dlc);
can_broadcast(sc->u, &msg);
}
}
}
int socan_init(json_t *config)
{
struct socan *sc;
const char *iface;
int ifindex;
int on = 1;
struct sockaddr_can addr;
if (!json_is_object(config)) {
lprintf("socketcan config must be an object/dictionary");
return 1;
}
if (!json_is_string(json_object_get(config, "interface"))) {
lprintf("socketcan 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("socketcan interface '%s' error: %s",
iface, strerror(errno));
return 1;
}
sc = calloc(sizeof(*sc), 1);
sc->ifname = strdup(iface);
sc->ifindex = ifindex;
sc->sock = socket(PF_CAN, SOCK_RAW, CAN_RAW);
if (sc->sock == -1) {
lprintf("socketcan interface '%s' socket() error: %s",
iface, strerror(errno));
free(sc);
return 1;
}
if (setsockopt(sc->sock, SOL_CAN_RAW, CAN_RAW_FD_FRAMES,
&on, sizeof(on))) {
lprintf("socketcan interface '%s' raw_fd_frames error: %s",
iface, strerror(errno));
return 1;
}
/* if (setsockopt(sc->sock, SOL_SOCKET, SO_TIMESTAMP, &on, sizeof(on))) {
lprintf("socketcan interface '%s' so_timestamp error: %s",
iface, strerror(errno));
return 1;
} */
addr.can_family = AF_CAN;
addr.can_ifindex = ifindex;
if (bind(sc->sock, (struct sockaddr *)&addr, sizeof(addr))) {
lprintf("socketcan interface '%s' bind error: %s",
iface, strerror(errno));
return 1;
}
sc->u = can_register_alloc(sc, socan_handler, "socketcan[%s]", iface);
sc->ev = event_new(ev_base, sc->sock, EV_READ | EV_PERSIST,
socan_event, sc);
event_add(sc->ev, NULL);
return 0;
}
|
