Subaru checksum counter (#455)

* Added Subaru Global checksum and counter checks
commaai · Feb 28, 2020 · fb02390 · fb02390
1 parent 9a44499
commit fb02390
Show file tree

Hide file tree

Showing 2 changed files with 45 additions and 5 deletions.
diff --git a/board/safety/safety_subaru.h b/board/safety/safety_subaru.h
@@ -13,12 +13,12 @@ const AddrBus SUBARU_L_TX_MSGS[] = {{0x164, 0}, {0x221, 0}, {0x322, 0}};
 const int SUBARU_TX_MSGS_LEN = sizeof(SUBARU_TX_MSGS) / sizeof(SUBARU_TX_MSGS[0]);
 const int SUBARU_L_TX_MSGS_LEN = sizeof(SUBARU_L_TX_MSGS) / sizeof(SUBARU_L_TX_MSGS[0]);
 
-// TODO: do checksum and counter checks after adding the signals to the outback dbc file
 AddrCheckStruct subaru_rx_checks[] = {
-  {.addr = { 0x40}, .bus = 0, .expected_timestep = 10000U},
-  {.addr = {0x119}, .bus = 0, .expected_timestep = 20000U},
-  {.addr = {0x240}, .bus = 0, .expected_timestep = 50000U},
+  {.addr = { 0x40}, .bus = 0, .check_checksum = true, .max_counter = 15U, .expected_timestep = 10000U},
+  {.addr = {0x119}, .bus = 0, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U},
+  {.addr = {0x240}, .bus = 0, .check_checksum = true, .max_counter = 15U, .expected_timestep = 50000U},
 };
+// TODO: do checksum and counter checks after adding the signals to the outback dbc file
 AddrCheckStruct subaru_l_rx_checks[] = {
   {.addr = {0x140}, .bus = 0, .expected_timestep = 10000U},
   {.addr = {0x371}, .bus = 0, .expected_timestep = 20000U},
@@ -35,12 +35,30 @@ bool subaru_gas_last = false;
 bool subaru_global = false;
 struct sample_t subaru_torque_driver;         // last few driver torques measured
 
+static uint8_t subaru_get_checksum(CAN_FIFOMailBox_TypeDef *to_push) {
+  return (uint8_t)GET_BYTE(to_push, 0);
+}
+
+static uint8_t subaru_get_counter(CAN_FIFOMailBox_TypeDef *to_push) {
+  return (uint8_t)(GET_BYTE(to_push, 1) & 0xF);
+}
+
+static uint8_t subaru_compute_checksum(CAN_FIFOMailBox_TypeDef *to_push) {
+  int addr = GET_ADDR(to_push);
+  int len = GET_LEN(to_push);
+  uint8_t checksum = (uint8_t)(addr) + (uint8_t)((unsigned int)(addr) >> 8U);
+  for (int i = 1; i < len; i++) {
+    checksum += (uint8_t)GET_BYTE(to_push, i);
+  }
+  return checksum;
+}
+
 static int subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
 
   bool valid = false;
   if (subaru_global) {
     valid = addr_safety_check(to_push, subaru_rx_checks, SUBARU_RX_CHECK_LEN,
-                              NULL, NULL, NULL);
+                              subaru_get_checksum, subaru_compute_checksum, subaru_get_counter);
   } else {
     valid = addr_safety_check(to_push, subaru_l_rx_checks, SUBARU_L_RX_CHECK_LEN,
                               NULL, NULL, NULL);

diff --git a/tests/safety/test_subaru.py b/tests/safety/test_subaru.py
@@ -30,12 +30,25 @@ def sign(a):
   else:
     return -1
 
+def subaru_checksum(msg, addr, len_msg):
+  checksum = addr + (addr >> 8)
+  for i in range(len_msg):
+    if i < 4:
+      checksum += (msg.RDLR >> (8 * i))
+    else:
+      checksum += (msg.RDHR >> (8 * (i - 4)))
+  return checksum & 0xff
+
+
 class TestSubaruSafety(unittest.TestCase):
   @classmethod
   def setUp(cls):
     cls.safety = libpandasafety_py.libpandasafety
     cls.safety.set_safety_hooks(Panda.SAFETY_SUBARU, 0)
     cls.safety.init_tests_subaru()
+    cls.cnt_gas = 0
+    cls.cnt_torque_driver = 0
+    cls.cnt_cruise = 0
 
   def _set_prev_torque(self, t):
     self.safety.set_subaru_desired_torque_last(t)
@@ -46,6 +59,9 @@ def _torque_driver_msg(self, torque):
     if self.safety.get_subaru_global():
       to_send = make_msg(0, 0x119)
       to_send[0].RDLR = ((t & 0x7FF) << 16)
+      to_send[0].RDLR |= (self.cnt_torque_driver & 0xF) << 8
+      to_send[0].RDLR |= subaru_checksum(to_send, 0x119, 8)
+      self.cnt_torque_driver += 1
     else:
       to_send = make_msg(0, 0x371)
       to_send[0].RDLR = (t & 0x7) << 29
@@ -66,6 +82,9 @@ def _gas_msg(self, gas):
     if self.safety.get_subaru_global():
       to_send = make_msg(0, 0x40)
       to_send[0].RDHR = gas & 0xFF
+      to_send[0].RDLR |= (self.cnt_gas & 0xF) << 8
+      to_send[0].RDLR |= subaru_checksum(to_send, 0x40, 8)
+      self.cnt_gas += 1
     else:
       to_send = make_msg(0, 0x140)
       to_send[0].RDLR = gas & 0xFF
@@ -75,6 +94,9 @@ def _cruise_msg(self, cruise):
     if self.safety.get_subaru_global():
       to_send = make_msg(0, 0x240)
       to_send[0].RDHR = cruise << 9
+      to_send[0].RDLR |= (self.cnt_cruise & 0xF) << 8
+      to_send[0].RDLR |= subaru_checksum(to_send, 0x240, 8)
+      self.cnt_cruise += 1
     else:
       to_send = make_msg(0, 0x144)
       to_send[0].RDHR = cruise << 17