Merge pull request #10 from arduino/fix-can-extended-id-rx

Fix: correctly pass-through received frames with both standard and extended CAN id

Author: aentinger

recipes-kernel/kernel-modules/x8h7/x8h7_can.c

@@ -70,8 +70,8 @@ RX1IE: Receive Buffer 1 Full I      questo non serve
 #define X8H7_CAN_STS_FLG_EWARN   0x40  // Error Warning
 #define X8H7_CAN_STS_FLG_TX_OVR  0x80  // Transmit Buffer Overflow
 
-#define CAN_FRAME_MAX_DATA_LEN	8
-#define X8H7_CAN_HEADER_SIZE    5
+#define X8H7_CAN_HEADER_SIZE        5
+#define X8H7_CAN_FRAME_MAX_DATA_LEN 8
 
 #define AFTER_SUSPEND_UP      1
 #define AFTER_SUSPEND_DOWN    2
@@ -89,6 +89,17 @@ struct x8h7_can_filter {
   u32   mask;
 };
 
+union x8h7_can_message
+{
+  struct __attribute__((packed))
+  {
+    uint32_t id;
+    uint8_t  len;
+    uint8_t  data[X8H7_CAN_FRAME_MAX_DATA_LEN];
+  } field;
+  uint8_t buf[X8H7_CAN_HEADER_SIZE + X8H7_CAN_FRAME_MAX_DATA_LEN];
+};
+
 /**
  */
 struct x8h7_can_priv {
@@ -257,7 +268,7 @@ static void x8h7_can_hook(void *arg, x8h7_pkt_t *pkt)
 
   switch(pkt->opcode) {
   case X8H7_CAN_OC_RECV:
-    if (pkt->size < 5) {
+    if (pkt->size < X8H7_CAN_HEADER_SIZE) {
       DBG_ERROR("received packed is too short (%d)\n", pkt->size);
       return;
     } else {
@@ -270,10 +281,19 @@ static void x8h7_can_hook(void *arg, x8h7_pkt_t *pkt)
         priv->net->stats.rx_dropped++;
         return;
       }
-      frame->can_id = (pkt->data[3] << 24) | (pkt->data[2] << 16) |
-                      (pkt->data[1] <<  8) | pkt->data[0];
-      frame->can_dlc = get_can_dlc(pkt->data[4] & 0x0F);
-      memcpy(frame->data, pkt->data + 5, frame->can_dlc);
+
+      /* Copy header from raw byte-stream onto union. */
+      union x8h7_can_message x8h7_can_msg;
+      memcpy(x8h7_can_msg.buf, pkt->data, X8H7_CAN_HEADER_SIZE);
+
+      /* Extract can_id and can_dlc. Note: x8h7_can_message uses the exact
+       * same flags for signaling extended/standard id mode or remote
+       * retransmit request as struct can_frame.
+       */
+      frame->can_id  = x8h7_can_msg.field.id;
+      frame->can_dlc = x8h7_can_msg.field.len;
+      memcpy(frame->data, pkt->data + X8H7_CAN_HEADER_SIZE, frame->can_dlc);
+
       priv->net->stats.rx_packets++;
       priv->net->stats.rx_bytes += frame->can_dlc;
       can_led_event(priv->net, CAN_LED_EVENT_RX);
@@ -502,29 +522,20 @@ static void x8h7_can_hw_rx(struct x8h7_can_priv *priv)
  */
 static void x8h7_can_hw_tx(struct x8h7_can_priv *priv, struct can_frame *frame)
 {
-  union
-  {
-    struct __attribute__((packed))
-    {
-      uint32_t id;                           // 29 bit identifier
-      uint8_t  len;                          // Length of data field in bytes
-      uint8_t  data[CAN_FRAME_MAX_DATA_LEN]; // Data field
-    } field;
-    uint8_t buf[X8H7_CAN_HEADER_SIZE + CAN_FRAME_MAX_DATA_LEN];
-  } can_msg;
+  union x8h7_can_message x8h7_can_msg;
 
   DBG_PRINT("\n");
 
   if (frame->can_id & CAN_EFF_FLAG)
-    can_msg.field.id  = CAN_EFF_FLAG | (frame->can_id & CAN_EFF_MASK);
+    x8h7_can_msg.field.id  = CAN_EFF_FLAG | (frame->can_id & CAN_EFF_MASK);
   else
-    can_msg.field.id  =                (frame->can_id & CAN_SFF_MASK);
+    x8h7_can_msg.field.id  =                (frame->can_id & CAN_SFF_MASK);
 
-  can_msg.field.len = (frame->can_dlc <= CAN_FRAME_MAX_DATA_LEN) ? frame->can_dlc : CAN_FRAME_MAX_DATA_LEN;
-  memcpy(can_msg.field.data, frame->data, can_msg.field.len);
+  x8h7_can_msg.field.len = (frame->can_dlc <= X8H7_CAN_FRAME_MAX_DATA_LEN) ? frame->can_dlc : X8H7_CAN_FRAME_MAX_DATA_LEN;
+  memcpy(x8h7_can_msg.field.data, frame->data, x8h7_can_msg.field.len);
 
 #ifdef DEBUG
-  char  data_str[CAN_FRAME_MAX_DATA_LEN * 4] = {0};
+  char  data_str[X8H7_CAN_FRAME_MAX_DATA_LEN * 4] = {0};
   int   i = 0, len = 0;
 
   for (i = 0; (i < frame->can_dlc) && (len < sizeof(data_str)); i++)
@@ -534,8 +545,8 @@ static void x8h7_can_hw_tx(struct x8h7_can_priv *priv, struct can_frame *frame)
   DBG_PRINT("Send CAN frame to H7: id = %08X, len = %d, data = [%s ]\n", can_msg.field.id, can_msg.field.len, data_str);
 #endif
 
-  uint16_t const bytes_to_send = X8H7_CAN_HEADER_SIZE + can_msg.field.len; /* Send 4-Byte ID, 1-Byte Length and the required number of data bytes. */
-  x8h7_pkt_enq(priv->periph, X8H7_CAN_OC_SEND, bytes_to_send, can_msg.buf);
+  uint16_t const bytes_to_send = X8H7_CAN_HEADER_SIZE + x8h7_can_msg.field.len; /* Send 4-Byte ID, 1-Byte Length and the required number of data bytes. */
+  x8h7_pkt_enq(priv->periph, X8H7_CAN_OC_SEND, bytes_to_send, x8h7_can_msg.buf);
   x8h7_pkt_send();
 }
 
@@ -557,8 +568,8 @@ static void x8h7_can_tx_work_handler(struct work_struct *ws)
       DBG_PRINT("Send frame\n");
       frame = (struct can_frame *)priv->tx_skb->data;
 
-      if (frame->can_dlc > CAN_FRAME_MAX_DATA_LEN) {
-        frame->can_dlc = CAN_FRAME_MAX_DATA_LEN;
+      if (frame->can_dlc > X8H7_CAN_FRAME_MAX_DATA_LEN) {
+        frame->can_dlc = X8H7_CAN_FRAME_MAX_DATA_LEN;
       }
 
       x8h7_can_hw_tx(priv, frame);

recipes-kernel/linux-firmware/linux-firmware-arduino-portenta-x8-stm32h7_git.bb

@@ -16,7 +16,7 @@ SRC_URI = " \
     file://monitor-m4-elf-file.path \
     file://monitor-m4-elf-file.service \
 "
-SRCREV = "96bcacb22b87f6f9169d1d01d531a4de641a3d63"
+SRCREV = "665930e420b74c685a07bb05feb5a5677d8f1e65"
 PV = "0.0.2"
 
 S = "${WORKDIR}/git"