canbus: Rework CAN command protocol

Rework the micro-controller command protocol so that it supports
direct communication with the serialqueue.c code.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>

1 files changed, 10 insertions, 18 deletions

diff --git a/src/stm32/can.c b/src/stm32/can.c
index c4d45f9c..867bbfd7 100644
--- a/src/stm32/can.c
+++ b/src/stm32/can.c
@@ -159,43 +159,35 @@ canbus_send(uint32_t id, uint32_t len, uint8_t *data)
     return len;
 }
 
-#define CAN_FILTER_NUMBER 0
-
 // Setup the receive packet filter
 void
 canbus_set_filter(uint32_t id)
 {
-    uint32_t filternbrbitpos = 1 << CAN_FILTER_NUMBER;
-
     /* Select the start slave bank */
     SOC_CAN->FMR |= CAN_FMR_FINIT;
     /* Initialisation mode for the filter */
     SOC_CAN->FA1R = 0;
 
-    uint32_t idadmin = CANBUS_ID_UUID;
-    SOC_CAN->sFilterRegister[CAN_FILTER_NUMBER].FR1 = idadmin << (5 + 16);
-    SOC_CAN->sFilterRegister[CAN_FILTER_NUMBER].FR2 = id << (5 + 16);
+    uint32_t mask = CAN_RI0R_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;
+    SOC_CAN->sFilterRegister[1].FR2 = mask;
+    SOC_CAN->sFilterRegister[2].FR1 = id << CAN_RI0R_STID_Pos;
+    SOC_CAN->sFilterRegister[2].FR2 = mask;
 
-    /* Identifier list mode for the filter */
-    SOC_CAN->FM1R = filternbrbitpos;
     /* 32-bit scale for the filter */
-    SOC_CAN->FS1R = filternbrbitpos;
+    SOC_CAN->FS1R = (1<<0) | (1<<1) | (1<<2);
 
     /* FIFO 0 assigned for the filter */
     SOC_CAN->FFA1R = 0;
 
     /* Filter activation */
-    SOC_CAN->FA1R = filternbrbitpos;
+    SOC_CAN->FA1R = (1<<0) | (id ? (1<<1) | (1<<2) : 0);
     /* Leave the initialisation mode for the filter */
     SOC_CAN->FMR &= ~CAN_FMR_FINIT;
 }
 
-void
-canbus_reboot(void)
-{
-    NVIC_SystemReset();
-}
-
 // This function handles CAN global interrupts
 void
 CAN_IRQHandler(void)
@@ -292,7 +284,7 @@ can_init(void)
         ;
 
     /*##-2- Configure the CAN Filter #######################################*/
-    canbus_set_filter(CANBUS_ID_SET);
+    canbus_set_filter(0);
 
     /*##-3- Configure Interrupts #################################*/
     armcm_enable_irq(CAN_IRQHandler, CAN_RX0_IRQn, 0);