Import QEMU upstream snapshot d2e570c

Upstream: https://gitlab.com/qemu-project/qemu.git

Upstream-Commit: d2e570cc0f97b936902a5b1b86b73c0f5998b475

net/can/can_core.c

@@ -0,0 +1,178 @@
+/*
+ * CAN common CAN bus emulation support
+ *
+ * Copyright (c) 2013-2014 Jin Yang
+ * Copyright (c) 2014-2018 Pavel Pisa
+ *
+ * Initial development supported by Google GSoC 2013 from RTEMS project slot
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include "qemu/osdep.h"
+#include "chardev/char.h"
+#include "qemu/module.h"
+#include "qemu/sockets.h"
+#include "qapi/error.h"
+#include "net/can_emu.h"
+#include "qom/object_interfaces.h"
+
+/* CAN DLC to real data length conversion helpers */
+
+static const uint8_t dlc2len[] = {
+    0, 1, 2, 3, 4, 5, 6, 7,
+    8, 12, 16, 20, 24, 32, 48, 64
+};
+
+/* get data length from can_dlc with sanitized can_dlc */
+uint8_t can_dlc2len(uint8_t can_dlc)
+{
+    return dlc2len[can_dlc & 0x0F];
+}
+
+static const uint8_t len2dlc[] = {
+    0, 1, 2, 3, 4, 5, 6, 7, 8,                              /* 0 - 8 */
+    9, 9, 9, 9,                                             /* 9 - 12 */
+    10, 10, 10, 10,                                         /* 13 - 16 */
+    11, 11, 11, 11,                                         /* 17 - 20 */
+    12, 12, 12, 12,                                         /* 21 - 24 */
+    13, 13, 13, 13, 13, 13, 13, 13,                         /* 25 - 32 */
+    14, 14, 14, 14, 14, 14, 14, 14,                         /* 33 - 40 */
+    14, 14, 14, 14, 14, 14, 14, 14,                         /* 41 - 48 */
+    15, 15, 15, 15, 15, 15, 15, 15,                         /* 49 - 56 */
+    15, 15, 15, 15, 15, 15, 15, 15                          /* 57 - 64 */
+};
+
+/* map the sanitized data length to an appropriate data length code */
+uint8_t can_len2dlc(uint8_t len)
+{
+    if (unlikely(len > 64)) {
+        return 0xF;
+    }
+
+    return len2dlc[len];
+}
+
+struct CanBusState {
+    Object object;
+
+    QTAILQ_HEAD(, CanBusClientState) clients;
+};
+
+static void can_bus_instance_init(Object *object)
+{
+    CanBusState *bus = (CanBusState *)object;
+
+    QTAILQ_INIT(&bus->clients);
+}
+
+int can_bus_insert_client(CanBusState *bus, CanBusClientState *client)
+{
+    client->bus = bus;
+    QTAILQ_INSERT_TAIL(&bus->clients, client, next);
+    return 0;
+}
+
+int can_bus_remove_client(CanBusClientState *client)
+{
+    CanBusState *bus = client->bus;
+    if (bus == NULL) {
+        return 0;
+    }
+
+    QTAILQ_REMOVE(&bus->clients, client, next);
+    client->bus = NULL;
+    return 1;
+}
+
+ssize_t can_bus_client_send(CanBusClientState *client,
+             const struct qemu_can_frame *frames, size_t frames_cnt)
+{
+    int ret = 0;
+    CanBusState *bus = client->bus;
+    CanBusClientState *peer;
+    if (bus == NULL) {
+        return -1;
+    }
+
+    QTAILQ_FOREACH(peer, &bus->clients, next) {
+        if (peer->info->can_receive(peer)) {
+            if (peer == client) {
+                /* No loopback support for now */
+                continue;
+            }
+            if (peer->info->receive(peer, frames, frames_cnt) > 0) {
+                ret = 1;
+            }
+        }
+    }
+
+    return ret;
+}
+
+int can_bus_filter_match(struct qemu_can_filter *filter, qemu_canid_t can_id)
+{
+    int m;
+    if (((can_id | filter->can_mask) & QEMU_CAN_ERR_FLAG)) {
+        return (filter->can_mask & QEMU_CAN_ERR_FLAG) != 0;
+    }
+    m = (can_id & filter->can_mask) == (filter->can_id & filter->can_mask);
+    return filter->can_id & QEMU_CAN_INV_FILTER ? !m : m;
+}
+
+int can_bus_client_set_filters(CanBusClientState *client,
+             const struct qemu_can_filter *filters, size_t filters_cnt)
+{
+    return 0;
+}
+
+
+static bool can_bus_prepare_delete(UserCreatable *uc, Error **errp)
+{
+    error_setg(errp, "Deleting CAN bus device '%s' is not supported",
+               object_get_canonical_path_component(OBJECT(uc)));
+    return false;
+}
+
+static void can_bus_class_init(ObjectClass *klass,
+                               const void *class_data G_GNUC_UNUSED)
+{
+    UserCreatableClass *uc_klass = USER_CREATABLE_CLASS(klass);
+
+    uc_klass->prepare_delete = can_bus_prepare_delete;
+}
+
+static const TypeInfo can_bus_info = {
+    .parent = TYPE_OBJECT,
+    .name = TYPE_CAN_BUS,
+    .instance_size = sizeof(CanBusState),
+    .instance_init = can_bus_instance_init,
+    .class_init = can_bus_class_init,
+    .interfaces = (const InterfaceInfo[]) {
+        { TYPE_USER_CREATABLE },
+        { }
+    }
+};
+
+static void can_bus_register_types(void)
+{
+    type_register_static(&can_bus_info);
+}
+
+type_init(can_bus_register_types);

net/can/can_host.c

@@ -0,0 +1,106 @@
+/*
+ * CAN generic CAN host connection support
+ *
+ * Copyright (c) 2013-2014 Jin Yang
+ * Copyright (c) 2014-2018 Pavel Pisa
+ *
+ * Initial development supported by Google GSoC 2013 from RTEMS project slot
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include "qemu/osdep.h"
+#include "chardev/char.h"
+#include "qemu/module.h"
+#include "qemu/sockets.h"
+#include "qapi/error.h"
+#include "qom/object_interfaces.h"
+#include "net/can_emu.h"
+#include "net/can_host.h"
+
+static void can_host_disconnect(CanHostState *ch)
+{
+    CanHostClass *chc = CAN_HOST_GET_CLASS(ch);
+
+    can_bus_remove_client(&ch->bus_client);
+    chc->disconnect(ch);
+}
+
+static void can_host_connect(CanHostState *ch, Error **errp)
+{
+    CanHostClass *chc = CAN_HOST_GET_CLASS(ch);
+    Error *local_err = NULL;
+
+    if (ch->bus == NULL) {
+        error_setg(errp, "'canbus' property not set");
+        return;
+    }
+
+    chc->connect(ch, &local_err);
+    if (local_err) {
+        error_propagate(errp, local_err);
+        return;
+    }
+
+    can_bus_insert_client(ch->bus, &ch->bus_client);
+}
+
+static void can_host_unparent(Object *obj)
+{
+    can_host_disconnect(CAN_HOST(obj));
+}
+
+static void can_host_complete(UserCreatable *uc, Error **errp)
+{
+    can_host_connect(CAN_HOST(uc), errp);
+}
+
+static void can_host_class_init(ObjectClass *klass,
+                                const void *class_data G_GNUC_UNUSED)
+{
+    UserCreatableClass *uc_klass = USER_CREATABLE_CLASS(klass);
+
+    object_class_property_add_link(klass, "canbus", TYPE_CAN_BUS,
+                                   offsetof(CanHostState, bus),
+                                   object_property_allow_set_link,
+                                   OBJ_PROP_LINK_STRONG);
+
+    klass->unparent = can_host_unparent;
+    uc_klass->complete = can_host_complete;
+}
+
+static const TypeInfo can_host_info = {
+    .parent = TYPE_OBJECT,
+    .name = TYPE_CAN_HOST,
+    .instance_size = sizeof(CanHostState),
+    .class_size = sizeof(CanHostClass),
+    .abstract = true,
+    .class_init = can_host_class_init,
+    .interfaces = (const InterfaceInfo[]) {
+        { TYPE_USER_CREATABLE },
+        { }
+    }
+};
+
+static void can_host_register_types(void)
+{
+    type_register_static(&can_host_info);
+}
+
+type_init(can_host_register_types);

net/can/can_socketcan.c

@@ -0,0 +1,343 @@
+/*
+ * CAN c support to connect to the Linux host SocketCAN interfaces
+ *
+ * Copyright (c) 2013-2014 Jin Yang
+ * Copyright (c) 2014-2018 Pavel Pisa
+ *
+ * Initial development supported by Google GSoC 2013 from RTEMS project slot
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include "qemu/osdep.h"
+#include "qemu/log.h"
+#include "qemu/main-loop.h"
+#include "qemu/module.h"
+#include "qapi/error.h"
+#include "chardev/char.h"
+#include "qemu/sockets.h"
+#include "qemu/error-report.h"
+#include "net/can_emu.h"
+#include "net/can_host.h"
+
+#include <sys/ioctl.h>
+#include <net/if.h>
+#include <linux/can.h>
+#include <linux/can/raw.h>
+#include "qom/object.h"
+
+#ifndef DEBUG_CAN
+#define DEBUG_CAN 0
+#endif /*DEBUG_CAN*/
+
+#define TYPE_CAN_HOST_SOCKETCAN "can-host-socketcan"
+OBJECT_DECLARE_SIMPLE_TYPE(CanHostSocketCAN, CAN_HOST_SOCKETCAN)
+
+#define CAN_READ_BUF_LEN  5
+struct CanHostSocketCAN {
+    CanHostState       parent;
+    char               *ifname;
+
+    qemu_can_filter    *rfilter;
+    int                rfilter_num;
+    can_err_mask_t     err_mask;
+
+    qemu_can_frame     buf[CAN_READ_BUF_LEN];
+    int                bufcnt;
+    int                bufptr;
+
+    int                fd;
+};
+
+/* Check that QEMU and Linux kernel flags encoding and structure matches */
+QEMU_BUILD_BUG_ON(QEMU_CAN_EFF_FLAG != CAN_EFF_FLAG);
+QEMU_BUILD_BUG_ON(QEMU_CAN_RTR_FLAG != CAN_RTR_FLAG);
+QEMU_BUILD_BUG_ON(QEMU_CAN_ERR_FLAG != CAN_ERR_FLAG);
+QEMU_BUILD_BUG_ON(QEMU_CAN_INV_FILTER != CAN_INV_FILTER);
+QEMU_BUILD_BUG_ON(offsetof(qemu_can_frame, data)
+                  != offsetof(struct can_frame, data));
+
+static void can_host_socketcan_display_msg(struct qemu_can_frame *msg)
+{
+    int i;
+    FILE *logfile = qemu_log_trylock();
+
+    if (logfile) {
+        fprintf(logfile, "[cansocketcan]: %03X [%01d] %s %s",
+                msg->can_id & QEMU_CAN_EFF_MASK,
+                msg->can_dlc,
+                msg->can_id & QEMU_CAN_EFF_FLAG ? "EFF" : "SFF",
+                msg->can_id & QEMU_CAN_RTR_FLAG ? "RTR" : "DAT");
+
+        for (i = 0; i < msg->can_dlc; i++) {
+            fprintf(logfile, " %02X", msg->data[i]);
+        }
+        fprintf(logfile, "\n");
+        qemu_log_unlock(logfile);
+    }
+}
+
+static void can_host_socketcan_read(void *opaque)
+{
+    CanHostSocketCAN *c = opaque;
+    CanHostState *ch = CAN_HOST(c);
+
+    /* CAN_READ_BUF_LEN for multiple messages syscall is possible for future */
+    c->bufcnt = read(c->fd, c->buf, sizeof(qemu_can_frame));
+    if (c->bufcnt < 0) {
+        warn_report("CAN bus host read failed (%s)", strerror(errno));
+        return;
+    }
+
+    if (!ch->bus_client.fd_mode) {
+        c->buf[0].flags = 0;
+    } else {
+        if (c->bufcnt > CAN_MTU) {
+            c->buf[0].flags |= QEMU_CAN_FRMF_TYPE_FD;
+        }
+    }
+
+    can_bus_client_send(&ch->bus_client, c->buf, 1);
+
+    if (DEBUG_CAN) {
+        can_host_socketcan_display_msg(c->buf);
+    }
+}
+
+static bool can_host_socketcan_can_receive(CanBusClientState *client)
+{
+    return true;
+}
+
+static ssize_t can_host_socketcan_receive(CanBusClientState *client,
+                            const qemu_can_frame *frames, size_t frames_cnt)
+{
+    CanHostState *ch = container_of(client, CanHostState, bus_client);
+    CanHostSocketCAN *c = CAN_HOST_SOCKETCAN(ch);
+
+    size_t len;
+    int res;
+
+    if (c->fd < 0) {
+        return -1;
+    }
+    if (frames->flags & QEMU_CAN_FRMF_TYPE_FD) {
+        if (!ch->bus_client.fd_mode) {
+            return 0;
+        }
+        len = CANFD_MTU;
+    } else {
+        len = CAN_MTU;
+
+    }
+
+    res = write(c->fd, frames, len);
+
+    if (!res) {
+        warn_report("[cansocketcan]: write message to host returns zero");
+        return -1;
+    }
+
+    if (res != len) {
+        if (res < 0) {
+            warn_report("[cansocketcan]: write to host failed (%s)",
+                        strerror(errno));
+        } else {
+            warn_report("[cansocketcan]: write to host truncated");
+        }
+        return -1;
+    }
+
+    return 1;
+}
+
+static void can_host_socketcan_disconnect(CanHostState *ch)
+{
+    CanHostSocketCAN *c = CAN_HOST_SOCKETCAN(ch);
+
+    if (c->fd >= 0) {
+        qemu_set_fd_handler(c->fd, NULL, NULL, c);
+        close(c->fd);
+        c->fd = -1;
+    }
+
+    g_free(c->rfilter);
+    c->rfilter = NULL;
+    c->rfilter_num = 0;
+}
+
+static CanBusClientInfo can_host_socketcan_bus_client_info = {
+    .can_receive = can_host_socketcan_can_receive,
+    .receive = can_host_socketcan_receive,
+};
+
+static void can_host_socketcan_connect(CanHostState *ch, Error **errp)
+{
+    CanHostSocketCAN *c = CAN_HOST_SOCKETCAN(ch);
+    int s; /* can raw socket */
+    int mtu;
+    int enable_canfd = 1;
+    struct sockaddr_can addr;
+    struct ifreq ifr;
+
+    if (!c->ifname) {
+        error_setg(errp, "'if' property not set");
+        return;
+    }
+
+    /* open socket */
+    s = qemu_socket(PF_CAN, SOCK_RAW, CAN_RAW);
+    if (s < 0) {
+        error_setg_errno(errp, errno, "failed to create CAN_RAW socket");
+        return;
+    }
+
+    addr.can_family = AF_CAN;
+    memset(&ifr.ifr_name, 0, sizeof(ifr.ifr_name));
+    strcpy(ifr.ifr_name, c->ifname);
+    /* check if the frame fits into the CAN netdevice */
+    if (ioctl(s, SIOCGIFINDEX, &ifr) < 0) {
+        error_setg_errno(errp, errno,
+                         "SocketCAN host interface %s not available",
+                         c->ifname);
+        goto fail;
+    }
+    addr.can_ifindex = ifr.ifr_ifindex;
+
+    if (ioctl(s, SIOCGIFMTU, &ifr) < 0) {
+        error_setg_errno(errp, errno,
+                         "SocketCAN host interface %s SIOCGIFMTU failed",
+                         c->ifname);
+        goto fail;
+    }
+    mtu = ifr.ifr_mtu;
+
+    if (mtu >= CANFD_MTU) {
+        /* interface is ok - try to switch the socket into CAN FD mode */
+        if (setsockopt(s, SOL_CAN_RAW, CAN_RAW_FD_FRAMES,
+                        &enable_canfd, sizeof(enable_canfd))) {
+            warn_report("SocketCAN host interface %s enabling CAN FD failed",
+                        c->ifname);
+        } else {
+            c->parent.bus_client.fd_mode = true;
+        }
+    }
+
+    c->err_mask = 0xffffffff; /* Receive error frame. */
+    setsockopt(s, SOL_CAN_RAW, CAN_RAW_ERR_FILTER,
+                   &c->err_mask, sizeof(c->err_mask));
+
+    c->rfilter_num = 1;
+    c->rfilter = g_new(struct qemu_can_filter, c->rfilter_num);
+
+    /* Receive all data frame. If |= CAN_INV_FILTER no data. */
+    c->rfilter[0].can_id = 0;
+    c->rfilter[0].can_mask = 0;
+    c->rfilter[0].can_mask &= ~CAN_ERR_FLAG;
+
+    setsockopt(s, SOL_CAN_RAW, CAN_RAW_FILTER, c->rfilter,
+               c->rfilter_num * sizeof(struct qemu_can_filter));
+
+    if (bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
+        error_setg_errno(errp, errno, "failed to bind to host interface %s",
+                         c->ifname);
+        goto fail;
+    }
+
+    c->fd = s;
+    ch->bus_client.info = &can_host_socketcan_bus_client_info;
+    qemu_set_fd_handler(c->fd, can_host_socketcan_read, NULL, c);
+    return;
+
+fail:
+    close(s);
+    g_free(c->rfilter);
+    c->rfilter = NULL;
+    c->rfilter_num = 0;
+}
+
+static char *can_host_socketcan_get_if(Object *obj, Error **errp)
+{
+    CanHostSocketCAN *c = CAN_HOST_SOCKETCAN(obj);
+
+    return g_strdup(c->ifname);
+}
+
+static void can_host_socketcan_set_if(Object *obj, const char *value,
+                                      Error **errp)
+{
+    CanHostSocketCAN *c = CAN_HOST_SOCKETCAN(obj);
+    struct ifreq ifr;
+
+    if (strlen(value) >= sizeof(ifr.ifr_name)) {
+        error_setg(errp, "CAN interface name longer than %zd characters",
+                   sizeof(ifr.ifr_name) - 1);
+        return;
+    }
+
+    if (c->fd != -1) {
+        error_setg(errp, "CAN interface already connected");
+        return;
+    }
+
+    g_free(c->ifname);
+    c->ifname = g_strdup(value);
+}
+
+static void can_host_socketcan_instance_init(Object *obj)
+{
+    CanHostSocketCAN *c = CAN_HOST_SOCKETCAN(obj);
+
+    c->fd = -1;
+}
+
+static void can_host_socketcan_class_init(ObjectClass *klass,
+                                          const void *class_data G_GNUC_UNUSED)
+{
+    CanHostClass *chc = CAN_HOST_CLASS(klass);
+
+    object_class_property_add_str(klass, "if",
+                                  can_host_socketcan_get_if,
+                                  can_host_socketcan_set_if);
+    chc->connect = can_host_socketcan_connect;
+    chc->disconnect = can_host_socketcan_disconnect;
+}
+
+static void can_host_socketcan_finalize(Object *obj)
+{
+    CanHostSocketCAN *c = CAN_HOST_SOCKETCAN(obj);
+
+    g_free(c->ifname);
+}
+
+static const TypeInfo can_host_socketcan_info = {
+    .parent = TYPE_CAN_HOST,
+    .name = TYPE_CAN_HOST_SOCKETCAN,
+    .instance_size = sizeof(CanHostSocketCAN),
+    .instance_init = can_host_socketcan_instance_init,
+    .instance_finalize = can_host_socketcan_finalize,
+    .class_init = can_host_socketcan_class_init,
+};
+
+static void can_host_register_types(void)
+{
+    type_register_static(&can_host_socketcan_info);
+}
+
+type_init(can_host_register_types);

net/can/meson.build

@@ -0,0 +1,7 @@
+can_ss = ss.source_set()
+can_ss.add(files('can_core.c', 'can_host.c'))
+if host_os == 'linux'
+  can_ss.add(files('can_socketcan.c'))
+endif
+
+system_ss.add_all(when: 'CONFIG_CAN_BUS', if_true: can_ss)