1 files changed, 44 insertions, 3 deletions

diff --git a/net/can/can_socketcan.c b/net/can/can_socketcan.c
index ce8c2549ed..92b1f79385 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;