Signed-off-by: Gerhard Uttenthaler <uttenthaler@xxxxxxxxxxxxxxxx>
---
drivers/net/can/usb/ems_usb.c | 45 +++++++++++++++++++++++++++++++++++
1 file changed, 45 insertions(+)
diff --git a/drivers/net/can/usb/ems_usb.c b/drivers/net/can/usb/ems_usb.c
index d6b52b265536..a4d9a1b2d2f0 100644
--- a/drivers/net/can/usb/ems_usb.c
+++ b/drivers/net/can/usb/ems_usb.c
@@ -389,6 +389,47 @@ static void ems_usb_rx_can_msg(struct ems_usb *dev, struct ems_cpc_msg *msg)
netif_rx(skb);
}
+static void ems_usb_rx_canfd_msg(struct ems_usb *dev, struct ems_cpc_msg *msg)
+{
+ struct cpc_canfd_msg *fd_msg = &msg->msg.canfd_msg;
+
+ /* Although the CPC_FDFLAG_NONCANFD_MSG flag
+ * should not be set with a received message,
+ * it seems better to have checked it anyway.
+ */
+ if (!(fd_msg->flags & CPC_FDFLAG_NONCANFD_MSG)) {
+ /* CAN FD frame */
+ struct canfd_frame *cfd;
+ struct sk_buff *skb;
+ int i;
+ struct net_device_stats *stats = &dev->netdev->stats;
+
+ skb = alloc_canfd_skb(dev->netdev, &cfd);
+ if (!skb)
+ return;
+
+ cfd->can_id = le32_to_cpu(fd_msg->id);
+ cfd->len = fd_msg->length;
+
+ for (i = 0; i < cfd->len; i++)
+ cfd->data[i] = fd_msg->msg[i];
+
+ cfd->flags = 0;
+ if (fd_msg->flags & CPC_FDFLAG_BRS)
+ cfd->flags |= CANFD_BRS;
+
+ if (fd_msg->flags & CPC_FDFLAG_ESI)
+ cfd->flags |= CANFD_ESI;
+
+ if (fd_msg->flags & CPC_FDFLAG_XTD)
+ cfd->can_id |= CAN_EFF_FLAG;
+
+ stats->rx_packets++;
+ stats->rx_bytes += cfd->len;
+ netif_rx(skb);
+ }
+}
+
static void ems_usb_rx_err(struct ems_usb *dev, struct ems_cpc_msg *msg)
{
struct can_frame *cf;
@@ -513,6 +554,10 @@ static void ems_usb_read_bulk_callback(struct urb *urb)
ems_usb_rx_can_msg(dev, msg);
break;
+ case CPC_MSG_TYPE_CANFD_FRAME:
+ ems_usb_rx_canfd_msg(dev, msg);
+ break;
+
case CPC_MSG_TYPE_CAN_FRAME_ERROR:
/* Process errorframe */
ems_usb_rx_err(dev, msg);
--
2.26.2
--
EMS Dr. Thomas Wuensche e.K.
Sonnenhang 3
85304 Ilmmuenster
HR Ingolstadt, HRA 170106
Phone: +49-8441-490260
Fax : +49-8441-81860
http://www.ems-wuensche.com
