From 3f7d05dd18469927dff1cf5a7d35d67ec9fd7cdc Mon Sep 17 00:00:00 2001
From: Kevin O'Connor <kevin@koconnor.net>
Date: Sat, 11 Jun 2022 19:00:54 -0400
Subject: [PATCH] stm32: Support passing through RTR and EFF canbus frames

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
---
 src/generic/canbus.h |  3 +++
 src/stm32/can.c      | 15 ++++++++++++--
 src/stm32/fdcan.c    | 49 +++++++++++++++++++++-----------------------
 3 files changed, 39 insertions(+), 28 deletions(-)

diff --git a/src/generic/canbus.h b/src/generic/canbus.h
index a6463246..224d7037 100644
--- a/src/generic/canbus.h
+++ b/src/generic/canbus.h
@@ -16,6 +16,9 @@ struct canbus_msg {
     };
 };
 
+#define CANMSG_ID_RTR (1<<30)
+#define CANMSG_ID_EFF (1<<31)
+
 #define CANMSG_DATA_LEN(msg) ((msg)->dlc > 8 ? 8 : (msg)->dlc)
 
 // callbacks provided by board specific code
diff --git a/src/stm32/can.c b/src/stm32/can.c
index 99fe98dc..16623f4b 100644
--- a/src/stm32/can.c
+++ b/src/stm32/can.c
@@ -118,6 +118,12 @@ canbus_send(struct canbus_msg *msg)
     mb->TDHR = msg->data32[1];
 
     /* Request transmission */
+    uint32_t tir;
+    if (msg->id & CANMSG_ID_EFF)
+        tir = ((msg->id & 0x1fffffff) << CAN_TI0R_EXID_Pos) | CAN_TI0R_IDE;
+    else
+        tir = (msg->id & 0x7ff) << CAN_TI0R_STID_Pos;
+    tir |= msg->id & CANMSG_ID_RTR ? CAN_TI0R_RTR : 0;
     mb->TIR = (msg->id << CAN_TI0R_STID_Pos) | CAN_TI0R_TXRQ;
     return CANMSG_DATA_LEN(msg);
 }
@@ -131,7 +137,7 @@ canbus_set_filter(uint32_t id)
     /* Initialisation mode for the filter */
     SOC_CAN->FA1R = 0;
 
-    uint32_t mask = CAN_RI0R_STID | CAN_TI0R_IDE | CAN_TI0R_RTR;
+    uint32_t mask = CAN_TI0R_STID | CAN_TI0R_IDE | CAN_TI0R_RTR;
     SOC_CAN->sFilterRegister[0].FR1 = CANBUS_ID_ADMIN << CAN_RI0R_STID_Pos;
     SOC_CAN->sFilterRegister[0].FR2 = mask;
     SOC_CAN->sFilterRegister[1].FR1 = (id + 1) << CAN_RI0R_STID_Pos;
@@ -155,8 +161,13 @@ CAN_IRQHandler(void)
     if (SOC_CAN->RF0R & CAN_RF0R_FMP0) {
         // Read and ack data packet
         CAN_FIFOMailBox_TypeDef *mb = &SOC_CAN->sFIFOMailBox[0];
+        uint32_t rir = mb->RIR;
         struct canbus_msg msg;
-        msg.id = (mb->RIR >> CAN_RI0R_STID_Pos) & 0x7FF;
+        if (rir & CAN_RI0R_IDE)
+            msg.id = ((rir >> CAN_RI0R_EXID_Pos) & 0x1fffffff) | CANMSG_ID_EFF;
+        else
+            msg.id = (rir >> CAN_RI0R_STID_Pos) & 0x7ff;
+        msg.id |= rir & CAN_RI0R_RTR ? CANMSG_ID_RTR : 0;
         msg.dlc = mb->RDTR & CAN_RDT0R_DLC;
         msg.data32[0] = mb->RDLR;
         msg.data32[1] = mb->RDHR;
diff --git a/src/stm32/fdcan.c b/src/stm32/fdcan.c
index afd59b5d..8a462f76 100755
--- a/src/stm32/fdcan.c
+++ b/src/stm32/fdcan.c
@@ -17,38 +17,24 @@
 #include "internal.h" // enable_pclock
 #include "sched.h" // DECL_INIT
 
-typedef struct
-{
-       uint32_t RESERVED0 : 18;
-  __IO uint32_t ID : 11;
-  __IO uint32_t RTR : 1;
-  __IO uint32_t XTD : 1;
-  __IO uint32_t ESI : 1;
-  __IO uint32_t RXTS : 16;
-  __IO uint32_t DLC : 4;
-  __IO uint32_t BRS : 1;
-  __IO uint32_t FDF : 1;
-       uint32_t RESERVED1 : 2;
-  __IO uint32_t FIDX : 7;
-  __IO uint32_t ANMF : 1;
-  __IO uint32_t data[64 / 4];
-}FDCAN_RX_FIFO_TypeDef;
-
 typedef struct
 {
   __IO uint32_t id_section;
   __IO uint32_t dlc_section;
   __IO uint32_t data[64 / 4];
-}FDCAN_TX_FIFO_TypeDef;
+}FDCAN_FIFO_TypeDef;
+
+#define FDCAN_XTD (1<<30)
+#define FDCAN_RTR (1<<29)
 
 typedef struct
 {
   __IO uint32_t FLS[28]; // Filter list standard
   __IO uint32_t FLE[16]; // Filter list extended
-  FDCAN_RX_FIFO_TypeDef RXF0[3];
-  FDCAN_RX_FIFO_TypeDef RXF1[3];
+  FDCAN_FIFO_TypeDef RXF0[3];
+  FDCAN_FIFO_TypeDef RXF1[3];
   __IO uint32_t TEF[6]; // Tx event FIFO
-  FDCAN_TX_FIFO_TypeDef TXFIFO[3];
+  FDCAN_FIFO_TypeDef TXFIFO[3];
 }FDCAN_MSG_RAM_TypeDef;
 
 typedef struct
@@ -100,8 +86,14 @@ canbus_send(struct canbus_msg *msg)
         return -1;
 
     uint32_t w_index = ((txfqs & FDCAN_TXFQS_TFQPI) >> FDCAN_TXFQS_TFQPI_Pos);
-    FDCAN_TX_FIFO_TypeDef *txfifo = &MSG_RAM.TXFIFO[w_index];
-    txfifo->id_section = (msg->id & 0x1fffffff) << 18;
+    FDCAN_FIFO_TypeDef *txfifo = &MSG_RAM.TXFIFO[w_index];
+    uint32_t ids;
+    if (msg->id & CANMSG_ID_EFF)
+        ids = (msg->id & 0x1fffffff) | FDCAN_XTD;
+    else
+        ids = (msg->id & 0x7ff) << 18;
+    ids |= msg->id & CANMSG_ID_RTR ? FDCAN_RTR : 0;
+    txfifo->id_section = ids;
     txfifo->dlc_section = (msg->dlc & 0x0f) << 16;
     txfifo->data[0] = msg->data32[0];
     txfifo->data[1] = msg->data32[1];
@@ -155,10 +147,15 @@ CAN_IRQHandler(void)
         if (rxf0s & FDCAN_RXF0S_F0FL) {
             // Read and ack data packet
             uint32_t idx = (rxf0s & FDCAN_RXF0S_F0GI) >> FDCAN_RXF0S_F0GI_Pos;
-            FDCAN_RX_FIFO_TypeDef *rxf0 = &MSG_RAM.RXF0[idx];
+            FDCAN_FIFO_TypeDef *rxf0 = &MSG_RAM.RXF0[idx];
+            uint32_t ids = rxf0->id_section;
             struct canbus_msg msg;
-            msg.id = rxf0->ID;
-            msg.dlc = rxf0->DLC;
+            if (ids & FDCAN_XTD)
+                msg.id = (ids & 0x1fffffff) | CANMSG_ID_EFF;
+            else
+                msg.id = (ids >> 18) & 0x7ff;
+            msg.id |= ids & FDCAN_RTR ? CANMSG_ID_RTR : 0;
+            msg.dlc = (rxf0->dlc_section >> 16) & 0x0f;
             msg.data32[0] = rxf0->data[0];
             msg.data32[1] = rxf0->data[1];
             SOC_CAN->RXF0A = idx;