diff --git a/demos/STM32/RT-STM32H755ZI-NUCLEO144/cfg/mcuconf.h b/demos/STM32/RT-STM32H755ZI-NUCLEO144/cfg/mcuconf.h
index 5efb41767..070e5d0c7 100644
--- a/demos/STM32/RT-STM32H755ZI-NUCLEO144/cfg/mcuconf.h
+++ b/demos/STM32/RT-STM32H755ZI-NUCLEO144/cfg/mcuconf.h
@@ -184,6 +184,11 @@
 #define STM32_IRQ_EXTI19_PRIORITY           6
 #define STM32_IRQ_EXTI20_21_PRIORITY        6
 
+#define STM32_IRQ_FDCAN1_IT0_PRIORITY       10
+#define STM32_IRQ_FDCAN1_IT1_PRIORITY       10
+#define STM32_IRQ_FDCAN2_IT0_PRIORITY       10
+#define STM32_IRQ_FDCAN2_IT1_PRIORITY       10
+
 #define STM32_IRQ_MDMA_PRIORITY             9
 
 #define STM32_IRQ_QUADSPI1_PRIORITY         10
@@ -235,7 +240,7 @@
 /*
  * CAN driver system settings.
  */
-#define STM32_CAN_USE_FDCAN1                FALSE
+#define STM32_CAN_USE_FDCAN1                TRUE
 #define STM32_CAN_USE_FDCAN2                FALSE
 
 /*
diff --git a/os/hal/ports/STM32/LLD/ADCv1/hal_adc_lld.c b/os/hal/ports/STM32/LLD/ADCv1/hal_adc_lld.c
index eeeca40d2..8c0eadc41 100644
--- a/os/hal/ports/STM32/LLD/ADCv1/hal_adc_lld.c
+++ b/os/hal/ports/STM32/LLD/ADCv1/hal_adc_lld.c
@@ -90,7 +90,7 @@ static void adc_lld_stop_adc(ADC_TypeDef *adc) {
 }
 
 /**
- * @brief   ADC DMA ISR service routine.
+ * @brief   ADC DMA service routine.
  *
  * @param[in] adcp      pointer to the @p ADCDriver object
  * @param[in] flags     pre-shifted content of the ISR register
diff --git a/os/hal/ports/STM32/LLD/ADCv2/hal_adc_lld.c b/os/hal/ports/STM32/LLD/ADCv2/hal_adc_lld.c
index c983ee372..53a5439cd 100644
--- a/os/hal/ports/STM32/LLD/ADCv2/hal_adc_lld.c
+++ b/os/hal/ports/STM32/LLD/ADCv2/hal_adc_lld.c
@@ -67,7 +67,7 @@ ADCDriver ADCD3;
 /*===========================================================================*/
 
 /**
- * @brief   ADC DMA ISR service routine.
+ * @brief   ADC DMA service routine.
  *
  * @param[in] adcp      pointer to the @p ADCDriver object
  * @param[in] flags     pre-shifted content of the ISR register
diff --git a/os/hal/ports/STM32/LLD/ADCv3/hal_adc_lld.c b/os/hal/ports/STM32/LLD/ADCv3/hal_adc_lld.c
index ed2e311b2..af7a861eb 100644
--- a/os/hal/ports/STM32/LLD/ADCv3/hal_adc_lld.c
+++ b/os/hal/ports/STM32/LLD/ADCv3/hal_adc_lld.c
@@ -244,7 +244,7 @@ static void adc_lld_stop_adc(ADCDriver *adcp) {
 }
 
 /**
- * @brief   ADC DMA ISR service routine.
+ * @brief   ADC DMA service routine.
  *
  * @param[in] adcp      pointer to the @p ADCDriver object
  * @param[in] flags     pre-shifted content of the ISR register
@@ -274,7 +274,7 @@ static void adc_lld_serve_dma_interrupt(ADCDriver *adcp, uint32_t flags) {
 }
 
 /**
- * @brief   ADC ISR service routine.
+ * @brief   ADC IRQ service routine.
  *
  * @param[in] adcp      pointer to the @p ADCDriver object
  * @param[in] isr       content of the ISR register
diff --git a/os/hal/ports/STM32/LLD/ADCv4/hal_adc_lld.c b/os/hal/ports/STM32/LLD/ADCv4/hal_adc_lld.c
index 77d72c215..cc4e5eec7 100644
--- a/os/hal/ports/STM32/LLD/ADCv4/hal_adc_lld.c
+++ b/os/hal/ports/STM32/LLD/ADCv4/hal_adc_lld.c
@@ -187,7 +187,7 @@ static void adc_lld_stop_adc(ADCDriver *adcp) {
 }
 
 /**
- * @brief   ADC DMA ISR service routine.
+ * @brief   ADC DMA service routine.
  *
  * @param[in] adcp      pointer to the @p ADCDriver object
  * @param[in] flags     pre-shifted content of the ISR register
@@ -217,7 +217,7 @@ static void adc_lld_serve_dma_interrupt(ADCDriver *adcp, uint32_t flags) {
 }
 
 /**
- * @brief   ADC BDMA ISR service routine.
+ * @brief   ADC BDMA service routine.
  *
  * @param[in] adcp      pointer to the @p ADCDriver object
  * @param[in] flags     pre-shifted content of the ISR register
@@ -247,7 +247,7 @@ static void adc_lld_serve_bdma_interrupt(ADCDriver *adcp, uint32_t flags) {
 }
 
 /**
- * @brief   ADC ISR service routine.
+ * @brief   ADC IRQ service routine.
  *
  * @param[in] adcp      pointer to the @p ADCDriver object
  * @param[in] isr       content of the ISR register
diff --git a/os/hal/ports/STM32/LLD/FDCANv1/driver.mk b/os/hal/ports/STM32/LLD/FDCANv1/driver.mk
index 608a9273f..734c3a020 100644
--- a/os/hal/ports/STM32/LLD/FDCANv1/driver.mk
+++ b/os/hal/ports/STM32/LLD/FDCANv1/driver.mk
@@ -1,9 +1,9 @@
 ifeq ($(USE_SMART_BUILD),yes)
 ifneq ($(findstring HAL_USE_CAN TRUE,$(HALCONF)),)
-PLATFORMSRC += $(CHIBIOS)/os/hal/ports/STM32/LLD/CANv1/hal_can_lld.c
+PLATFORMSRC += $(CHIBIOS)/os/hal/ports/STM32/LLD/FDCANv1/hal_can_lld.c
 endif
 else
-PLATFORMSRC += $(CHIBIOS)/os/hal/ports/STM32/LLD/CANv1/hal_can_lld.c
+PLATFORMSRC += $(CHIBIOS)/os/hal/ports/STM32/LLD/FDCANv1/hal_can_lld.c
 endif
 
-PLATFORMINC += $(CHIBIOS)/os/hal/ports/STM32/LLD/CANv1
+PLATFORMINC += $(CHIBIOS)/os/hal/ports/STM32/LLD/FDCANv1
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 d3c32400d..aa31c884a 100644
--- a/os/hal/ports/STM32/LLD/FDCANv1/hal_can_lld.c
+++ b/os/hal/ports/STM32/LLD/FDCANv1/hal_can_lld.c
@@ -52,544 +52,10 @@ CANDriver CAND2;
 /* Driver local functions.                                                   */
 /*===========================================================================*/
 
-/**
- * @brief   Programs the filters of CAN 1 and CAN 2.
- *
- * @param[in] canp      pointer to the @p CANDriver object
- * @param[in] can2sb    number of the first filter assigned to CAN2
- * @param[in] num       number of entries in the filters array, if zero then
- *                      a default filter is programmed
- * @param[in] cfp       pointer to the filters array, can be @p NULL if
- *                      (num == 0)
- *
- * @notapi
- */
-static void can_lld_set_filters(CANDriver* canp,
-                                uint32_t can2sb,
-                                uint32_t num,
-                                const CANFilter *cfp) {
-
-#if STM32_CAN_USE_CAN2
-  if (canp == &CAND2) {
-    /* Set handle to CAN1, because CAN1 manages the filters of CAN2.*/
-    canp = &CAND1;
-  }
-#endif
-
-  /* Temporarily enabling CAN clock.*/
-#if STM32_CAN_USE_CAN1
-  if (canp == &CAND1) {
-    rccEnableCAN1(true);
-    /* Filters initialization.*/
-    canp->can->FMR = (canp->can->FMR & 0xFFFF0000) | CAN_FMR_FINIT;
-    canp->can->FMR = (canp->can->FMR & 0xFFFF0000) | (can2sb << 8) | CAN_FMR_FINIT;
-  }
-#endif
-
-#if STM32_CAN_USE_CAN3
-  if (canp == &CAND3) {
-    rccEnableCAN3(true);
-    /* Filters initialization.*/
-    canp->can->FMR = (canp->can->FMR & 0xFFFF0000) | CAN_FMR_FINIT;
-  }
-#endif
-
-  if (num > 0) {
-    uint32_t i, fmask;
-
-    /* All filters cleared.*/
-    canp->can->FA1R = 0;
-    canp->can->FM1R = 0;
-    canp->can->FS1R = 0;
-    canp->can->FFA1R = 0;
-
-#if STM32_CAN_USE_CAN1
-    if (canp == &CAND1) {
-      for (i = 0; i < STM32_CAN_MAX_FILTERS; i++) {
-        canp->can->sFilterRegister[i].FR1 = 0;
-        canp->can->sFilterRegister[i].FR2 = 0;
-      }
-    }
-#endif
-
-#if STM32_CAN_USE_CAN3
-    if (canp == &CAND3) {
-      for (i = 0; i < STM32_CAN3_MAX_FILTERS; i++) {
-        canp->can->sFilterRegister[i].FR1 = 0;
-        canp->can->sFilterRegister[i].FR2 = 0;
-      }
-    }
-#endif
-
-    /* Scanning the filters array.*/
-    for (i = 0; i < num; i++) {
-      fmask = 1 << cfp->filter;
-      if (cfp->mode)
-        canp->can->FM1R |= fmask;
-      if (cfp->scale)
-        canp->can->FS1R |= fmask;
-      if (cfp->assignment)
-        canp->can->FFA1R |= fmask;
-      canp->can->sFilterRegister[cfp->filter].FR1 = cfp->register1;
-      canp->can->sFilterRegister[cfp->filter].FR2 = cfp->register2;
-      canp->can->FA1R |= fmask;
-      cfp++;
-    }
-  }
-  else {
-    /* Setting up a single default filter that enables everything for both
-       CANs.*/
-    canp->can->sFilterRegister[0].FR1 = 0;
-    canp->can->sFilterRegister[0].FR2 = 0;
-#if STM32_CAN_USE_CAN2
-    if (canp == &CAND1) {
-      canp->can->sFilterRegister[can2sb].FR1 = 0;
-      canp->can->sFilterRegister[can2sb].FR2 = 0;
-    }
-#endif
-    canp->can->FM1R = 0;
-    canp->can->FFA1R = 0;
-    canp->can->FS1R = 1;
-    canp->can->FA1R = 1;
-#if STM32_CAN_USE_CAN2
-    if (canp == &CAND1) {
-      canp->can->FS1R |= 1 << can2sb;
-      canp->can->FA1R |= 1 << can2sb;
-    }
-#endif
-  }
-  canp->can->FMR &= ~CAN_FMR_FINIT;
-
-  /* Clock disabled, it will be enabled again in can_lld_start().*/
-  /* Temporarily enabling CAN clock.*/
-#if STM32_CAN_USE_CAN1
-  if (canp == &CAND1) {
-    rccDisableCAN1();
-  }
-#endif
-#if STM32_CAN_USE_CAN3
-  if (canp == &CAND3) {
-    rccDisableCAN3();
-  }
-#endif
-}
-
-/**
- * @brief   Common TX ISR handler.
- *
- * @param[in] canp      pointer to the @p CANDriver object
- *
- * @notapi
- */
-static void can_lld_tx_handler(CANDriver *canp) {
-  uint32_t tsr;
-  eventflags_t flags;
-
-  /* Clearing IRQ sources.*/
-  tsr = canp->can->TSR;
-  canp->can->TSR = tsr;
-
-  /* Flags to be signaled through the TX event source.*/
-  flags = 0U;
-
-  /* Checking mailbox 0.*/
-  if ((tsr & CAN_TSR_RQCP0) != 0U) {
-    if ((tsr & (CAN_TSR_ALST0 | CAN_TSR_TERR0)) != 0U) {
-      flags |= CAN_MAILBOX_TO_MASK(1U) << 16U;
-    }
-    else {
-      flags |= CAN_MAILBOX_TO_MASK(1U);
-    }
-  }
-
-  /* Checking mailbox 1.*/
-  if ((tsr & CAN_TSR_RQCP1) != 0U) {
-    if ((tsr & (CAN_TSR_ALST1 | CAN_TSR_TERR1)) != 0U) {
-      flags |= CAN_MAILBOX_TO_MASK(2U) << 16U;
-    }
-    else {
-      flags |= CAN_MAILBOX_TO_MASK(2U);
-    }
-  }
-
-  /* Checking mailbox 2.*/
-  if ((tsr & CAN_TSR_RQCP2) != 0U) {
-    if ((tsr & (CAN_TSR_ALST2 | CAN_TSR_TERR2)) != 0U) {
-      flags |= CAN_MAILBOX_TO_MASK(3U) << 16U;
-    }
-    else {
-      flags |= CAN_MAILBOX_TO_MASK(3U);
-    }
-  }
-
-  /* Signaling flags and waking up threads waiting for a transmission slot.*/
-  _can_tx_empty_isr(canp, flags);
-}
-
-/**
- * @brief   Common RX0 ISR handler.
- *
- * @param[in] canp      pointer to the @p CANDriver object
- *
- * @notapi
- */
-static void can_lld_rx0_handler(CANDriver *canp) {
-  uint32_t rf0r;
-
-  rf0r = canp->can->RF0R;
-  if ((rf0r & CAN_RF0R_FMP0) > 0) {
-    /* No more receive events until the queue 0 has been emptied.*/
-    canp->can->IER &= ~CAN_IER_FMPIE0;
-    _can_rx_full_isr(canp, CAN_MAILBOX_TO_MASK(1U));
-  }
-  if ((rf0r & CAN_RF0R_FOVR0) > 0) {
-    /* Overflow events handling.*/
-    canp->can->RF0R = CAN_RF0R_FOVR0;
-    _can_error_isr(canp, CAN_OVERFLOW_ERROR);
-  }
-}
-
-/**
- * @brief   Common RX1 ISR handler.
- *
- * @param[in] canp      pointer to the @p CANDriver object
- *
- * @notapi
- */
-static void can_lld_rx1_handler(CANDriver *canp) {
-  uint32_t rf1r;
-
-  rf1r = canp->can->RF1R;
-  if ((rf1r & CAN_RF1R_FMP1) > 0) {
-    /* No more receive events until the queue 0 has been emptied.*/
-    canp->can->IER &= ~CAN_IER_FMPIE1;
-    _can_rx_full_isr(canp, CAN_MAILBOX_TO_MASK(2U));
-  }
-  if ((rf1r & CAN_RF1R_FOVR1) > 0) {
-    /* Overflow events handling.*/
-    canp->can->RF1R = CAN_RF1R_FOVR1;
-    _can_error_isr(canp, CAN_OVERFLOW_ERROR);
-  }
-}
-
-/**
- * @brief   Common SCE ISR handler.
- *
- * @param[in] canp      pointer to the @p CANDriver object
- *
- * @notapi
- */
-static void can_lld_sce_handler(CANDriver *canp) {
-  uint32_t msr;
-
-  /* Clearing IRQ sources.*/
-  msr = canp->can->MSR;
-  canp->can->MSR = msr;
-
-  /* Wakeup event.*/
-#if CAN_USE_SLEEP_MODE
-  if (msr & CAN_MSR_WKUI) {
-    canp->state = CAN_READY;
-    canp->can->MCR &= ~CAN_MCR_SLEEP;
-    _can_wakeup_isr(canp);
-  }
-#endif /* CAN_USE_SLEEP_MODE */
-  /* Error event.*/
-  if (msr & CAN_MSR_ERRI) {
-    eventflags_t flags;
-    uint32_t esr = canp->can->ESR;
-
-#if STM32_CAN_REPORT_ALL_ERRORS
-    flags = (eventflags_t)(esr & 7);
-    if ((esr & CAN_ESR_LEC) > 0)
-      flags |= CAN_FRAMING_ERROR;
-#else
-    flags = 0;
-#endif
-
-    /* The content of the ESR register is copied unchanged in the upper
-       half word of the listener flags mask.*/
-    _can_error_isr(canp, flags | (eventflags_t)(esr << 16U));
-  }
-}
-
 /*===========================================================================*/
 /* Driver interrupt handlers.                                                */
 /*===========================================================================*/
 
-#if STM32_CAN_USE_CAN1 || defined(__DOXYGEN__)
-#if defined(STM32_CAN1_UNIFIED_HANDLER)
-/**
- * @brief   CAN1 unified interrupt handler.
- *
- * @isr
- */
-OSAL_IRQ_HANDLER(STM32_CAN1_UNIFIED_HANDLER) {
-
-  OSAL_IRQ_PROLOGUE();
-
-  can_lld_tx_handler(&CAND1);
-  can_lld_rx0_handler(&CAND1);
-  can_lld_rx1_handler(&CAND1);
-  can_lld_sce_handler(&CAND1);
-
-  OSAL_IRQ_EPILOGUE();
-}
-#else /* !defined(STM32_CAN1_UNIFIED_HANDLER) */
-
-#if !defined(STM32_CAN1_TX_HANDLER)
-#error "STM32_CAN1_TX_HANDLER not defined"
-#endif
-#if !defined(STM32_CAN1_RX0_HANDLER)
-#error "STM32_CAN1_RX0_HANDLER not defined"
-#endif
-#if !defined(STM32_CAN1_RX1_HANDLER)
-#error "STM32_CAN1_RX1_HANDLER not defined"
-#endif
-#if !defined(STM32_CAN1_SCE_HANDLER)
-#error "STM32_CAN1_SCE_HANDLER not defined"
-#endif
-
-/**
- * @brief   CAN1 TX interrupt handler.
- *
- * @isr
- */
-OSAL_IRQ_HANDLER(STM32_CAN1_TX_HANDLER) {
-
-  OSAL_IRQ_PROLOGUE();
-
-  can_lld_tx_handler(&CAND1);
-
-  OSAL_IRQ_EPILOGUE();
-}
-
-/**
- * @brief   CAN1 RX0 interrupt handler.
- *
- * @isr
- */
-OSAL_IRQ_HANDLER(STM32_CAN1_RX0_HANDLER) {
-
-  OSAL_IRQ_PROLOGUE();
-
-  can_lld_rx0_handler(&CAND1);
-
-  OSAL_IRQ_EPILOGUE();
-}
-
-/**
- * @brief   CAN1 RX1 interrupt handler.
- *
- * @isr
- */
-OSAL_IRQ_HANDLER(STM32_CAN1_RX1_HANDLER) {
-
-  OSAL_IRQ_PROLOGUE();
-
-  can_lld_rx1_handler(&CAND1);
-
-  OSAL_IRQ_EPILOGUE();
-}
-
-/**
- * @brief   CAN1 SCE interrupt handler.
- *
- * @isr
- */
-OSAL_IRQ_HANDLER(STM32_CAN1_SCE_HANDLER) {
-
-  OSAL_IRQ_PROLOGUE();
-
-  can_lld_sce_handler(&CAND1);
-
-  OSAL_IRQ_EPILOGUE();
-}
-#endif /* !defined(STM32_CAN1_UNIFIED_HANDLER) */
-#endif /* STM32_CAN_USE_CAN1 */
-
-#if STM32_CAN_USE_CAN2 || defined(__DOXYGEN__)
-#if defined(STM32_CAN2_UNIFIED_HANDLER)
-/**
- * @brief   CAN1 unified interrupt handler.
- *
- * @isr
- */
-OSAL_IRQ_HANDLER(STM32_CAN2_UNIFIED_HANDLER) {
-
-  OSAL_IRQ_PROLOGUE();
-
-  can_lld_tx_handler(&CAND2);
-  can_lld_rx0_handler(&CAND2);
-  can_lld_rx1_handler(&CAND2);
-  can_lld_sce_handler(&CAND2);
-
-  OSAL_IRQ_EPILOGUE();
-}
-#else /* !defined(STM32_CAN2_UNIFIED_HANDLER) */
-
-#if !defined(STM32_CAN1_TX_HANDLER)
-#error "STM32_CAN1_TX_HANDLER not defined"
-#endif
-#if !defined(STM32_CAN1_RX0_HANDLER)
-#error "STM32_CAN1_RX0_HANDLER not defined"
-#endif
-#if !defined(STM32_CAN1_RX1_HANDLER)
-#error "STM32_CAN1_RX1_HANDLER not defined"
-#endif
-#if !defined(STM32_CAN1_SCE_HANDLER)
-#error "STM32_CAN1_SCE_HANDLER not defined"
-#endif
-
-/**
- * @brief   CAN2 TX interrupt handler.
- *
- * @isr
- */
-OSAL_IRQ_HANDLER(STM32_CAN2_TX_HANDLER) {
-
-  OSAL_IRQ_PROLOGUE();
-
-  can_lld_tx_handler(&CAND2);
-
-  OSAL_IRQ_EPILOGUE();
-}
-
-/**
- * @brief   CAN2 RX0 interrupt handler.
- *
- * @isr
- */
-OSAL_IRQ_HANDLER(STM32_CAN2_RX0_HANDLER) {
-
-  OSAL_IRQ_PROLOGUE();
-
-  can_lld_rx0_handler(&CAND2);
-
-  OSAL_IRQ_EPILOGUE();
-}
-
-/**
- * @brief   CAN2 RX1 interrupt handler.
- *
- * @isr
- */
-OSAL_IRQ_HANDLER(STM32_CAN2_RX1_HANDLER) {
-
-  OSAL_IRQ_PROLOGUE();
-
-  can_lld_rx1_handler(&CAND2);
-
-  OSAL_IRQ_EPILOGUE();
-}
-
-/**
- * @brief   CAN2 SCE interrupt handler.
- *
- * @isr
- */
-OSAL_IRQ_HANDLER(STM32_CAN2_SCE_HANDLER) {
-
-  OSAL_IRQ_PROLOGUE();
-
-  can_lld_sce_handler(&CAND2);
-
-  OSAL_IRQ_EPILOGUE();
-}
-#endif /* !defined(STM32_CAN2_UNIFIED_HANDLER) */
-#endif /* STM32_CAN_USE_CAN2 */
-
-#if STM32_CAN_USE_CAN3 || defined(__DOXYGEN__)
-#if defined(STM32_CAN3_UNIFIED_HANDLER)
-/**
- * @brief   CAN1 unified interrupt handler.
- *
- * @isr
- */
-OSAL_IRQ_HANDLER(STM32_CAN3_UNIFIED_HANDLER) {
-
-  OSAL_IRQ_PROLOGUE();
-
-  can_lld_tx_handler(&CAND3);
-  can_lld_rx0_handler(&CAND3);
-  can_lld_rx1_handler(&CAND3);
-  can_lld_sce_handler(&CAND3);
-
-  OSAL_IRQ_EPILOGUE();
-}
-#else /* !defined(STM32_CAN3_UNIFIED_HANDLER) */
-
-#if !defined(STM32_CAN3_TX_HANDLER)
-#error "STM32_CAN3_TX_HANDLER not defined"
-#endif
-#if !defined(STM32_CAN3_RX0_HANDLER)
-#error "STM32_CAN3_RX0_HANDLER not defined"
-#endif
-#if !defined(STM32_CAN3_RX1_HANDLER)
-#error "STM32_CAN3_RX1_HANDLER not defined"
-#endif
-#if !defined(STM32_CAN3_SCE_HANDLER)
-#error "STM32_CAN3_SCE_HANDLER not defined"
-#endif
-
-/**
- * @brief   CAN3 TX interrupt handler.
- *
- * @isr
- */
-OSAL_IRQ_HANDLER(STM32_CAN3_TX_HANDLER) {
-
-  OSAL_IRQ_PROLOGUE();
-
-  can_lld_tx_handler(&CAND3);
-
-  OSAL_IRQ_EPILOGUE();
-}
-
-/**
- * @brief   CAN3 RX0 interrupt handler.
- *
- * @isr
- */
-OSAL_IRQ_HANDLER(STM32_CAN3_RX0_HANDLER) {
-
-  OSAL_IRQ_PROLOGUE();
-
-  can_lld_rx0_handler(&CAND3);
-
-  OSAL_IRQ_EPILOGUE();
-}
-
-/**
- * @brief   CAN1 RX3 interrupt handler.
- *
- * @isr
- */
-OSAL_IRQ_HANDLER(STM32_CAN3_RX1_HANDLER) {
-
-  OSAL_IRQ_PROLOGUE();
-
-  can_lld_rx1_handler(&CAND3);
-
-  OSAL_IRQ_EPILOGUE();
-}
-
-/**
- * @brief   CAN1 SCE interrupt handler.
- *
- * @isr
- */
-OSAL_IRQ_HANDLER(STM32_CAN3_SCE_HANDLER) {
-
-  OSAL_IRQ_PROLOGUE();
-
-  can_lld_sce_handler(&CAND3);
-
-  OSAL_IRQ_EPILOGUE();
-}
-#endif /* !defined(STM32_CAN1_UNIFIED_HANDLER) */
-#endif /* STM32_CAN_USE_CAN1 */
-
 /*===========================================================================*/
 /* Driver exported functions.                                                */
 /*===========================================================================*/
@@ -601,61 +67,16 @@ OSAL_IRQ_HANDLER(STM32_CAN3_SCE_HANDLER) {
  */
 void can_lld_init(void) {
 
-#if STM32_CAN_USE_CAN1
+#if STM32_CAN_USE_FDCAN1
   /* Driver initialization.*/
   canObjectInit(&CAND1);
-  CAND1.can = CAN1;
-#if defined(STM32_CAN1_UNIFIED_NUMBER)
-    nvicEnableVector(STM32_CAN1_UNIFIED_NUMBER, STM32_CAN_CAN1_IRQ_PRIORITY);
-#else
-    nvicEnableVector(STM32_CAN1_TX_NUMBER, STM32_CAN_CAN1_IRQ_PRIORITY);
-    nvicEnableVector(STM32_CAN1_RX0_NUMBER, STM32_CAN_CAN1_IRQ_PRIORITY);
-    nvicEnableVector(STM32_CAN1_RX1_NUMBER, STM32_CAN_CAN1_IRQ_PRIORITY);
-    nvicEnableVector(STM32_CAN1_SCE_NUMBER, STM32_CAN_CAN1_IRQ_PRIORITY);
-#endif
+  CAND1.fdcan = FDCAN1;
 #endif
 
-#if STM32_CAN_USE_CAN2
+#if STM32_CAN_USE_FDCAN2
   /* Driver initialization.*/
   canObjectInit(&CAND2);
-  CAND2.can = CAN2;
-#if defined(STM32_CAN2_UNIFIED_NUMBER)
-    nvicEnableVector(STM32_CAN2_UNIFIED_NUMBER, STM32_CAN_CAN2_IRQ_PRIORITY);
-#else
-    nvicEnableVector(STM32_CAN2_TX_NUMBER, STM32_CAN_CAN2_IRQ_PRIORITY);
-    nvicEnableVector(STM32_CAN2_RX0_NUMBER, STM32_CAN_CAN2_IRQ_PRIORITY);
-    nvicEnableVector(STM32_CAN2_RX1_NUMBER, STM32_CAN_CAN2_IRQ_PRIORITY);
-    nvicEnableVector(STM32_CAN2_SCE_NUMBER, STM32_CAN_CAN2_IRQ_PRIORITY);
-#endif
-#endif
-
-#if STM32_CAN_USE_CAN3
-  /* Driver initialization.*/
-  canObjectInit(&CAND3);
-  CAND3.can = CAN3;
-#if defined(STM32_CAN3_UNIFIED_NUMBER)
-    nvicEnableVector(STM32_CAN3_UNIFIED_NUMBER, STM32_CAN_CAN3_IRQ_PRIORITY);
-#else
-    nvicEnableVector(STM32_CAN3_TX_NUMBER, STM32_CAN_CAN3_IRQ_PRIORITY);
-    nvicEnableVector(STM32_CAN3_RX0_NUMBER, STM32_CAN_CAN3_IRQ_PRIORITY);
-    nvicEnableVector(STM32_CAN3_RX1_NUMBER, STM32_CAN_CAN3_IRQ_PRIORITY);
-    nvicEnableVector(STM32_CAN3_SCE_NUMBER, STM32_CAN_CAN3_IRQ_PRIORITY);
-#endif
-#endif
-
-  /* Filters initialization.*/
-#if STM32_CAN_USE_CAN1
-#if STM32_HAS_CAN2
-  can_lld_set_filters(&CAND1, STM32_CAN_MAX_FILTERS / 2, 0, NULL);
-#else
-  can_lld_set_filters(&CAND1, STM32_CAN_MAX_FILTERS, 0, NULL);
-#endif
-#endif
-
-#if STM32_HAS_CAN3
-#if STM32_CAN_USE_CAN3
-  can_lld_set_filters(&CAND3, STM32_CAN3_MAX_FILTERS, 0, NULL);
-#endif
+  CAND2.fdcan = FDCAN2;
 #endif
 }
 
@@ -669,44 +90,17 @@ void can_lld_init(void) {
 void can_lld_start(CANDriver *canp) {
 
   /* Clock activation.*/
-#if STM32_CAN_USE_CAN1
+#if STM32_CAN_USE_FDCAN1
   if (&CAND1 == canp) {
-    rccEnableCAN1(true);
+    rccEnableFDCAN1(true);
   }
 #endif
 
-#if STM32_CAN_USE_CAN2
+#if STM32_CAN_USE_FDCAN2
   if (&CAND2 == canp) {
-    rccEnableCAN1(true);    /* CAN 2 requires CAN1, so enabling it first.*/
-    rccEnableCAN2(true);
+    rccEnableFDCAN2(true);
   }
 #endif
-
-#if STM32_CAN_USE_CAN3
-  if (&CAND3 == canp) {
-    rccEnableCAN3(true);
-  }
-#endif
-
-  /* Configuring CAN. */
-  canp->can->MCR = CAN_MCR_INRQ;
-  while ((canp->can->MSR & CAN_MSR_INAK) == 0)
-    osalThreadSleepS(1);
-  canp->can->BTR = canp->config->btr;
-  canp->can->MCR = canp->config->mcr;
-
-  /* Interrupt sources initialization.*/
-#if STM32_CAN_REPORT_ALL_ERRORS
-  canp->can->IER = CAN_IER_TMEIE  | CAN_IER_FMPIE0 | CAN_IER_FMPIE1 |
-                   CAN_IER_WKUIE  | CAN_IER_ERRIE  | CAN_IER_LECIE  |
-                   CAN_IER_BOFIE  | CAN_IER_EPVIE  | CAN_IER_EWGIE  |
-                   CAN_IER_FOVIE0 | CAN_IER_FOVIE1;
-#else
-  canp->can->IER = CAN_IER_TMEIE  | CAN_IER_FMPIE0 | CAN_IER_FMPIE1 |
-                   CAN_IER_WKUIE  | CAN_IER_ERRIE  |
-                   CAN_IER_BOFIE  | CAN_IER_EPVIE  | CAN_IER_EWGIE  |
-                   CAN_IER_FOVIE0 | CAN_IER_FOVIE1;
-#endif
 }
 
 /**
@@ -720,40 +114,15 @@ void can_lld_stop(CANDriver *canp) {
 
   /* If in ready state then disables the CAN peripheral.*/
   if (canp->state == CAN_READY) {
-#if STM32_CAN_USE_CAN1
+#if STM32_CAN_USE_FDCAN1
     if (&CAND1 == canp) {
-      CAN1->MCR = 0x00010002;                   /* Register reset value.    */
-      CAN1->IER = 0x00000000;                   /* All sources disabled.    */
-#if STM32_CAN_USE_CAN2
-      /* If CAND2 is stopped then CAN1 clock is stopped here.*/
-      if (CAND2.state == CAN_STOP)
-#endif
-      {
-        rccDisableCAN1();
-      }
+      rccDisableFDCAN1();
     }
 #endif
 
-#if STM32_CAN_USE_CAN2
+#if STM32_CAN_USE_FDCAN2
     if (&CAND2 == canp) {
-      CAN2->MCR = 0x00010002;                   /* Register reset value.    */
-      CAN2->IER = 0x00000000;                   /* All sources disabled.    */
-#if STM32_CAN_USE_CAN1
-      /* If CAND1 is stopped then CAN1 clock is stopped here.*/
-      if (CAND1.state == CAN_STOP)
-#endif
-      {
-        rccDisableCAN1();
-      }
-      rccDisableCAN2();
-    }
-#endif
-
-#if STM32_CAN_USE_CAN3
-    if (&CAND3 == canp) {
-      CAN3->MCR = 0x00010002;                   /* Register reset value.    */
-      CAN3->IER = 0x00000000;                   /* All sources disabled.    */
-      rccDisableCAN3();
+      rccDisableFDCAN2();
     }
 #endif
   }
@@ -773,18 +142,6 @@ void can_lld_stop(CANDriver *canp) {
  */
 bool can_lld_is_tx_empty(CANDriver *canp, canmbx_t mailbox) {
 
-  switch (mailbox) {
-  case CAN_ANY_MAILBOX:
-    return (canp->can->TSR & CAN_TSR_TME) != 0;
-  case 1:
-    return (canp->can->TSR & CAN_TSR_TME0) != 0;
-  case 2:
-    return (canp->can->TSR & CAN_TSR_TME1) != 0;
-  case 3:
-    return (canp->can->TSR & CAN_TSR_TME2) != 0;
-  default:
-    return false;
-  }
 }
 
 /**
@@ -799,37 +156,7 @@ bool can_lld_is_tx_empty(CANDriver *canp, canmbx_t mailbox) {
 void can_lld_transmit(CANDriver *canp,
                       canmbx_t mailbox,
                       const CANTxFrame *ctfp) {
-  uint32_t tir;
-  CAN_TxMailBox_TypeDef *tmbp;
 
-  /* Pointer to a free transmission mailbox.*/
-  switch (mailbox) {
-  case CAN_ANY_MAILBOX:
-    tmbp = &canp->can->sTxMailBox[(canp->can->TSR & CAN_TSR_CODE) >> 24];
-    break;
-  case 1:
-    tmbp = &canp->can->sTxMailBox[0];
-    break;
-  case 2:
-    tmbp = &canp->can->sTxMailBox[1];
-    break;
-  case 3:
-    tmbp = &canp->can->sTxMailBox[2];
-    break;
-  default:
-    return;
-  }
-
-  /* Preparing the message.*/
-  if (ctfp->IDE)
-    tir = ((uint32_t)ctfp->EID << 3) | ((uint32_t)ctfp->RTR << 1) |
-          CAN_TI0R_IDE;
-  else
-    tir = ((uint32_t)ctfp->SID << 21) | ((uint32_t)ctfp->RTR << 1);
-  tmbp->TDTR = ctfp->DLC;
-  tmbp->TDLR = ctfp->data32[0];
-  tmbp->TDHR = ctfp->data32[1];
-  tmbp->TIR  = tir | CAN_TI0R_TXRQ;
 }
 
 /**
@@ -846,17 +173,6 @@ void can_lld_transmit(CANDriver *canp,
  */
 bool can_lld_is_rx_nonempty(CANDriver *canp, canmbx_t mailbox) {
 
-  switch (mailbox) {
-  case CAN_ANY_MAILBOX:
-    return ((canp->can->RF0R & CAN_RF0R_FMP0) != 0 ||
-            (canp->can->RF1R & CAN_RF1R_FMP1) != 0);
-  case 1:
-    return (canp->can->RF0R & CAN_RF0R_FMP0) != 0;
-  case 2:
-    return (canp->can->RF1R & CAN_RF1R_FMP1) != 0;
-  default:
-    return false;
-  }
 }
 
 /**
@@ -868,67 +184,8 @@ bool can_lld_is_rx_nonempty(CANDriver *canp, canmbx_t mailbox) {
  *
  * @notapi
  */
-void can_lld_receive(CANDriver *canp,
-                     canmbx_t mailbox,
-                     CANRxFrame *crfp) {
-  uint32_t rir, rdtr;
+void can_lld_receive(CANDriver *canp, canmbx_t mailbox, CANRxFrame *crfp) {
 
-  if (mailbox == CAN_ANY_MAILBOX) {
-    if ((canp->can->RF0R & CAN_RF0R_FMP0) != 0)
-      mailbox = 1;
-    else if ((canp->can->RF1R & CAN_RF1R_FMP1) != 0)
-      mailbox = 2;
-    else {
-      /* Should not happen, do nothing.*/
-      return;
-    }
-  }
-  switch (mailbox) {
-  case 1:
-    /* Fetches the message.*/
-    rir  = canp->can->sFIFOMailBox[0].RIR;
-    rdtr = canp->can->sFIFOMailBox[0].RDTR;
-    crfp->data32[0] = canp->can->sFIFOMailBox[0].RDLR;
-    crfp->data32[1] = canp->can->sFIFOMailBox[0].RDHR;
-
-    /* Releases the mailbox.*/
-    canp->can->RF0R = CAN_RF0R_RFOM0;
-
-    /* If the queue is empty re-enables the interrupt in order to generate
-       events again.*/
-    if ((canp->can->RF0R & CAN_RF0R_FMP0) == 0)
-      canp->can->IER |= CAN_IER_FMPIE0;
-    break;
-  case 2:
-    /* Fetches the message.*/
-    rir  = canp->can->sFIFOMailBox[1].RIR;
-    rdtr = canp->can->sFIFOMailBox[1].RDTR;
-    crfp->data32[0] = canp->can->sFIFOMailBox[1].RDLR;
-    crfp->data32[1] = canp->can->sFIFOMailBox[1].RDHR;
-
-    /* Releases the mailbox.*/
-    canp->can->RF1R = CAN_RF1R_RFOM1;
-
-    /* If the queue is empty re-enables the interrupt in order to generate
-       events again.*/
-    if ((canp->can->RF1R & CAN_RF1R_FMP1) == 0)
-      canp->can->IER |= CAN_IER_FMPIE1;
-    break;
-  default:
-    /* Should not happen, do nothing.*/
-    return;
-  }
-
-  /* Decodes the various fields in the RX frame.*/
-  crfp->RTR = (rir & CAN_RI0R_RTR) >> 1;
-  crfp->IDE = (rir & CAN_RI0R_IDE) >> 2;
-  if (crfp->IDE)
-    crfp->EID = rir >> 3;
-  else
-    crfp->SID = rir >> 21;
-  crfp->DLC = rdtr & CAN_RDT0R_DLC;
-  crfp->FMI = (uint8_t)(rdtr >> 8);
-  crfp->TIME = (uint16_t)(rdtr >> 16);
 }
 
 /**
@@ -939,10 +196,8 @@ void can_lld_receive(CANDriver *canp,
  *
  * @notapi
  */
-void can_lld_abort(CANDriver *canp,
-                   canmbx_t mailbox) {
+void can_lld_abort(CANDriver *canp, canmbx_t mailbox) {
 
-  canp->can->TSR = 128U << ((mailbox - 1U) * 8U);
 }
 
 #if CAN_USE_SLEEP_MODE || defined(__DOXYGEN__)
@@ -955,7 +210,6 @@ void can_lld_abort(CANDriver *canp,
  */
 void can_lld_sleep(CANDriver *canp) {
 
-  canp->can->MCR |= CAN_MCR_SLEEP;
 }
 
 /**
@@ -967,53 +221,31 @@ void can_lld_sleep(CANDriver *canp) {
  */
 void can_lld_wakeup(CANDriver *canp) {
 
-  canp->can->MCR &= ~CAN_MCR_SLEEP;
 }
 #endif /* CAN_USE_SLEEP_MODE */
 
-/**
- * @brief   Programs the filters.
- * @note    This is an STM32-specific API.
- *
- * @param[in] canp      pointer to the @p CANDriver object
- * @param[in] can2sb    number of the first filter assigned to CAN2
- * @param[in] num       number of entries in the filters array, if zero then
- *                      a default filter is programmed
- * @param[in] cfp       pointer to the filters array, can be @p NULL if
- *                      (num == 0)
- *
- * @api
- */
-void canSTM32SetFilters(CANDriver *canp, uint32_t can2sb,
-                        uint32_t num, const CANFilter *cfp) {
-
-#if STM32_CAN_USE_CAN2
-  osalDbgCheck((can2sb <= STM32_CAN_MAX_FILTERS) &&
-               (num <= STM32_CAN_MAX_FILTERS));
-#endif
-
-#if STM32_CAN_USE_CAN1
-  osalDbgAssert(CAND1.state == CAN_STOP, "invalid state");
-#endif
-#if STM32_CAN_USE_CAN2
-  osalDbgAssert(CAND2.state == CAN_STOP, "invalid state");
-#endif
-#if STM32_CAN_USE_CAN3
-  osalDbgAssert(CAND3.state == CAN_STOP, "invalid state");
-#endif
-
-#if STM32_CAN_USE_CAN1
-  if (canp == &CAND1) {
-    can_lld_set_filters(canp, can2sb, num, cfp);
-  }
-#endif
-#if STM32_CAN_USE_CAN3
-  if (canp == &CAND3) {
-    can_lld_set_filters(canp, can2sb, num, cfp);
-  }
-#endif
-}
-
 #endif /* HAL_USE_CAN */
 
+/**
+ * @brief   FDCAN IRQ0 service routine.
+ *
+ * @param[in] canp      pointer to the @p CANDriver object
+ *
+ * @notapi
+ */
+void can_lld_serve_interrupt0(CANDriver *canp) {
+
+}
+
+/**
+ * @brief   FDCAN IRQ1 service routine.
+ *
+ * @param[in] canp      pointer to the @p CANDriver object
+ *
+ * @notapi
+ */
+void can_lld_serve_interrupt1(CANDriver *canp) {
+
+}
+
 /** @} */
diff --git a/os/hal/ports/STM32/LLD/FDCANv1/hal_can_lld.h b/os/hal/ports/STM32/LLD/FDCANv1/hal_can_lld.h
index cb7eb1e7b..0b8a127be 100644
--- a/os/hal/ports/STM32/LLD/FDCANv1/hal_can_lld.h
+++ b/os/hal/ports/STM32/LLD/FDCANv1/hal_can_lld.h
@@ -402,6 +402,8 @@ extern "C" {
   void can_lld_sleep(CANDriver *canp);
   void can_lld_wakeup(CANDriver *canp);
 #endif /* CAN_USE_SLEEP_MODE */
+  void can_lld_serve_interrupt0(CANDriver *canp);
+  void can_lld_serve_interrupt1(CANDriver *canp);
 #ifdef __cplusplus
 }
 #endif
diff --git a/os/hal/ports/STM32/STM32H7xx/platform.mk b/os/hal/ports/STM32/STM32H7xx/platform.mk
index 8bcfd6c5b..05c12ef6a 100644
--- a/os/hal/ports/STM32/STM32H7xx/platform.mk
+++ b/os/hal/ports/STM32/STM32H7xx/platform.mk
@@ -31,6 +31,7 @@ include $(CHIBIOS)/os/hal/ports/STM32/LLD/CRYPv1/driver.mk
 include $(CHIBIOS)/os/hal/ports/STM32/LLD/DACv1/driver.mk
 include $(CHIBIOS)/os/hal/ports/STM32/LLD/DMAv2/driver.mk
 include $(CHIBIOS)/os/hal/ports/STM32/LLD/EXTIv1/driver.mk
+include $(CHIBIOS)/os/hal/ports/STM32/LLD/FDCANv1/driver.mk
 include $(CHIBIOS)/os/hal/ports/STM32/LLD/GPIOv2/driver.mk
 include $(CHIBIOS)/os/hal/ports/STM32/LLD/I2Cv3/driver.mk
 include $(CHIBIOS)/os/hal/ports/STM32/LLD/MDMAv1/driver.mk
diff --git a/os/hal/ports/STM32/STM32H7xx/stm32_isr.c b/os/hal/ports/STM32/STM32H7xx/stm32_isr.c
index b3fc58651..9cebbe5b2 100644
--- a/os/hal/ports/STM32/STM32H7xx/stm32_isr.c
+++ b/os/hal/ports/STM32/STM32H7xx/stm32_isr.c
@@ -64,10 +64,12 @@
 #include "stm32_exti19.inc"
 #include "stm32_exti20_21.inc"
 
-#include <stm32_quadspi1.inc>
+#include "stm32_fdcan1.inc"
 
-#include <stm32_sdmmc1.inc>
-#include <stm32_sdmmc2.inc>
+#include "stm32_quadspi1.inc"
+
+#include "stm32_sdmmc1.inc"
+#include "stm32_sdmmc2.inc"
 
 #include <stm32_usart1.inc>
 #include "stm32_usart2.inc"
@@ -112,6 +114,8 @@ void irqInit(void) {
   exti19_irq_init();
   exti20_exti21_irq_init();
 
+  fdcan1_irq_init();
+
   mdma_irq_init();
 
   quadspi1_irq_init();
@@ -158,6 +162,8 @@ void irqDeinit(void) {
   exti19_irq_deinit();
   exti20_exti21_irq_deinit();
 
+  fdcan1_irq_deinit();
+
   mdma_irq_deinit();
 
   quadspi1_irq_deinit();
diff --git a/os/hal/ports/STM32/STM32H7xx/stm32_isr.h b/os/hal/ports/STM32/STM32H7xx/stm32_isr.h
index 38ff7c7af..df3c6dc78 100644
--- a/os/hal/ports/STM32/STM32H7xx/stm32_isr.h
+++ b/os/hal/ports/STM32/STM32H7xx/stm32_isr.h
@@ -172,6 +172,19 @@
 #define STM32_EXTI19_NUMBER                 3
 #define STM32_EXTI2021_NUMBER               137
 
+/*
+ * FDCAN units.
+ */
+#define STM32_FDCAN1_IT0_HANDLER            Vector8C
+#define STM32_FDCAN1_IT1_HANDLER            Vector94
+#define STM32_FDCAN2_IT0_HANDLER            Vector90
+#define STM32_FDCAN2_IT1_HANDLER            Vector98
+
+#define STM32_FDCAN1_IT0_NUMBER             19
+#define STM32_FDCAN1_IT1_NUMBER             21
+#define STM32_FDCAN2_IT0_NUMBER             20
+#define STM32_FDCAN2_IT1_NUMBER             22
+
 /*
  * I2C units.
  */