diff --git a/os/hal/ports/STM32/LLD/FDCANv1/hal_can_lld.c b/os/hal/ports/STM32/LLD/FDCANv1/hal_can_lld.c
index 928c7e644a..b6b46ee070 100644
--- a/os/hal/ports/STM32/LLD/FDCANv1/hal_can_lld.c
+++ b/os/hal/ports/STM32/LLD/FDCANv1/hal_can_lld.c
@@ -134,7 +134,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;
     }
@@ -276,7 +276,8 @@ 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;
@@ -284,7 +285,13 @@ bool can_lld_start(CANDriver *canp) {
   canp->fdcan->CCCR  |= canp->config->CCCR & ~(FDCAN_CCCR_CSR | FDCAN_CCCR_CSA |
                                                FDCAN_CCCR_CCE | FDCAN_CCCR_INIT);
   canp->fdcan->TEST   = canp->config->TEST;
+#ifdef STM32G4XX
   canp->fdcan->RXGFC  = canp->config->RXGFC;
+#elif defined(STM32H7XX)
+  canp->fdcan->GFC    = canp->config->RXGFC;
+#else
+#error "Unsupported STM32 for FDCAN LLD driver"
+#endif
 
   /* Enabling interrupts, only using interrupt zero.*/
   canp->fdcan->IR     = (uint32_t)-1;
@@ -294,6 +301,22 @@ bool can_lld_start(CANDriver *canp) {
   canp->fdcan->TXBTIE = FDCAN_TXBTIE_TIE;
   canp->fdcan->ILE    = FDCAN_ILE_EINT0;
 
+#ifdef STM32H7XX
+  /* H7 version of FDCAN has configurable memory layout, so configure it */
+  canp->fdcan->SIDFC = STM32_FDCAN_FLS_NBR << 16 | SRAMCAN_FLSSA;
+  canp->fdcan->XIDFC = STM32_FDCAN_FLE_NBR << 16 | SRAMCAN_FLESA;
+  canp->fdcan->RXF0C = STM32_FDCAN_RF0_NBR << 16 | SRAMCAN_RF0SA;
+  canp->fdcan->RXF1C = STM32_FDCAN_RF1_NBR << 16 | SRAMCAN_RF1SA;
+  canp->fdcan->RXBC  = SRAMCAN_RBSA;
+  canp->fdcan->TXEFC = STM32_FDCAN_TEF_NBR << 16 | SRAMCAN_TEFSA;
+  /* NB: this doesn't set NDTB, but sets TFQS to run in queue mode with no dedicated buffers */
+  canp->fdcan->TXBC  = STM32_FDCAN_TB_NBR  << 24 | SRAMCAN_TBSA;
+
+  /* set to use the full 18-byte size buffer elements */
+  canp->fdcan->TXESC = 0x007;
+  canp->fdcan->RXESC = 0x777;
+#endif /* STM32H7XX */
+
   /* Going in active mode.*/
   if (fdcan_active_mode(canp)) {
     osalDbgAssert(false, "CAN initialization failed, check clocks and pin config");
diff --git a/os/hal/ports/STM32/STM32H7xx/stm32_registry.h b/os/hal/ports/STM32/STM32H7xx/stm32_registry.h
index 7ae52a3e90..d3118dd7a7 100644
--- a/os/hal/ports/STM32/STM32H7xx/stm32_registry.h
+++ b/os/hal/ports/STM32/STM32H7xx/stm32_registry.h
@@ -123,14 +123,14 @@
 #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_FLS_NBR                 64U
+#define STM32_FDCAN_FLE_NBR                 64U
+#define STM32_FDCAN_RF0_NBR                 56U
+#define STM32_FDCAN_RF1_NBR                 56U
+#define STM32_FDCAN_RB_NBR                  0U
+#define STM32_FDCAN_TEF_NBR                 0U
 #define STM32_FDCAN_TB_NBR                  32U
-#define STM32_FDCAN_TM_NBR                  64U
+#define STM32_FDCAN_TM_NBR                  0U
 
 /* DAC attributes.*/
 #define STM32_HAS_DAC1_CH1                  TRUE