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