From 53959082e7e3e200fb62e1c44219fa4e656eb8de Mon Sep 17 00:00:00 2001
From: Igor Biletskyy <briskspirit@users.noreply.github.com>
Date: Tue, 20 Sep 2022 12:39:12 -0700
Subject: [PATCH] boardd: add CAN health to pandaStates (#25800)

* init

* try this

* mistake

* fix

* bump cereal

* make obvious

* fixes

* remove comment

* one helath header

* ..

* preallocate vectors
---
 cereal                     |  2 +-
 selfdrive/boardd/boardd.cc | 46 ++++++++++++++++++++++++++++++++++++--
 selfdrive/boardd/panda.cc  |  6 +++++
 selfdrive/boardd/panda.h   |  2 ++
 4 files changed, 53 insertions(+), 3 deletions(-)

diff --git a/cereal b/cereal
index e4130c9055..3baa20e1da 160000
--- a/cereal
+++ b/cereal
@@ -1 +1 @@
-Subproject commit e4130c90558dfb491e132992dce36e0e620e070a
+Subproject commit 3baa20e1da5d88dcb1d3ae9678471eb8013958f2
diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc
index 0872a94712..7009f28e12 100644
--- a/selfdrive/boardd/boardd.cc
+++ b/selfdrive/boardd/boardd.cc
@@ -295,13 +295,19 @@ void send_empty_panda_state(PubMaster *pm) {
 
 std::optional<bool> send_panda_states(PubMaster *pm, const std::vector<Panda *> &pandas, bool spoofing_started) {
   bool ignition_local = false;
+  const uint32_t pandas_cnt = pandas.size();
 
   // build msg
   MessageBuilder msg;
   auto evt = msg.initEvent();
-  auto pss = evt.initPandaStates(pandas.size());
+  auto pss = evt.initPandaStates(pandas_cnt);
 
   std::vector<health_t> pandaStates;
+  pandaStates.reserve(pandas_cnt);
+
+  std::vector<std::array<can_health_t, PANDA_CAN_CNT>> pandaCanStates;
+  pandaCanStates.reserve(pandas_cnt);
+
   for (const auto& panda : pandas){
     auto health_opt = panda->get_state();
     if (!health_opt) {
@@ -310,6 +316,16 @@ std::optional<bool> send_panda_states(PubMaster *pm, const std::vector<Panda *>
 
     health_t health = *health_opt;
 
+    std::array<can_health_t, PANDA_CAN_CNT> can_health{};
+    for (uint32_t i = 0; i < PANDA_CAN_CNT; i++) {
+      auto can_health_opt = panda->get_can_state(i);
+      if (!can_health_opt) {
+        return std::nullopt;
+      }
+      can_health[i] = *can_health_opt;
+    }
+    pandaCanStates.push_back(can_health);
+
     if (spoofing_started) {
       health.ignition_line_pkt = 1;
     }
@@ -319,7 +335,7 @@ std::optional<bool> send_panda_states(PubMaster *pm, const std::vector<Panda *>
     pandaStates.push_back(health);
   }
 
-  for (uint32_t i = 0; i < pandas.size(); i++) {
+  for (uint32_t i = 0; i < pandas_cnt; i++) {
     auto panda = pandas[i];
     const auto &health = pandaStates[i];
 
@@ -366,6 +382,32 @@ std::optional<bool> send_panda_states(PubMaster *pm, const std::vector<Panda *>
     ps.setInterruptLoad(health.interrupt_load);
     ps.setFanPower(health.fan_power);
 
+    std::array<cereal::PandaState::PandaCanState::Builder, PANDA_CAN_CNT> cs = {ps.initCanState0(), ps.initCanState1(), ps.initCanState2()};
+
+    for (uint32_t j = 0; j < PANDA_CAN_CNT; j++) {
+      const auto &can_health = pandaCanStates[i][j];
+      cs[j].setBusOff((bool)can_health.bus_off);
+      cs[j].setBusOffCnt(can_health.bus_off_cnt);
+      cs[j].setErrorWarning((bool)can_health.error_warning);
+      cs[j].setErrorPassive((bool)can_health.error_passive);
+      cs[j].setLastError(cereal::PandaState::PandaCanState::LecErrorCode(can_health.last_error));
+      cs[j].setLastStoredError(cereal::PandaState::PandaCanState::LecErrorCode(can_health.last_stored_error));
+      cs[j].setLastDataError(cereal::PandaState::PandaCanState::LecErrorCode(can_health.last_data_error));
+      cs[j].setLastDataStoredError(cereal::PandaState::PandaCanState::LecErrorCode(can_health.last_data_stored_error));
+      cs[j].setReceiveErrorCnt(can_health.receive_error_cnt);
+      cs[j].setTransmitErrorCnt(can_health.transmit_error_cnt);
+      cs[j].setTotalErrorCnt(can_health.total_error_cnt);
+      cs[j].setTotalTxLostCnt(can_health.total_tx_lost_cnt);
+      cs[j].setTotalRxLostCnt(can_health.total_rx_lost_cnt);
+      cs[j].setTotalTxCnt(can_health.total_tx_cnt);
+      cs[j].setTotalRxCnt(can_health.total_rx_cnt);
+      cs[j].setTotalFwdCnt(can_health.total_fwd_cnt);
+      cs[j].setCanSpeed(can_health.can_speed);
+      cs[j].setCanDataSpeed(can_health.can_data_speed);
+      cs[j].setCanfdEnabled(can_health.canfd_enabled);
+      cs[j].setBrsEnabled(can_health.canfd_enabled);
+    }
+
     // Convert faults bitset to capnp list
     std::bitset<sizeof(health.faults_pkt) * 8> fault_bits(health.faults_pkt);
     auto faults = ps.initFaults(fault_bits.count());
diff --git a/selfdrive/boardd/panda.cc b/selfdrive/boardd/panda.cc
index 4d72bc9040..7ddf15f9ce 100644
--- a/selfdrive/boardd/panda.cc
+++ b/selfdrive/boardd/panda.cc
@@ -317,6 +317,12 @@ std::optional<health_t> Panda::get_state() {
   return err >= 0 ? std::make_optional(health) : std::nullopt;
 }
 
+std::optional<can_health_t> Panda::get_can_state(uint16_t can_number) {
+  can_health_t can_health {0};
+  int err = usb_read(0xc2, can_number, 0, (unsigned char*)&can_health, sizeof(can_health));
+  return err >= 0 ? std::make_optional(can_health) : std::nullopt;
+}
+
 void Panda::set_loopback(bool loopback) {
   usb_write(0xe5, loopback, 0);
 }
diff --git a/selfdrive/boardd/panda.h b/selfdrive/boardd/panda.h
index 1cefc3cb4d..a4afbdac1a 100644
--- a/selfdrive/boardd/panda.h
+++ b/selfdrive/boardd/panda.h
@@ -16,6 +16,7 @@
 #include "panda/board/health.h"
 
 #define TIMEOUT 0
+#define PANDA_CAN_CNT 3
 #define PANDA_BUS_CNT 4
 #define RECV_SIZE (0x4000U)
 #define USB_TX_SOFT_LIMIT   (0x100U)
@@ -81,6 +82,7 @@ class Panda {
   uint16_t get_fan_speed();
   void set_ir_pwr(uint16_t ir_pwr);
   std::optional<health_t> get_state();
+  std::optional<can_health_t> get_can_state(uint16_t can_number);
   void set_loopback(bool loopback);
   std::optional<std::vector<uint8_t>> get_firmware_version();
   std::optional<std::string> get_serial();