[PATCH v8] can: usb: etas_es58X: add support for ETAS ES58X CAN USB interfaces

@@ -20,6 +20,16 @@ config CAN_ESD_USB2
       This driver supports the CAN-USB/2 interface
       from esd electronic system design gmbh (http://www.esd.eu).

+config CAN_ETAS_ES58X
+    tristate "ETAS ES58X CAN/USB interfaces"
+    select CRC16
+    help
+      This driver supports the ES581.4, ES582.1 and ES584.1 interfaces
+      from ETAS GmbH (https://www.etas.com/en/products/es58x.php).
+
+      To compile this driver as a module, choose M here: the module
+      will be called etas_es58x.
+
 config CAN_GS_USB
     tristate "Geschwister Schneider UG interfaces"
     help

@@ -6,6 +6,7 @@
 obj-$(CONFIG_CAN_8DEV_USB) += usb_8dev.o
 obj-$(CONFIG_CAN_EMS_USB) += ems_usb.o
 obj-$(CONFIG_CAN_ESD_USB2) += esd_usb2.o
+obj-$(CONFIG_CAN_ETAS_ES58X) += etas_es58x/
 obj-$(CONFIG_CAN_GS_USB) += gs_usb.o
 obj-$(CONFIG_CAN_KVASER_USB) += kvaser_usb/
 obj-$(CONFIG_CAN_MCBA_USB) += mcba_usb.o

@@ -0,0 +1,3 @@
+# SPDX-License-Identifier: GPL-2.0
+obj-$(CONFIG_CAN_ETAS_ES58X) += etas_es58x.o
+etas_es58x-y = es58x_core.o es581_4.o es58x_fd.o

@@ -0,0 +1,552 @@
+// SPDX-License-Identifier: GPL-2.0
+
+/* Driver for ETAS GmbH ES58X USB CAN(-FD) Bus Interfaces.
+ *
+ * File es581_4.c: Adds support to ETAS ES581.4.
+ *
+ * Copyright (C) 2019 Robert Bosch Engineering and Business
+ * Solutions. All rights reserved.
+ * Copyright (C) 2020 ETAS K.K.. All rights reserved.
+ */
+
+#include <linux/kernel.h>
+#include <asm/unaligned.h>
+
+#include "es58x_core.h"
+#include "es581_4.h"
+
+/**
+ * es58x_sizeof_rx_tx_msg() - Calculate the actual length of the
+ *    structure of a rx or tx message.
+ * @msg: message of variable length, must have a dlc field.
+ *
+ * Even if RTR frames have actually no payload, the ES58X devices
+ * still expect it. Must be a macro in order to retrieve the actual
+ * size using offsetof() and typeof().
+ *
+ * Return: length of the message.
+ */
+#define es581_4_sizeof_rx_tx_msg(msg)                \
+    offsetof(typeof(msg), data[can_cc_dlc2len((msg).dlc)])
+
+static u16 es581_4_get_msg_len(const union es58x_urb_cmd *urb_cmd)
+{
+    return get_unaligned_le16(&urb_cmd->es581_4_urb_cmd.msg_len);
+}
+
+static int es581_4_rx_loopback_msg(struct es58x_device *es58x_dev,
+                   const struct es581_4_urb_cmd
+                   *es581_4_urb_cmd)
+{
+    struct net_device *netdev;
+    const struct es581_4_bulk_rx_loopback_msg *bulk_rx_loopback_msg;
+    const struct es581_4_rx_loopback_msg *rx_loopback_msg;
+    u64 *tstamps = es58x_dev->timestamps;
+    u16 msg_len;
+    u32 first_packet_idx, packet_idx;
+    unsigned int dropped = 0;
+    int i, num_element, ret;
+
+    bulk_rx_loopback_msg = &es581_4_urb_cmd->bulk_rx_loopback_msg;
+    msg_len = get_unaligned_le16(&es581_4_urb_cmd->msg_len) -
+        sizeof(bulk_rx_loopback_msg->channel_no);
+    num_element =
+        es58x_msg_num_element(es58x_dev->dev,
+                  bulk_rx_loopback_msg->rx_loopback_msg,
+                  msg_len);
+    if (num_element <= 0)
+        return num_element;
+
+    ret = es58x_get_netdev(es58x_dev, bulk_rx_loopback_msg->channel_no,
+                   &netdev);
+    if (ret)
+        return ret;
+
+    rx_loopback_msg = &bulk_rx_loopback_msg->rx_loopback_msg[0];
+    first_packet_idx = get_unaligned_le32(&rx_loopback_msg->packet_idx);
+    packet_idx = first_packet_idx;
+    for (i = 0; i < num_element; i++) {
+        rx_loopback_msg = &bulk_rx_loopback_msg->rx_loopback_msg[i];
+        if (get_unaligned_le32(&rx_loopback_msg->packet_idx)
+            == packet_idx - 1) {
+            if (net_ratelimit())
+                netdev_warn(netdev,
+                        "Received loopback packet idx %u twice\n",
+                        packet_idx - 1);
+            dropped++;
+            continue;
+        }
+        if (get_unaligned_le32(&rx_loopback_msg->packet_idx)
+            != packet_idx) {
+            netdev_err(netdev, "Packet idx jumped from %u to %u\n",
+                   packet_idx - 1, rx_loopback_msg->packet_idx);
+            return -EBADMSG;
+        }
+
+        tstamps[i] = get_unaligned_le64(&rx_loopback_msg->timestamp);
+        packet_idx++;
+    }
+
+    netdev->stats.tx_dropped += dropped;
+    return es58x_can_get_echo_skb(netdev, first_packet_idx,
+                      tstamps, num_element - dropped);
+}
+
+static int es581_4_rx_can_msg(struct es58x_device *es58x_dev,
+                  const struct es581_4_urb_cmd *es581_4_urb_cmd,
+                  u16 msg_len)
+{
+    const struct device *dev = es58x_dev->dev;
+    struct net_device *netdev;
+    int pkts, num_element, channel_no, ret;
+
+    num_element = es58x_msg_num_element(dev, es581_4_urb_cmd->rx_can_msg,
+                        msg_len);
+    if (num_element <= 0)
+        return num_element;
+
+    channel_no = es581_4_urb_cmd->rx_can_msg[0].channel_no;
+    ret = es58x_get_netdev(es58x_dev, channel_no, &netdev);
+    if (ret)
+        return ret;
+
+    if (!netif_running(netdev)) {
+        if (net_ratelimit())
+            netdev_info(netdev,
+                    "%s: %s is down, dropping %d rx packets\n",
+                    __func__, netdev->name, num_element);
+        netdev->stats.rx_dropped += num_element;
+        return 0;
+    }
+
+    for (pkts = 0; pkts < num_element; pkts++) {
+        const struct es581_4_rx_can_msg *rx_can_msg =
+            &es581_4_urb_cmd->rx_can_msg[pkts];
+        u64 tstamp = get_unaligned_le64(&rx_can_msg->timestamp);
+        canid_t can_id = get_unaligned_le32(&rx_can_msg->can_id);
+
+        if (channel_no != rx_can_msg->channel_no) {
+            netdev_err(netdev,
+                   "%s: channel_no changed from %d to %d\n",
+                   __func__,
+                   channel_no, rx_can_msg->channel_no);
+            return -EBADMSG;
+        }
+
+        ret = es58x_rx_can_msg(netdev, tstamp, rx_can_msg->data,
+                       can_id, rx_can_msg->flags,
+                       rx_can_msg->dlc);
+        if (ret)
+            break;
+    }
+
+    return ret;
+}
+
+static int es581_4_rx_err_msg(struct es58x_device *es58x_dev,
+                  const struct es581_4_rx_err_msg *rx_err_msg)
+{
+    struct net_device *netdev;
+    enum es58x_err error = get_unaligned_le32(&rx_err_msg->error);
+    int ret;
+
+    ret = es58x_get_netdev(es58x_dev, rx_err_msg->channel_no, &netdev);
+    if (ret)
+        return ret;
+
+    return es58x_rx_err_msg(netdev, error, 0,
+                get_unaligned_le64(&rx_err_msg->timestamp));
+}
+
+static int es581_4_rx_event_msg(struct es58x_device *es58x_dev,
+                const struct es581_4_rx_event_msg *rx_event_msg)
+{
+    struct net_device *netdev;
+    enum es58x_event event = get_unaligned_le32(&rx_event_msg->event);
+    int ret;
+
+    ret = es58x_get_netdev(es58x_dev, rx_event_msg->channel_no, &netdev);
+    if (ret)
+        return ret;
+
+    return es58x_rx_err_msg(netdev, 0, event,
+                get_unaligned_le64(&rx_event_msg->timestamp));
+}
+
+static int es581_4_rx_cmd_ret_u32(struct es58x_device *es58x_dev,
+                  const struct es581_4_urb_cmd *es581_4_urb_cmd,
+                  enum es58x_ret_type ret_type)
+{
+    struct net_device *netdev;
+    const struct es581_4_rx_dev_ret *rx_dev_ret;
+    u16 msg_len = get_unaligned_le16(&es581_4_urb_cmd->msg_len);
+    enum es58x_ret_u32 rx_cmd_ret_u32;
+    int ret;
+
+    ret = es58x_check_msg_len(es58x_dev->dev,
+                  es581_4_urb_cmd->rx_dev_ret, msg_len);
+    if (ret)
+        return ret;
+
+    rx_dev_ret = &es581_4_urb_cmd->rx_dev_ret;
+
+    ret = es58x_get_netdev(es58x_dev, rx_dev_ret->channel_no, &netdev);
+    if (ret)
+        return ret;
+
+    rx_cmd_ret_u32 = get_unaligned_le32(&rx_dev_ret->rx_cmd_ret_le32);
+    return es58x_rx_cmd_ret_u32(netdev, ret_type, rx_cmd_ret_u32);
+}
+
+static int es581_4_tx_ack_msg(struct es58x_device *es58x_dev,
+                  const struct es581_4_urb_cmd *es581_4_urb_cmd)
+{
+    struct net_device *netdev;
+    const struct es581_4_tx_ack_msg *tx_ack_msg;
+    u16 msg_len = get_unaligned_le16(&es581_4_urb_cmd->msg_len);
+    int ret;
+
+    tx_ack_msg = &es581_4_urb_cmd->tx_ack_msg;
+    ret = es58x_check_msg_len(es58x_dev->dev, *tx_ack_msg, msg_len);
+    if (ret)
+        return ret;
+
+    if (tx_ack_msg->rx_dev_ret_u8 != ES58X_RET_U8_OK)
+        return es58x_rx_dev_ret_u8(es58x_dev->dev,
+                       ES58X_RET_TYPE_TX_MSG,
+                       tx_ack_msg->rx_dev_ret_u8);
+
+    ret = es58x_get_netdev(es58x_dev, tx_ack_msg->channel_no, &netdev);
+    if (ret)
+        return ret;
+
+    return
+        es58x_tx_ack_msg(netdev,
+                 get_unaligned_le16(&tx_ack_msg->tx_free_entries),
+                 ES58X_RET_U32_OK);
+}
+
+static int es581_4_dispatch_rx_cmd(struct es58x_device *es58x_dev,
+                   const struct es581_4_urb_cmd
+                   *es581_4_urb_cmd)
+{
+    const struct device *dev = es58x_dev->dev;
+    u16 msg_len = get_unaligned_le16(&es581_4_urb_cmd->msg_len);
+    enum es581_4_rx_type rx_type = es581_4_urb_cmd->rx_can_msg[0].rx_type;
+    int ret = 0;
+
+    switch (rx_type) {
+    case ES581_4_RX_TYPE_MESSAGE:
+        return es581_4_rx_can_msg(es58x_dev, es581_4_urb_cmd, msg_len);
+
+    case ES581_4_RX_TYPE_ERROR:
+        ret = es58x_check_msg_len(dev, es581_4_urb_cmd->rx_err_msg,
+                      msg_len);
+        if (ret < 0)
+            return ret;
+        return es581_4_rx_err_msg(es58x_dev,
+                      &es581_4_urb_cmd->rx_err_msg);
+
+    case ES581_4_RX_TYPE_EVENT:
+        ret = es58x_check_msg_len(dev, es581_4_urb_cmd->rx_event_msg,
+                      msg_len);
+        if (ret < 0)
+            return ret;
+        return es581_4_rx_event_msg(es58x_dev,
+                        &es581_4_urb_cmd->rx_event_msg);
+
+    default:
+        dev_err(dev, "%s: Unknown rx_type 0x%02X\n", __func__, rx_type);
+        return -EBADRQC;
+    }
+}
+
+static int es581_4_handle_urb_cmd(struct es58x_device *es58x_dev,
+                  const union es58x_urb_cmd *urb_cmd)
+{
+    const struct es581_4_urb_cmd *es581_4_urb_cmd;
+    const struct device *dev = es58x_dev->dev;
+    u16 msg_len = es581_4_get_msg_len(urb_cmd);
+    int ret;
+
+    es581_4_urb_cmd = &urb_cmd->es581_4_urb_cmd;
+
+    if (es581_4_urb_cmd->cmd_type != ES581_4_CAN_COMMAND_TYPE) {
+        dev_err(dev, "%s: Unknown command type (0x%02X)\n",
+            __func__, es581_4_urb_cmd->cmd_type);
+        return -EBADRQC;
+    }
+
+    switch ((enum es581_4_cmd_id)es581_4_urb_cmd->cmd_id) {
+    case ES581_4_CMD_ID_SET_BITTIMING:
+        return es581_4_rx_cmd_ret_u32(es58x_dev, es581_4_urb_cmd,
+                          ES58X_RET_TYPE_SET_BITTIMING);
+
+    case ES581_4_CMD_ID_ENABLE_CHANNEL:
+        return es581_4_rx_cmd_ret_u32(es58x_dev, es581_4_urb_cmd,
+                          ES58X_RET_TYPE_ENABLE_CHANNEL);
+
+    case ES581_4_CMD_ID_TX_MSG:
+        return es581_4_tx_ack_msg(es58x_dev, es581_4_urb_cmd);
+
+    case ES581_4_CMD_ID_RX_MSG:
+        return es581_4_dispatch_rx_cmd(es58x_dev, es581_4_urb_cmd);
+
+    case ES581_4_CMD_ID_RESET_RX:
+        ret = es581_4_rx_cmd_ret_u32(es58x_dev, es581_4_urb_cmd,
+                         ES58X_RET_TYPE_RESET_RX);
+        return ret;
+
+    case ES581_4_CMD_ID_RESET_TX:
+        ret = es581_4_rx_cmd_ret_u32(es58x_dev, es581_4_urb_cmd,
+                         ES58X_RET_TYPE_RESET_TX);
+        return ret;
+
+    case ES581_4_CMD_ID_DISABLE_CHANNEL:
+        return es581_4_rx_cmd_ret_u32(es58x_dev, es581_4_urb_cmd,
+                          ES58X_RET_TYPE_DISABLE_CHANNEL);
+
+    case ES581_4_CMD_ID_TIMESTAMP:
+        ret = es58x_check_msg_len(dev, es581_4_urb_cmd->timestamp,
+                      msg_len);
+        if (ret < 0)
+            return ret;
+        return es58x_rx_timestamp(es58x_dev,
+                      get_unaligned_le64
+                      (&es581_4_urb_cmd->timestamp));
+
+    case ES581_4_CMD_ID_SELF_RECEPTION:
+        return es581_4_rx_loopback_msg(es58x_dev, es581_4_urb_cmd);
+
+    case ES581_4_CMD_ID_DEVICE_ERR:
+        ret = es58x_check_msg_len(dev, es581_4_urb_cmd->rx_dev_ret_u8,
+                      msg_len);
+        if (ret)
+            return ret;
+        return es58x_rx_dev_ret_u8(es58x_dev->dev,
+                       ES58X_RET_TYPE_DEVICE_ERR,
+                       es581_4_urb_cmd->rx_dev_ret_u8);
+
+    default:
+        dev_warn(dev, "%s: Unexpected command ID: 0x%02X\n",
+             __func__, es581_4_urb_cmd->cmd_id);
+        return -EBADRQC;
+    }
+}
+
+static void es581_4_fill_urb_header(union es58x_urb_cmd *urb_cmd, u8 cmd_type,
+                    u8 cmd_id, u8 channel_idx, u16 msg_len)
+{
+    struct es581_4_urb_cmd *es581_4_urb_cmd = &urb_cmd->es581_4_urb_cmd;
+
+    es581_4_urb_cmd->SOF = cpu_to_le16(es581_4_param.tx_start_of_frame);
+    es581_4_urb_cmd->cmd_type = cmd_type;
+    es581_4_urb_cmd->cmd_id = cmd_id;
+    es581_4_urb_cmd->msg_len = cpu_to_le16(msg_len);
+}
+
+static int es581_4_tx_can_msg(struct es58x_priv *priv, int channel_idx,
+                  u32 packet_idx, const struct sk_buff *skb,
+                  union es58x_urb_cmd *urb_cmd, u32 *urb_cmd_len)
+{
+    struct es58x_device *es58x_dev = priv->es58x_dev;
+    struct es581_4_urb_cmd *es581_4_urb_cmd = &urb_cmd->es581_4_urb_cmd;
+    struct can_frame *cf = (struct can_frame *)skb->data;
+    struct es581_4_tx_can_msg *tx_can_msg;
+    u16 msg_len;
+    int ret;
+
+    ret = es58x_check_msg_max_len(es58x_dev->dev,
+                      es581_4_urb_cmd->bulk_tx_can_msg,
+                      es581_4_get_msg_len(urb_cmd) +
+                      sizeof(*tx_can_msg));
+    if (ret)
+        return ret;
+
+    if (can_is_canfd_skb(skb))
+        return -EMSGSIZE;
+
+    /* Fill message contents. */
+    if (*urb_cmd_len == es58x_dev->param->urb_cmd_header_len) {
+        es581_4_fill_urb_header(urb_cmd, ES581_4_CAN_COMMAND_TYPE,
+                    ES581_4_CMD_ID_TX_MSG,
+                    channel_idx, *urb_cmd_len);
+        msg_len = 1;
+    } else {
+        msg_len = le16_to_cpu(es581_4_urb_cmd->msg_len);
+    }
+    tx_can_msg = (struct es581_4_tx_can_msg *)
+        &es581_4_urb_cmd->bulk_tx_can_msg.tx_can_msg_buf[msg_len - 1];
+
+    put_unaligned_le32(es58x_get_raw_can_id(cf), &tx_can_msg->can_id);
+    put_unaligned_le32(packet_idx, &tx_can_msg->packet_idx);
+    put_unaligned_le16((u16)es58x_get_flags(skb), &tx_can_msg->flags);
+    tx_can_msg->channel_no = channel_idx + 1;
+    tx_can_msg->dlc = can_get_cc_dlc(cf, priv->can.ctrlmode);
+
+    memcpy(tx_can_msg->data, cf->data, cf->len);
+
+    /* Calculate new sizes. */
+    es581_4_urb_cmd->bulk_tx_can_msg.num_can_msg++;
+    msg_len += es581_4_sizeof_rx_tx_msg(*tx_can_msg);
+    *urb_cmd_len = es58x_get_urb_cmd_len(es58x_dev, msg_len);
+    es581_4_urb_cmd->msg_len = cpu_to_le16(msg_len);
+
+    return 0;
+}
+
+static void es581_4_print_conf(struct net_device *netdev,
+                   struct es581_4_tx_conf_msg *tx_conf_msg)
+{
+    netdev_vdbg(netdev, "bitrate         = %d\n",
+            le32_to_cpu(tx_conf_msg->bitrate));
+    netdev_vdbg(netdev, "sample_point    = %d\n",
+            le32_to_cpu(tx_conf_msg->sample_point));
+    netdev_vdbg(netdev, "samples_per_bit = %d\n",
+            le32_to_cpu(tx_conf_msg->samples_per_bit));
+    netdev_vdbg(netdev, "bit_time        = %d\n",
+            le32_to_cpu(tx_conf_msg->bit_time));
+    netdev_vdbg(netdev, "sjw             = %d\n",
+            le32_to_cpu(tx_conf_msg->sjw));
+    netdev_vdbg(netdev, "sync_edge       = %d\n",
+            le32_to_cpu(tx_conf_msg->sync_edge));
+    netdev_vdbg(netdev, "physical_layer  = %d\n",
+            le32_to_cpu(tx_conf_msg->physical_layer));
+    netdev_vdbg(netdev, "self_reception  = %d\n",
+            le32_to_cpu(tx_conf_msg->self_reception_mode));
+    netdev_vdbg(netdev, "channel_no      = %d\n", tx_conf_msg->channel_no);
+}
+
+static int es581_4_set_bittiming(struct es58x_device *es58x_dev,
+                 int channel_idx)
+{
+    struct es581_4_tx_conf_msg tx_conf_msg = { 0 };
+    struct net_device *netdev = es58x_dev->netdev[channel_idx];
+    struct es58x_priv *priv = es58x_priv(netdev);
+    struct can_bittiming *bt = &priv->can.bittiming;
+
+    tx_conf_msg.bitrate = cpu_to_le32(bt->bitrate);
+    /* bt->sample_point is in tenth of percent. Convert it to percent. */
+    tx_conf_msg.sample_point = cpu_to_le32(bt->sample_point / 10U);
+    tx_conf_msg.samples_per_bit = cpu_to_le32(ES58X_SAMPLES_PER_BIT_ONE);
+    tx_conf_msg.bit_time = cpu_to_le32(can_bit_time(bt));
+    tx_conf_msg.sjw = cpu_to_le32(bt->sjw);
+    tx_conf_msg.sync_edge = cpu_to_le32(ES58X_SYNC_EDGE_SINGLE);
+    tx_conf_msg.physical_layer =
+        cpu_to_le32(ES58X_PHYSICAL_LAYER_HIGH_SPEED);
+    tx_conf_msg.self_reception_mode = cpu_to_le32(ES58X_SELF_RECEPTION_ON);
+    tx_conf_msg.channel_no = channel_idx + es581_4_param.channel_idx_offset;
+
+    es581_4_print_conf(netdev, &tx_conf_msg);
+
+    return es58x_send_msg(es58x_dev, ES581_4_CAN_COMMAND_TYPE,
+                  ES581_4_CMD_ID_SET_BITTIMING, &tx_conf_msg,
+                  sizeof(tx_conf_msg), channel_idx);
+}
+
+static int es581_4_enable_channel(struct es58x_device *es58x_dev,
+                  int channel_idx)
+{
+    u8 msg = channel_idx + 1;
+
+    return es58x_send_msg(es58x_dev, ES581_4_CAN_COMMAND_TYPE,
+                  ES581_4_CMD_ID_ENABLE_CHANNEL, &msg, sizeof(msg),
+                  channel_idx);
+}
+
+static int es581_4_disable_channel(struct es58x_device *es58x_dev,
+                   int channel_idx)
+{
+    u8 msg = channel_idx + 1;
+
+    return es58x_send_msg(es58x_dev, ES581_4_CAN_COMMAND_TYPE,
+                  ES581_4_CMD_ID_DISABLE_CHANNEL, &msg, sizeof(msg),
+                  channel_idx);
+}
+
+static int es581_4_reset_rx(struct es58x_device *es58x_dev, int channel_idx)
+{
+    u8 msg = channel_idx + 1;
+
+    return es58x_send_msg(es58x_dev, ES581_4_CAN_COMMAND_TYPE,
+                  ES581_4_CMD_ID_RESET_RX, &msg, sizeof(msg),
+                  channel_idx);
+}
+
+static int es581_4_reset_tx(struct es58x_device *es58x_dev, int channel_idx)
+{
+    u8 msg = channel_idx + 1;
+
+    return es58x_send_msg(es58x_dev, ES581_4_CAN_COMMAND_TYPE,
+                  ES581_4_CMD_ID_RESET_TX, &msg, sizeof(msg),
+                  channel_idx);
+}
+
+static int es581_4_get_timestamp(struct es58x_device *es58x_dev)
+{
+    return es58x_send_msg(es58x_dev, ES581_4_CAN_COMMAND_TYPE,
+                  ES581_4_CMD_ID_TIMESTAMP, ES58X_EMPTY_MSG,
+                  0, ES58X_CHANNEL_IDX_NA);
+}
+
+/* struct es581_4_bittiming_const - Nominal bittiming constants.
+ *
+ * Nominal bittiming constants for ES581.4 as specified in the
+ * microcontroller datasheet: "Stellaris(R) LM3S5B91 Microcontroller"
+ * table 17-4 "CAN Protocol Ranges" from Texas Instruments.
+ */
+static const struct can_bittiming_const es581_4_bittiming_const = {
+    .name = "ES581.4",
+    .tseg1_min = 1,
+    .tseg1_max = 8,
+    .tseg2_min = 1,
+    .tseg2_max = 8,
+    .sjw_max = 4,
+    .brp_min = 1,
+    .brp_max = 128,
+    .brp_inc = 1
+};
+
+const struct es58x_parameters es581_4_param = {
+    .bittiming_const = &es581_4_bittiming_const,
+    .data_bittiming_const = NULL,
+    .bitrate_max = ES58X_MBPS(1),
+    .clock = {.freq = ES58X_MHZ(50)},
+    .ctrlmode_supported = CAN_CTRLMODE_LOOPBACK | CAN_CTRLMODE_CC_LEN8_DLC,
+    .tx_start_of_frame = 0xAFAF,
+    .rx_start_of_frame = 0xFAFA,
+    .tx_urb_cmd_max_len = ES581_4_TX_URB_CMD_MAX_LEN,
+    .rx_urb_cmd_max_len = ES581_4_RX_URB_CMD_MAX_LEN,
+    /* Size of internal device TX queue is 330.
+     *
+     * However, we witnessed some ES58X_ERR_PROT_CRC errors from
+     * the device and thus, echo_skb_max was lowered to the
+     * empirical value of 75 which seems stable.
+     *
+     * Root cause of those ES58X_ERR_PROT_CRC errors is still
+     * unclear.
+     */
+    .echo_skb_max = 75,
+#ifdef CONFIG_BQL
+    /* Empirical value. */
+    .dql_limit_min = ES58X_CAN_FRAME_BYTES_MAX * 50,
+#endif
+    .tx_bulk_max = ES581_4_TX_BULK_MAX,
+    .urb_cmd_header_len = ES581_4_URB_CMD_HEADER_LEN,
+    .rx_urb_max = ES58X_RX_URBS_MAX,
+    .tx_urb_max = ES58X_TX_URBS_MAX,
+    .channel_idx_offset = 1
+};
+
+const struct es58x_operators es581_4_ops = {
+    .get_msg_len = es581_4_get_msg_len,
+    .handle_urb_cmd = es581_4_handle_urb_cmd,
+    .fill_urb_header = es581_4_fill_urb_header,
+    .tx_can_msg = es581_4_tx_can_msg,
+    .set_bittiming = es581_4_set_bittiming,
+    .enable_channel = es581_4_enable_channel,
+    .disable_channel = es581_4_disable_channel,
+    .reset_rx = es581_4_reset_rx,
+    .reset_tx = es581_4_reset_tx,
+    .get_timestamp = es581_4_get_timestamp
+};

@@ -0,0 +1,206 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+
+/* Driver for ETAS GmbH ES58X USB CAN(-FD) Bus Interfaces.
+ *
+ * File es581_4.h: Definitions and declarations specific to ETAS
+ * ES581.4.
+ *
+ * Copyright (C) 2019 Robert Bosch Engineering and Business
+ * Solutions. All rights reserved.
+ * Copyright (C) 2020 ETAS K.K.. All rights reserved.
+ */
+
+#ifndef __ES581_4_H__
+#define __ES581_4_H__
+
+#include <linux/types.h>
+
+#define ES581_4_NUM_CAN_CH 2
+
+#define ES581_4_TX_BULK_MAX 25
+#define ES581_4_RX_BULK_MAX 30
+#define ES581_4_RX_LOOPBACK_BULK_MAX 30
+
+enum es581_4_cmd_type {
+    ES581_4_CAN_COMMAND_TYPE = 0x45
+};
+
+enum es581_4_cmd_id {
+    ES581_4_CMD_ID_OPEN_CHANNEL = 0x01,
+    ES581_4_CMD_ID_CLOSE_CHANNEL = 0x02,
+    ES581_4_CMD_ID_SET_BITTIMING = 0x03,
+    ES581_4_CMD_ID_ENABLE_CHANNEL = 0x04,
+    ES581_4_CMD_ID_TX_MSG = 0x05,
+    ES581_4_CMD_ID_RX_MSG = 0x06,
+    ES581_4_CMD_ID_RESET_RX = 0x0A,
+    ES581_4_CMD_ID_RESET_TX = 0x0B,
+    ES581_4_CMD_ID_DISABLE_CHANNEL = 0x0C,
+    ES581_4_CMD_ID_TIMESTAMP = 0x0E,
+    ES581_4_CMD_ID_RESET_DEVICE = 0x28,
+    ES581_4_CMD_ID_SELF_RECEPTION = 0x71,
+    ES581_4_CMD_ID_DEVICE_ERR = 0x72
+};
+
+enum es581_4_rx_type {
+    ES581_4_RX_TYPE_MESSAGE = 1,
+    ES581_4_RX_TYPE_ERROR = 3,
+    ES581_4_RX_TYPE_EVENT = 4
+};
+
+/**
+ * struct es581_4_tx_conf_msg - Channel configuration.
+ * @bitrate: Bitrate.
+ * @sample_point: Sample point is in percent [0..100].
+ * @samples_per_bit: type enum es58x_samples_per_bit.
+ * @bit_time: Number of time quanta in one bit.
+ * @sjw: Synchronization Jump Width.
+ * @sync_edge: type enum es58x_sync_edge.
+ * @physical_layer: type enum es58x_physical_layer.
+ * @self_reception_mode: type enum es58x_self_reception_mode.
+ * @channel_no: Channel number, starting from 1. Not to be confused
+ *    with channed_idx of the ES58X FD which starts from 0.
+ */
+struct es581_4_tx_conf_msg {
+    __le32 bitrate;
+    __le32 sample_point;
+    __le32 samples_per_bit;
+    __le32 bit_time;
+    __le32 sjw;
+    __le32 sync_edge;
+    __le32 physical_layer;
+    __le32 self_reception_mode;
+    u8 channel_no;
+} __packed;
+
+struct es581_4_tx_can_msg {
+    __le32 can_id;
+    __le32 packet_idx;
+    __le16 flags;
+    u8 channel_no;
+    u8 dlc;
+    u8 data[CAN_MAX_DLEN];
+} __packed;
+
+/* The ES581.4 allows bulk transfer.  */
+struct es581_4_bulk_tx_can_msg {
+    u8 num_can_msg;
+    /* Using type "u8[]" instead of "struct es581_4_tx_can_msg[]"
+     * for tx_msg_buf because each member has a flexible size.
+     */
+    u8 tx_can_msg_buf[ES581_4_TX_BULK_MAX *
+              sizeof(struct es581_4_tx_can_msg)];
+} __packed;
+
+struct es581_4_rx_loopback_msg {
+    __le64 timestamp;
+    __le32 packet_idx;
+} __packed;
+
+struct es581_4_bulk_rx_loopback_msg {
+    u8 channel_no;
+    struct es581_4_rx_loopback_msg
+     rx_loopback_msg[ES581_4_RX_LOOPBACK_BULK_MAX];
+} __packed;
+
+/* Normal Rx CAN Message */
+struct es581_4_rx_can_msg {
+    __le64 timestamp;
+    u8 rx_type;        /* type enum es581_4_rx_type */
+    u8 flags;        /* type enum es58x_flag */
+    u8 channel_no;
+    u8 dlc;
+    __le32 can_id;
+    u8 data[CAN_MAX_DLEN];
+} __packed;
+
+struct es581_4_rx_err_msg {
+    __le64 timestamp;
+    __le16 rx_type;        /* type enum es581_4_rx_type */
+    __le16 flags;        /* type enum es58x_flag */
+    u8 channel_no;
+    u8 __padding[2];
+    u8 dlc;
+    __le32 tag;        /* Related to the CAN filtering. Unused in

@@ -0,0 +1,2628 @@
+// SPDX-License-Identifier: GPL-2.0
+
+/* Driver for ETAS GmbH ES58X USB CAN(-FD) Bus Interfaces.
+ *
+ * File es58x_core.c: Core logic to manage the network devices and the
+ * USB interface.
+ *
+ * Copyright (C) 2019 Robert Bosch Engineering and Business
+ * Solutions. All rights reserved.
+ * Copyright (C) 2020 ETAS K.K.. All rights reserved.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/usb.h>
+#include <linux/crc16.h>
+#include <linux/spinlock.h>
+#include <asm/unaligned.h>
+
+#include "es58x_core.h"
+
+#define DRV_VERSION "1.03"
+MODULE_AUTHOR("Mailhol Vincent <mailhol.vincent@wanadoo.fr>");
+MODULE_AUTHOR("Arunachalam Santhanam <arunachalam.santhanam@in.bosch.com>");
+MODULE_DESCRIPTION("Socket CAN driver for ETAS ES58X USB adapters");
+MODULE_VERSION(DRV_VERSION);
+MODULE_LICENSE("GPL v2");
+
+/* Vendor and product id */
+#define ES58X_MODULE_NAME "etas_es58x"
+#define ES58X_VENDOR_ID 0x108C
+#define ES581_4_PRODUCT_ID 0x0159
+#define ES582_1_PRODUCT_ID 0x0168
+#define ES584_1_PRODUCT_ID 0x0169
+
+/* Table of devices which work with this driver */
+static const struct usb_device_id es58x_id_table[] = {
+    {USB_DEVICE(ES58X_VENDOR_ID, ES581_4_PRODUCT_ID)},
+    {USB_DEVICE(ES58X_VENDOR_ID, ES582_1_PRODUCT_ID)},
+    {USB_DEVICE(ES58X_VENDOR_ID, ES584_1_PRODUCT_ID)},
+    {}            /* Terminating entry */
+};
+
+MODULE_DEVICE_TABLE(usb, es58x_id_table);
+
+#define es58x_print_hex_dump(buf, len)                    \
+    print_hex_dump(KERN_DEBUG,                    \
+               ES58X_MODULE_NAME " " __stringify(buf) ": ",    \
+               DUMP_PREFIX_NONE, 16, 1, buf, len, false)
+
+#define es58x_print_hex_dump_debug(buf, len)                 \
+    print_hex_dump_debug(ES58X_MODULE_NAME " " __stringify(buf) ": ",\
+                 DUMP_PREFIX_NONE, 16, 1, buf, len, false)
+
+/* The last two bytes of an ES58X command is a CRC16. The first two
+ * bytes (the start of frame) are skipped and the CRC calculation
+ * starts on the third byte.
+ */
+#define ES58X_CRC_CALC_OFFSET    2
+
+/**
+ * es58x_calculate_crc() - Compute the crc16 of a given URB.
+ * @urb_cmd: The URB command for which we want to calculate the CRC.
+ * @urb_len: Length of @urb_cmd. Must be at least bigger than 4
+ *    (ES58X_CRC_CALC_OFFSET + sizeof(crc))
+ *
+ * Return: crc16 value.
+ */
+static u16 es58x_calculate_crc(const union es58x_urb_cmd *urb_cmd, u16 urb_len)
+{
+    u16 crc;
+    ssize_t len = urb_len - ES58X_CRC_CALC_OFFSET - sizeof(crc);
+
+    WARN_ON(len < 0);
+    crc = crc16(0, &urb_cmd->raw_cmd[ES58X_CRC_CALC_OFFSET], len);
+    return crc;
+}
+
+/**
+ * es58x_get_crc() - Get the CRC value of a given URB.
+ * @urb_cmd: The URB command for which we want to get the CRC.
+ * @urb_len: Length of @urb_cmd. Must be at least bigger than 4
+ *    (ES58X_CRC_CALC_OFFSET + sizeof(crc))
+ *
+ * Return: crc16 value.
+ */
+static u16 es58x_get_crc(const union es58x_urb_cmd *urb_cmd, u16 urb_len)
+{
+    u16 crc;
+    const __le16 *crc_addr;
+
+    WARN_ON(urb_len - (ssize_t)sizeof(crc) < 0);
+    crc_addr = (__le16 *)&urb_cmd->raw_cmd[urb_len - sizeof(crc)];
+    crc = get_unaligned_le16(crc_addr);
+    return crc;
+}
+
+/**
+ * es58x_set_crc() - Set the CRC value of a given URB.
+ * @urb_cmd: The URB command for which we want to get the CRC.
+ * @urb_len: Length of @urb_cmd. Must be at least bigger than 4
+ *    (ES58X_CRC_CALC_OFFSET + sizeof(crc))
+ */
+static void es58x_set_crc(union es58x_urb_cmd *urb_cmd, u16 urb_len)
+{
+    u16 crc;
+    __le16 *crc_addr;
+
+    crc = es58x_calculate_crc(urb_cmd, urb_len);
+    crc_addr = (__le16 *)&urb_cmd->raw_cmd[urb_len - sizeof(crc)];
+    put_unaligned_le16(crc, crc_addr);
+}
+
+/**
+ * es58x_check_crc() - Validate the CRC value of a given URB.
+ * @es58x_dev: ES58X device.
+ * @urb_cmd: The URB command for which we want to check the CRC.
+ * @urb_len: Length of @urb_cmd. Must be at least bigger than 4
+ *    (ES58X_CRC_CALC_OFFSET + sizeof(crc))
+ *
+ * Return: zero on success, -EBADMSG if the CRC check fails.
+ */
+static int es58x_check_crc(struct es58x_device *es58x_dev,
+               const union es58x_urb_cmd *urb_cmd, u16 urb_len)
+{
+    u16 calculated_crc = es58x_calculate_crc(urb_cmd, urb_len);
+    u16 expected_crc = es58x_get_crc(urb_cmd, urb_len);
+
+    if (expected_crc != calculated_crc) {
+        dev_err_ratelimited(es58x_dev->dev,
+                    "%s: Bad CRC, urb_len: %d\n",
+                    __func__, urb_len);
+        return -EBADMSG;
+    }
+
+    return 0;
+}
+
+/**
+ * es58x_timestamp_to_ns() - Convert a timestamp value received from a
+ *    ES58X device to nanoseconds.
+ * @timestamp: Timestamp received from a ES58X device.
+ *
+ * The timestamp received from ES58X is expressed in multiple of 0.5
+ * micro seconds. This function converts it in to nanoseconds.
+ *
+ * Return: Timestamp value in nanoseconds.
+ */
+static u64 es58x_timestamp_to_ns(u64 timestamp)
+{
+    const u64 es58x_timestamp_ns_mult_coef = 500ULL;
+
+    return es58x_timestamp_ns_mult_coef * timestamp;
+}
+
+/**
+ * es58x_set_skb_timestamp() - Set the hardware timestamp of an skb.
+ * @netdev: CAN network device.
+ * @skb: socket buffer of a CAN message.
+ * @timestamp: Timestamp received from an ES58X device.
+ *
+ * Used for both received and loopback messages.
+ *
+ * Return: zero on success, -EFAULT if @skb is NULL.
+ */
+static int es58x_set_skb_timestamp(struct net_device *netdev,
+                   struct sk_buff *skb, u64 timestamp)
+{
+    struct es58x_device *es58x_dev = es58x_priv(netdev)->es58x_dev;
+    struct skb_shared_hwtstamps *hwts;
+
+    hwts = skb_hwtstamps(skb);
+    /* Ignoring overflow (overflow on 64 bits timestamp with nano
+     * second precision would occur after more than 500 years).
+     */
+    hwts->hwtstamp = ns_to_ktime(es58x_timestamp_to_ns(timestamp) +
+                     es58x_dev->realtime_diff_ns);
+
+    return 0;
+}
+
+/**
+ * es58x_rx_timestamp() - Handle a received timestamp.
+ * @es58x_dev: ES58X device.
+ * @timestamp: Timestamp received from a ES58X device.
+ *
+ * Calculate the difference between the ES58X device and the kernel
+ * internal clocks. This difference will be later used as an offset to
+ * convert the timestamps of RX and loopback messages to match the
+ * kernel system time (e.g. convert to UNIX time).
+ *
+ * Return: zero on success.
+ */
+int es58x_rx_timestamp(struct es58x_device *es58x_dev, u64 timestamp)
+{
+    u64 ktime_real_ns = ktime_get_real_ns();
+    u64 device_timestamp = es58x_timestamp_to_ns(timestamp);
+
+    dev_dbg(es58x_dev->dev, "%s: request round-trip time: %llu\n",
+        __func__, ktime_real_ns - es58x_dev->ktime_req_ns);
+
+    es58x_dev->realtime_diff_ns =
+        (es58x_dev->ktime_req_ns + ktime_real_ns) / 2 - device_timestamp;
+    es58x_dev->ktime_req_ns = 0;
+
+    dev_dbg(es58x_dev->dev,
+        "%s: Device timestamp: %llu, diff with kernel: %llu\n",
+        __func__, device_timestamp, es58x_dev->realtime_diff_ns);
+
+    return 0;
+}
+
+/**
+ * es58x_set_realtime_diff_ns() - Calculate difference between the
+ *    clocks of the ES58X device and the kernel
+ * @es58x_dev: ES58X device.
+ *
+ * Request a timestamp from the ES58X device. Once the answer is
+ * received, the timestamp difference will be set by the callback
+ * function es58x_rx_timestamp().
+ *
+ * Return: zero on success, errno when any error occurs.
+ */
+static int es58x_set_realtime_diff_ns(struct es58x_device *es58x_dev)
+{
+    if (es58x_dev->ktime_req_ns) {
+        dev_warn(es58x_dev->dev,
+             "%s: Previous request to set timestamp has not completed yet\n",
+             __func__);
+        return -EBUSY;
+    }
+
+    es58x_dev->ktime_req_ns = ktime_get_real_ns();
+    return es58x_dev->ops->get_timestamp(es58x_dev);
+}
+
+/**
+ * es58x_is_can_state_active() - Is the network device in an active
+ *    CAN state?
+ * @netdev: CAN network device.
+ *
+ * The device is considered active if it is able to send or receive
+ * CAN frames, that is to say if it is in any of
+ * CAN_STATE_ERROR_ACTIVE, CAN_STATE_ERROR_WARNING or
+ * CAN_STATE_ERROR_PASSIVE states.
+ *
+ * Caution: when recovering from a bus-off,
+ * net/core/dev.c#can_restart() will call
+ * net/core/dev.c#can_flush_echo_skb() without using any kind of
+ * locks. For this reason, it is critical to guaranty that no TX or
+ * loopback operations (i.e. any access to priv->echo_skb[]) can be
+ * done while this function is returning false.
+ *
+ * Return: true if the device is active, else returns false.
+ */
+static bool es58x_is_can_state_active(struct net_device *netdev)
+{
+    return es58x_priv(netdev)->can.state < CAN_STATE_BUS_OFF;
+}
+
+/**
+ * es58x_echo_skb_stop_threshold() - Determine the limit of how many
+ *    skb slots can be taken before we should stop the network
+ *    queue.
+ * @priv: ES58X private parameters related to the network device.
+ *
+ * We need to save enough free skb slots in order to be able to do
+ * bulk send. This function can be used to determine when to wake or
+ * stop the network queue in regard to the number of skb slots already
+ * taken if the loopback FIFO.
+ *
+ * Return: stop threshold limit.
+ */
+static int es58x_echo_skb_stop_threshold(struct es58x_priv *priv)
+{
+    return priv->can.echo_skb_max - priv->es58x_dev->param->tx_bulk_max + 1;
+}
+
+/**
+ * es58x_netif_wake_queue() - Wake-up the network queue if conditions
+ *    are met.
+ * @netdev: CAN network device.
+ *
+ * If the queue is already awoken, do nothing.
+ *
+ * The conditions to wake up the queue are: a.) The device should be
+ * active (e.g. not in bus off state), b.) The network device must be
+ * up, c.) The echo_skb must have enough free slots for a bulk sent
+ * (c.f. es58x_echo_skb_stop_threshold()), d.) Enough idle URBs should
+ * be available.
+ */
+static void es58x_netif_wake_queue(struct net_device *netdev)
+{
+    struct es58x_priv *priv = es58x_priv(netdev);
+    struct es58x_device *es58x_dev = priv->es58x_dev;
+    unsigned long echo_skb_flags, tx_urbs_flags;
+
+    if (!netif_queue_stopped(netdev))
+        return;
+
+    spin_lock_irqsave(&priv->echo_skb_spinlock, echo_skb_flags);
+    if (!es58x_is_can_state_active(netdev))
+        goto echo_skb_unlock;
+    if (netif_running(netdev) &&
+        priv->num_echo_skb < es58x_echo_skb_stop_threshold(priv)) {
+        /* Confirm that we have at least one idle URB per CAN
+         * network device. Otherwise, we might run into race
+         * conditions in the function es58x_start_xmit() which
+         * would force us to return NETDEV_TX_BUSY.
+         */
+        spin_lock_irqsave(&es58x_dev->tx_urbs_idle.lock, tx_urbs_flags);
+        if (atomic_read(&es58x_dev->tx_urbs_idle_cnt) >=
+            es58x_dev->num_can_ch)
+            netif_wake_queue(netdev);
+        spin_unlock_irqrestore(&es58x_dev->tx_urbs_idle.lock,
+                       tx_urbs_flags);
+    }
+ echo_skb_unlock:
+    spin_unlock_irqrestore(&priv->echo_skb_spinlock, echo_skb_flags);
+}
+
+/**
+ * es58x_netif_wake_all_queues() - Wake-up all the network queues if
+ *    conditions are met.
+ * @es58x_dev: ES58X device.
+ *
+ * Try to wake-up all the queues associated with a ES58X device. This
+ * function should be used when idle URB resources become available.
+ */
+static void es58x_netif_wake_all_queues(struct es58x_device *es58x_dev)
+{
+    int i;
+
+    for (i = 0; i < es58x_dev->num_can_ch; i++)
+        es58x_netif_wake_queue(es58x_dev->netdev[i]);
+}
+
+/**
+ * es58x_netif_stop_all_queues() - Unconditionally stop all the
+ *    network queues.
+ * @es58x_dev: ES58X device.
+ *
+ * Stop all the queues associated with a ES58X device. This
+ * function should be used when idle URB resources become unavailable.
+ */
+static void es58x_netif_stop_all_queues(struct es58x_device *es58x_dev)
+{
+    int i;
+
+    for (i = 0; i < es58x_dev->num_can_ch; i++)
+        if (es58x_dev->netdev[i])
+            netif_stop_queue(es58x_dev->netdev[i]);
+}
+
+/**
+ * es58x_can_frame_bytes() - Calculate the CAN frame length in bytes
+ *    of a given skb.
+ * @skb: socket buffer of a CAN message.
+ *
+ * Do a rough calculation: bitstuffing is ignored and length in bits
+ * is rounded up to a length in bytes.
+ *
+ * Rationale: this function is to be used for the BQL functions
+ * (__netdev_sent_queue() and netdev_completed_queue()) which expect a
+ * value in bytes. Just using skb->len is insufficient because it will
+ * return the constant value of CAN(FD)_MTU. Doing the bitstuffing
+ * calculation would be too expensive in term of computing resources
+ * for no noticeable gain.
+ *
+ * Remarks: a.)Tthe payload of CAN FD frames with BRS flag are sent at
+ * a different bitrate. Currently, the can-utils canbusload tool does
+ * not support CAN-FD yet and so we could not run any benchmark to
+ * measure the impact. There might be possible improvement here. b.)
+ * The macro ES58X_EFF_BYTES() and ES58X_SFF_BYTES() are for classical
+ * CAN. Need to do the addition for CAN-FD (the value are expected to
+ * be close enough so the impact should be minimal or none).
+ *
+ * Return: length in bytes.
+ */
+static unsigned int es58x_can_frame_bytes(const struct sk_buff *skb)
+{
+    const struct canfd_frame *cf = (const struct canfd_frame *)skb->data;
+    u8 len = cf->can_id & CAN_RTR_FLAG ? 0 : cf->len;
+
+    if (cf->can_id & CAN_EFF_FLAG)
+        return ES58X_EFF_BYTES(len);
+    else
+        return ES58X_SFF_BYTES(len);
+}
+
+/**
+ * es58x_add_skb_idx() - Increment an index of the loopback FIFO.
+ * @priv: ES58X private parameters related to the network device.
+ * @idx: address of the index to be incremented.
+ * @a: the increment. Must be positive and less or equal to
+ *    @priv->can.echo_skb_max.
+ *
+ * Do a modulus addition: set *@idx to (*@idx + @a) %
+ * @priv->can.echo_skb_max.
+ *
+ * Rationale: the modulus operator % takes a decent amount of CPU
+ * cycles (c.f. other division functions such as
+ * include/linux/math64.h:iter_div_u64_rem()).
+ */
+static __always_inline void es58x_add_skb_idx(struct es58x_priv *priv,
+                          u16 *idx, u16 a)
+{
+    *idx += a;
+    if (*idx >= priv->can.echo_skb_max)
+        *idx -= priv->can.echo_skb_max;
+}
+
+/**
+ * es58x_can_free_echo_skb_tail() - Remove the oldest echo skb of the
+ *    loopback FIFO.
+ * @netdev: CAN network device.
+ *
+ * Naming convention: the tail is the beginning of the FIFO, i.e. the
+ * first skb to have entered the FIFO.
+ */
+static void es58x_can_free_echo_skb_tail(struct net_device *netdev)
+{
+    struct es58x_priv *priv = es58x_priv(netdev);
+    u16 tail_idx = priv->echo_skb_tail_idx;
+    struct sk_buff *skb = priv->can.echo_skb[tail_idx];
+    unsigned long flags;
+
+    netdev_completed_queue(netdev, 1, es58x_can_frame_bytes(skb));
+    can_free_echo_skb(netdev, tail_idx);
+
+    spin_lock_irqsave(&priv->echo_skb_spinlock, flags);
+    es58x_add_skb_idx(priv, &priv->echo_skb_tail_idx, 1);
+    priv->num_echo_skb--;
+    spin_unlock_irqrestore(&priv->echo_skb_spinlock, flags);
+
+    netdev->stats.tx_dropped++;
+}
+
+/**
+ * es58x_can_get_echo_skb_recovery() - Try to re-sync the loopback FIFO.
+ * @netdev: CAN network device.
+ * @packet_idx: Index
+ *
+ * This function should not be called under normal circumstances. In
+ * the unlikely case that one or several URB packages get dropped by
+ * the device, the index will get out of sync. Try to recover by
+ * dropping the echo skb packets with older indexes.
+ *
+ * Return: zero if recovery was successful, -EINVAL otherwise.
+ */
+static int es58x_can_get_echo_skb_recovery(struct net_device *netdev,
+                       u32 packet_idx)
+{
+    struct es58x_priv *priv = es58x_priv(netdev);
+    u32 current_packet_idx, first_packet_idx, num_echo_skb;
+    unsigned long flags;
+    int ret = 0;
+
+    netdev->stats.tx_errors++;
+
+    spin_lock_irqsave(&priv->echo_skb_spinlock, flags);
+    current_packet_idx = priv->current_packet_idx;
+    num_echo_skb = priv->num_echo_skb;
+    spin_unlock_irqrestore(&priv->echo_skb_spinlock, flags);
+    first_packet_idx = current_packet_idx - num_echo_skb;
+
+    if (net_ratelimit())
+        netdev_warn(netdev,
+                "Bad loopback packet index: %u. First index: %u, end