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
44#ifndef DEBUG_CAN
45#define DEBUG_CAN 0
46#endif
47
48#define TYPE_CAN_HOST_SOCKETCAN "can-host-socketcan"
49#define CAN_HOST_SOCKETCAN(obj) \
50 OBJECT_CHECK(CanHostSocketCAN, (obj), TYPE_CAN_HOST_SOCKETCAN)
51
52#define CAN_READ_BUF_LEN 5
53typedef struct 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} CanHostSocketCAN;
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
80 qemu_log_lock();
81 qemu_log("[cansocketcan]: %03X [%01d] %s %s",
82 msg->can_id & QEMU_CAN_EFF_MASK,
83 msg->can_dlc,
84 msg->can_id & QEMU_CAN_EFF_FLAG ? "EFF" : "SFF",
85 msg->can_id & QEMU_CAN_RTR_FLAG ? "RTR" : "DAT");
86
87 for (i = 0; i < msg->can_dlc; i++) {
88 qemu_log(" %02X", msg->data[i]);
89 }
90 qemu_log("\n");
91 qemu_log_flush();
92 qemu_log_unlock();
93}
94
95static void can_host_socketcan_read(void *opaque)
96{
97 CanHostSocketCAN *c = opaque;
98 CanHostState *ch = CAN_HOST(c);
99
100
101 c->bufcnt = read(c->fd, c->buf, sizeof(qemu_can_frame));
102 if (c->bufcnt < 0) {
103 warn_report("CAN bus host read failed (%s)", strerror(errno));
104 return;
105 }
106
107 can_bus_client_send(&ch->bus_client, c->buf, 1);
108
109 if (DEBUG_CAN) {
110 can_host_socketcan_display_msg(c->buf);
111 }
112}
113
114static int can_host_socketcan_can_receive(CanBusClientState *client)
115{
116 return 1;
117}
118
119static ssize_t can_host_socketcan_receive(CanBusClientState *client,
120 const qemu_can_frame *frames, size_t frames_cnt)
121{
122 CanHostState *ch = container_of(client, CanHostState, bus_client);
123 CanHostSocketCAN *c = CAN_HOST_SOCKETCAN(ch);
124
125 size_t len = sizeof(qemu_can_frame);
126 int res;
127
128 if (c->fd < 0) {
129 return -1;
130 }
131
132 res = write(c->fd, frames, len);
133
134 if (!res) {
135 warn_report("[cansocketcan]: write message to host returns zero");
136 return -1;
137 }
138
139 if (res != len) {
140 if (res < 0) {
141 warn_report("[cansocketcan]: write to host failed (%s)",
142 strerror(errno));
143 } else {
144 warn_report("[cansocketcan]: write to host truncated");
145 }
146 return -1;
147 }
148
149 return 1;
150}
151
152static void can_host_socketcan_disconnect(CanHostState *ch)
153{
154 CanHostSocketCAN *c = CAN_HOST_SOCKETCAN(ch);
155
156 if (c->fd >= 0) {
157 qemu_set_fd_handler(c->fd, NULL, NULL, c);
158 close(c->fd);
159 c->fd = -1;
160 }
161
162 g_free(c->rfilter);
163 c->rfilter = NULL;
164 c->rfilter_num = 0;
165}
166
167static CanBusClientInfo can_host_socketcan_bus_client_info = {
168 .can_receive = can_host_socketcan_can_receive,
169 .receive = can_host_socketcan_receive,
170};
171
172static void can_host_socketcan_connect(CanHostState *ch, Error **errp)
173{
174 CanHostSocketCAN *c = CAN_HOST_SOCKETCAN(ch);
175 int s;
176 struct sockaddr_can addr;
177 struct ifreq ifr;
178
179
180 s = qemu_socket(PF_CAN, SOCK_RAW, CAN_RAW);
181 if (s < 0) {
182 error_setg_errno(errp, errno, "failed to create CAN_RAW socket");
183 return;
184 }
185
186 addr.can_family = AF_CAN;
187 memset(&ifr.ifr_name, 0, sizeof(ifr.ifr_name));
188 strcpy(ifr.ifr_name, c->ifname);
189 if (ioctl(s, SIOCGIFINDEX, &ifr) < 0) {
190 error_setg_errno(errp, errno,
191 "SocketCAN host interface %s not available", c->ifname);
192 goto fail;
193 }
194 addr.can_ifindex = ifr.ifr_ifindex;
195
196 c->err_mask = 0xffffffff;
197 setsockopt(s, SOL_CAN_RAW, CAN_RAW_ERR_FILTER,
198 &c->err_mask, sizeof(c->err_mask));
199
200 c->rfilter_num = 1;
201 c->rfilter = g_new(struct qemu_can_filter, c->rfilter_num);
202
203
204 c->rfilter[0].can_id = 0;
205 c->rfilter[0].can_mask = 0;
206 c->rfilter[0].can_mask &= ~CAN_ERR_FLAG;
207
208 setsockopt(s, SOL_CAN_RAW, CAN_RAW_FILTER, c->rfilter,
209 c->rfilter_num * sizeof(struct qemu_can_filter));
210
211 if (bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
212 error_setg_errno(errp, errno, "failed to bind to host interface %s",
213 c->ifname);
214 goto fail;
215 }
216
217 c->fd = s;
218 ch->bus_client.info = &can_host_socketcan_bus_client_info;
219 qemu_set_fd_handler(c->fd, can_host_socketcan_read, NULL, c);
220 return;
221
222fail:
223 close(s);
224 g_free(c->rfilter);
225 c->rfilter = NULL;
226 c->rfilter_num = 0;
227}
228
229static char *can_host_socketcan_get_if(Object *obj, Error **errp)
230{
231 CanHostSocketCAN *c = CAN_HOST_SOCKETCAN(obj);
232
233 return g_strdup(c->ifname);
234}
235
236static void can_host_socketcan_set_if(Object *obj, const char *value, Error **errp)
237{
238 CanHostSocketCAN *c = CAN_HOST_SOCKETCAN(obj);
239 struct ifreq ifr;
240
241 if (strlen(value) >= sizeof(ifr.ifr_name)) {
242 error_setg(errp, "CAN interface name longer than %zd characters",
243 sizeof(ifr.ifr_name) - 1);
244 return;
245 }
246
247 if (c->fd != -1) {
248 error_setg(errp, "CAN interface already connected");
249 return;
250 }
251
252 g_free(c->ifname);
253 c->ifname = g_strdup(value);
254}
255
256static void can_host_socketcan_instance_init(Object *obj)
257{
258 CanHostSocketCAN *c = CAN_HOST_SOCKETCAN(obj);
259
260 c->fd = -1;
261}
262
263static void can_host_socketcan_class_init(ObjectClass *klass,
264 void *class_data G_GNUC_UNUSED)
265{
266 CanHostClass *chc = CAN_HOST_CLASS(klass);
267
268 object_class_property_add_str(klass, "if",
269 can_host_socketcan_get_if,
270 can_host_socketcan_set_if,
271 &error_abort);
272 chc->connect = can_host_socketcan_connect;
273 chc->disconnect = can_host_socketcan_disconnect;
274}
275
276static const TypeInfo can_host_socketcan_info = {
277 .parent = TYPE_CAN_HOST,
278 .name = TYPE_CAN_HOST_SOCKETCAN,
279 .instance_size = sizeof(CanHostSocketCAN),
280 .instance_init = can_host_socketcan_instance_init,
281 .class_init = can_host_socketcan_class_init,
282};
283
284static void can_host_register_types(void)
285{
286 type_register_static(&can_host_socketcan_info);
287}
288
289type_init(can_host_register_types);
290
