[PATCH 10/17] can: ems_usb: Added receive routine for CAN FD messages and its call in ems_usb_read_bulk_callback

[Gerhard Uttenthaler <uttenthaler@ems-wuensche.com>]

Signed-off-by: Gerhard Uttenthaler <uttenthaler@ems-wuensche.com>
---
 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