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 #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 /*DEBUG_CAN*/
  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
  50 typedef 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 /* Check that QEMU and Linux kernel flags encoding and structure matches */
  66 QEMU_BUILD_BUG_ON(QEMU_CAN_EFF_FLAG != CAN_EFF_FLAG);
  67 QEMU_BUILD_BUG_ON(QEMU_CAN_RTR_FLAG != CAN_RTR_FLAG);
  68 QEMU_BUILD_BUG_ON(QEMU_CAN_ERR_FLAG != CAN_ERR_FLAG);
  69 QEMU_BUILD_BUG_ON(QEMU_CAN_INV_FILTER != CAN_INV_FILTER);
  70 QEMU_BUILD_BUG_ON(offsetof(qemu_can_frame, data)
  71                   != offsetof(struct can_frame, data));
  72
  73 static 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
  92 static void can_host_socketcan_read(void *opaque)
  93 {
  94     CanHostSocketCAN *c = opaque;
  95     CanHostState *ch = CAN_HOST(c);
  96
  97     /* CAN_READ_BUF_LEN for multiple messages syscall is possible for future */
  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
 111 static int can_host_socketcan_can_receive(CanBusClientState *client)
 112 {
 113     return 1;
 114 }
 115
 116 static 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
 149 static 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
 164 static CanBusClientInfo can_host_socketcan_bus_client_info = {
 165     .can_receive = can_host_socketcan_can_receive,
 166     .receive = can_host_socketcan_receive,
 167 };
 168
 169 static void can_host_socketcan_connect(CanHostState *ch, Error **errp)
 170 {
 171     CanHostSocketCAN *c = CAN_HOST_SOCKETCAN(ch);
 172     int s; /* can raw socket */
 173     struct sockaddr_can addr;
 174     struct ifreq ifr;
 175
 176     /* open socket */
 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; /* Receive error frame. */
 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     /* Receive all data frame. If |= CAN_INV_FILTER no data. */
 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
 219 fail:
 220     close(s);
 221     g_free(c->rfilter);
 222     c->rfilter = NULL;
 223     c->rfilter_num = 0;
 224 }
 225
 226 static 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
 233 static 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
 253 static void can_host_socketcan_instance_init(Object *obj)
 254 {
 255     CanHostSocketCAN *c = CAN_HOST_SOCKETCAN(obj);
 256
 257     c->fd = -1;
 258 }
 259
 260 static 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
 273 static 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
 281 static void can_host_register_types(void)
 282 {
 283     type_register_static(&can_host_socketcan_info);
 284 }
 285
 286 type_init(can_host_register_types);