1 | /* |
2 | * CAN c support to connect to the Linux host SocketCAN interfaces |
3 | * |
4 | * Copyright (c) 2013-2014 Jin Yang |
5 | * Copyright (c) 2014-2018 Pavel Pisa |
6 | * |
7 | * Initial development supported by Google GSoC 2013 from RTEMS project slot |
8 | * |
9 | * Permission is hereby granted, free of charge, to any person obtaining a copy |
10 | * of this software and associated documentation files (the "Software"), to deal |
11 | * in the Software without restriction, including without limitation the rights |
12 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell |
13 | * copies of the Software, and to permit persons to whom the Software is |
14 | * furnished to do so, subject to the following conditions: |
15 | * |
16 | * The above copyright notice and this permission notice shall be included in |
17 | * all copies or substantial portions of the Software. |
18 | * |
19 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR |
20 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, |
21 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL |
22 | * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER |
23 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, |
24 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN |
25 | * THE SOFTWARE. |
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 /*DEBUG_CAN*/ |
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 |
53 | typedef 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 | /* Check that QEMU and Linux kernel flags encoding and structure matches */ |
69 | QEMU_BUILD_BUG_ON(QEMU_CAN_EFF_FLAG != CAN_EFF_FLAG); |
70 | QEMU_BUILD_BUG_ON(QEMU_CAN_RTR_FLAG != CAN_RTR_FLAG); |
71 | QEMU_BUILD_BUG_ON(QEMU_CAN_ERR_FLAG != CAN_ERR_FLAG); |
72 | QEMU_BUILD_BUG_ON(QEMU_CAN_INV_FILTER != CAN_INV_FILTER); |
73 | QEMU_BUILD_BUG_ON(offsetof(qemu_can_frame, data) |
74 | != offsetof(struct can_frame, data)); |
75 | |
76 | static 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 | |
95 | static void can_host_socketcan_read(void *opaque) |
96 | { |
97 | CanHostSocketCAN *c = opaque; |
98 | CanHostState *ch = CAN_HOST(c); |
99 | |
100 | /* CAN_READ_BUF_LEN for multiple messages syscall is possible for future */ |
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 | |
114 | static int can_host_socketcan_can_receive(CanBusClientState *client) |
115 | { |
116 | return 1; |
117 | } |
118 | |
119 | static 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 | |
152 | static 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 | |
167 | static CanBusClientInfo can_host_socketcan_bus_client_info = { |
168 | .can_receive = can_host_socketcan_can_receive, |
169 | .receive = can_host_socketcan_receive, |
170 | }; |
171 | |
172 | static void can_host_socketcan_connect(CanHostState *ch, Error **errp) |
173 | { |
174 | CanHostSocketCAN *c = CAN_HOST_SOCKETCAN(ch); |
175 | int s; /* can raw socket */ |
176 | struct sockaddr_can addr; |
177 | struct ifreq ifr; |
178 | |
179 | /* open socket */ |
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; /* Receive error frame. */ |
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 | /* Receive all data frame. If |= CAN_INV_FILTER no data. */ |
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 | |
222 | fail: |
223 | close(s); |
224 | g_free(c->rfilter); |
225 | c->rfilter = NULL; |
226 | c->rfilter_num = 0; |
227 | } |
228 | |
229 | static 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 | |
236 | static 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 | |
256 | static void can_host_socketcan_instance_init(Object *obj) |
257 | { |
258 | CanHostSocketCAN *c = CAN_HOST_SOCKETCAN(obj); |
259 | |
260 | c->fd = -1; |
261 | } |
262 | |
263 | static 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 | |
276 | static 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 | |
284 | static void can_host_register_types(void) |
285 | { |
286 | type_register_static(&can_host_socketcan_info); |
287 | } |
288 | |
289 | type_init(can_host_register_types); |
290 | |