diff --git a/os/hal/platforms/SPC5xx/FlexCAN_v1/can_lld.c b/os/hal/platforms/SPC5xx/FlexCAN_v1/can_lld.c
index 9f9ee3814..7d9ce4ebb 100644
--- a/os/hal/platforms/SPC5xx/FlexCAN_v1/can_lld.c
+++ b/os/hal/platforms/SPC5xx/FlexCAN_v1/can_lld.c
@@ -83,11 +83,47 @@ CANDriver CAND6;
 static void can_lld_tx_handler(CANDriver *canp) {
   uint32_t iflag1, iflag2;
 
-  iflag1 = canp->flexcan->IFRL.R;
-  iflag2 = canp->flexcan->IFRH.R;
   /* No more events until a message is transmitted.*/
+  iflag1 = canp->flexcan->IFRL.R;
   canp->flexcan->IFRL.R = iflag1 & 0xFFFFFF00;
-  canp->flexcan->IFRH.R = canp->flexcan->IFRH.R & 0xFFFFFFFF;
+
+#if SPC5_CAN_USE_FLEXCAN0 && (SPC5_FLEXCAN0_MB == 64)
+  if(&CAND1 == canp) {
+    iflag2 = canp->flexcan->IFRH.R;
+    canp->flexcan->IFRH.R = canp->flexcan->IFRH.R & 0xFFFFFFFF;
+  }
+#endif
+#if SPC5_CAN_USE_FLEXCAN1 && (SPC5_FLEXCAN1_MB == 64)
+  if(&CAND2 == canp) {
+    iflag2 = canp->flexcan->IFRH.R;
+    canp->flexcan->IFRH.R = canp->flexcan->IFRH.R & 0xFFFFFFFF;
+  }
+#endif
+#if SPC5_CAN_USE_FLEXCAN2 && (SPC5_FLEXCAN2_MB == 64)
+  if(&CAND3 == canp) {
+    iflag2 = canp->flexcan->IFRH.R;
+    canp->flexcan->IFRH.R = canp->flexcan->IFRH.R & 0xFFFFFFFF;
+  }
+#endif
+#if SPC5_CAN_USE_FLEXCAN3 && (SPC5_FLEXCAN3_MB == 64)
+  if(&CAND4 == canp) {
+    iflag2 = canp->flexcan->IFRH.R;
+    canp->flexcan->IFRH.R = canp->flexcan->IFRH.R & 0xFFFFFFFF;
+  }
+#endif
+#if SPC5_CAN_USE_FLEXCAN4 && (SPC5_FLEXCAN4_MB == 64)
+  if(&CAND5 == canp) {
+    iflag2 = canp->flexcan->IFRH.R;
+    canp->flexcan->IFRH.R = canp->flexcan->IFRH.R & 0xFFFFFFFF;
+  }
+#endif
+#if SPC5_CAN_USE_FLEXCAN5 && (SPC5_FLEXCAN5_MB == 64)
+  if(&CAND6 == canp) {
+    iflag2 = canp->flexcan->IFRH.R;
+    canp->flexcan->IFRH.R = canp->flexcan->IFRH.R & 0xFFFFFFFF;
+  }
+#endif
+
   chSysLockFromIsr();
   while (chSemGetCounterI(&canp->txsem) < 0)
     chSemSignalI(&canp->txsem);
diff --git a/testhal/SPC560BCxx/CAN/mcuconf.h b/testhal/SPC560BCxx/CAN/mcuconf.h
index dc667bb74..fc8b78dba 100644
--- a/testhal/SPC560BCxx/CAN/mcuconf.h
+++ b/testhal/SPC560BCxx/CAN/mcuconf.h
@@ -181,42 +181,42 @@
 #define SPC5_CAN_USE_FILTERS                TRUE
 
 #define SPC5_CAN_USE_FLEXCAN0               FALSE
-#define SPC5_CAN_FLEXCAN0_PRIORITY          11
+#define SPC5_CAN_FLEXCAN0_IRQ_PRIORITY      11
 #define SPC5_CAN_FLEXCAN0_START_PCTL        (SPC5_ME_PCTL_RUN(1) |          \
                                              SPC5_ME_PCTL_LP(2))
 #define SPC5_CAN_FLEXCAN0_STOP_PCTL         (SPC5_ME_PCTL_RUN(0) |          \
                                              SPC5_ME_PCTL_LP(0))
 
 #define SPC5_CAN_USE_FLEXCAN1               FALSE
-#define SPC5_CAN_FLEXCAN1_PRIORITY          11
+#define SPC5_CAN_FLEXCAN1_IRQ_PRIORITY      11
 #define SPC5_CAN_FLEXCAN1_START_PCTL        (SPC5_ME_PCTL_RUN(1) |          \
                                              SPC5_ME_PCTL_LP(2))
 #define SPC5_CAN_FLEXCAN1_STOP_PCTL         (SPC5_ME_PCTL_RUN(0) |          \
                                              SPC5_ME_PCTL_LP(0))
 
 #define SPC5_CAN_USE_FLEXCAN2               FALSE
-#define SPC5_CAN_FLEXCAN2_PRIORITY          11
+#define SPC5_CAN_FLEXCAN2_IRQ_PRIORITY      11
 #define SPC5_CAN_FLEXCAN2_START_PCTL        (SPC5_ME_PCTL_RUN(1) |          \
                                              SPC5_ME_PCTL_LP(2))
 #define SPC5_CAN_FLEXCAN2_STOP_PCTL         (SPC5_ME_PCTL_RUN(0) |          \
                                              SPC5_ME_PCTL_LP(0))
 
 #define SPC5_CAN_USE_FLEXCAN3               FALSE
-#define SPC5_CAN_FLEXCAN3_PRIORITY          11
+#define SPC5_CAN_FLEXCAN3_IRQ_PRIORITY      11
 #define SPC5_CAN_FLEXCAN3_START_PCTL        (SPC5_ME_PCTL_RUN(1) |          \
                                              SPC5_ME_PCTL_LP(2))
 #define SPC5_CAN_FLEXCAN3_STOP_PCTL         (SPC5_ME_PCTL_RUN(0) |          \
                                              SPC5_ME_PCTL_LP(0))
 
 #define SPC5_CAN_USE_FLEXCAN4               TRUE
-#define SPC5_CAN_FLEXCAN4_PRIORITY          11
+#define SPC5_CAN_FLEXCAN4_IRQ_PRIORITY      11
 #define SPC5_CAN_FLEXCAN4_START_PCTL        (SPC5_ME_PCTL_RUN(1) |          \
                                              SPC5_ME_PCTL_LP(2))
 #define SPC5_CAN_FLEXCAN4_STOP_PCTL         (SPC5_ME_PCTL_RUN(0) |          \
                                              SPC5_ME_PCTL_LP(0))
 
 #define SPC5_CAN_USE_FLEXCAN5               FALSE
-#define SPC5_CAN_FLEXCAN5_PRIORITY          11
+#define SPC5_CAN_FLEXCAN5_IRQ_PRIORITY      11
 #define SPC5_CAN_FLEXCAN5_START_PCTL        (SPC5_ME_PCTL_RUN(1) |          \
                                              SPC5_ME_PCTL_LP(2))
 #define SPC5_CAN_FLEXCAN5_STOP_PCTL         (SPC5_ME_PCTL_RUN(0) |          \
diff --git a/testhal/SPC560Pxx/CAN/mcuconf.h b/testhal/SPC560Pxx/CAN/mcuconf.h
index bdd11f6ae..4d18be947 100644
--- a/testhal/SPC560Pxx/CAN/mcuconf.h
+++ b/testhal/SPC560Pxx/CAN/mcuconf.h
@@ -183,7 +183,7 @@
 #define SPC5_CAN_USE_FILTERS                TRUE
 
 #define SPC5_CAN_USE_FLEXCAN0               TRUE
-#define SPC5_CAN_FLEXCAN0_PRIORITY          11
+#define SPC5_CAN_FLEXCAN0_IRQ_PRIORITY      11
 #define SPC5_CAN_FLEXCAN0_START_PCTL        (SPC5_ME_PCTL_RUN(1) |          \
                                              SPC5_ME_PCTL_LP(2))
 #define SPC5_CAN_FLEXCAN0_STOP_PCTL         (SPC5_ME_PCTL_RUN(0) |          \
diff --git a/testhal/SPC56ELxx/CAN/mcuconf.h b/testhal/SPC56ELxx/CAN/mcuconf.h
index 212082fc9..a310fd439 100644
--- a/testhal/SPC56ELxx/CAN/mcuconf.h
+++ b/testhal/SPC56ELxx/CAN/mcuconf.h
@@ -222,14 +222,14 @@
 #define SPC5_CAN_USE_FILTERS                TRUE
 
 #define SPC5_CAN_USE_FLEXCAN0               TRUE
-#define SPC5_CAN_FLEXCAN0_PRIORITY          11
+#define SPC5_CAN_FLEXCAN0_IRQ_PRIORITY      11
 #define SPC5_CAN_FLEXCAN0_START_PCTL        (SPC5_ME_PCTL_RUN(1) |          \
                                              SPC5_ME_PCTL_LP(2))
 #define SPC5_CAN_FLEXCAN0_STOP_PCTL         (SPC5_ME_PCTL_RUN(0) |          \
                                              SPC5_ME_PCTL_LP(0))
 
 #define SPC5_CAN_USE_FLEXCAN1               FALSE
-#define SPC5_CAN_FLEXCAN1_PRIORITY          11
+#define SPC5_CAN_FLEXCAN1_IRQ_PRIORITY      11
 #define SPC5_CAN_FLEXCAN1_START_PCTL        (SPC5_ME_PCTL_RUN(1) |          \
                                              SPC5_ME_PCTL_LP(2))
 #define SPC5_CAN_FLEXCAN1_STOP_PCTL         (SPC5_ME_PCTL_RUN(0) |          \