Added ems_usb_rx_canfd_msg routine for CAN FD frame reception
Signed-off-by: Gerhard Uttenthaler <uttenthaler@xxxxxxxxxxxxxxxx>
---
drivers/net/can/usb/ems_usb.c | 44 +++++++++++++++++++++++++++++++++++
1 file changed, 44 insertions(+)
diff --git a/drivers/net/can/usb/ems_usb.c b/drivers/net/can/usb/ems_usb.c
index 96012052a375..8ada663350fa 100644
--- a/drivers/net/can/usb/ems_usb.c
+++ b/drivers/net/can/usb/ems_usb.c
@@ -40,6 +40,7 @@ MODULE_LICENSE("GPL v2");
#define CPC_MSG_TYPE_OVERRUN 21 /* overrun events */
#define CPC_MSG_TYPE_CAN_FRAME_ERROR 23 /* detected bus errors */
#define CPC_MSG_TYPE_ERR_COUNTER 25 /* RX/TX error counter */
+#define CPC_MSG_TYPE_CANFD_FRAME 26 /* CAN FD frame */
/* Messages from the PC to the CPC interface */
#define CPC_CMD_TYPE_CAN_FRAME 1 /* CAN data frame */
@@ -388,6 +389,45 @@ 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;
+ 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;
+
+ memcpy(cfd->data, fd_msg->msg, cfd->len);
+
+ 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;
@@ -520,6 +560,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
