From 0eef8cf333d02256d47db70558ff0e35f903a6cd Mon Sep 17 00:00:00 2001
From: Igor Biletskyy <briskspirit@users.noreply.github.com>
Date: Thu, 15 Sep 2022 13:04:10 -0700
Subject: [PATCH] CAN: health message for each CAN module (#1067)

* initial can health pkt

* MISRA: variable scope

* MISRA 10.4

* typo

* add total_fwd_cnt

* check size of can_health_t

* improve

* comment

* that's better

* move

* add health check to tests

* why?

* ...

* revert later

* meh

* Revert "meh"

This reverts commit 0eb84321942a494454c17c242e7309deb8a92341.

* Revert "revert later"

This reverts commit 2d98957a8fd5350d50ebb1d26c9912f984d72043.

* adjust test
---
 board/can_health.h              | 20 ++++++++
 board/drivers/bxcan.h           | 85 +++++++++++++++++----------------
 board/drivers/can_common.h      | 10 +---
 board/drivers/fdcan.h           | 61 ++++++++++++++++++-----
 board/main.c                    |  1 +
 board/main_comms.h              | 18 +++----
 board/stm32fx/llbxcan.h         |  8 ++--
 board/stm32h7/llfdcan.h         |  6 ++-
 python/__init__.py              | 55 +++++++++++++++++++--
 tests/automated/3_usb_to_can.py |  4 +-
 tests/automated/helpers.py      | 11 +++++
 11 files changed, 193 insertions(+), 86 deletions(-)
 create mode 100644 board/can_health.h

diff --git a/board/can_health.h b/board/can_health.h
new file mode 100644
index 00000000..45e0f006
--- /dev/null
+++ b/board/can_health.h
@@ -0,0 +1,20 @@
+// When changing this struct, python/__init__.py needs to be kept up to date!
+#define CAN_HEALTH_PACKET_VERSION 1
+typedef struct __attribute__((packed)) {
+  uint8_t bus_off;
+  uint32_t bus_off_cnt;
+  uint8_t error_warning;
+  uint8_t error_passive;
+  uint8_t last_error; // real time LEC value
+  uint8_t last_stored_error; // last LEC positive error code stored
+  uint8_t last_data_error; // DLEC (for CANFD only)
+  uint8_t last_data_stored_error; // last DLEC positive error code stored (for CANFD only)
+  uint8_t receive_error_cnt; // REC
+  uint8_t transmit_error_cnt; // TEC
+  uint32_t total_error_cnt; // How many times any error interrupt were invoked
+  uint32_t total_tx_lost_cnt; // Tx event FIFO element Lost
+  uint32_t total_rx_lost_cnt; // Rx FIFO 0 message Lost
+  uint32_t total_tx_cnt;
+  uint32_t total_rx_cnt;
+  uint32_t total_fwd_cnt; // Messages forwarded from one bus to another
+} can_health_t;
diff --git a/board/drivers/bxcan.h b/board/drivers/bxcan.h
index 41b22373..9f3fd555 100644
--- a/board/drivers/bxcan.h
+++ b/board/drivers/bxcan.h
@@ -68,31 +68,33 @@ void can_set_gmlan(uint8_t bus) {
   }
 }
 
+void update_can_health_pkt(uint8_t can_number, bool error_irq) {
+  CAN_TypeDef *CAN = CANIF_FROM_CAN_NUM(can_number);
+  uint32_t esr_reg = CAN->ESR;
+
+  can_health[can_number].bus_off = ((esr_reg & CAN_ESR_BOFF) >> CAN_ESR_BOFF_Pos);
+  can_health[can_number].bus_off_cnt += can_health[can_number].bus_off;
+  can_health[can_number].error_warning = ((esr_reg & CAN_ESR_EWGF) >> CAN_ESR_EWGF_Pos);
+  can_health[can_number].error_passive = ((esr_reg & CAN_ESR_EPVF) >> CAN_ESR_EPVF_Pos);
+
+  can_health[can_number].last_error = ((esr_reg & CAN_ESR_LEC) >> CAN_ESR_LEC_Pos);
+  if ((can_health[can_number].last_error != 0U) && (can_health[can_number].last_error != 7U)) {
+    can_health[can_number].last_stored_error = can_health[can_number].last_error;
+  }
+
+  can_health[can_number].receive_error_cnt = ((esr_reg & CAN_ESR_REC) >> CAN_ESR_REC_Pos);
+  can_health[can_number].transmit_error_cnt = ((esr_reg & CAN_ESR_TEC) >> CAN_ESR_TEC_Pos);
+
+  if (error_irq) {
+    can_health[can_number].total_error_cnt += 1U;
+    llcan_clear_send(CAN);
+  }
+}
+
 // CAN error
-void can_sce(CAN_TypeDef *CAN) {
+void can_sce(uint8_t can_number) {
   ENTER_CRITICAL();
-
-  #ifdef DEBUG
-    if (CAN==CAN1) puts("CAN1:  ");
-    if (CAN==CAN2) puts("CAN2:  ");
-    #ifdef CAN3
-      if (CAN==CAN3) puts("CAN3:  ");
-    #endif
-    puts("MSR:");
-    puth(CAN->MSR);
-    puts(" TSR:");
-    puth(CAN->TSR);
-    puts(" RF0R:");
-    puth(CAN->RF0R);
-    puts(" RF1R:");
-    puth(CAN->RF1R);
-    puts(" ESR:");
-    puth(CAN->ESR);
-    puts("\n");
-  #endif
-
-  can_err_cnt += 1;
-  llcan_clear_send(CAN);
+  update_can_health_pkt(can_number, true);
   EXIT_CRITICAL();
 }
 
@@ -107,11 +109,13 @@ void process_can(uint8_t can_number) {
 
     // check for empty mailbox
     CANPacket_t to_send;
+    if ((CAN->TSR & (CAN_TSR_TERR0 | CAN_TSR_ALST0)) != 0) { // last TX failed due to error arbitration lost
+      can_health[can_number].total_rx_lost_cnt += 1U;
+      CAN->TSR |= (CAN_TSR_TERR0 | CAN_TSR_ALST0);
+    }
     if ((CAN->TSR & CAN_TSR_TME0) == CAN_TSR_TME0) {
       // add successfully transmitted message to my fifo
       if ((CAN->TSR & CAN_TSR_RQCP0) == CAN_TSR_RQCP0) {
-        can_txd_cnt += 1;
-
         if ((CAN->TSR & CAN_TSR_TXOK0) == CAN_TSR_TXOK0) {
           CANPacket_t to_push;
           to_push.returned = 1U;
@@ -126,25 +130,13 @@ void process_can(uint8_t can_number) {
           can_send_errs += can_push(&can_rx_q, &to_push) ? 0U : 1U;
         }
 
-        if ((CAN->TSR & CAN_TSR_TERR0) == CAN_TSR_TERR0) {
-          #ifdef DEBUG
-            puts("CAN TX ERROR!\n");
-          #endif
-        }
-
-        if ((CAN->TSR & CAN_TSR_ALST0) == CAN_TSR_ALST0) {
-          #ifdef DEBUG
-            puts("CAN TX ARBITRATION LOST!\n");
-          #endif
-        }
-
         // clear interrupt
         // careful, this can also be cleared by requesting a transmission
         CAN->TSR |= CAN_TSR_RQCP0;
       }
 
       if (can_pop(can_queues[bus_number], &to_send)) {
-        can_tx_cnt += 1;
+        can_health[can_number].total_tx_cnt += 1U;
         // only send if we have received a packet
         CAN->sTxMailBox[0].TIR = ((to_send.extended != 0U) ? (to_send.addr << 3) : (to_send.addr << 21)) | (to_send.extended << 2);
         CAN->sTxMailBox[0].TDTR = to_send.data_len_code;
@@ -157,6 +149,7 @@ void process_can(uint8_t can_number) {
       }
     }
 
+    update_can_health_pkt(can_number, false);
     EXIT_CRITICAL();
   }
 }
@@ -166,8 +159,14 @@ void process_can(uint8_t can_number) {
 void can_rx(uint8_t can_number) {
   CAN_TypeDef *CAN = CANIF_FROM_CAN_NUM(can_number);
   uint8_t bus_number = BUS_NUM_FROM_CAN_NUM(can_number);
+
+  if ((CAN->RF0R & (CAN_RF0R_FOVR0)) != 0) { // RX message lost due to FIFO overrun
+    can_health[can_number].total_tx_lost_cnt += 1U;
+    CAN->RF0R &= ~(CAN_RF0R_FOVR0);
+  }
+
   while ((CAN->RF0R & CAN_RF0R_FMP0) != 0) {
-    can_rx_cnt += 1;
+    can_health[can_number].total_rx_cnt += 1U;
 
     // can is live
     pending_can_live = 1;
@@ -197,6 +196,7 @@ void can_rx(uint8_t can_number) {
       to_send.data_len_code = to_push.data_len_code;
       (void)memcpy(to_send.data, to_push.data, dlc_to_len[to_push.data_len_code]);
       can_send(&to_send, bus_fwd_num, true);
+      can_health[can_number].total_fwd_cnt += 1U;
     }
 
     can_rx_errs += safety_rx_hook(&to_push) ? 0U : 1U;
@@ -206,21 +206,22 @@ void can_rx(uint8_t can_number) {
     can_send_errs += can_push(&can_rx_q, &to_push) ? 0U : 1U;
 
     // next
+    update_can_health_pkt(can_number, false);
     CAN->RF0R |= CAN_RF0R_RFOM0;
   }
 }
 
 void CAN1_TX_IRQ_Handler(void) { process_can(0); }
 void CAN1_RX0_IRQ_Handler(void) { can_rx(0); }
-void CAN1_SCE_IRQ_Handler(void) { can_sce(CAN1); }
+void CAN1_SCE_IRQ_Handler(void) { can_sce(0); }
 
 void CAN2_TX_IRQ_Handler(void) { process_can(1); }
 void CAN2_RX0_IRQ_Handler(void) { can_rx(1); }
-void CAN2_SCE_IRQ_Handler(void) { can_sce(CAN2); }
+void CAN2_SCE_IRQ_Handler(void) { can_sce(1); }
 
 void CAN3_TX_IRQ_Handler(void) { process_can(2); }
 void CAN3_RX0_IRQ_Handler(void) { can_rx(2); }
-void CAN3_SCE_IRQ_Handler(void) { can_sce(CAN3); }
+void CAN3_SCE_IRQ_Handler(void) { can_sce(2); }
 
 bool can_init(uint8_t can_number) {
   bool ret = false;
diff --git a/board/drivers/can_common.h b/board/drivers/can_common.h
index e38f2066..b9056547 100644
--- a/board/drivers/can_common.h
+++ b/board/drivers/can_common.h
@@ -20,6 +20,8 @@ uint32_t can_fwd_errs = 0;
 uint32_t gmlan_send_errs = 0;
 uint32_t blocked_msg_cnt = 0;
 
+can_health_t can_health[] = {{0}, {0}, {0}};
+
 extern int can_live;
 extern int pending_can_live;
 
@@ -62,13 +64,6 @@ can_buffer(tx3_q, 0x1A0)
 // cppcheck-suppress misra-c2012-9.3
 can_ring *can_queues[] = {&can_tx1_q, &can_tx2_q, &can_tx3_q, &can_txgmlan_q};
 
-// global CAN stats
-int can_rx_cnt = 0;
-int can_tx_cnt = 0;
-int can_txd_cnt = 0;
-int can_err_cnt = 0;
-int can_overflow_cnt = 0;
-
 // ********************* interrupt safe queue *********************
 bool can_pop(can_ring *q, CANPacket_t *elem) {
   bool ret = 0;
@@ -105,7 +100,6 @@ bool can_push(can_ring *q, CANPacket_t *elem) {
   }
   EXIT_CRITICAL();
   if (!ret) {
-    can_overflow_cnt++;
     #ifdef DEBUG
       puts("can_push to ");
       if (q == &can_rx_q) {
diff --git a/board/drivers/fdcan.h b/board/drivers/fdcan.h
index 3ea1377b..3ba14566 100644
--- a/board/drivers/fdcan.h
+++ b/board/drivers/fdcan.h
@@ -32,6 +32,45 @@ void can_set_gmlan(uint8_t bus) {
 }
 
 // ***************************** CAN *****************************
+void update_can_health_pkt(uint8_t can_number, bool error_irq) {
+  ENTER_CRITICAL();
+
+  FDCAN_GlobalTypeDef *CANx = CANIF_FROM_CAN_NUM(can_number);
+  uint32_t psr_reg = CANx->PSR;
+  uint32_t ecr_reg = CANx->ECR;
+
+  can_health[can_number].bus_off = ((psr_reg & FDCAN_PSR_BO) >> FDCAN_PSR_BO_Pos);
+  can_health[can_number].bus_off_cnt += can_health[can_number].bus_off;
+  can_health[can_number].error_warning = ((psr_reg & FDCAN_PSR_EW) >> FDCAN_PSR_EW_Pos);
+  can_health[can_number].error_passive = ((psr_reg & FDCAN_PSR_EP) >> FDCAN_PSR_EP_Pos);
+
+  can_health[can_number].last_error = ((psr_reg & FDCAN_PSR_LEC) >> FDCAN_PSR_LEC_Pos);
+  if ((can_health[can_number].last_error != 0U) && (can_health[can_number].last_error != 7U)) {
+    can_health[can_number].last_stored_error = can_health[can_number].last_error;
+  }
+
+  can_health[can_number].last_data_error = ((psr_reg & FDCAN_PSR_DLEC) >> FDCAN_PSR_DLEC_Pos);
+  if ((can_health[can_number].last_data_error != 0U) && (can_health[can_number].last_data_error != 7U)) {
+    can_health[can_number].last_data_stored_error = can_health[can_number].last_data_error;
+  }
+
+  can_health[can_number].receive_error_cnt = ((ecr_reg & FDCAN_ECR_REC) >> FDCAN_ECR_REC_Pos);
+  can_health[can_number].transmit_error_cnt = ((ecr_reg & FDCAN_ECR_TEC) >> FDCAN_ECR_TEC_Pos);
+
+
+  if (error_irq) {
+    can_health[can_number].total_error_cnt += 1U;
+    if ((CANx->IR & (FDCAN_IR_RF0L)) != 0) {
+      can_health[can_number].total_rx_lost_cnt += 1U;
+    }
+    if ((CANx->IR & (FDCAN_IR_TEFL)) != 0) {
+      can_health[can_number].total_tx_lost_cnt += 1U;
+    }
+    llcan_clear_send(CANx);
+  }
+  EXIT_CRITICAL();
+}
+
 void process_can(uint8_t can_number) {
   if (can_number != 0xffU) {
     ENTER_CRITICAL();
@@ -44,7 +83,8 @@ void process_can(uint8_t can_number) {
     if ((CANx->TXFQS & FDCAN_TXFQS_TFQF) == 0) {
       CANPacket_t to_send;
       if (can_pop(can_queues[bus_number], &to_send)) {
-        can_tx_cnt += 1;
+        can_health[can_number].total_tx_cnt += 1U;
+
         uint32_t TxFIFOSA = FDCAN_START_ADDRESS + (can_number * FDCAN_OFFSET) + (FDCAN_RX_FIFO_0_EL_CNT * FDCAN_RX_FIFO_0_EL_SIZE);
         uint8_t tx_index = (CANx->TXFQS >> FDCAN_TXFQS_TFQPI_Pos) & 0x1F;
         // only send if we have received a packet
@@ -63,7 +103,6 @@ void process_can(uint8_t can_number) {
         CANx->TXBAR = (1UL << tx_index);
 
         // Send back to USB
-        can_txd_cnt += 1;
         CANPacket_t to_push;
 
         to_push.returned = 1U;
@@ -79,6 +118,7 @@ void process_can(uint8_t can_number) {
       }
     }
 
+    update_can_health_pkt(can_number, false);
     EXIT_CRITICAL();
   }
 }
@@ -88,19 +128,18 @@ void process_can(uint8_t can_number) {
 void can_rx(uint8_t can_number) {
   FDCAN_GlobalTypeDef *CANx = CANIF_FROM_CAN_NUM(can_number);
   uint8_t bus_number = BUS_NUM_FROM_CAN_NUM(can_number);
-  uint8_t rx_fifo_idx;
 
   // Rx FIFO 0 new message
   if((CANx->IR & FDCAN_IR_RF0N) != 0) {
     CANx->IR |= FDCAN_IR_RF0N;
     while((CANx->RXF0S & FDCAN_RXF0S_F0FL) != 0) {
-      can_rx_cnt += 1;
+      can_health[can_number].total_rx_cnt += 1U;
 
       // can is live
       pending_can_live = 1;
 
       // getting new message index (0 to 63)
-      rx_fifo_idx = (uint8_t)((CANx->RXF0S >> FDCAN_RXF0S_F0GI_Pos) & 0x3F);
+      uint8_t rx_fifo_idx = (uint8_t)((CANx->RXF0S >> FDCAN_RXF0S_F0GI_Pos) & 0x3F);
 
       uint32_t RxFIFO0SA = FDCAN_START_ADDRESS + (can_number * FDCAN_OFFSET);
       CANPacket_t to_push;
@@ -138,6 +177,7 @@ void can_rx(uint8_t can_number) {
         to_send.data_len_code = to_push.data_len_code;
         (void)memcpy(to_send.data, to_push.data, dlc_to_len[to_push.data_len_code]);
         can_send(&to_send, bus_fwd_num, true);
+        can_health[can_number].total_fwd_cnt += 1U;
       }
 
       can_rx_errs += safety_rx_hook(&to_push) ? 0U : 1U;
@@ -158,15 +198,10 @@ void can_rx(uint8_t can_number) {
       CANx->RXF0A = rx_fifo_idx;
     }
 
-  } else if((CANx->IR & (FDCAN_IR_PEA | FDCAN_IR_PED | FDCAN_IR_RF0L | FDCAN_IR_RF0F | FDCAN_IR_EW | FDCAN_IR_MRAF | FDCAN_IR_TOO)) != 0) {
-    #ifdef DEBUG
-      puts("FDCAN error, FDCAN_IR: ");puth(CANx->IR);puts("\n");
-    #endif
-    CANx->IR |= (FDCAN_IR_PEA | FDCAN_IR_PED | FDCAN_IR_RF0L | FDCAN_IR_RF0F | FDCAN_IR_EW | FDCAN_IR_MRAF | FDCAN_IR_TOO); // Clean all error flags
-    can_err_cnt += 1;
-  } else {
-
   }
+  // Error handling
+  bool error_irq = ((CANx->IR & (FDCAN_IR_PED | FDCAN_IR_PEA | FDCAN_IR_EW | FDCAN_IR_EP | FDCAN_IR_ELO | FDCAN_IR_BO | FDCAN_IR_TEFL | FDCAN_IR_RF0L)) != 0);
+  update_can_health_pkt(can_number, error_irq);
 }
 
 void FDCAN1_IT0_IRQ_Handler(void) { can_rx(0); }
diff --git a/board/main.c b/board/main.c
index 29a3ff8a..5672c94e 100644
--- a/board/main.c
+++ b/board/main.c
@@ -12,6 +12,7 @@
 #include "power_saving.h"
 #include "safety.h"
 
+#include "can_health.h"
 #include "drivers/can_common.h"
 
 #ifdef STM32H7
diff --git a/board/main_comms.h b/board/main_comms.h
index 770c3daa..fcbc7c40 100644
--- a/board/main_comms.h
+++ b/board/main_comms.h
@@ -242,20 +242,19 @@ int comms_control_handler(ControlPacket_t *req, uint8_t *resp) {
     case 0xb3:
       current_board->set_phone_power(req->param1 > 0U);
       break;
-    // **** 0xc0: get CAN debug info
-    case 0xc0:
-      puts("can tx: "); puth(can_tx_cnt);
-      puts(" txd: "); puth(can_txd_cnt);
-      puts(" rx: "); puth(can_rx_cnt);
-      puts(" err: "); puth(can_err_cnt);
-      puts("\n");
-      break;
     // **** 0xc1: get hardware type
     case 0xc1:
       resp[0] = hw_type;
       resp_len = 1;
       break;
     // **** 0xd0: fetch serial number
+    case 0xc2:
+      COMPILE_TIME_ASSERT(sizeof(can_health_t) <= USBPACKET_MAX_SIZE);
+      if (req->param1 < 3U) {
+        resp_len = sizeof(can_health[req->param1]);
+        (void)memcpy(resp, &can_health[req->param1], resp_len);
+      }
+      break;
     case 0xd0:
       // addresses are OTP
       if (req->param1 == 1U) {
@@ -376,7 +375,8 @@ int comms_control_handler(ControlPacket_t *req, uint8_t *resp) {
     case 0xdd:
       resp[0] = HEALTH_PACKET_VERSION;
       resp[1] = CAN_PACKET_VERSION;
-      resp_len = 2;
+      resp[2] = CAN_HEALTH_PACKET_VERSION;
+      resp_len = 3;
       break;
     // **** 0xde: set can bitrate
     case 0xde:
diff --git a/board/stm32fx/llbxcan.h b/board/stm32fx/llbxcan.h
index 4da45473..6f5d84c7 100644
--- a/board/stm32fx/llbxcan.h
+++ b/board/stm32fx/llbxcan.h
@@ -99,7 +99,7 @@ bool llcan_init(CAN_TypeDef *CAN_obj) {
     register_clear_bits(&(CAN_obj->FMR), CAN_FMR_FINIT);
 
     // enable certain CAN interrupts
-    register_set_bits(&(CAN_obj->IER), CAN_IER_TMEIE | CAN_IER_FMPIE0 |  CAN_IER_WKUIE | CAN_IER_ERRIE | CAN_IER_BOFIE);
+    register_set_bits(&(CAN_obj->IER), CAN_IER_TMEIE | CAN_IER_FMPIE0 | CAN_IER_ERRIE | CAN_IER_LECIE | CAN_IER_BOFIE | CAN_IER_EPVIE | CAN_IER_EWGIE | CAN_IER_FOVIE0 | CAN_IER_FFIE0);
 
     if (CAN_obj == CAN1) {
       NVIC_EnableIRQ(CAN1_TX_IRQn);
@@ -123,8 +123,6 @@ bool llcan_init(CAN_TypeDef *CAN_obj) {
 }
 
 void llcan_clear_send(CAN_TypeDef *CAN_obj) {
-  CAN_obj->TSR |= CAN_TSR_ABRQ0;
-  register_clear_bits(&(CAN_obj->MSR), CAN_MSR_ERRI);
-  // cppcheck-suppress selfAssignment ; needed to clear the register
-  CAN_obj->MSR = CAN_obj->MSR;
+  CAN_obj->TSR |= CAN_TSR_ABRQ0; // Abort message transmission on error interrupt
+  CAN_obj->MSR |= CAN_MSR_ERRI; // Clear error interrupt
 }
diff --git a/board/stm32h7/llfdcan.h b/board/stm32h7/llfdcan.h
index ae44a95f..b10163b0 100644
--- a/board/stm32h7/llfdcan.h
+++ b/board/stm32h7/llfdcan.h
@@ -200,6 +200,7 @@ bool llcan_init(FDCAN_GlobalTypeDef *CANx) {
     CANx->IE &= 0x0U; // Reset all interrupts
     // Messages for INT0
     CANx->IE |= FDCAN_IE_RF0NE; // Rx FIFO 0 new message
+    CANx->IE |= FDCAN_IE_PEDE | FDCAN_IE_PEAE | FDCAN_IE_BOE | FDCAN_IE_EPE | FDCAN_IE_ELOE | FDCAN_IE_TEFLE | FDCAN_IE_RF0LE;
 
     // Messages for INT1 (Only TFE works??)
     CANx->ILS |= FDCAN_ILS_TFEL;
@@ -230,6 +231,7 @@ bool llcan_init(FDCAN_GlobalTypeDef *CANx) {
 }
 
 void llcan_clear_send(FDCAN_GlobalTypeDef *CANx) {
-  // From H7 datasheet: Transmit cancellation is not intended for Tx FIFO operation.
-  UNUSED(CANx);
+  CANx->TXBCR = 0xFFFFU; // Abort message transmission on error interrupt
+  // Clear error interrupts
+  CANx->IR |= (FDCAN_IR_PED | FDCAN_IR_PEA | FDCAN_IR_EW | FDCAN_IR_EP | FDCAN_IR_ELO | FDCAN_IR_BO | FDCAN_IR_TEFL | FDCAN_IR_RF0L);
 }
diff --git a/python/__init__.py b/python/__init__.py
index 5c54a443..7caec49f 100644
--- a/python/__init__.py
+++ b/python/__init__.py
@@ -117,6 +117,16 @@ def ensure_can_packet_version(fn):
     return fn(self, *args, **kwargs)
   return wrapper
 
+def ensure_can_health_packet_version(fn):
+  @wraps(fn)
+  def wrapper(self, *args, **kwargs):
+    if self.can_health_version < self.CAN_HEALTH_PACKET_VERSION:
+      raise RuntimeError("Panda firmware has outdated CAN health packet definition. Reflash panda firmware.")
+    elif self.can_health_version > self.CAN_HEALTH_PACKET_VERSION:
+      raise RuntimeError("Panda python library has outdated CAN health packet definition. Update panda python library.")
+    return fn(self, *args, **kwargs)
+  return wrapper
+
 class ALTERNATIVE_EXPERIENCE:
   DEFAULT = 0
   DISABLE_DISENGAGE_ON_GAS = 1
@@ -176,7 +186,9 @@ class Panda:
 
   CAN_PACKET_VERSION = 2
   HEALTH_PACKET_VERSION = 9
+  CAN_HEALTH_PACKET_VERSION = 1
   HEALTH_STRUCT = struct.Struct("<IIIIIIIIBBBBBBHBBBHIfB")
+  CAN_HEALTH_STRUCT = struct.Struct("<BIBBBBBBBBIIIIII")
 
   F2_DEVICES = (HW_TYPE_PEDAL, )
   F4_DEVICES = (HW_TYPE_WHITE_PANDA, HW_TYPE_GREY_PANDA, HW_TYPE_BLACK_PANDA, HW_TYPE_UNO, HW_TYPE_DOS)
@@ -273,7 +285,7 @@ class Panda:
 
     assert self._handle is not None
     self._mcu_type = self.get_mcu_type()
-    self.health_version, self.can_version = self.get_packets_versions()
+    self.health_version, self.can_version, self.can_health_version = self.get_packets_versions()
     print("connected")
 
     # disable openpilot's heartbeat checks
@@ -460,6 +472,39 @@ class Panda:
       "fan_power": a[21],
     }
 
+  @ensure_can_health_packet_version
+  def can_health(self, can_number):
+    LEC_ERROR_CODE = {
+      0: "No error",
+      1: "Stuff error",
+      2: "Form error",
+      3: "AckError",
+      4: "Bit1Error",
+      5: "Bit0Error",
+      6: "CRCError",
+      7: "NoChange",
+    }
+    dat = self._handle.controlRead(Panda.REQUEST_IN, 0xc2, int(can_number), 0, self.CAN_HEALTH_STRUCT.size)
+    a = self.CAN_HEALTH_STRUCT.unpack(dat)
+    return {
+      "bus_off": a[0],
+      "bus_off_cnt": a[1],
+      "error_warning": a[2],
+      "error_passive": a[3],
+      "last_error": LEC_ERROR_CODE[a[4]],
+      "last_stored_error": LEC_ERROR_CODE[a[5]],
+      "last_data_error": LEC_ERROR_CODE[a[6]],
+      "last_data_stored_error": LEC_ERROR_CODE[a[7]],
+      "receive_error_cnt": a[8],
+      "transmit_error_cnt": a[9],
+      "total_error_cnt": a[10],
+      "total_tx_lost_cnt": a[11],
+      "total_rx_lost_cnt": a[12],
+      "total_tx_cnt": a[13],
+      "total_rx_cnt": a[14],
+      "total_fwd_cnt": a[15],
+    }
+
   # ******************* control *******************
 
   def enter_bootloader(self):
@@ -494,12 +539,12 @@ class Panda:
 
   # Returns tuple with health packet version and CAN packet/USB packet version
   def get_packets_versions(self):
-    dat = self._handle.controlRead(Panda.REQUEST_IN, 0xdd, 0, 0, 2)
+    dat = self._handle.controlRead(Panda.REQUEST_IN, 0xdd, 0, 0, 3)
     if dat:
-      a = struct.unpack("BB", dat)
-      return (a[0], a[1])
+      a = struct.unpack("BBB", dat)
+      return (a[0], a[1], a[2])
     else:
-      return (0, 0)
+      return (0, 0, 0)
 
   def get_mcu_type(self):
     hw_type = self.get_type()
diff --git a/tests/automated/3_usb_to_can.py b/tests/automated/3_usb_to_can.py
index d63f301e..a2035bd0 100644
--- a/tests/automated/3_usb_to_can.py
+++ b/tests/automated/3_usb_to_can.py
@@ -158,5 +158,5 @@ def test_gmlan_bad_toggle(p):
 @panda_connect_and_init
 def test_serial_debug(p):
   _ = p.serial_read(Panda.SERIAL_DEBUG)  # junk
-  p.call_control_api(0xc0)
-  assert(p.serial_read(Panda.SERIAL_DEBUG).startswith(b"can "))
+  p.call_control_api(0x01)
+  assert(p.serial_read(Panda.SERIAL_DEBUG).startswith(b"NO HANDLER"))
diff --git a/tests/automated/helpers.py b/tests/automated/helpers.py
index a6fe0f05..5e833937 100644
--- a/tests/automated/helpers.py
+++ b/tests/automated/helpers.py
@@ -207,6 +207,17 @@ def panda_connect_and_init(fn=None, full_reset=True):
         if not panda.bootstub:
           panda.reconnect()
           assert panda.health()['fault_status'] == 0
+          # Check health of each CAN core after test, normal to fail for test_gen2_loopback on OBD bus, so skipping
+          if fn.__name__ != "test_gen2_loopback":
+            for i in range(3):
+              print(fn.__name__)
+              can_health = panda.can_health(i)
+              assert can_health['bus_off_cnt'] == 0
+              assert can_health['receive_error_cnt'] == 0
+              assert can_health['transmit_error_cnt'] == 0
+              assert can_health['total_rx_lost_cnt'] == 0
+              assert can_health['total_tx_lost_cnt'] == 0
+              assert can_health['total_error_cnt'] == 0
     finally:
       for p in pandas:
         try: