Pavel Pisa | ea15ea8 | 2018-01-27 17:59:27 +0100 | [diff] [blame] | 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 | */ |
Markus Armbruster | 0b8fa32 | 2019-05-23 16:35:07 +0200 | [diff] [blame] | 27 | |
Pavel Pisa | ea15ea8 | 2018-01-27 17:59:27 +0100 | [diff] [blame] | 28 | #include "qemu/osdep.h" |
| 29 | #include "qemu/log.h" |
Markus Armbruster | db72581 | 2019-08-12 07:23:50 +0200 | [diff] [blame] | 30 | #include "qemu/main-loop.h" |
Markus Armbruster | 0b8fa32 | 2019-05-23 16:35:07 +0200 | [diff] [blame] | 31 | #include "qemu/module.h" |
Pavel Pisa | ea15ea8 | 2018-01-27 17:59:27 +0100 | [diff] [blame] | 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> |
Eduardo Habkost | db1015e | 2020-09-03 16:43:22 -0400 | [diff] [blame] | 43 | #include "qom/object.h" |
Pavel Pisa | ea15ea8 | 2018-01-27 17:59:27 +0100 | [diff] [blame] | 44 | |
| 45 | #ifndef DEBUG_CAN |
| 46 | #define DEBUG_CAN 0 |
| 47 | #endif /*DEBUG_CAN*/ |
| 48 | |
| 49 | #define TYPE_CAN_HOST_SOCKETCAN "can-host-socketcan" |
Eduardo Habkost | 8063396 | 2020-09-16 14:25:19 -0400 | [diff] [blame] | 50 | OBJECT_DECLARE_SIMPLE_TYPE(CanHostSocketCAN, CAN_HOST_SOCKETCAN) |
Pavel Pisa | ea15ea8 | 2018-01-27 17:59:27 +0100 | [diff] [blame] | 51 | |
| 52 | #define CAN_READ_BUF_LEN 5 |
Eduardo Habkost | db1015e | 2020-09-03 16:43:22 -0400 | [diff] [blame] | 53 | struct CanHostSocketCAN { |
Pavel Pisa | ea15ea8 | 2018-01-27 17:59:27 +0100 | [diff] [blame] | 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; |
Eduardo Habkost | db1015e | 2020-09-03 16:43:22 -0400 | [diff] [blame] | 66 | }; |
Pavel Pisa | ea15ea8 | 2018-01-27 17:59:27 +0100 | [diff] [blame] | 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; |
Richard Henderson | c60f599 | 2022-04-17 11:29:47 -0700 | [diff] [blame] | 79 | FILE *logfile = qemu_log_trylock(); |
Pavel Pisa | ea15ea8 | 2018-01-27 17:59:27 +0100 | [diff] [blame] | 80 | |
Richard Henderson | 78b5485 | 2022-04-17 11:29:49 -0700 | [diff] [blame] | 81 | if (logfile) { |
| 82 | fprintf(logfile, "[cansocketcan]: %03X [%01d] %s %s", |
| 83 | msg->can_id & QEMU_CAN_EFF_MASK, |
| 84 | msg->can_dlc, |
| 85 | msg->can_id & QEMU_CAN_EFF_FLAG ? "EFF" : "SFF", |
| 86 | msg->can_id & QEMU_CAN_RTR_FLAG ? "RTR" : "DAT"); |
| 87 | |
| 88 | for (i = 0; i < msg->can_dlc; i++) { |
| 89 | fprintf(logfile, " %02X", msg->data[i]); |
| 90 | } |
| 91 | fprintf(logfile, "\n"); |
Richard Henderson | 78b5485 | 2022-04-17 11:29:49 -0700 | [diff] [blame] | 92 | qemu_log_unlock(logfile); |
Pavel Pisa | ea15ea8 | 2018-01-27 17:59:27 +0100 | [diff] [blame] | 93 | } |
Pavel Pisa | ea15ea8 | 2018-01-27 17:59:27 +0100 | [diff] [blame] | 94 | } |
| 95 | |
| 96 | static void can_host_socketcan_read(void *opaque) |
| 97 | { |
| 98 | CanHostSocketCAN *c = opaque; |
| 99 | CanHostState *ch = CAN_HOST(c); |
| 100 | |
| 101 | /* CAN_READ_BUF_LEN for multiple messages syscall is possible for future */ |
| 102 | c->bufcnt = read(c->fd, c->buf, sizeof(qemu_can_frame)); |
| 103 | if (c->bufcnt < 0) { |
| 104 | warn_report("CAN bus host read failed (%s)", strerror(errno)); |
| 105 | return; |
| 106 | } |
| 107 | |
Jan Charvat | d44948c | 2020-09-14 10:09:02 +0200 | [diff] [blame] | 108 | if (!ch->bus_client.fd_mode) { |
| 109 | c->buf[0].flags = 0; |
| 110 | } else { |
| 111 | if (c->bufcnt > CAN_MTU) { |
| 112 | c->buf[0].flags |= QEMU_CAN_FRMF_TYPE_FD; |
| 113 | } |
| 114 | } |
| 115 | |
Pavel Pisa | ea15ea8 | 2018-01-27 17:59:27 +0100 | [diff] [blame] | 116 | can_bus_client_send(&ch->bus_client, c->buf, 1); |
| 117 | |
| 118 | if (DEBUG_CAN) { |
| 119 | can_host_socketcan_display_msg(c->buf); |
| 120 | } |
| 121 | } |
| 122 | |
Philippe Mathieu-Daudé | 767cc9a | 2020-03-05 18:56:50 +0100 | [diff] [blame] | 123 | static bool can_host_socketcan_can_receive(CanBusClientState *client) |
Pavel Pisa | ea15ea8 | 2018-01-27 17:59:27 +0100 | [diff] [blame] | 124 | { |
Philippe Mathieu-Daudé | 767cc9a | 2020-03-05 18:56:50 +0100 | [diff] [blame] | 125 | return true; |
Pavel Pisa | ea15ea8 | 2018-01-27 17:59:27 +0100 | [diff] [blame] | 126 | } |
| 127 | |
| 128 | static ssize_t can_host_socketcan_receive(CanBusClientState *client, |
| 129 | const qemu_can_frame *frames, size_t frames_cnt) |
| 130 | { |
| 131 | CanHostState *ch = container_of(client, CanHostState, bus_client); |
| 132 | CanHostSocketCAN *c = CAN_HOST_SOCKETCAN(ch); |
| 133 | |
Jan Charvat | d44948c | 2020-09-14 10:09:02 +0200 | [diff] [blame] | 134 | size_t len; |
Pavel Pisa | ea15ea8 | 2018-01-27 17:59:27 +0100 | [diff] [blame] | 135 | int res; |
| 136 | |
| 137 | if (c->fd < 0) { |
| 138 | return -1; |
| 139 | } |
Jan Charvat | d44948c | 2020-09-14 10:09:02 +0200 | [diff] [blame] | 140 | if (frames->flags & QEMU_CAN_FRMF_TYPE_FD) { |
| 141 | if (!ch->bus_client.fd_mode) { |
| 142 | return 0; |
| 143 | } |
| 144 | len = CANFD_MTU; |
| 145 | } else { |
| 146 | len = CAN_MTU; |
| 147 | |
| 148 | } |
Pavel Pisa | ea15ea8 | 2018-01-27 17:59:27 +0100 | [diff] [blame] | 149 | |
| 150 | res = write(c->fd, frames, len); |
| 151 | |
| 152 | if (!res) { |
| 153 | warn_report("[cansocketcan]: write message to host returns zero"); |
| 154 | return -1; |
| 155 | } |
| 156 | |
| 157 | if (res != len) { |
| 158 | if (res < 0) { |
| 159 | warn_report("[cansocketcan]: write to host failed (%s)", |
| 160 | strerror(errno)); |
| 161 | } else { |
| 162 | warn_report("[cansocketcan]: write to host truncated"); |
| 163 | } |
| 164 | return -1; |
| 165 | } |
| 166 | |
| 167 | return 1; |
| 168 | } |
| 169 | |
| 170 | static void can_host_socketcan_disconnect(CanHostState *ch) |
| 171 | { |
| 172 | CanHostSocketCAN *c = CAN_HOST_SOCKETCAN(ch); |
| 173 | |
| 174 | if (c->fd >= 0) { |
| 175 | qemu_set_fd_handler(c->fd, NULL, NULL, c); |
| 176 | close(c->fd); |
| 177 | c->fd = -1; |
| 178 | } |
| 179 | |
| 180 | g_free(c->rfilter); |
| 181 | c->rfilter = NULL; |
| 182 | c->rfilter_num = 0; |
| 183 | } |
| 184 | |
| 185 | static CanBusClientInfo can_host_socketcan_bus_client_info = { |
| 186 | .can_receive = can_host_socketcan_can_receive, |
| 187 | .receive = can_host_socketcan_receive, |
| 188 | }; |
| 189 | |
| 190 | static void can_host_socketcan_connect(CanHostState *ch, Error **errp) |
| 191 | { |
| 192 | CanHostSocketCAN *c = CAN_HOST_SOCKETCAN(ch); |
| 193 | int s; /* can raw socket */ |
Jan Charvat | d44948c | 2020-09-14 10:09:02 +0200 | [diff] [blame] | 194 | int mtu; |
| 195 | int enable_canfd = 1; |
Pavel Pisa | ea15ea8 | 2018-01-27 17:59:27 +0100 | [diff] [blame] | 196 | struct sockaddr_can addr; |
| 197 | struct ifreq ifr; |
| 198 | |
Eduardo Habkost | d9753cc | 2020-10-08 16:27:12 -0400 | [diff] [blame] | 199 | if (!c->ifname) { |
| 200 | error_setg(errp, "'if' property not set"); |
| 201 | return; |
| 202 | } |
| 203 | |
Pavel Pisa | ea15ea8 | 2018-01-27 17:59:27 +0100 | [diff] [blame] | 204 | /* open socket */ |
| 205 | s = qemu_socket(PF_CAN, SOCK_RAW, CAN_RAW); |
| 206 | if (s < 0) { |
| 207 | error_setg_errno(errp, errno, "failed to create CAN_RAW socket"); |
| 208 | return; |
| 209 | } |
| 210 | |
| 211 | addr.can_family = AF_CAN; |
| 212 | memset(&ifr.ifr_name, 0, sizeof(ifr.ifr_name)); |
| 213 | strcpy(ifr.ifr_name, c->ifname); |
Jan Charvat | d44948c | 2020-09-14 10:09:02 +0200 | [diff] [blame] | 214 | /* check if the frame fits into the CAN netdevice */ |
Pavel Pisa | ea15ea8 | 2018-01-27 17:59:27 +0100 | [diff] [blame] | 215 | if (ioctl(s, SIOCGIFINDEX, &ifr) < 0) { |
| 216 | error_setg_errno(errp, errno, |
Jan Charvat | d44948c | 2020-09-14 10:09:02 +0200 | [diff] [blame] | 217 | "SocketCAN host interface %s not available", |
| 218 | c->ifname); |
Pavel Pisa | ea15ea8 | 2018-01-27 17:59:27 +0100 | [diff] [blame] | 219 | goto fail; |
| 220 | } |
| 221 | addr.can_ifindex = ifr.ifr_ifindex; |
| 222 | |
Jan Charvat | d44948c | 2020-09-14 10:09:02 +0200 | [diff] [blame] | 223 | if (ioctl(s, SIOCGIFMTU, &ifr) < 0) { |
| 224 | error_setg_errno(errp, errno, |
| 225 | "SocketCAN host interface %s SIOCGIFMTU failed", |
| 226 | c->ifname); |
| 227 | goto fail; |
| 228 | } |
| 229 | mtu = ifr.ifr_mtu; |
| 230 | |
| 231 | if (mtu >= CANFD_MTU) { |
| 232 | /* interface is ok - try to switch the socket into CAN FD mode */ |
| 233 | if (setsockopt(s, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, |
| 234 | &enable_canfd, sizeof(enable_canfd))) { |
| 235 | warn_report("SocketCAN host interface %s enabling CAN FD failed", |
| 236 | c->ifname); |
| 237 | } else { |
| 238 | c->parent.bus_client.fd_mode = true; |
| 239 | } |
| 240 | } |
| 241 | |
Pavel Pisa | ea15ea8 | 2018-01-27 17:59:27 +0100 | [diff] [blame] | 242 | c->err_mask = 0xffffffff; /* Receive error frame. */ |
| 243 | setsockopt(s, SOL_CAN_RAW, CAN_RAW_ERR_FILTER, |
| 244 | &c->err_mask, sizeof(c->err_mask)); |
| 245 | |
| 246 | c->rfilter_num = 1; |
| 247 | c->rfilter = g_new(struct qemu_can_filter, c->rfilter_num); |
| 248 | |
| 249 | /* Receive all data frame. If |= CAN_INV_FILTER no data. */ |
| 250 | c->rfilter[0].can_id = 0; |
| 251 | c->rfilter[0].can_mask = 0; |
| 252 | c->rfilter[0].can_mask &= ~CAN_ERR_FLAG; |
| 253 | |
| 254 | setsockopt(s, SOL_CAN_RAW, CAN_RAW_FILTER, c->rfilter, |
| 255 | c->rfilter_num * sizeof(struct qemu_can_filter)); |
| 256 | |
| 257 | if (bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) { |
| 258 | error_setg_errno(errp, errno, "failed to bind to host interface %s", |
| 259 | c->ifname); |
| 260 | goto fail; |
| 261 | } |
| 262 | |
| 263 | c->fd = s; |
| 264 | ch->bus_client.info = &can_host_socketcan_bus_client_info; |
| 265 | qemu_set_fd_handler(c->fd, can_host_socketcan_read, NULL, c); |
| 266 | return; |
| 267 | |
| 268 | fail: |
| 269 | close(s); |
| 270 | g_free(c->rfilter); |
| 271 | c->rfilter = NULL; |
| 272 | c->rfilter_num = 0; |
| 273 | } |
| 274 | |
| 275 | static char *can_host_socketcan_get_if(Object *obj, Error **errp) |
| 276 | { |
| 277 | CanHostSocketCAN *c = CAN_HOST_SOCKETCAN(obj); |
| 278 | |
| 279 | return g_strdup(c->ifname); |
| 280 | } |
| 281 | |
Jan Charvat | d44948c | 2020-09-14 10:09:02 +0200 | [diff] [blame] | 282 | static void can_host_socketcan_set_if(Object *obj, const char *value, |
| 283 | Error **errp) |
Pavel Pisa | ea15ea8 | 2018-01-27 17:59:27 +0100 | [diff] [blame] | 284 | { |
| 285 | CanHostSocketCAN *c = CAN_HOST_SOCKETCAN(obj); |
| 286 | struct ifreq ifr; |
| 287 | |
| 288 | if (strlen(value) >= sizeof(ifr.ifr_name)) { |
| 289 | error_setg(errp, "CAN interface name longer than %zd characters", |
| 290 | sizeof(ifr.ifr_name) - 1); |
| 291 | return; |
| 292 | } |
| 293 | |
| 294 | if (c->fd != -1) { |
| 295 | error_setg(errp, "CAN interface already connected"); |
| 296 | return; |
| 297 | } |
| 298 | |
| 299 | g_free(c->ifname); |
| 300 | c->ifname = g_strdup(value); |
| 301 | } |
| 302 | |
| 303 | static void can_host_socketcan_instance_init(Object *obj) |
| 304 | { |
| 305 | CanHostSocketCAN *c = CAN_HOST_SOCKETCAN(obj); |
| 306 | |
| 307 | c->fd = -1; |
| 308 | } |
| 309 | |
| 310 | static void can_host_socketcan_class_init(ObjectClass *klass, |
| 311 | void *class_data G_GNUC_UNUSED) |
| 312 | { |
| 313 | CanHostClass *chc = CAN_HOST_CLASS(klass); |
| 314 | |
| 315 | object_class_property_add_str(klass, "if", |
| 316 | can_host_socketcan_get_if, |
Markus Armbruster | d262312 | 2020-05-05 17:29:22 +0200 | [diff] [blame] | 317 | can_host_socketcan_set_if); |
Pavel Pisa | ea15ea8 | 2018-01-27 17:59:27 +0100 | [diff] [blame] | 318 | chc->connect = can_host_socketcan_connect; |
| 319 | chc->disconnect = can_host_socketcan_disconnect; |
| 320 | } |
| 321 | |
| 322 | static const TypeInfo can_host_socketcan_info = { |
| 323 | .parent = TYPE_CAN_HOST, |
| 324 | .name = TYPE_CAN_HOST_SOCKETCAN, |
| 325 | .instance_size = sizeof(CanHostSocketCAN), |
| 326 | .instance_init = can_host_socketcan_instance_init, |
| 327 | .class_init = can_host_socketcan_class_init, |
| 328 | }; |
| 329 | |
| 330 | static void can_host_register_types(void) |
| 331 | { |
| 332 | type_register_static(&can_host_socketcan_info); |
| 333 | } |
| 334 | |
| 335 | type_init(can_host_register_types); |