can_socketcan.c (9732B)
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#include "qom/object.h" 44 45#ifndef DEBUG_CAN 46#define DEBUG_CAN 0 47#endif /*DEBUG_CAN*/ 48 49#define TYPE_CAN_HOST_SOCKETCAN "can-host-socketcan" 50OBJECT_DECLARE_SIMPLE_TYPE(CanHostSocketCAN, CAN_HOST_SOCKETCAN) 51 52#define CAN_READ_BUF_LEN 5 53struct 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}; 67 68/* Check that QEMU and Linux kernel flags encoding and structure matches */ 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 FILE *logfile = 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(logfile); 92} 93 94static void can_host_socketcan_read(void *opaque) 95{ 96 CanHostSocketCAN *c = opaque; 97 CanHostState *ch = CAN_HOST(c); 98 99 /* CAN_READ_BUF_LEN for multiple messages syscall is possible for future */ 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 if (!ch->bus_client.fd_mode) { 107 c->buf[0].flags = 0; 108 } else { 109 if (c->bufcnt > CAN_MTU) { 110 c->buf[0].flags |= QEMU_CAN_FRMF_TYPE_FD; 111 } 112 } 113 114 can_bus_client_send(&ch->bus_client, c->buf, 1); 115 116 if (DEBUG_CAN) { 117 can_host_socketcan_display_msg(c->buf); 118 } 119} 120 121static bool can_host_socketcan_can_receive(CanBusClientState *client) 122{ 123 return true; 124} 125 126static ssize_t can_host_socketcan_receive(CanBusClientState *client, 127 const qemu_can_frame *frames, size_t frames_cnt) 128{ 129 CanHostState *ch = container_of(client, CanHostState, bus_client); 130 CanHostSocketCAN *c = CAN_HOST_SOCKETCAN(ch); 131 132 size_t len; 133 int res; 134 135 if (c->fd < 0) { 136 return -1; 137 } 138 if (frames->flags & QEMU_CAN_FRMF_TYPE_FD) { 139 if (!ch->bus_client.fd_mode) { 140 return 0; 141 } 142 len = CANFD_MTU; 143 } else { 144 len = CAN_MTU; 145 146 } 147 148 res = write(c->fd, frames, len); 149 150 if (!res) { 151 warn_report("[cansocketcan]: write message to host returns zero"); 152 return -1; 153 } 154 155 if (res != len) { 156 if (res < 0) { 157 warn_report("[cansocketcan]: write to host failed (%s)", 158 strerror(errno)); 159 } else { 160 warn_report("[cansocketcan]: write to host truncated"); 161 } 162 return -1; 163 } 164 165 return 1; 166} 167 168static void can_host_socketcan_disconnect(CanHostState *ch) 169{ 170 CanHostSocketCAN *c = CAN_HOST_SOCKETCAN(ch); 171 172 if (c->fd >= 0) { 173 qemu_set_fd_handler(c->fd, NULL, NULL, c); 174 close(c->fd); 175 c->fd = -1; 176 } 177 178 g_free(c->rfilter); 179 c->rfilter = NULL; 180 c->rfilter_num = 0; 181} 182 183static CanBusClientInfo can_host_socketcan_bus_client_info = { 184 .can_receive = can_host_socketcan_can_receive, 185 .receive = can_host_socketcan_receive, 186}; 187 188static void can_host_socketcan_connect(CanHostState *ch, Error **errp) 189{ 190 CanHostSocketCAN *c = CAN_HOST_SOCKETCAN(ch); 191 int s; /* can raw socket */ 192 int mtu; 193 int enable_canfd = 1; 194 struct sockaddr_can addr; 195 struct ifreq ifr; 196 197 if (!c->ifname) { 198 error_setg(errp, "'if' property not set"); 199 return; 200 } 201 202 /* open socket */ 203 s = qemu_socket(PF_CAN, SOCK_RAW, CAN_RAW); 204 if (s < 0) { 205 error_setg_errno(errp, errno, "failed to create CAN_RAW socket"); 206 return; 207 } 208 209 addr.can_family = AF_CAN; 210 memset(&ifr.ifr_name, 0, sizeof(ifr.ifr_name)); 211 strcpy(ifr.ifr_name, c->ifname); 212 /* check if the frame fits into the CAN netdevice */ 213 if (ioctl(s, SIOCGIFINDEX, &ifr) < 0) { 214 error_setg_errno(errp, errno, 215 "SocketCAN host interface %s not available", 216 c->ifname); 217 goto fail; 218 } 219 addr.can_ifindex = ifr.ifr_ifindex; 220 221 if (ioctl(s, SIOCGIFMTU, &ifr) < 0) { 222 error_setg_errno(errp, errno, 223 "SocketCAN host interface %s SIOCGIFMTU failed", 224 c->ifname); 225 goto fail; 226 } 227 mtu = ifr.ifr_mtu; 228 229 if (mtu >= CANFD_MTU) { 230 /* interface is ok - try to switch the socket into CAN FD mode */ 231 if (setsockopt(s, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, 232 &enable_canfd, sizeof(enable_canfd))) { 233 warn_report("SocketCAN host interface %s enabling CAN FD failed", 234 c->ifname); 235 } else { 236 c->parent.bus_client.fd_mode = true; 237 } 238 } 239 240 c->err_mask = 0xffffffff; /* Receive error frame. */ 241 setsockopt(s, SOL_CAN_RAW, CAN_RAW_ERR_FILTER, 242 &c->err_mask, sizeof(c->err_mask)); 243 244 c->rfilter_num = 1; 245 c->rfilter = g_new(struct qemu_can_filter, c->rfilter_num); 246 247 /* Receive all data frame. If |= CAN_INV_FILTER no data. */ 248 c->rfilter[0].can_id = 0; 249 c->rfilter[0].can_mask = 0; 250 c->rfilter[0].can_mask &= ~CAN_ERR_FLAG; 251 252 setsockopt(s, SOL_CAN_RAW, CAN_RAW_FILTER, c->rfilter, 253 c->rfilter_num * sizeof(struct qemu_can_filter)); 254 255 if (bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) { 256 error_setg_errno(errp, errno, "failed to bind to host interface %s", 257 c->ifname); 258 goto fail; 259 } 260 261 c->fd = s; 262 ch->bus_client.info = &can_host_socketcan_bus_client_info; 263 qemu_set_fd_handler(c->fd, can_host_socketcan_read, NULL, c); 264 return; 265 266fail: 267 close(s); 268 g_free(c->rfilter); 269 c->rfilter = NULL; 270 c->rfilter_num = 0; 271} 272 273static char *can_host_socketcan_get_if(Object *obj, Error **errp) 274{ 275 CanHostSocketCAN *c = CAN_HOST_SOCKETCAN(obj); 276 277 return g_strdup(c->ifname); 278} 279 280static void can_host_socketcan_set_if(Object *obj, const char *value, 281 Error **errp) 282{ 283 CanHostSocketCAN *c = CAN_HOST_SOCKETCAN(obj); 284 struct ifreq ifr; 285 286 if (strlen(value) >= sizeof(ifr.ifr_name)) { 287 error_setg(errp, "CAN interface name longer than %zd characters", 288 sizeof(ifr.ifr_name) - 1); 289 return; 290 } 291 292 if (c->fd != -1) { 293 error_setg(errp, "CAN interface already connected"); 294 return; 295 } 296 297 g_free(c->ifname); 298 c->ifname = g_strdup(value); 299} 300 301static void can_host_socketcan_instance_init(Object *obj) 302{ 303 CanHostSocketCAN *c = CAN_HOST_SOCKETCAN(obj); 304 305 c->fd = -1; 306} 307 308static void can_host_socketcan_class_init(ObjectClass *klass, 309 void *class_data G_GNUC_UNUSED) 310{ 311 CanHostClass *chc = CAN_HOST_CLASS(klass); 312 313 object_class_property_add_str(klass, "if", 314 can_host_socketcan_get_if, 315 can_host_socketcan_set_if); 316 chc->connect = can_host_socketcan_connect; 317 chc->disconnect = can_host_socketcan_disconnect; 318} 319 320static const TypeInfo can_host_socketcan_info = { 321 .parent = TYPE_CAN_HOST, 322 .name = TYPE_CAN_HOST_SOCKETCAN, 323 .instance_size = sizeof(CanHostSocketCAN), 324 .instance_init = can_host_socketcan_instance_init, 325 .class_init = can_host_socketcan_class_init, 326}; 327 328static void can_host_register_types(void) 329{ 330 type_register_static(&can_host_socketcan_info); 331} 332 333type_init(can_host_register_types);