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#include "qemu/osdep.h"
29#include "qemu/log.h"
30#include "qemu/main-loop.h"
31#include "qemu/module.h"
32#include "qapi/error.h"
33#include "chardev/char.h"
34#include "qemu/sockets.h"
35#include "qemu/error-report.h"
36#include "net/can_emu.h"
37#include "net/can_host.h"
38
39#include <sys/ioctl.h>
40#include <net/if.h>
41#include <linux/can.h>
42#include <linux/can/raw.h>
43#include "qom/object.h"
44
45#ifndef DEBUG_CAN
46#define DEBUG_CAN 0
47#endif
48
49#define TYPE_CAN_HOST_SOCKETCAN "can-host-socketcan"
50OBJECT_DECLARE_SIMPLE_TYPE(CanHostSocketCAN, CAN_HOST_SOCKETCAN)
51
52#define CAN_READ_BUF_LEN 5
53struct CanHostSocketCAN {
54 CanHostState parent;
55 char *ifname;
56
57 qemu_can_filter *rfilter;
58 int rfilter_num;
59 can_err_mask_t err_mask;
60
61 qemu_can_frame buf[CAN_READ_BUF_LEN];
62 int bufcnt;
63 int bufptr;
64
65 int fd;
66};
67
68
69QEMU_BUILD_BUG_ON(QEMU_CAN_EFF_FLAG != CAN_EFF_FLAG);
70QEMU_BUILD_BUG_ON(QEMU_CAN_RTR_FLAG != CAN_RTR_FLAG);
71QEMU_BUILD_BUG_ON(QEMU_CAN_ERR_FLAG != CAN_ERR_FLAG);
72QEMU_BUILD_BUG_ON(QEMU_CAN_INV_FILTER != CAN_INV_FILTER);
73QEMU_BUILD_BUG_ON(offsetof(qemu_can_frame, data)
74 != offsetof(struct can_frame, data));
75
76static void can_host_socketcan_display_msg(struct qemu_can_frame *msg)
77{
78 int i;
79 FILE *logfile = qemu_log_lock();
80 qemu_log("[cansocketcan]: %03X [%01d] %s %s",
81 msg->can_id & QEMU_CAN_EFF_MASK,
82 msg->can_dlc,
83 msg->can_id & QEMU_CAN_EFF_FLAG ? "EFF" : "SFF",
84 msg->can_id & QEMU_CAN_RTR_FLAG ? "RTR" : "DAT");
85
86 for (i = 0; i < msg->can_dlc; i++) {
87 qemu_log(" %02X", msg->data[i]);
88 }
89 qemu_log("\n");
90 qemu_log_flush();
91 qemu_log_unlock(logfile);
92}
93
94static void can_host_socketcan_read(void *opaque)
95{
96 CanHostSocketCAN *c = opaque;
97 CanHostState *ch = CAN_HOST(c);
98
99
100 c->bufcnt = read(c->fd, c->buf, sizeof(qemu_can_frame));
101 if (c->bufcnt < 0) {
102 warn_report("CAN bus host read failed (%s)", strerror(errno));
103 return;
104 }
105
106 if (!ch->bus_client.fd_mode) {
107 c->buf[0].flags = 0;
108 } else {
109 if (c->bufcnt > CAN_MTU) {
110 c->buf[0].flags |= QEMU_CAN_FRMF_TYPE_FD;
111 }
112 }
113
114 can_bus_client_send(&ch->bus_client, c->buf, 1);
115
116 if (DEBUG_CAN) {
117 can_host_socketcan_display_msg(c->buf);
118 }
119}
120
121static bool can_host_socketcan_can_receive(CanBusClientState *client)
122{
123 return true;
124}
125
126static ssize_t can_host_socketcan_receive(CanBusClientState *client,
127 const qemu_can_frame *frames, size_t frames_cnt)
128{
129 CanHostState *ch = container_of(client, CanHostState, bus_client);
130 CanHostSocketCAN *c = CAN_HOST_SOCKETCAN(ch);
131
132 size_t len;
133 int res;
134
135 if (c->fd < 0) {
136 return -1;
137 }
138 if (frames->flags & QEMU_CAN_FRMF_TYPE_FD) {
139 if (!ch->bus_client.fd_mode) {
140 return 0;
141 }
142 len = CANFD_MTU;
143 } else {
144 len = CAN_MTU;
145
146 }
147
148 res = write(c->fd, frames, len);
149
150 if (!res) {
151 warn_report("[cansocketcan]: write message to host returns zero");
152 return -1;
153 }
154
155 if (res != len) {
156 if (res < 0) {
157 warn_report("[cansocketcan]: write to host failed (%s)",
158 strerror(errno));
159 } else {
160 warn_report("[cansocketcan]: write to host truncated");
161 }
162 return -1;
163 }
164
165 return 1;
166}
167
168static void can_host_socketcan_disconnect(CanHostState *ch)
169{
170 CanHostSocketCAN *c = CAN_HOST_SOCKETCAN(ch);
171
172 if (c->fd >= 0) {
173 qemu_set_fd_handler(c->fd, NULL, NULL, c);
174 close(c->fd);
175 c->fd = -1;
176 }
177
178 g_free(c->rfilter);
179 c->rfilter = NULL;
180 c->rfilter_num = 0;
181}
182
183static CanBusClientInfo can_host_socketcan_bus_client_info = {
184 .can_receive = can_host_socketcan_can_receive,
185 .receive = can_host_socketcan_receive,
186};
187
188static void can_host_socketcan_connect(CanHostState *ch, Error **errp)
189{
190 CanHostSocketCAN *c = CAN_HOST_SOCKETCAN(ch);
191 int s;
192 int mtu;
193 int enable_canfd = 1;
194 struct sockaddr_can addr;
195 struct ifreq ifr;
196
197 if (!c->ifname) {
198 error_setg(errp, "'if' property not set");
199 return;
200 }
201
202
203 s = qemu_socket(PF_CAN, SOCK_RAW, CAN_RAW);
204 if (s < 0) {
205 error_setg_errno(errp, errno, "failed to create CAN_RAW socket");
206 return;
207 }
208
209 addr.can_family = AF_CAN;
210 memset(&ifr.ifr_name, 0, sizeof(ifr.ifr_name));
211 strcpy(ifr.ifr_name, c->ifname);
212
213 if (ioctl(s, SIOCGIFINDEX, &ifr) < 0) {
214 error_setg_errno(errp, errno,
215 "SocketCAN host interface %s not available",
216 c->ifname);
217 goto fail;
218 }
219 addr.can_ifindex = ifr.ifr_ifindex;
220
221 if (ioctl(s, SIOCGIFMTU, &ifr) < 0) {
222 error_setg_errno(errp, errno,
223 "SocketCAN host interface %s SIOCGIFMTU failed",
224 c->ifname);
225 goto fail;
226 }
227 mtu = ifr.ifr_mtu;
228
229 if (mtu >= CANFD_MTU) {
230
231 if (setsockopt(s, SOL_CAN_RAW, CAN_RAW_FD_FRAMES,
232 &enable_canfd, sizeof(enable_canfd))) {
233 warn_report("SocketCAN host interface %s enabling CAN FD failed",
234 c->ifname);
235 } else {
236 c->parent.bus_client.fd_mode = true;
237 }
238 }
239
240 c->err_mask = 0xffffffff;
241 setsockopt(s, SOL_CAN_RAW, CAN_RAW_ERR_FILTER,
242 &c->err_mask, sizeof(c->err_mask));
243
244 c->rfilter_num = 1;
245 c->rfilter = g_new(struct qemu_can_filter, c->rfilter_num);
246
247
248 c->rfilter[0].can_id = 0;
249 c->rfilter[0].can_mask = 0;
250 c->rfilter[0].can_mask &= ~CAN_ERR_FLAG;
251
252 setsockopt(s, SOL_CAN_RAW, CAN_RAW_FILTER, c->rfilter,
253 c->rfilter_num * sizeof(struct qemu_can_filter));
254
255 if (bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
256 error_setg_errno(errp, errno, "failed to bind to host interface %s",
257 c->ifname);
258 goto fail;
259 }
260
261 c->fd = s;
262 ch->bus_client.info = &can_host_socketcan_bus_client_info;
263 qemu_set_fd_handler(c->fd, can_host_socketcan_read, NULL, c);
264 return;
265
266fail:
267 close(s);
268 g_free(c->rfilter);
269 c->rfilter = NULL;
270 c->rfilter_num = 0;
271}
272
273static char *can_host_socketcan_get_if(Object *obj, Error **errp)
274{
275 CanHostSocketCAN *c = CAN_HOST_SOCKETCAN(obj);
276
277 return g_strdup(c->ifname);
278}
279
280static void can_host_socketcan_set_if(Object *obj, const char *value,
281 Error **errp)
282{
283 CanHostSocketCAN *c = CAN_HOST_SOCKETCAN(obj);
284 struct ifreq ifr;
285
286 if (strlen(value) >= sizeof(ifr.ifr_name)) {
287 error_setg(errp, "CAN interface name longer than %zd characters",
288 sizeof(ifr.ifr_name) - 1);
289 return;
290 }
291
292 if (c->fd != -1) {
293 error_setg(errp, "CAN interface already connected");
294 return;
295 }
296
297 g_free(c->ifname);
298 c->ifname = g_strdup(value);
299}
300
301static void can_host_socketcan_instance_init(Object *obj)
302{
303 CanHostSocketCAN *c = CAN_HOST_SOCKETCAN(obj);
304
305 c->fd = -1;
306}
307
308static void can_host_socketcan_class_init(ObjectClass *klass,
309 void *class_data G_GNUC_UNUSED)
310{
311 CanHostClass *chc = CAN_HOST_CLASS(klass);
312
313 object_class_property_add_str(klass, "if",
314 can_host_socketcan_get_if,
315 can_host_socketcan_set_if);
316 chc->connect = can_host_socketcan_connect;
317 chc->disconnect = can_host_socketcan_disconnect;
318}
319
320static const TypeInfo can_host_socketcan_info = {
321 .parent = TYPE_CAN_HOST,
322 .name = TYPE_CAN_HOST_SOCKETCAN,
323 .instance_size = sizeof(CanHostSocketCAN),
324 .instance_init = can_host_socketcan_instance_init,
325 .class_init = can_host_socketcan_class_init,
326};
327
328static void can_host_register_types(void)
329{
330 type_register_static(&can_host_socketcan_info);
331}
332
333type_init(can_host_register_types);
334
