1 files changed, 64 insertions, 2 deletions
diff --git a/os/hal/platforms/SPC5xx/FlexCAN_v1/can_lld.c b/os/hal/platforms/SPC5xx/FlexCAN_v1/can_lld.c
index 09e8ed55d..b20cc1334 100644
--- a/os/hal/platforms/SPC5xx/FlexCAN_v1/can_lld.c
+++ b/os/hal/platforms/SPC5xx/FlexCAN_v1/can_lld.c
@@ -81,16 +81,72 @@ CANDriver CAND6;
  * @notapi
  */
 static void can_lld_tx_handler(CANDriver *canp) {
-  uint32_t iflag1;
+  uint32_t iflag1, iflag2;
 
   iflag1 = canp->flexcan->IFRL.R;
+  iflag2 = canp->flexcan->IFRH.R;
   /* No more events until a message is transmitted.*/
   canp->flexcan->IFRL.R |= iflag1 & 0xFFFFFF00;
   canp->flexcan->IFRH.R |= canp->flexcan->IFRH.R & 0xFFFFFFFF;
   chSysLockFromIsr();
   while (chSemGetCounterI(&canp->txsem) < 0)
     chSemSignalI(&canp->txsem);
-  chEvtBroadcastFlagsI(&canp->txempty_event, iflag1 & 0xFFFFFF00);
+
+#if SPC5_CAN_USE_FLEXCAN0 && (SPC5_FLEXCAN0_MB == 32)
+  if(&CAND1 == canp) {
+    chEvtBroadcastFlagsI(&canp->txempty_event, iflag1 & 0xFFFFFF00);
+  }
+#elif SPC5_CAN_USE_FLEXCAN0 && (SPC5_FLEXCAN0_MB == 64)
+  if(&CAND1 == canp) {
+    chEvtBroadcastFlagsI(&canp->txempty_event, iflag2 | (iflag1 & 0xFFFFFF00));
+  }
+#endif
+#if SPC5_CAN_USE_FLEXCAN1 && (SPC5_FLEXCAN1_MB == 32)
+  if(&CAND2 == canp) {
+    chEvtBroadcastFlagsI(&canp->txempty_event, iflag1 & 0xFFFFFF00);
+  }
+#elif SPC5_CAN_USE_FLEXCAN1 && (SPC5_FLEXCAN1_MB == 64)
+  if(&CAND2 == canp) {
+    chEvtBroadcastFlagsI(&canp->txempty_event, iflag2 | (iflag1 & 0xFFFFFF00));
+  }
+#endif
+#if SPC5_CAN_USE_FLEXCAN2 && (SPC5_FLEXCAN2_MB == 32)
+  if(&CAND3 == canp) {
+    chEvtBroadcastFlagsI(&canp->txempty_event, iflag1 & 0xFFFFFF00);
+  }
+#elif SPC5_CAN_USE_FLEXCAN2 && (SPC5_FLEXCAN2_MB == 64)
+  if(&CAND3 == canp) {
+    chEvtBroadcastFlagsI(&canp->txempty_event, iflag2 | (iflag1 & 0xFFFFFF00));
+  }
+#endif
+#if SPC5_CAN_USE_FLEXCAN3 && (SPC5_FLEXCAN3_MB == 32)
+  if(&CAND4 == canp) {
+    chEvtBroadcastFlagsI(&canp->txempty_event, iflag1 & 0xFFFFFF00);
+  }
+#elif SPC5_CAN_USE_FLEXCAN3 && (SPC5_FLEXCAN3_MB == 64)
+  if(&CAND4 == canp) {
+    chEvtBroadcastFlagsI(&canp->txempty_event, iflag2 | (iflag1 & 0xFFFFFF00));
+  }
+#endif
+#if SPC5_CAN_USE_FLEXCAN4 && (SPC5_FLEXCAN4_MB == 32)
+  if(&CAND5 == canp) {
+    chEvtBroadcastFlagsI(&canp->txempty_event, iflag1 & 0xFFFFFF00);
+  }
+#elif SPC5_CAN_USE_FLEXCAN4 && (SPC5_FLEXCAN4_MB == 64)
+  if(&CAND5 == canp) {
+    chEvtBroadcastFlagsI(&canp->txempty_event, iflag2 | (iflag1 & 0xFFFFFF00));
+  }
+#endif
+#if SPC5_CAN_USE_FLEXCAN5 && (SPC5_FLEXCAN5_MB == 32)
+  if(&CAND6 == canp) {
+    chEvtBroadcastFlagsI(&canp->txempty_event, iflag1 & 0xFFFFFF00);
+  }
+#elif SPC5_CAN_USE_FLEXCAN5 && (SPC5_FLEXCAN5_MB == 64)
+  if(&CAND6 == canp) {
+    chEvtBroadcastFlagsI(&canp->txempty_event, iflag2 | (iflag1 & 0xFFFFFF00));
+  }
+#endif
+
   chSysUnlockFromIsr();
 }
 
@@ -1141,6 +1197,12 @@ void can_lld_start(CANDriver *canp) {
   /* RX MB initialization.*/
   for(mb_index = 0; mb_index < CAN_RX_MAILBOXES; mb_index++) {
     canp->flexcan->BUF[mb_index].CS.B.CODE = 0U;
+    if(mb_index < 4) {
+      canp->flexcan->BUF[mb_index].CS.B.IDE = 0U;
+    }
+    else {
+      canp->flexcan->BUF[mb_index].CS.B.IDE = 1U;
+    }
     canp->flexcan->BUF[mb_index].ID.R = 0U;
     canp->flexcan->BUF[mb_index].CS.B.CODE = 4U;
   }