WIP: Checksum checks (commaai#403)

* add lag message check for all supported cars
* add checksum and counter checks for toyota and honda
* add rx hook regression tests

VERSION

@@ -1 +1 @@
-v1.7.1
+v1.7.2

board/drivers/can.h

@@ -382,7 +382,7 @@ void can_rx(uint8_t can_number) {
       can_send(&to_send, bus_fwd_num, true);
     }
 
-    safety_rx_hook(&to_push);
+    can_rx_errs += safety_rx_hook(&to_push) ? 0U : 1U;
     ignition_can_hook(&to_push);
 
     current_board->set_led(LED_BLUE, true);

board/main.c

@@ -667,7 +667,7 @@ void __attribute__ ((noinline)) enable_fpu(void) {
 #define EON_HEARTBEAT_IGNITION_CNT_ON 5U
 #define EON_HEARTBEAT_IGNITION_CNT_OFF 2U
 
-// called once per second
+// called at 1Hz
 void TIM1_BRK_TIM9_IRQ_Handler(void) {
   if (TIM9->SR != 0) {
     can_live = pending_can_live;
@@ -736,6 +736,9 @@ void TIM1_BRK_TIM9_IRQ_Handler(void) {
     uptime_cnt += 1U;
     safety_mode_cnt += 1U;
     ignition_can_cnt += 1U;
+
+    // synchronous safety check
+    safety_tick(current_hooks);
   }
   TIM9->SR = 0;
 }

board/safety.h

@@ -41,8 +41,8 @@
 uint16_t current_safety_mode = SAFETY_SILENT;
 const safety_hooks *current_hooks = &nooutput_hooks;
 
-void safety_rx_hook(CAN_FIFOMailBox_TypeDef *to_push){
-  current_hooks->rx(to_push);
+int safety_rx_hook(CAN_FIFOMailBox_TypeDef *to_push){
+  return current_hooks->rx(to_push);
 }
 
 int safety_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
@@ -68,6 +68,106 @@ bool msg_allowed(int addr, int bus, const AddrBus addr_list[], int len) {
   return allowed;
 }
 
+// compute the time elapsed (in microseconds) from 2 counter samples
+// case where ts < ts_last is ok: overflow is properly re-casted into uint32_t
+uint32_t get_ts_elapsed(uint32_t ts, uint32_t ts_last) {
+  return ts - ts_last;
+}
+
+int get_addr_check_index(CAN_FIFOMailBox_TypeDef *to_push, AddrCheckStruct addr_list[], const int len) {
+  int bus = GET_BUS(to_push);
+  int addr = GET_ADDR(to_push);
+
+  int index = -1;
+  for (int i = 0; i < len; i++) {
+    for (uint8_t j = 0U; addr_list[i].addr[j] != 0; j++) {
+      if ((addr == addr_list[i].addr[j]) && (bus == addr_list[i].bus)) {
+        index = i;
+        goto Return;
+      }
+    }
+  }
+Return:
+  return index;
+}
+
+// 1Hz safety function called by main. Now just a check for lagging safety messages
+void safety_tick(const safety_hooks *hooks) {
+  uint32_t ts = TIM2->CNT;
+  if (hooks->addr_check != NULL) {
+    for (int i=0; i < hooks->addr_check_len; i++) {
+      uint32_t elapsed_time = get_ts_elapsed(ts, hooks->addr_check[i].last_timestamp);
+      // lag threshold is max of: 1s and MAX_MISSED_MSGS * expected timestep.
+      // Quite conservative to not risk false triggers.
+      // 2s of lag is worse case, since the function is called at 1Hz
+      bool lagging = elapsed_time > MAX(hooks->addr_check[i].expected_timestep * MAX_MISSED_MSGS, 1e6);
+      hooks->addr_check[i].lagging = lagging;
+      if (lagging) {
+        controls_allowed = 0;
+      }
+    }
+  }
+}
+
+void update_counter(AddrCheckStruct addr_list[], int index, uint8_t counter) {
+  if (index != -1) {
+    uint8_t expected_counter = (addr_list[index].last_counter + 1U) % (addr_list[index].max_counter + 1U);
+    addr_list[index].wrong_counters += (expected_counter == counter) ? -1 : 1;
+    addr_list[index].wrong_counters = MAX(MIN(addr_list[index].wrong_counters, MAX_WRONG_COUNTERS), 0);
+    addr_list[index].last_counter = counter;
+  }
+}
+
+bool is_msg_valid(AddrCheckStruct addr_list[], int index) {
+  bool valid = true;
+  if (index != -1) {
+    if ((!addr_list[index].valid_checksum) || (addr_list[index].wrong_counters >= MAX_WRONG_COUNTERS)) {
+      valid = false;
+      controls_allowed = 0;
+    }
+  }
+  return valid;
+}
+
+void update_addr_timestamp(AddrCheckStruct addr_list[], int index) {
+  if (index != -1) {
+    uint32_t ts = TIM2->CNT;
+    addr_list[index].last_timestamp = ts;
+  }
+}
+
+bool addr_safety_check(CAN_FIFOMailBox_TypeDef *to_push,
+                       AddrCheckStruct *rx_checks,
+                       const int rx_checks_len,
+                       uint8_t (*get_checksum)(CAN_FIFOMailBox_TypeDef *to_push),
+                       uint8_t (*compute_checksum)(CAN_FIFOMailBox_TypeDef *to_push),
+                       uint8_t (*get_counter)(CAN_FIFOMailBox_TypeDef *to_push)) {
+
+  int index = get_addr_check_index(to_push, rx_checks, rx_checks_len);
+  update_addr_timestamp(rx_checks, index);
+
+  if (index != -1) {
+    // checksum check
+    if ((get_checksum != NULL) && (compute_checksum != NULL)) {
+      if (rx_checks[index].check_checksum) {
+        uint8_t checksum = get_checksum(to_push);
+        uint8_t checksum_comp = compute_checksum(to_push);
+        rx_checks[index].valid_checksum = checksum_comp == checksum;
+      }
+    }
+
+    // counter check
+    if (get_counter != NULL) {
+      if (rx_checks[index].max_counter > 0U) {
+        uint8_t counter = get_counter(to_push);
+        update_counter(rx_checks, index, counter);
+      }
+    }
+  }
+  return is_msg_valid(rx_checks, index);
+}
+
+
 typedef struct {
   uint16_t id;
   const safety_hooks *hooks;
@@ -115,12 +215,6 @@ int set_safety_hooks(uint16_t mode, int16_t param) {
   return set_status;
 }
 
-// compute the time elapsed (in microseconds) from 2 counter samples
-// case where ts < ts_last is ok: overflow is properly re-casted into uint32_t
-uint32_t get_ts_elapsed(uint32_t ts, uint32_t ts_last) {
-  return ts - ts_last;
-}
-
 // convert a trimmed integer to signed 32 bit int
 int to_signed(int d, int bits) {
   int d_signed = d;

board/safety/safety_cadillac.h

@@ -23,7 +23,7 @@ int cadillac_get_torque_idx(int addr, int array_size) {
   return MIN(MAX(addr - 0x151, 0), array_size);  // 0x151 is id 0, 0x152 is id 1 and so on...
 }
 
-static void cadillac_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
+static int cadillac_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
   int bus = GET_BUS(to_push);
   int addr = GET_ADDR(to_push);
 
@@ -51,6 +51,7 @@ static void cadillac_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
   if ((addr == 0x152) || (addr == 0x154)) {
     cadillac_supercruise_on = (GET_BYTE(to_push, 4) & 0x10) != 0;
   }
+  return 1;
 }
 
 static int cadillac_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {

board/safety/safety_chrysler.h

@@ -6,13 +6,20 @@ const int CHRYSLER_MAX_RATE_DOWN = 3;
 const int CHRYSLER_MAX_TORQUE_ERROR = 80;    // max torque cmd in excess of torque motor
 const AddrBus CHRYSLER_TX_MSGS[] = {{571, 0}, {658, 0}, {678, 0}};
 
+// TODO: do checksum and counter checks
+AddrCheckStruct chrysler_rx_checks[] = {
+  {.addr = {544}, .bus = 0, .expected_timestep = 10000U},
+  {.addr = {500}, .bus = 0, .expected_timestep = 20000U},
+};
+const int CHRYSLER_RX_CHECK_LEN = sizeof(chrysler_rx_checks) / sizeof(chrysler_rx_checks[0]);
+
 int chrysler_rt_torque_last = 0;
 int chrysler_desired_torque_last = 0;
 int chrysler_cruise_engaged_last = 0;
 uint32_t chrysler_ts_last = 0;
 struct sample_t chrysler_torque_meas;         // last few torques measured
 
-static void chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
+static int chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
   int bus = GET_BUS(to_push);
   int addr = GET_ADDR(to_push);
 
@@ -36,10 +43,13 @@ static void chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
     chrysler_cruise_engaged_last = cruise_engaged;
   }
 
+  // TODO: add gas pressed check
+
   // check if stock camera ECU is on bus 0
   if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && (addr == 0x292)) {
     relay_malfunction = true;
   }
+  return 1;
 }
 
 static int chrysler_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
@@ -137,4 +147,6 @@ const safety_hooks chrysler_hooks = {
   .tx = chrysler_tx_hook,
   .tx_lin = nooutput_tx_lin_hook,
   .fwd = chrysler_fwd_hook,
+  .addr_check = chrysler_rx_checks,
+  .addr_check_len = sizeof(chrysler_rx_checks) / sizeof(chrysler_rx_checks[0]),
 };

board/safety/safety_defaults.h

@@ -1,5 +1,6 @@
-void default_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
+int default_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
   UNUSED(to_push);
+  return true;
 }
 
 // *** no output safety mode ***

board/safety/safety_ford.h

@@ -11,7 +11,7 @@ int ford_brake_prev = 0;
 int ford_gas_prev = 0;
 bool ford_moving = false;
 
-static void ford_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
+static int ford_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
 
   int addr = GET_ADDR(to_push);
   int bus = GET_BUS(to_push);
@@ -58,6 +58,7 @@ static void ford_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
   if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && (addr == 0x3CA)) {
     relay_malfunction = true;
   }
+  return 1;
 }
 
 // all commands: just steering

board/safety/safety_gm.h

@@ -23,6 +23,16 @@ const AddrBus GM_TX_MSGS[] = {{384, 0}, {1033, 0}, {1034, 0}, {715, 0}, {880, 0}
                               {789, 2},  // ch bus
                               {0x104c006c, 3}, {0x10400060, 3}};  // gmlan
 
+// TODO: do checksum and counter checks. Add correct timestep, 0.1s for now.
+AddrCheckStruct gm_rx_checks[] = {
+  {.addr = {388}, .bus = 0, .expected_timestep = 100000U},
+  {.addr = {842}, .bus = 0, .expected_timestep = 100000U},
+  {.addr = {481}, .bus = 0, .expected_timestep = 100000U},
+  {.addr = {241}, .bus = 0, .expected_timestep = 100000U},
+  {.addr = {417}, .bus = 0, .expected_timestep = 100000U},
+};
+const int GM_RX_CHECK_LEN = sizeof(gm_rx_checks) / sizeof(gm_rx_checks[0]);
+
 int gm_brake_prev = 0;
 int gm_gas_prev = 0;
 bool gm_moving = false;
@@ -31,7 +41,7 @@ int gm_desired_torque_last = 0;
 uint32_t gm_ts_last = 0;
 struct sample_t gm_torque_driver;         // last few driver torques measured
 
-static void gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
+static int gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
   int bus = GET_BUS(to_push);
   int addr = GET_ADDR(to_push);
 
@@ -103,6 +113,7 @@ static void gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
   if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && ((addr == 384) || (addr == 715))) {
     relay_malfunction = true;
   }
+  return 1;
 }
 
 // all commands: gas/regen, friction brake and steering
@@ -219,4 +230,6 @@ const safety_hooks gm_hooks = {
   .tx = gm_tx_hook,
   .tx_lin = nooutput_tx_lin_hook,
   .fwd = default_fwd_hook,
+  .addr_check = gm_rx_checks,
+  .addr_check_len = sizeof(gm_rx_checks) / sizeof(gm_rx_checks[0]),
 };

board/safety/safety_gm_ascm.h

@@ -41,4 +41,3 @@ const safety_hooks gm_ascm_hooks = {
   .tx_lin = nooutput_tx_lin_hook,
   .fwd = gm_ascm_fwd_hook,
 };
-