Volkswagen safety updates: Phase 2 (#445)

* CRC and counter checks, standstill/brake checks

* Clean up a tsk_06 that snuck through

* Be consistent about how we call _msg_esp_05

* Reduce scope: haunted by the ghost of MISRA future

* Improved check/test for in-motion braking

* MISRA styling fix

Author: jyoung8607

board/safety.h

@@ -57,6 +57,21 @@ int safety_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
   return current_hooks->fwd(bus_num, to_fwd);
 }
 
+// Given a CRC-8 poly, generate a static lookup table to use with a fast CRC-8
+// algorithm. Called at init time for safety modes using CRC-8.
+void gen_crc_lookup_table(uint8_t poly, uint8_t crc_lut[]) {
+  for (int i = 0; i < 256; i++) {
+    uint8_t crc = i;
+    for (int j = 0; j < 8; j++) {
+      if ((crc & 0x80U) != 0U)
+        crc = (uint8_t)((crc << 1) ^ poly);
+      else
+        crc <<= 1;
+    }
+    crc_lut[i] = crc;
+  }
+}
+
 bool msg_allowed(int addr, int bus, const AddrBus addr_list[], int len) {
   bool allowed = false;
   for (int i = 0; i < len; i++) {

board/safety/safety_volkswagen.h

@@ -8,7 +8,9 @@ const int VOLKSWAGEN_DRIVER_TORQUE_ALLOWANCE = 80;
 const int VOLKSWAGEN_DRIVER_TORQUE_FACTOR = 3;
 
 // Safety-relevant CAN messages for the Volkswagen MQB platform
+#define MSG_ESP_19      0x0B2   // RX from ABS, for wheel speeds
 #define MSG_EPS_01      0x09F   // RX from EPS, for driver steering torque
+#define MSG_ESP_05      0x106   // RX from ABS, for brake switch state
 #define MSG_MOTOR_20    0x121   // RX from ECU, for driver throttle input
 #define MSG_ACC_06      0x122   // RX from ACC radar, for status and engagement
 #define MSG_HCA_01      0x126   // TX by OP, Heading Control Assist steering torque
@@ -19,11 +21,12 @@ const int VOLKSWAGEN_DRIVER_TORQUE_FACTOR = 3;
 const AddrBus VOLKSWAGEN_MQB_TX_MSGS[] = {{MSG_HCA_01, 0}, {MSG_GRA_ACC_01, 0}, {MSG_GRA_ACC_01, 2}, {MSG_LDW_02, 0}};
 const int VOLKSWAGEN_MQB_TX_MSGS_LEN = sizeof(VOLKSWAGEN_MQB_TX_MSGS) / sizeof(VOLKSWAGEN_MQB_TX_MSGS[0]);
 
-// TODO: do checksum and counter checks
 AddrCheckStruct volkswagen_mqb_rx_checks[] = {
-  {.addr = {MSG_EPS_01}, .bus = 0, .expected_timestep = 10000U},
-  {.addr = {MSG_ACC_06}, .bus = 0, .expected_timestep = 20000U},
-  {.addr = {MSG_MOTOR_20}, .bus = 0, .expected_timestep = 20000U},
+  {.addr = {MSG_ESP_19},   .bus = 0, .check_checksum = false, .max_counter = 0U,  .expected_timestep = 10000U},
+  {.addr = {MSG_EPS_01},   .bus = 0, .check_checksum = true,  .max_counter = 15U, .expected_timestep = 10000U},
+  {.addr = {MSG_ESP_05},   .bus = 0, .check_checksum = true,  .max_counter = 15U, .expected_timestep = 20000U},
+  {.addr = {MSG_MOTOR_20}, .bus = 0, .check_checksum = true,  .max_counter = 15U, .expected_timestep = 20000U},
+  {.addr = {MSG_ACC_06},   .bus = 0, .check_checksum = true,  .max_counter = 15U, .expected_timestep = 20000U},
 };
 const int VOLKSWAGEN_MQB_RX_CHECKS_LEN = sizeof(volkswagen_mqb_rx_checks) / sizeof(volkswagen_mqb_rx_checks[0]);
 
@@ -32,9 +35,56 @@ struct sample_t volkswagen_torque_driver; // Last few driver torques measured
 int volkswagen_rt_torque_last = 0;
 int volkswagen_desired_torque_last = 0;
 uint32_t volkswagen_ts_last = 0;
+bool volkswagen_moving = false;
+bool volkswagen_brake_pressed_prev = false;
 int volkswagen_gas_prev = 0;
 int volkswagen_torque_msg = 0;
 int volkswagen_lane_msg = 0;
+uint8_t volkswagen_crc8_lut_8h2f[256]; // Static lookup table for CRC8 poly 0x2F, aka 8H2F/AUTOSAR
+
+
+static uint8_t volkswagen_get_checksum(CAN_FIFOMailBox_TypeDef *to_push) {
+  return (uint8_t)GET_BYTE(to_push, 0);
+}
+
+static uint8_t volkswagen_get_counter(CAN_FIFOMailBox_TypeDef *to_push) {
+  return (uint8_t)GET_BYTE(to_push, 1) & 0xFU;
+}
+
+static uint8_t volkswagen_mqb_compute_crc(CAN_FIFOMailBox_TypeDef *to_push) {
+  int addr = GET_ADDR(to_push);
+  int len = GET_LEN(to_push);
+
+  // This is CRC-8H2F/AUTOSAR with a twist. See the OpenDBC implementation
+  // of this algorithm for a version with explanatory comments.
+
+  uint8_t crc = 0xFFU;
+  for (int i = 1; i < len; i++) {
+    crc ^= (uint8_t)GET_BYTE(to_push, i);
+    crc = volkswagen_crc8_lut_8h2f[crc];
+  }
+
+  uint8_t counter = volkswagen_get_counter(to_push);
+  switch(addr) {
+    case MSG_EPS_01:
+      crc ^= (uint8_t[]){0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5}[counter];
+      break;
+    case MSG_ESP_05:
+      crc ^= (uint8_t[]){0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07}[counter];
+      break;
+    case MSG_MOTOR_20:
+      crc ^= (uint8_t[]){0xE9,0x65,0xAE,0x6B,0x7B,0x35,0xE5,0x5F,0x4E,0xC7,0x86,0xA2,0xBB,0xDD,0xEB,0xB4}[counter];
+      break;
+    case MSG_ACC_06:
+      crc ^= (uint8_t[]){0x37,0x7D,0xF3,0xA9,0x18,0x46,0x6D,0x4D,0x3D,0x71,0x92,0x9C,0xE5,0x32,0x10,0xB9}[counter];
+      break;
+    default: // Undefined CAN message, CRC check expected to fail
+      break;
+  }
+  crc = volkswagen_crc8_lut_8h2f[crc];
+
+  return crc ^ 0xFFU;
+}
 
 static void volkswagen_mqb_init(int16_t param) {
   UNUSED(param);
@@ -43,17 +93,29 @@ static void volkswagen_mqb_init(int16_t param) {
   relay_malfunction = false;
   volkswagen_torque_msg = MSG_HCA_01;
   volkswagen_lane_msg = MSG_LDW_02;
+  gen_crc_lookup_table(0x2F, volkswagen_crc8_lut_8h2f);
 }
 
 static int volkswagen_mqb_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
 
   bool valid = addr_safety_check(to_push, volkswagen_mqb_rx_checks, VOLKSWAGEN_MQB_RX_CHECKS_LEN,
-                                 NULL, NULL, NULL);
+                                 volkswagen_get_checksum, volkswagen_mqb_compute_crc, volkswagen_get_counter);
 
   if (valid) {
     int bus = GET_BUS(to_push);
     int addr = GET_ADDR(to_push);
 
+    // Update in-motion state by sampling front wheel speeds
+    // Signal: ESP_19.ESP_VL_Radgeschw_02 (front left) in scaled km/h
+    // Signal: ESP_19.ESP_VR_Radgeschw_02 (front right) in scaled km/h
+    if ((bus == 0) && (addr == MSG_ESP_19)) {
+      int wheel_speed_fl = GET_BYTE(to_push, 4) | (GET_BYTE(to_push, 5) << 8);
+      int wheel_speed_fr = GET_BYTE(to_push, 6) | (GET_BYTE(to_push, 7) << 8);
+      // Check for average front speed in excess of 0.3m/s, 1.08km/h
+      // DBC speed scale 0.0075: 0.3m/s = 144, sum both wheels to compare
+      volkswagen_moving = (wheel_speed_fl + wheel_speed_fr) > 288;
+    }
+
     // Update driver input torque samples
     // Signal: EPS_01.Driver_Strain (absolute torque)
     // Signal: EPS_01.Driver_Strain_VZ (direction)
@@ -83,6 +145,16 @@ static int volkswagen_mqb_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
       volkswagen_gas_prev = gas;
     }
 
+    // Exit controls on rising edge of brake press
+    // Signal: ESP_05.ESP_Fahrer_bremst
+    if ((bus == 0) && (addr == MSG_ESP_05)) {
+      bool brake_pressed = (GET_BYTE(to_push, 3) & 0x4) >> 2;
+      if (brake_pressed && (!(volkswagen_brake_pressed_prev) || volkswagen_moving)) {
+        controls_allowed = 0;
+      }
+      volkswagen_brake_pressed_prev = brake_pressed;
+    }
+
     // If there are HCA messages on bus 0 not sent by OP, there's a relay problem
     if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && (addr == MSG_HCA_01)) {
       relay_malfunction = true;

board/safety_declarations.h

@@ -49,6 +49,7 @@ bool driver_limit_check(int val, int val_last, struct sample_t *val_driver,
   const int MAX_ALLOWANCE, const int DRIVER_FACTOR);
 bool rt_rate_limit_check(int val, int val_last, const int MAX_RT_DELTA);
 float interpolate(struct lookup_t xy, float x);
+void gen_crc_lookup_table(uint8_t poly, uint8_t crc_lut[]);
 bool msg_allowed(int addr, int bus, const AddrBus addr_list[], int len);
 int get_addr_check_index(CAN_FIFOMailBox_TypeDef *to_push, AddrCheckStruct addr_list[], const int len);
 void update_counter(AddrCheckStruct addr_list[], int index, uint8_t counter);

requirements.txt

@@ -9,3 +9,4 @@ requests
 flake8==3.7.9
 pylint==2.4.3
 cffi==1.11.4
+crcmod

tests/safety/libpandasafety_py.py

@@ -94,6 +94,8 @@
 int get_volkswagen_gas_prev(void);
 int get_volkswagen_torque_driver_min(void);
 int get_volkswagen_torque_driver_max(void);
+bool get_volkswagen_moving(void);
+bool get_volkswagen_brake_pressed_prev(void);
 void set_volkswagen_desired_torque_last(int t);
 void set_volkswagen_rt_torque_last(int t);
 void set_volkswagen_torque_driver(int min, int max);

tests/safety/test.c

@@ -232,6 +232,14 @@ void set_volkswagen_desired_torque_last(int t){
   volkswagen_desired_torque_last = t;
 }
 
+int get_volkswagen_moving(void){
+  return volkswagen_moving;
+}
+
+int get_volkswagen_brake_pressed_prev(void){
+  return volkswagen_brake_pressed_prev;
+}
+
 int get_volkswagen_gas_prev(void){
   return volkswagen_gas_prev;
 }
@@ -334,6 +342,9 @@ void init_tests_subaru(void){
 
 void init_tests_volkswagen(void){
   init_tests();
+  volkswagen_moving = false;
+  volkswagen_brake_pressed_prev = false;
+  volkswagen_gas_prev = 0;
   volkswagen_torque_driver.min = 0;
   volkswagen_torque_driver.max = 0;
   volkswagen_desired_torque_last = 0;