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/module.h"
31#include "qapi/error.h"
32#include "chardev/char.h"
33#include "qemu/sockets.h"
34#include "qemu/error-report.h"
35#include "net/can_emu.h"
36#include "net/can_host.h"
37
38#include <sys/ioctl.h>
39#include <net/if.h>
40#include <linux/can.h>
41#include <linux/can/raw.h>
42
43#ifndef DEBUG_CAN
44#define DEBUG_CAN 0
45#endif
46
47#define TYPE_CAN_HOST_SOCKETCAN "can-host-socketcan"
48#define CAN_HOST_SOCKETCAN(obj) \
49 OBJECT_CHECK(CanHostSocketCAN, (obj), TYPE_CAN_HOST_SOCKETCAN)
50
51#define CAN_READ_BUF_LEN 5
52typedef struct CanHostSocketCAN {
53 CanHostState parent;
54 char *ifname;
55
56 qemu_can_filter *rfilter;
57 int rfilter_num;
58 can_err_mask_t err_mask;
59
60 qemu_can_frame buf[CAN_READ_BUF_LEN];
61 int bufcnt;
62 int bufptr;
63
64 int fd;
65} CanHostSocketCAN;
66
67
68QEMU_BUILD_BUG_ON(QEMU_CAN_EFF_FLAG != CAN_EFF_FLAG);
69QEMU_BUILD_BUG_ON(QEMU_CAN_RTR_FLAG != CAN_RTR_FLAG);
70QEMU_BUILD_BUG_ON(QEMU_CAN_ERR_FLAG != CAN_ERR_FLAG);
71QEMU_BUILD_BUG_ON(QEMU_CAN_INV_FILTER != CAN_INV_FILTER);
72QEMU_BUILD_BUG_ON(offsetof(qemu_can_frame, data)
73 != offsetof(struct can_frame, data));
74
75static void can_host_socketcan_display_msg(struct qemu_can_frame *msg)
76{
77 int i;
78
79 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();
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 can_bus_client_send(&ch->bus_client, c->buf, 1);
107
108 if (DEBUG_CAN) {
109 can_host_socketcan_display_msg(c->buf);
110 }
111}
112
113static int can_host_socketcan_can_receive(CanBusClientState *client)
114{
115 return 1;
116}
117
118static ssize_t can_host_socketcan_receive(CanBusClientState *client,
119 const qemu_can_frame *frames, size_t frames_cnt)
120{
121 CanHostState *ch = container_of(client, CanHostState, bus_client);
122 CanHostSocketCAN *c = CAN_HOST_SOCKETCAN(ch);
123
124 size_t len = sizeof(qemu_can_frame);
125 int res;
126
127 if (c->fd < 0) {
128 return -1;
129 }
130
131 res = write(c->fd, frames, len);
132
133 if (!res) {
134 warn_report("[cansocketcan]: write message to host returns zero");
135 return -1;
136 }
137
138 if (res != len) {
139 if (res < 0) {
140 warn_report("[cansocketcan]: write to host failed (%s)",
141 strerror(errno));
142 } else {
143 warn_report("[cansocketcan]: write to host truncated");
144 }
145 return -1;
146 }
147
148 return 1;
149}
150
151static void can_host_socketcan_disconnect(CanHostState *ch)
152{
153 CanHostSocketCAN *c = CAN_HOST_SOCKETCAN(ch);
154
155 if (c->fd >= 0) {
156 qemu_set_fd_handler(c->fd, NULL, NULL, c);
157 close(c->fd);
158 c->fd = -1;
159 }
160
161 g_free(c->rfilter);
162 c->rfilter = NULL;
163 c->rfilter_num = 0;
164}
165
166static CanBusClientInfo can_host_socketcan_bus_client_info = {
167 .can_receive = can_host_socketcan_can_receive,
168 .receive = can_host_socketcan_receive,
169};
170
171static void can_host_socketcan_connect(CanHostState *ch, Error **errp)
172{
173 CanHostSocketCAN *c = CAN_HOST_SOCKETCAN(ch);
174 int s;
175 struct sockaddr_can addr;
176 struct ifreq ifr;
177
178
179 s = qemu_socket(PF_CAN, SOCK_RAW, CAN_RAW);
180 if (s < 0) {
181 error_setg_errno(errp, errno, "failed to create CAN_RAW socket");
182 return;
183 }
184
185 addr.can_family = AF_CAN;
186 memset(&ifr.ifr_name, 0, sizeof(ifr.ifr_name));
187 strcpy(ifr.ifr_name, c->ifname);
188 if (ioctl(s, SIOCGIFINDEX, &ifr) < 0) {
189 error_setg_errno(errp, errno,
190 "SocketCAN host interface %s not available", c->ifname);
191 goto fail;
192 }
193 addr.can_ifindex = ifr.ifr_ifindex;
194
195 c->err_mask = 0xffffffff;
196 setsockopt(s, SOL_CAN_RAW, CAN_RAW_ERR_FILTER,
197 &c->err_mask, sizeof(c->err_mask));
198
199 c->rfilter_num = 1;
200 c->rfilter = g_new(struct qemu_can_filter, c->rfilter_num);
201
202
203 c->rfilter[0].can_id = 0;
204 c->rfilter[0].can_mask = 0;
205 c->rfilter[0].can_mask &= ~CAN_ERR_FLAG;
206
207 setsockopt(s, SOL_CAN_RAW, CAN_RAW_FILTER, c->rfilter,
208 c->rfilter_num * sizeof(struct qemu_can_filter));
209
210 if (bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
211 error_setg_errno(errp, errno, "failed to bind to host interface %s",
212 c->ifname);
213 goto fail;
214 }
215
216 c->fd = s;
217 ch->bus_client.info = &can_host_socketcan_bus_client_info;
218 qemu_set_fd_handler(c->fd, can_host_socketcan_read, NULL, c);
219 return;
220
221fail:
222 close(s);
223 g_free(c->rfilter);
224 c->rfilter = NULL;
225 c->rfilter_num = 0;
226}
227
228static char *can_host_socketcan_get_if(Object *obj, Error **errp)
229{
230 CanHostSocketCAN *c = CAN_HOST_SOCKETCAN(obj);
231
232 return g_strdup(c->ifname);
233}
234
235static void can_host_socketcan_set_if(Object *obj, const char *value, Error **errp)
236{
237 CanHostSocketCAN *c = CAN_HOST_SOCKETCAN(obj);
238 struct ifreq ifr;
239
240 if (strlen(value) >= sizeof(ifr.ifr_name)) {
241 error_setg(errp, "CAN interface name longer than %zd characters",
242 sizeof(ifr.ifr_name) - 1);
243 return;
244 }
245
246 if (c->fd != -1) {
247 error_setg(errp, "CAN interface already connected");
248 return;
249 }
250
251 g_free(c->ifname);
252 c->ifname = g_strdup(value);
253}
254
255static void can_host_socketcan_instance_init(Object *obj)
256{
257 CanHostSocketCAN *c = CAN_HOST_SOCKETCAN(obj);
258
259 c->fd = -1;
260}
261
262static void can_host_socketcan_class_init(ObjectClass *klass,
263 void *class_data G_GNUC_UNUSED)
264{
265 CanHostClass *chc = CAN_HOST_CLASS(klass);
266
267 object_class_property_add_str(klass, "if",
268 can_host_socketcan_get_if,
269 can_host_socketcan_set_if,
270 &error_abort);
271 chc->connect = can_host_socketcan_connect;
272 chc->disconnect = can_host_socketcan_disconnect;
273}
274
275static const TypeInfo can_host_socketcan_info = {
276 .parent = TYPE_CAN_HOST,
277 .name = TYPE_CAN_HOST_SOCKETCAN,
278 .instance_size = sizeof(CanHostSocketCAN),
279 .instance_init = can_host_socketcan_instance_init,
280 .class_init = can_host_socketcan_class_init,
281};
282
283static void can_host_register_types(void)
284{
285 type_register_static(&can_host_socketcan_info);
286}
287
288type_init(can_host_register_types);
289
