[PATCH v2 1/7] net/can: Initial host SocketCan support for CAN FD.

[Pavel Pisa]

From: Jan Charvat <charvj10@fel.cvut.cz>

Signed-off-by: Jan Charvat <charvj10@fel.cvut.cz>
Signed-off-by: Pavel Pisa <pisa@cmp.felk.cvut.cz>
Reviewed-by: Vikram Garhwal <fnu.vikram@xilinx.com>
---
 hw/net/can/can_sja1000.c |  2 ++
 include/net/can_emu.h    |  8 ++++++-
 net/can/can_socketcan.c  | 47 +++++++++++++++++++++++++++++++++++++---
 3 files changed, 53 insertions(+), 4 deletions(-)

diff --git a/hw/net/can/can_sja1000.c b/hw/net/can/can_sja1000.c
index 299932998a..ec66d4232d 100644
--- a/hw/net/can/can_sja1000.c
+++ b/hw/net/can/can_sja1000.c
@@ -268,6 +268,7 @@ static void buff2frame_pel(const uint8_t *buff, 
qemu_can_frame *frame)
 {
     uint8_t i;
 
+    frame->flags = 0;
     frame->can_id = 0;
     if (buff[0] & 0x40) { /* RTR */
         frame->can_id = QEMU_CAN_RTR_FLAG;
@@ -303,6 +304,7 @@ static void buff2frame_bas(const uint8_t *buff, 
qemu_can_frame *frame)
 {
     uint8_t i;
 
+    frame->flags = 0;
     frame->can_id = ((buff[0] << 3) & (0xff << 3)) + ((buff[1] >> 5) & 0x07);
     if (buff[1] & 0x10) { /* RTR */
         frame->can_id = QEMU_CAN_RTR_FLAG;
diff --git a/include/net/can_emu.h b/include/net/can_emu.h
index 7e90fd8a45..1547091ff4 100644
--- a/include/net/can_emu.h
+++ b/include/net/can_emu.h
@@ -46,7 +46,8 @@ typedef uint32_t qemu_canid_t;
 typedef struct qemu_can_frame {
     qemu_canid_t    can_id;  /* 32 bit CAN_ID + EFF/RTR/ERR flags */
     uint8_t         can_dlc; /* data length code: 0 .. 8 */
-    uint8_t         data[8] QEMU_ALIGNED(8);
+    uint8_t         flags;
+    uint8_t         data[64] QEMU_ALIGNED(8);
 } qemu_can_frame;
 
 /* Keep defines for QEMU separate from Linux ones for now */
@@ -58,6 +59,10 @@ typedef struct qemu_can_frame {
 #define QEMU_CAN_SFF_MASK 0x000007FFU /* standard frame format (SFF) */
 #define QEMU_CAN_EFF_MASK 0x1FFFFFFFU /* extended frame format (EFF) */
 
+#define QEMU_CAN_FRMF_BRS     0x01 /* bit rate switch (2nd bitrate for data) */
+#define QEMU_CAN_FRMF_ESI     0x02 /* error state ind. of transmitting node */
+#define QEMU_CAN_FRMF_TYPE_FD 0x10 /* internal bit ind. of CAN FD frame */
+
 /**
  * struct qemu_can_filter - CAN ID based filter in can_register().
  * @can_id:   relevant bits of CAN ID which are not masked out.
@@ -97,6 +102,7 @@ struct CanBusClientState {
     char *model;
     char *name;
     void (*destructor)(CanBusClientState *);
+    bool fd_mode;
 };
 
 #define TYPE_CAN_BUS "can-bus"
diff --git a/net/can/can_socketcan.c b/net/can/can_socketcan.c
index b7ef63ec0e..fbc0b62ea4 100644
--- a/net/can/can_socketcan.c
+++ b/net/can/can_socketcan.c
@@ -103,6 +103,14 @@ static void can_host_socketcan_read(void *opaque)
         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) {
@@ -121,12 +129,21 @@ static ssize_t 
can_host_socketcan_receive(CanBusClientState *client,
     CanHostState *ch = container_of(client, CanHostState, bus_client);
     CanHostSocketCAN *c = CAN_HOST_SOCKETCAN(ch);
 
-    size_t len = sizeof(qemu_can_frame);
+    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);
 
@@ -172,6 +189,8 @@ 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;
 
@@ -185,13 +204,34 @@ static void can_host_socketcan_connect(CanHostState *ch, 
Error **errp)
     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);
+                         "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));
@@ -232,7 +272,8 @@ static char *can_host_socketcan_get_if(Object *obj, Error 
**errp)
     return g_strdup(c->ifname);
 }
 
-static void can_host_socketcan_set_if(Object *obj, const char *value, Error 
**errp)
+static void can_host_socketcan_set_if(Object *obj, const char *value,
+                                      Error **errp)
 {
     CanHostSocketCAN *c = CAN_HOST_SOCKETCAN(obj);
     struct ifreq ifr;
-- 
2.20.1