net/can/can_socketcan.c

   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"
  50 OBJECT_DECLARE_SIMPLE_TYPE(CanHostSocketCAN, CAN_HOST_SOCKETCAN)
  51
  52 #define CAN_READ_BUF_LEN  5
  53 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 };
  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     FILE *logfile = qemu_log_trylock();
  80
  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");
  92         qemu_log_unlock(logfile);
  93     }
  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
 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
 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
 123 static bool can_host_socketcan_can_receive(CanBusClientState *client)
 124 {
 125     return true;
 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
 134     size_t len;
 135     int res;
 136
 137     if (c->fd < 0) {
 138         return -1;
 139     }
 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     }
 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 */
 194     int mtu;
 195     int enable_canfd = 1;
 196     struct sockaddr_can addr;
 197     struct ifreq ifr;
 198
 199     if (!c->ifname) {
 200         error_setg(errp, "'if' property not set");
 201         return;
 202     }
 203
 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);
 214     /* check if the frame fits into the CAN netdevice */
 215     if (ioctl(s, SIOCGIFINDEX, &ifr) < 0) {
 216         error_setg_errno(errp, errno,
 217                          "SocketCAN host interface %s not available",
 218                          c->ifname);
 219         goto fail;
 220     }
 221     addr.can_ifindex = ifr.ifr_ifindex;
 222
 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
 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
 282 static void can_host_socketcan_set_if(Object *obj, const char *value,
 283                                       Error **errp)
 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,
 317                                   can_host_socketcan_set_if);
 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);