UNIMOC-Universal-Motor-Control · Tecnologic · Apr 19, 2022
diff --git a/os/hal/ports/STM32/LLD/FDCANv1/hal_can_lld.c b/os/hal/ports/STM32/LLD/FDCANv1/hal_can_lld.c
@@ -30,6 +30,7 @@
 /* Driver local definitions.                                                 */
 /*===========================================================================*/
 
+#if defined(STM32G4XX)
 /* Filter Standard Element Size in bytes.*/
 #define SRAMCAN_FLS_SIZE                (1U * 4U)
 
@@ -85,9 +86,31 @@
 #define SRAMCAN_TMSA  ((uint32_t)(SRAMCAN_TBSA +                            \
                                   (STM32_FDCAN_TB_NBR * SRAMCAN_TB_SIZE)))
 
-/* Message RAM size.*/
-#define SRAMCAN_SIZE  ((uint32_t)(SRAMCAN_TMSA +                            \
+/* Message RAM stride.*/
+#define SRAMCAN_STRIDE  ((uint32_t)(SRAMCAN_TMSA +                            \
                                   (STM32_FDCAN_TM_NBR * SRAMCAN_TM_SIZE)))
+#else
+/* Message RAM stride is 0 because it is shared.*/
+#define SRAMCAN_STRIDE  0
+
+/* RX FIFO 0 Start Address.*/
+#define SRAMCAN_RF0SA(fdcan)      ((fdcan->RXF0C & FDCAN_RXF0C_F0SA) >> FDCAN_RXF0C_F0SA_Pos)
+
+/* RX FIFO 0 Size */
+#define SRAMCAN_RF0_SIZE(fdcan)   bfs_to_element_size[((fdcan->RXESC & FDCAN_RXESC_F0DS) >> FDCAN_RXESC_F0DS_Pos)]
+
+/* RX FIFO 1 Start Address.*/
+#define SRAMCAN_RF1SA(fdcan)      ((fdcan->RXF1C & FDCAN_RXF1C_F1SA) >> FDCAN_RXF1C_F1SA_Pos)
+
+/* RX FIFO 1 Size */
+#define SRAMCAN_RF1_SIZE(fdcan)   bfs_to_element_size[((fdcan->RXESC & FDCAN_RXESC_F1DS) >> FDCAN_RXESC_F1DS_Pos)]
+
+/* TX Buffers Start Address.*/
+#define SRAMCAN_TBSA(fdcan)       ((fdcan->TXBC & FDCAN_TXBC_TBSA) >> FDCAN_TXBC_TBSA_Pos)
+
+/* TX Buffers Size */
+#define SRAMCAN_TB_SIZE(fdcan)     bfs_to_element_size[((fdcan->TXESC & FDCAN_TXESC_TBDS) >> FDCAN_TXESC_TBDS_Pos)]
+#endif /* STM32G4XX */
 
 #define TIMEOUT_INIT_MS                 250U
 #define TIMEOUT_CSA_MS                  250U
@@ -119,6 +142,9 @@ static const uint8_t dlc_to_bytes[] = {
   0U,  1U,  2U,  3U,  4U,  5U,  6U,  7U,
   8U, 12U, 16U, 20U, 24U, 32U, 48U, 64U
 };
+static const uint8_t bfs_to_element_size[] = {
+  4U,  5U,  6U,  7U,  8U, 10U, 14U, 18U
+};
 
 static uint32_t canclk;
 
@@ -133,7 +159,7 @@ static bool fdcan_clock_stop(CANDriver *canp) {
   canp->fdcan->CCCR |= FDCAN_CCCR_CSR;
   start = osalOsGetSystemTimeX();
   end = osalTimeAddX(start, TIME_MS2I(TIMEOUT_INIT_MS));
-  while ((canp->fdcan->CCCR & FDCAN_CCCR_CSA) != 0U) {
+  while ((canp->fdcan->CCCR & FDCAN_CCCR_CSA) == 0U) {
     if (!osalTimeIsInRangeX(osalOsGetSystemTimeX(), start, end)) {
       return true;
     }
@@ -201,21 +227,21 @@ void can_lld_init(void) {
   /* Driver initialization.*/
   canObjectInit(&CAND1);
   CAND1.fdcan = FDCAN1;
-  CAND1.ram_base = (uint32_t *)(SRAMCAN_BASE + 0U * SRAMCAN_SIZE);
+  CAND1.ram_base = (uint32_t *)(SRAMCAN_BASE + 0U * SRAMCAN_STRIDE);
 #endif
 
 #if STM32_CAN_USE_FDCAN2
   /* Driver initialization.*/
   canObjectInit(&CAND2);
   CAND2.fdcan = FDCAN2;
-  CAND2.ram_base = (uint32_t *)(SRAMCAN_BASE + 1U * SRAMCAN_SIZE);
+  CAND2.ram_base = (uint32_t *)(SRAMCAN_BASE + 1U * SRAMCAN_STRIDE);
 #endif
 
 #if STM32_CAN_USE_FDCAN3
   /* Driver initialization.*/
   canObjectInit(&CAND3);
   CAND3.fdcan = FDCAN3;
-  CAND3.ram_base = (uint32_t *)(SRAMCAN_BASE + 2U * SRAMCAN_SIZE);
+  CAND3.ram_base = (uint32_t *)(SRAMCAN_BASE + 2U * SRAMCAN_STRIDE);
 #endif
 }
 
@@ -237,8 +263,8 @@ bool can_lld_start(CANDriver *canp) {
   /* If it is the first activation then performing some extra
      initializations.*/
   if (canclk == 0U) {
-    for (uint32_t *wp = canp->ram_base;
-         wp < canp->ram_base + SRAMCAN_SIZE;
+    for (uint32_t *wp = (uint32_t *)SRAMCAN_BASE;
+         wp < (uint32_t *)(SRAMCAN_BASE + STM32_FDCAN_SRAM_SIZE);
          wp += 1U) {
       *wp = (uint32_t)0U;
     }
@@ -275,15 +301,15 @@ bool can_lld_start(CANDriver *canp) {
   }
 
   /* Configuration can be performed now.*/
-  canp->fdcan->CCCR   = FDCAN_CCCR_CCE;
+  canp->fdcan->CCCR  |= FDCAN_CCCR_CCE;
+  canp->fdcan->CCCR &= ~(FDCAN_CCCR_CSR | FDCAN_CCCR_CSA);
 
   /* Setting up operation mode except driver-controlled bits.*/
   canp->fdcan->NBTP   = canp->config->NBTP;
   canp->fdcan->DBTP   = canp->config->DBTP;
   canp->fdcan->CCCR  |= canp->config->CCCR & ~(FDCAN_CCCR_CSR | FDCAN_CCCR_CSA |
                                                FDCAN_CCCR_CCE | FDCAN_CCCR_INIT);
   canp->fdcan->TEST   = canp->config->TEST;
-  canp->fdcan->RXGFC  = canp->config->RXGFC;
 
   /* Enabling interrupts, only using interrupt zero.*/
   canp->fdcan->IR     = (uint32_t)-1;
@@ -293,6 +319,24 @@ bool can_lld_start(CANDriver *canp) {
   canp->fdcan->TXBTIE = FDCAN_TXBTIE_TIE;
   canp->fdcan->ILE    = FDCAN_ILE_EINT0;
 
+#if defined(STM32G4XX)
+  canp->fdcan->RXGFC = canp->config->RXGFC;
+  canp->fdcan->TXBC  = 0;
+#else
+  canp->fdcan->GFC   = canp->config->GFC;
+
+  /* H7 version of FDCAN has configurable memory layout, so configure it */
+  canp->fdcan->SIDFC = canp->config->SIDFC;
+  canp->fdcan->XIDFC = canp->config->XIDFC;
+  canp->fdcan->RXF0C = canp->config->RXF0C;
+  canp->fdcan->RXF1C = canp->config->RXF1C;
+  canp->fdcan->RXBC  = canp->config->RXBC;
+  canp->fdcan->TXEFC = canp->config->TXEFC;
+  canp->fdcan->TXBC  = canp->config->TXBC;
+  canp->fdcan->RXESC = canp->config->RXESC;
+  canp->fdcan->TXESC = canp->config->TXESC;
+#endif /* STM32G4XX */
+
   /* Going in active mode.*/
   if (fdcan_active_mode(canp)) {
     osalDbgAssert(false, "CAN initialization failed, check clocks and pin config");
@@ -386,8 +430,13 @@ void can_lld_transmit(CANDriver *canp, canmbx_t mailbox, const CANTxFrame *ctfp)
   put_index = ((canp->fdcan->TXFQS & FDCAN_TXFQS_TFQPI) >> FDCAN_TXFQS_TFQPI_Pos);
 
   /* Writing frame.*/
+#if defined(STM32G4XX)
   tx_address = canp->ram_base +
                ((SRAMCAN_TBSA + (put_index * SRAMCAN_TB_SIZE)) / sizeof (uint32_t));
+#else
+  tx_address = canp->ram_base +
+               ((SRAMCAN_TBSA(canp->fdcan) + (put_index * SRAMCAN_TB_SIZE(canp->fdcan))));
+#endif /* STM32G4XX */
 
   *tx_address++ = ctfp->header32[0];
   *tx_address++ = ctfp->header32[1];
@@ -454,14 +503,24 @@ void can_lld_receive(CANDriver *canp, canmbx_t mailbox, CANRxFrame *crfp) {
   if (mailbox == 1U) {
      /* GET index RXF0, add it and the length to the rx_address.*/
      get_index = (canp->fdcan->RXF0S & FDCAN_RXF0S_F0GI_Msk) >> FDCAN_RXF0S_F0GI_Pos;
+#if defined(STM32G4XX)
      rx_address = canp->ram_base + (SRAMCAN_RF0SA +
                                     (get_index * SRAMCAN_RF0_SIZE)) / sizeof (uint32_t);
+#else
+     rx_address = canp->ram_base + (SRAMCAN_RF0SA(canp->fdcan) +
+                                    (get_index * SRAMCAN_RF0_SIZE(canp->fdcan)));
+#endif /* STM32G4XX */
   }
   else {
      /* GET index RXF1, add it and the length to the rx_address.*/
      get_index = (canp->fdcan->RXF1S & FDCAN_RXF1S_F1GI_Msk) >> FDCAN_RXF1S_F1GI_Pos;
+#if defined(STM32G4XX)
      rx_address = canp->ram_base + (SRAMCAN_RF1SA +
                                     (get_index * SRAMCAN_RF1_SIZE)) / sizeof (uint32_t);
+#else
+     rx_address = canp->ram_base + (SRAMCAN_RF1SA(canp->fdcan) +
+                                    (get_index * SRAMCAN_RF1_SIZE(canp->fdcan)));
+#endif /* STM32G4XX */
   }
   crfp->header32[0] = *rx_address++;
   crfp->header32[1] = *rx_address++;

diff --git a/os/hal/ports/STM32/LLD/FDCANv1/hal_can_lld.h b/os/hal/ports/STM32/LLD/FDCANv1/hal_can_lld.h
@@ -111,6 +111,11 @@
 #error "CAN driver activated but no FDCAN peripheral assigned"
 #endif
 
+#if !defined(STM32_FDCAN_SRAM_SIZE)
+#error "STM32_FDCAN_SRAM_SIZE not defined in registry"
+#endif
+
+#if defined(STM32G4XX)
 #if !defined(STM32_FDCAN_FLS_NBR)
 #error "STM32_FDCAN_FLS_NBR not defined in registry"
 #endif
@@ -142,6 +147,7 @@
 #if !defined(STM32_FDCAN_TM_NBR)
 #error "STM32_FDCAN_TM_NBR not defined in registry"
 #endif
+#endif /* STM32G4XX */
 
 /*===========================================================================*/
 /* Driver data structures and types.                                         */
@@ -330,10 +336,53 @@ typedef struct hal_can_config {
    * @brief   Test configuration register.
    */
   uint32_t                  TEST;
+#if defined(STM32G4XX)
   /**
    * @brief   Global filter configuration register.
    */
   uint32_t                  RXGFC;
+#else
+  /**
+   * @brief   Global filter configuration register.
+   */
+  uint32_t                  GFC;
+  /**
+   * @brief   Standard ID filter configuration register.
+   */
+  uint32_t                  SIDFC;
+  /**
+   * @brief   Extended ID filter configuration register.
+   */
+  uint32_t                  XIDFC;
+  /**
+   * @brief   Rx FIFO 0 configuration register.
+   */
+  uint32_t                  RXF0C;
+  /**
+   * @brief   Rx FIFO 1 configuration register.
+   */
+  uint32_t                  RXF1C;
+  /**
+   * @brief   Rx buffer configuration register.
+   */
+  uint32_t                  RXBC;
+  /**
+   * @brief   Tx event FIFO configuration register.
+   */
+  uint32_t                  TXEFC;
+  /**
+   * @brief   Tx buffer configuration register.
+   */
+  uint32_t                  TXBC;
+  /**
+   * @brief   Rx buffer element size configuration register.
+   */
+  uint32_t                  RXESC;
+  /**
+   * @brief   Tx buffer element size configuration register.
+   */
+  uint32_t                  TXESC;
+#endif  /* STM32G4XX */
 } CANConfig;
 
 /**

diff --git a/os/hal/ports/STM32/STM32G4xx/stm32_registry.h b/os/hal/ports/STM32/STM32G4xx/stm32_registry.h
@@ -138,6 +138,7 @@
 #define STM32_FDCAN_TEF_NBR                 3U
 #define STM32_FDCAN_TB_NBR                  3U
 #define STM32_FDCAN_TM_NBR                  0U
+#define STM32_FDCAN_SRAM_SIZE               2544U
 
 /* DAC attributes.*/
 #define STM32_HAS_DAC1_CH1                  TRUE
@@ -357,6 +358,7 @@
 #define STM32_FDCAN_TEF_NBR                 3U
 #define STM32_FDCAN_TB_NBR                  3U
 #define STM32_FDCAN_TM_NBR                  0U
+#define STM32_FDCAN_SRAM_SIZE               1696U
 
 /* DAC attributes.*/
 #define STM32_HAS_DAC1_CH1                  TRUE
@@ -576,6 +578,7 @@
 #define STM32_FDCAN_TEF_NBR                 3U
 #define STM32_FDCAN_TB_NBR                  3U
 #define STM32_FDCAN_TM_NBR                  0U
+#define STM32_FDCAN_SRAM_SIZE               848U
 
 /* DAC attributes.*/
 #define STM32_HAS_DAC1_CH1                  TRUE

diff --git a/os/hal/ports/STM32/STM32H7xx/stm32_registry.h b/os/hal/ports/STM32/STM32H7xx/stm32_registry.h
@@ -124,14 +124,7 @@
 #define STM32_HAS_FDCAN1                    TRUE
 #define STM32_HAS_FDCAN2                    TRUE
 #define STM32_HAS_FDCAN3                    FALSE
-#define STM32_FDCAN_FLS_NBR                 128U
-#define STM32_FDCAN_FLE_NBR                 128U
-#define STM32_FDCAN_RF0_NBR                 64U
-#define STM32_FDCAN_RF1_NBR                 64U
-#define STM32_FDCAN_RB_NBR                  64U
-#define STM32_FDCAN_TEF_NBR                 32U
-#define STM32_FDCAN_TB_NBR                  32U
-#define STM32_FDCAN_TM_NBR                  64U
+#define STM32_FDCAN_SRAM_SIZE               10240U
 
 /* DAC attributes.*/
 #define STM32_HAS_DAC1_CH1                  TRUE
@@ -362,14 +355,7 @@
 #define STM32_HAS_FDCAN1                    TRUE
 #define STM32_HAS_FDCAN2                    TRUE
 #define STM32_HAS_FDCAN3                    TRUE
-#define STM32_FDCAN_FLS_NBR                 128U
-#define STM32_FDCAN_FLE_NBR                 128U
-#define STM32_FDCAN_RF0_NBR                 64U
-#define STM32_FDCAN_RF1_NBR                 64U
-#define STM32_FDCAN_RB_NBR                  64U
-#define STM32_FDCAN_TEF_NBR                 32U
-#define STM32_FDCAN_TB_NBR                  32U
-#define STM32_FDCAN_TM_NBR                  64U
+#define STM32_FDCAN_SRAM_SIZE               10240U
 
 /* DAC attributes.*/
 #define STM32_HAS_DAC1_CH1                  TRUE