diff --git a/VERSION b/VERSION index 6c4fc20..75e4760 100644 --- a/VERSION +++ b/VERSION @@ -1 +1 @@ -v1.7.1 \ No newline at end of file +v1.7.2 \ No newline at end of file diff --git a/board/drivers/can.h b/board/drivers/can.h index b69348e..93db256 100644 --- a/board/drivers/can.h +++ b/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); diff --git a/board/main.c b/board/main.c index 9f7a337..a758bf7 100644 --- a/board/main.c +++ b/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; } diff --git a/board/safety.h b/board/safety.h index 1a587a1..4588eec 100644 --- a/board/safety.h +++ b/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; diff --git a/board/safety/safety_cadillac.h b/board/safety/safety_cadillac.h index 2900ec5..0f500a8 100644 --- a/board/safety/safety_cadillac.h +++ b/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) { diff --git a/board/safety/safety_chrysler.h b/board/safety/safety_chrysler.h index 76d8ece..8c201a3 100644 --- a/board/safety/safety_chrysler.h +++ b/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]), }; diff --git a/board/safety/safety_defaults.h b/board/safety/safety_defaults.h index 4733438..ba96b7d 100644 --- a/board/safety/safety_defaults.h +++ b/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 *** diff --git a/board/safety/safety_ford.h b/board/safety/safety_ford.h index 47c7342..97e0f57 100644 --- a/board/safety/safety_ford.h +++ b/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 diff --git a/board/safety/safety_gm.h b/board/safety/safety_gm.h index 67d0f9e..5356a11 100644 --- a/board/safety/safety_gm.h +++ b/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]), }; diff --git a/board/safety/safety_gm_ascm.h b/board/safety/safety_gm_ascm.h index 36fd1d8..e7eddf4 100644 --- a/board/safety/safety_gm_ascm.h +++ b/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, }; - diff --git a/board/safety/safety_honda.h b/board/safety/safety_honda.h index 575ec6c..ad981f2 100644 --- a/board/safety/safety_honda.h +++ b/board/safety/safety_honda.h @@ -7,9 +7,26 @@ // brake rising edge // brake > 0mph const AddrBus HONDA_N_TX_MSGS[] = {{0xE4, 0}, {0x194, 0}, {0x1FA, 0}, {0x200, 0}, {0x30C, 0}, {0x33D, 0}, {0x39F, 0}}; -const AddrBus HONDA_BH_TX_MSGS[] = {{0xE4, 0}, {0x296, 1}, {0x33D, 0}}; // Bosch Harness const AddrBus HONDA_BG_TX_MSGS[] = {{0xE4, 2}, {0x296, 0}, {0x33D, 2}}; // Bosch Giraffe +const AddrBus HONDA_BH_TX_MSGS[] = {{0xE4, 0}, {0x296, 1}, {0x33D, 0}}; // Bosch Harness const int HONDA_GAS_INTERCEPTOR_THRESHOLD = 328; // ratio between offset and gain from dbc file + +// Nidec and Bosch giraffe have pt on bus 0 +AddrCheckStruct honda_rx_checks[] = { + {.addr = {0x1A6, 0x296}, .bus = 0, .check_checksum = true, .max_counter = 3U, .expected_timestep = 40000U}, + {.addr = { 0x158}, .bus = 0, .check_checksum = true, .max_counter = 3U, .expected_timestep = 10000U}, + {.addr = { 0x17C}, .bus = 0, .check_checksum = true, .max_counter = 3U, .expected_timestep = 10000U}, +}; +const int HONDA_RX_CHECKS_LEN = sizeof(honda_rx_checks) / sizeof(honda_rx_checks[0]); + +// Bosch harness has pt on bus 1 +AddrCheckStruct honda_bh_rx_checks[] = { + {.addr = {0x296}, .bus = 1, .check_checksum = true, .max_counter = 3U, .expected_timestep = 40000U}, + {.addr = {0x158}, .bus = 1, .check_checksum = true, .max_counter = 3U, .expected_timestep = 10000U}, + {.addr = {0x17C}, .bus = 1, .check_checksum = true, .max_counter = 3U, .expected_timestep = 10000U}, +}; +const int HONDA_BH_RX_CHECKS_LEN = sizeof(honda_bh_rx_checks) / sizeof(honda_bh_rx_checks[0]); + int honda_brake = 0; int honda_gas_prev = 0; bool honda_brake_pressed_prev = false; @@ -19,97 +36,136 @@ bool honda_fwd_brake = false; enum {HONDA_N_HW, HONDA_BG_HW, HONDA_BH_HW} honda_hw = HONDA_N_HW; -static void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { +static uint8_t honda_get_checksum(CAN_FIFOMailBox_TypeDef *to_push) { + int checksum_byte = GET_LEN(to_push) - 1; + return (uint8_t)(GET_BYTE(to_push, checksum_byte)) & 0xFU; +} - int addr = GET_ADDR(to_push); +static uint8_t honda_compute_checksum(CAN_FIFOMailBox_TypeDef *to_push) { int len = GET_LEN(to_push); - int bus = GET_BUS(to_push); - - // sample speed - if (addr == 0x158) { - // first 2 bytes - honda_moving = GET_BYTE(to_push, 0) | GET_BYTE(to_push, 1); + uint8_t checksum = 0U; + unsigned int addr = GET_ADDR(to_push); + while (addr > 0U) { + checksum += (addr & 0xFU); addr >>= 4; } - - // state machine to enter and exit controls - // 0x1A6 for the ILX, 0x296 for the Civic Touring - if ((addr == 0x1A6) || (addr == 0x296)) { - int button = (GET_BYTE(to_push, 0) & 0xE0) >> 5; - switch (button) { - case 2: // cancel - controls_allowed = 0; - break; - case 3: // set - case 4: // resume - controls_allowed = 1; - break; - default: - break; // any other button is irrelevant + for (int j = 0; (j < len); j++) { + uint8_t byte = GET_BYTE(to_push, j); + checksum += (byte & 0xFU) + (byte >> 4U); + if (j == (len - 1)) { + checksum -= (byte & 0xFU); // remove checksum in message } } + return (8U - checksum) & 0xFU; +} - // user brake signal on 0x17C reports applied brake from computer brake on accord - // and crv, which prevents the usual brake safety from working correctly. these - // cars have a signal on 0x1BE which only detects user's brake being applied so - // in these cases, this is used instead. - // most hondas: 0x17C bit 53 - // accord, crv: 0x1BE bit 4 - // exit controls on rising edge of brake press or on brake press when speed > 0 - bool is_user_brake_msg = honda_alt_brake_msg ? ((addr) == 0x1BE) : ((addr) == 0x17C); - if (is_user_brake_msg) { - bool brake_pressed = honda_alt_brake_msg ? (GET_BYTE((to_push), 0) & 0x10) : (GET_BYTE((to_push), 6) & 0x20); - if (brake_pressed && (!(honda_brake_pressed_prev) || honda_moving)) { - controls_allowed = 0; - } - honda_brake_pressed_prev = brake_pressed; +static uint8_t honda_get_counter(CAN_FIFOMailBox_TypeDef *to_push) { + int counter_byte = GET_LEN(to_push) - 1; + return ((uint8_t)(GET_BYTE(to_push, counter_byte)) >> 4U) & 0x3U; +} + +static int honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { + + bool valid; + if (honda_hw == HONDA_BH_HW) { + valid = addr_safety_check(to_push, honda_bh_rx_checks, HONDA_BH_RX_CHECKS_LEN, + honda_get_checksum, honda_compute_checksum, honda_get_counter); + } else { + valid = addr_safety_check(to_push, honda_rx_checks, HONDA_RX_CHECKS_LEN, + honda_get_checksum, honda_compute_checksum, honda_get_counter); } - // exit controls on rising edge of gas press if interceptor (0x201 w/ len = 6) - // length check because bosch hardware also uses this id (0x201 w/ len = 8) - if ((addr == 0x201) && (len == 6)) { - gas_interceptor_detected = 1; - int gas_interceptor = GET_INTERCEPTOR(to_push); - if ((gas_interceptor > HONDA_GAS_INTERCEPTOR_THRESHOLD) && - (gas_interceptor_prev <= HONDA_GAS_INTERCEPTOR_THRESHOLD)) { - controls_allowed = 0; - } - gas_interceptor_prev = gas_interceptor; - } + if (valid) { + int addr = GET_ADDR(to_push); + int len = GET_LEN(to_push); + int bus = GET_BUS(to_push); - // exit controls on rising edge of gas press if no interceptor - if (!gas_interceptor_detected) { - if (addr == 0x17C) { - int gas = GET_BYTE(to_push, 0); - if (gas && !honda_gas_prev) { + // sample speed + if (addr == 0x158) { + // first 2 bytes + honda_moving = GET_BYTE(to_push, 0) | GET_BYTE(to_push, 1); + } + + // state machine to enter and exit controls + // 0x1A6 for the ILX, 0x296 for the Civic Touring + if ((addr == 0x1A6) || (addr == 0x296)) { + int button = (GET_BYTE(to_push, 0) & 0xE0) >> 5; + switch (button) { + case 2: // cancel + controls_allowed = 0; + break; + case 3: // set + case 4: // resume + controls_allowed = 1; + break; + default: + break; // any other button is irrelevant + } + } + + // user brake signal on 0x17C reports applied brake from computer brake on accord + // and crv, which prevents the usual brake safety from working correctly. these + // cars have a signal on 0x1BE which only detects user's brake being applied so + // in these cases, this is used instead. + // most hondas: 0x17C bit 53 + // accord, crv: 0x1BE bit 4 + // exit controls on rising edge of brake press or on brake press when speed > 0 + bool is_user_brake_msg = honda_alt_brake_msg ? ((addr) == 0x1BE) : ((addr) == 0x17C); + if (is_user_brake_msg) { + bool brake_pressed = honda_alt_brake_msg ? (GET_BYTE((to_push), 0) & 0x10) : (GET_BYTE((to_push), 6) & 0x20); + if (brake_pressed && (!(honda_brake_pressed_prev) || honda_moving)) { controls_allowed = 0; } - honda_gas_prev = gas; + honda_brake_pressed_prev = brake_pressed; } - } - if ((bus == 2) && (addr == 0x1FA)) { - bool honda_stock_aeb = GET_BYTE(to_push, 3) & 0x20; - int honda_stock_brake = (GET_BYTE(to_push, 0) << 2) + ((GET_BYTE(to_push, 1) >> 6) & 0x3); - // Forward AEB when stock braking is higher than openpilot braking - // only stop forwarding when AEB event is over - if (!honda_stock_aeb) { - honda_fwd_brake = false; - } else if (honda_stock_brake >= honda_brake) { - honda_fwd_brake = true; - } else { - // Leave Honda forward brake as is + // exit controls on rising edge of gas press if interceptor (0x201 w/ len = 6) + // length check because bosch hardware also uses this id (0x201 w/ len = 8) + if ((addr == 0x201) && (len == 6)) { + gas_interceptor_detected = 1; + int gas_interceptor = GET_INTERCEPTOR(to_push); + if ((gas_interceptor > HONDA_GAS_INTERCEPTOR_THRESHOLD) && + (gas_interceptor_prev <= HONDA_GAS_INTERCEPTOR_THRESHOLD)) { + controls_allowed = 0; + } + gas_interceptor_prev = gas_interceptor; } - } - // if steering controls messages are received on the destination bus, it's an indication - // that the relay might be malfunctioning - int bus_rdr_car = (honda_hw == HONDA_BH_HW) ? 0 : 2; // radar bus, car side - if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && ((addr == 0xE4) || (addr == 0x194))) { - if (((honda_hw != HONDA_N_HW) && (bus == bus_rdr_car)) || - ((honda_hw == HONDA_N_HW) && (bus == 0))) { - relay_malfunction = true; + // exit controls on rising edge of gas press if no interceptor + if (!gas_interceptor_detected) { + if (addr == 0x17C) { + int gas = GET_BYTE(to_push, 0); + if (gas && !honda_gas_prev) { + controls_allowed = 0; + } + honda_gas_prev = gas; + } + } + if ((bus == 2) && (addr == 0x1FA)) { + bool honda_stock_aeb = GET_BYTE(to_push, 3) & 0x20; + int honda_stock_brake = (GET_BYTE(to_push, 0) << 2) + ((GET_BYTE(to_push, 1) >> 6) & 0x3); + + // Forward AEB when stock braking is higher than openpilot braking + // only stop forwarding when AEB event is over + if (!honda_stock_aeb) { + honda_fwd_brake = false; + } else if (honda_stock_brake >= honda_brake) { + honda_fwd_brake = true; + } else { + // Leave Honda forward brake as is + } + } + + // if steering controls messages are received on the destination bus, it's an indication + // that the relay might be malfunctioning + int bus_rdr_car = (honda_hw == HONDA_BH_HW) ? 0 : 2; // radar bus, car side + if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && ((addr == 0xE4) || (addr == 0x194))) { + if (((honda_hw != HONDA_N_HW) && (bus == bus_rdr_car)) || + ((honda_hw == HONDA_N_HW) && (bus == 0))) { + relay_malfunction = true; + } } } + return valid; } // all commands: gas, brake and steering @@ -124,16 +180,12 @@ static int honda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { int addr = GET_ADDR(to_send); int bus = GET_BUS(to_send); - switch (honda_hw) { - case HONDA_BG_HW: - tx = msg_allowed(addr, bus, HONDA_BG_TX_MSGS, sizeof(HONDA_BG_TX_MSGS)/sizeof(HONDA_BG_TX_MSGS[0])); - break; - case HONDA_BH_HW: - tx = msg_allowed(addr, bus, HONDA_BH_TX_MSGS, sizeof(HONDA_BH_TX_MSGS)/sizeof(HONDA_BH_TX_MSGS[0])); - break; - default: // nidec - tx = msg_allowed(addr, bus, HONDA_N_TX_MSGS, sizeof(HONDA_N_TX_MSGS)/sizeof(HONDA_N_TX_MSGS[0])); - break; + if (honda_hw == HONDA_BG_HW) { + tx = msg_allowed(addr, bus, HONDA_BG_TX_MSGS, sizeof(HONDA_BG_TX_MSGS)/sizeof(HONDA_BG_TX_MSGS[0])); + } else if (honda_hw == HONDA_BH_HW) { + tx = msg_allowed(addr, bus, HONDA_BH_TX_MSGS, sizeof(HONDA_BH_TX_MSGS)/sizeof(HONDA_BH_TX_MSGS[0])); + } else { + tx = msg_allowed(addr, bus, HONDA_N_TX_MSGS, sizeof(HONDA_N_TX_MSGS)/sizeof(HONDA_N_TX_MSGS[0])); } if (relay_malfunction) { @@ -271,6 +323,8 @@ const safety_hooks honda_nidec_hooks = { .tx = honda_tx_hook, .tx_lin = nooutput_tx_lin_hook, .fwd = honda_nidec_fwd_hook, + .addr_check = honda_rx_checks, + .addr_check_len = sizeof(honda_rx_checks) / sizeof(honda_rx_checks[0]), }; const safety_hooks honda_bosch_giraffe_hooks = { @@ -279,6 +333,8 @@ const safety_hooks honda_bosch_giraffe_hooks = { .tx = honda_tx_hook, .tx_lin = nooutput_tx_lin_hook, .fwd = honda_bosch_fwd_hook, + .addr_check = honda_rx_checks, + .addr_check_len = sizeof(honda_rx_checks) / sizeof(honda_rx_checks[0]), }; const safety_hooks honda_bosch_harness_hooks = { @@ -287,4 +343,6 @@ const safety_hooks honda_bosch_harness_hooks = { .tx = honda_tx_hook, .tx_lin = nooutput_tx_lin_hook, .fwd = honda_bosch_fwd_hook, + .addr_check = honda_bh_rx_checks, + .addr_check_len = sizeof(honda_bh_rx_checks) / sizeof(honda_bh_rx_checks[0]), }; diff --git a/board/safety/safety_hyundai.h b/board/safety/safety_hyundai.h index f5ce0e1..fcf112a 100644 --- a/board/safety/safety_hyundai.h +++ b/board/safety/safety_hyundai.h @@ -5,16 +5,22 @@ const int HYUNDAI_MAX_RATE_UP = 3; const int HYUNDAI_MAX_RATE_DOWN = 7; const int HYUNDAI_DRIVER_TORQUE_ALLOWANCE = 50; const int HYUNDAI_DRIVER_TORQUE_FACTOR = 2; - const AddrBus HYUNDAI_TX_MSGS[] = {{832, 0}, {1265, 0}}; +// TODO: do checksum and counter checks +AddrCheckStruct hyundai_rx_checks[] = { + {.addr = {897}, .bus = 0, .expected_timestep = 10000U}, + {.addr = {1057}, .bus = 0, .expected_timestep = 20000U}, +}; +const int HYUNDAI_RX_CHECK_LEN = sizeof(hyundai_rx_checks) / sizeof(hyundai_rx_checks[0]); + int hyundai_rt_torque_last = 0; int hyundai_desired_torque_last = 0; int hyundai_cruise_engaged_last = 0; uint32_t hyundai_ts_last = 0; struct sample_t hyundai_torque_driver; // last few driver torques measured -static void hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { +static int hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { int bus = GET_BUS(to_push); int addr = GET_ADDR(to_push); @@ -24,11 +30,6 @@ static void hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { update_sample(&hyundai_torque_driver, torque_driver_new); } - // check if stock camera ECU is on bus 0 - if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && (addr == 832)) { - relay_malfunction = true; - } - // enter controls on rising edge of ACC, exit controls on ACC off if (addr == 1057) { // 2 bits: 13-14 @@ -41,6 +42,14 @@ static void hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { } hyundai_cruise_engaged_last = cruise_engaged; } + + // TODO: check gas pressed + + // check if stock camera ECU is on bus 0 + if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && (addr == 832)) { + relay_malfunction = true; + } + return 1; } static int hyundai_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { @@ -140,4 +149,6 @@ const safety_hooks hyundai_hooks = { .tx = hyundai_tx_hook, .tx_lin = nooutput_tx_lin_hook, .fwd = hyundai_fwd_hook, + .addr_check = hyundai_rx_checks, + .addr_check_len = sizeof(hyundai_rx_checks) / sizeof(hyundai_rx_checks[0]), }; diff --git a/board/safety/safety_mazda.h b/board/safety/safety_mazda.h index f1215f2..f6b19c1 100644 --- a/board/safety/safety_mazda.h +++ b/board/safety/safety_mazda.h @@ -22,7 +22,6 @@ #define MAZDA_DRIVER_TORQUE_ALLOWANCE 15 #define MAZDA_DRIVER_TORQUE_FACTOR 1 - int mazda_cruise_engaged_last = 0; int mazda_rt_torque_last = 0; int mazda_desired_torque_last = 0; @@ -30,7 +29,7 @@ uint32_t mazda_ts_last = 0; struct sample_t mazda_torque_driver; // last few driver torques measured // track msgs coming from OP so that we know what CAM msgs to drop and what to forward -void mazda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { +static int mazda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { int bus = GET_BUS(to_push); int addr = GET_ADDR(to_push); @@ -58,6 +57,7 @@ void mazda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == MAZDA_CAM) && (addr == MAZDA_WHEEL_SPEED)) { relay_malfunction = true; } + return 1; } static int mazda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { @@ -146,4 +146,5 @@ const safety_hooks mazda_hooks = { .tx = mazda_tx_hook, .tx_lin = nooutput_tx_lin_hook, .fwd = mazda_fwd_hook, + // TODO: add addr safety checks }; diff --git a/board/safety/safety_subaru.h b/board/safety/safety_subaru.h index 5b90764..b4ff5f2 100644 --- a/board/safety/safety_subaru.h +++ b/board/safety/safety_subaru.h @@ -10,13 +10,20 @@ const int SUBARU_DRIVER_TORQUE_FACTOR = 10; const AddrBus SUBARU_TX_MSGS[] = {{0x122, 0}, {0x164, 0}, {0x221, 0}, {0x322, 0}}; +// TODO: do checksum and counter checks after adding the signals to the outback dbc file +AddrCheckStruct subaru_rx_checks[] = { + {.addr = {0x119, 0x371}, .bus = 0, .expected_timestep = 20000U}, + {.addr = {0x240, 0x144}, .bus = 0, .expected_timestep = 50000U}, +}; +const int SUBARU_RX_CHECK_LEN = sizeof(subaru_rx_checks) / sizeof(subaru_rx_checks[0]); + int subaru_cruise_engaged_last = 0; int subaru_rt_torque_last = 0; int subaru_desired_torque_last = 0; uint32_t subaru_ts_last = 0; struct sample_t subaru_torque_driver; // last few driver torques measured -static void subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { +static int subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { int bus = GET_BUS(to_push); int addr = GET_ADDR(to_push); @@ -41,9 +48,12 @@ static void subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { subaru_cruise_engaged_last = cruise_engaged; } + // TODO: enforce cancellation on gas pressed + if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && ((addr == 0x122) || (addr == 0x164))) { relay_malfunction = true; } + return 1; } static int subaru_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { @@ -141,4 +151,6 @@ const safety_hooks subaru_hooks = { .tx = subaru_tx_hook, .tx_lin = nooutput_tx_lin_hook, .fwd = subaru_fwd_hook, + .addr_check = subaru_rx_checks, + .addr_check_len = sizeof(subaru_rx_checks) / sizeof(subaru_rx_checks[0]), }; diff --git a/board/safety/safety_tesla.h b/board/safety/safety_tesla.h index 2e5d20c..fbb464d 100644 --- a/board/safety/safety_tesla.h +++ b/board/safety/safety_tesla.h @@ -44,7 +44,7 @@ void reset_gmlan_switch_timeout(void); void gmlan_switch_init(int timeout_enable); -static void tesla_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { +static int tesla_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { set_gmlan_digital_output(0); // #define GMLAN_HIGH 0 reset_gmlan_switch_timeout(); //we're still in tesla safety mode, reset the timeout counter and make sure our output is enabled @@ -120,6 +120,7 @@ static void tesla_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { tesla_controls_allowed_last = controls_allowed; } + return 1; } // all commands: gas/regen, friction brake and steering diff --git a/board/safety/safety_toyota.h b/board/safety/safety_toyota.h index 01ad330..844e387 100644 --- a/board/safety/safety_toyota.h +++ b/board/safety/safety_toyota.h @@ -18,11 +18,16 @@ const int TOYOTA_MIN_ACCEL = -3000; // 3.0 m/s2 const int TOYOTA_GAS_INTERCEPTOR_THRESHOLD = 475; // ratio between offset and gain from dbc file -// allowed DSU messages on bus 0 and 1 const AddrBus TOYOTA_TX_MSGS[] = {{0x283, 0}, {0x2E6, 0}, {0x2E7, 0}, {0x33E, 0}, {0x344, 0}, {0x365, 0}, {0x366, 0}, {0x4CB, 0}, // DSU bus 0 - {0x128, 1}, {0x141, 1}, {0x160, 1}, {0x161, 1}, {0x470, 1}, // DSU bus 1 - {0x2E4, 0}, {0x411, 0}, {0x412, 0}, {0x343, 0}, {0x1D2, 0}, // LKAS + ACC - {0x200, 0}}; // interceptor + {0x128, 1}, {0x141, 1}, {0x160, 1}, {0x161, 1}, {0x470, 1}, // DSU bus 1 + {0x2E4, 0}, {0x411, 0}, {0x412, 0}, {0x343, 0}, {0x1D2, 0}, // LKAS + ACC + {0x200, 0}}; // interceptor + +AddrCheckStruct toyota_rx_checks[] = { + {.addr = {0x260}, .bus = 0, .check_checksum = true, .max_counter = 0U, .expected_timestep = 20000U}, + {.addr = {0x1D2}, .bus = 0, .check_checksum = true, .max_counter = 0U, .expected_timestep = 30000U}, +}; +const int TOYOTA_RX_CHECKS_LEN = sizeof(toyota_rx_checks) / sizeof(toyota_rx_checks[0]); // global actuation limit states int toyota_dbc_eps_torque_factor = 100; // conversion factor for STEER_TORQUE_EPS in %: see dbc file @@ -36,64 +41,84 @@ int toyota_gas_prev = 0; struct sample_t toyota_torque_meas; // last 3 motor torques produced by the eps -static void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { - - int bus = GET_BUS(to_push); +static uint8_t toyota_compute_checksum(CAN_FIFOMailBox_TypeDef *to_push) { int addr = GET_ADDR(to_push); - - // get eps motor torque (0.66 factor in dbc) - if (addr == 0x260) { - int torque_meas_new = (GET_BYTE(to_push, 5) << 8) | GET_BYTE(to_push, 6); - torque_meas_new = to_signed(torque_meas_new, 16); - - // scale by dbc_factor - torque_meas_new = (torque_meas_new * toyota_dbc_eps_torque_factor) / 100; - - // update array of sample - update_sample(&toyota_torque_meas, torque_meas_new); - - // increase torque_meas by 1 to be conservative on rounding - toyota_torque_meas.min--; - toyota_torque_meas.max++; + int len = GET_LEN(to_push); + uint8_t checksum = (uint8_t)(addr) + (uint8_t)((unsigned int)(addr) >> 8U) + (uint8_t)(len); + for (int i = 0; i < (len - 1); i++) { + checksum += (uint8_t)GET_BYTE(to_push, i); } + return checksum; +} - // enter controls on rising edge of ACC, exit controls on ACC off - if (addr == 0x1D2) { - // 5th bit is CRUISE_ACTIVE - int cruise_engaged = GET_BYTE(to_push, 0) & 0x20; - if (!cruise_engaged) { - controls_allowed = 0; +static uint8_t toyota_get_checksum(CAN_FIFOMailBox_TypeDef *to_push) { + int checksum_byte = GET_LEN(to_push) - 1; + return (uint8_t)(GET_BYTE(to_push, checksum_byte)); +} + +static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { + + bool valid = addr_safety_check(to_push, toyota_rx_checks, TOYOTA_RX_CHECKS_LEN, + toyota_get_checksum, toyota_compute_checksum, NULL); + if (valid) { + int bus = GET_BUS(to_push); + int addr = GET_ADDR(to_push); + + // get eps motor torque (0.66 factor in dbc) + if (addr == 0x260) { + int torque_meas_new = (GET_BYTE(to_push, 5) << 8) | GET_BYTE(to_push, 6); + torque_meas_new = to_signed(torque_meas_new, 16); + + // scale by dbc_factor + torque_meas_new = (torque_meas_new * toyota_dbc_eps_torque_factor) / 100; + + // update array of sample + update_sample(&toyota_torque_meas, torque_meas_new); + + // increase torque_meas by 1 to be conservative on rounding + toyota_torque_meas.min--; + toyota_torque_meas.max++; } - if (cruise_engaged && !toyota_cruise_engaged_last) { - controls_allowed = 1; - } - toyota_cruise_engaged_last = cruise_engaged; - } - // exit controls on rising edge of interceptor gas press - if (addr == 0x201) { - gas_interceptor_detected = 1; - int gas_interceptor = GET_INTERCEPTOR(to_push); - if ((gas_interceptor > TOYOTA_GAS_INTERCEPTOR_THRESHOLD) && - (gas_interceptor_prev <= TOYOTA_GAS_INTERCEPTOR_THRESHOLD)) { - controls_allowed = 0; + // enter controls on rising edge of ACC, exit controls on ACC off + if (addr == 0x1D2) { + // 5th bit is CRUISE_ACTIVE + int cruise_engaged = GET_BYTE(to_push, 0) & 0x20; + if (!cruise_engaged) { + controls_allowed = 0; + } + if (cruise_engaged && !toyota_cruise_engaged_last) { + controls_allowed = 1; + } + toyota_cruise_engaged_last = cruise_engaged; } - gas_interceptor_prev = gas_interceptor; - } - // exit controls on rising edge of gas press - if (addr == 0x2C1) { - int gas = GET_BYTE(to_push, 6) & 0xFF; - if ((gas > 0) && (toyota_gas_prev == 0) && !gas_interceptor_detected) { - controls_allowed = 0; + // exit controls on rising edge of interceptor gas press + if (addr == 0x201) { + gas_interceptor_detected = 1; + int gas_interceptor = GET_INTERCEPTOR(to_push); + if ((gas_interceptor > TOYOTA_GAS_INTERCEPTOR_THRESHOLD) && + (gas_interceptor_prev <= TOYOTA_GAS_INTERCEPTOR_THRESHOLD)) { + controls_allowed = 0; + } + gas_interceptor_prev = gas_interceptor; } - toyota_gas_prev = gas; - } - // 0x2E4 is lkas cmd. If it is on bus 0, then relay is unexpectedly closed - if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (addr == 0x2E4) && (bus == 0)) { - relay_malfunction = true; + // exit controls on rising edge of gas press + if (addr == 0x2C1) { + int gas = GET_BYTE(to_push, 6) & 0xFF; + if ((gas > 0) && (toyota_gas_prev == 0) && !gas_interceptor_detected) { + controls_allowed = 0; + } + toyota_gas_prev = gas; + } + + // 0x2E4 is lkas cmd. If it is on bus 0, then relay is unexpectedly closed + if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (addr == 0x2E4) && (bus == 0)) { + relay_malfunction = true; + } } + return valid; } static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { @@ -224,4 +249,6 @@ const safety_hooks toyota_hooks = { .tx = toyota_tx_hook, .tx_lin = nooutput_tx_lin_hook, .fwd = toyota_fwd_hook, + .addr_check = toyota_rx_checks, + .addr_check_len = sizeof(toyota_rx_checks)/sizeof(toyota_rx_checks[0]), }; diff --git a/board/safety/safety_toyota_ipas.h b/board/safety/safety_toyota_ipas.h index d5833e4..e951565 100644 --- a/board/safety/safety_toyota_ipas.h +++ b/board/safety/safety_toyota_ipas.h @@ -31,9 +31,9 @@ uint32_t ts_angle_last = 0; int controls_allowed_last = 0; -static void toyota_ipas_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { +static int toyota_ipas_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // check standard toyota stuff as well - toyota_rx_hook(to_push); + bool valid = toyota_rx_hook(to_push); int addr = GET_ADDR(to_push); @@ -95,6 +95,7 @@ static void toyota_ipas_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { (ipas_state==5))) { controls_allowed = 0; } + return valid; } static int toyota_ipas_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { diff --git a/board/safety/safety_volkswagen.h b/board/safety/safety_volkswagen.h index cf602de..4b01fd4 100644 --- a/board/safety/safety_volkswagen.h +++ b/board/safety/safety_volkswagen.h @@ -17,13 +17,22 @@ const int VOLKSWAGEN_DRIVER_TORQUE_FACTOR = 3; // MSG_GRA_ACC_01 is allowed on bus 0 and 2 to keep compatibility with gateway and camera integration const AddrBus VOLKSWAGEN_TX_MSGS[] = {{MSG_HCA_01, 0}, {MSG_GRA_ACC_01, 0}, {MSG_GRA_ACC_01, 2}, {MSG_LDW_02, 0}}; -struct sample_t volkswagen_torque_driver; // last few driver torques measured +// TODO: do checksum and counter checks +AddrCheckStruct volkswagen_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}, +}; + +const int VOLKSWAGEN_RX_CHECK_LEN = sizeof(volkswagen_rx_checks) / sizeof(volkswagen_rx_checks[0]); + +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; int volkswagen_gas_prev = 0; -static void volkswagen_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { +static int volkswagen_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { int bus = GET_BUS(to_push); int addr = GET_ADDR(to_push); @@ -59,6 +68,7 @@ static void volkswagen_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && (addr == MSG_HCA_01)) { relay_malfunction = true; } + return 1; } static int volkswagen_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { @@ -174,4 +184,6 @@ const safety_hooks volkswagen_hooks = { .tx = volkswagen_tx_hook, .tx_lin = nooutput_tx_lin_hook, .fwd = volkswagen_fwd_hook, + .addr_check = volkswagen_rx_checks, + .addr_check_len = sizeof(volkswagen_rx_checks) / sizeof(volkswagen_rx_checks[0]), }; diff --git a/board/safety_declarations.h b/board/safety_declarations.h index 4a7a39d..1a6a8dd 100644 --- a/board/safety_declarations.h +++ b/board/safety_declarations.h @@ -1,3 +1,6 @@ +const int MAX_WRONG_COUNTERS = 5; +const uint8_t MAX_MISSED_MSGS = 10U; + // sample struct that keeps 3 samples in memory struct sample_t { int values[6]; @@ -16,7 +19,23 @@ typedef struct { int bus; } AddrBus; -void safety_rx_hook(CAN_FIFOMailBox_TypeDef *to_push); +// params and flags about checksum, counter and frequency checks for each monitored address +typedef struct { + // const params + const int addr[3]; // check either messages (e.g. honda steer). Array MUST terminate with a zero to know its length. + const int bus; // bus where to expect the addr. Temp hack: -1 means skip the bus check + const bool check_checksum; // true is checksum check is performed + const uint8_t max_counter; // maximum value of the counter. 0 means that the counter check is skipped + const uint32_t expected_timestep; // expected time between message updates [us] + // dynamic flags + bool valid_checksum; // true if and only if checksum check is passed + int wrong_counters; // counter of wrong counters, saturated between 0 and MAX_WRONG_COUNTERS + uint8_t last_counter; // last counter value + uint32_t last_timestamp; // micro-s + bool lagging; // true if and only if the time between updates is excessive +} AddrCheckStruct; + +int safety_rx_hook(CAN_FIFOMailBox_TypeDef *to_push); int safety_tx_hook(CAN_FIFOMailBox_TypeDef *to_send); int safety_tx_lin_hook(int lin_num, uint8_t *data, int len); uint32_t get_ts_elapsed(uint32_t ts, uint32_t ts_last); @@ -31,9 +50,19 @@ bool driver_limit_check(int val, int val_last, struct sample_t *val_driver, bool rt_rate_limit_check(int val, int val_last, const int MAX_RT_DELTA); float interpolate(struct lookup_t xy, float x); 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); +void update_addr_timestamp(AddrCheckStruct addr_list[], int index); +bool is_msg_valid(AddrCheckStruct addr_list[], int index); +bool addr_safety_check(CAN_FIFOMailBox_TypeDef *to_push, + AddrCheckStruct *addr_check, + const int addr_check_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)); typedef void (*safety_hook_init)(int16_t param); -typedef void (*rx_hook)(CAN_FIFOMailBox_TypeDef *to_push); +typedef int (*rx_hook)(CAN_FIFOMailBox_TypeDef *to_push); typedef int (*tx_hook)(CAN_FIFOMailBox_TypeDef *to_send); typedef int (*tx_lin_hook)(int lin_num, uint8_t *data, int len); typedef int (*fwd_hook)(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd); @@ -44,8 +73,12 @@ typedef struct { tx_hook tx; tx_lin_hook tx_lin; fwd_hook fwd; + AddrCheckStruct *addr_check; + const int addr_check_len; } safety_hooks; +void safety_tick(const safety_hooks *hooks); + // This can be set by the safety hooks bool controls_allowed = false; bool relay_malfunction = false; diff --git a/tests/misra/suppressions.txt b/tests/misra/suppressions.txt index e0a2708..0a479bc 100644 --- a/tests/misra/suppressions.txt +++ b/tests/misra/suppressions.txt @@ -1,8 +1,10 @@ -# Advisory: union types can be used -misra.19.2 # Advisory: casting from void pointer to type pointer is ok. Done by STM libraries as well misra.11.4 # Advisory: casting from void pointer to type pointer is ok. Done by STM libraries as well misra.11.5 +# Advisory: as stated in the Misra document, use of goto statements in accordance to 15.2 and 15.3 is ok +misra.15.1 +# Advisory: union types can be used +misra.19.2 # Required: it's ok re-defining potentially reserved Macro names. Not likely to cause confusion misra.21.1 diff --git a/tests/safety/common.py b/tests/safety/common.py index 99f7237..63408ce 100644 --- a/tests/safety/common.py +++ b/tests/safety/common.py @@ -1,5 +1,7 @@ from panda.tests.safety import libpandasafety_py +MAX_WRONG_COUNTERS = 5 + def make_msg(bus, addr, length=8): to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') if addr >= 0x800: diff --git a/tests/safety/libpandasafety_py.py b/tests/safety/libpandasafety_py.py index 5115a91..77131c1 100644 --- a/tests/safety/libpandasafety_py.py +++ b/tests/safety/libpandasafety_py.py @@ -41,7 +41,7 @@ int get_hw_type(void); void set_timer(uint32_t t); void reset_angle_control(void); -void safety_rx_hook(CAN_FIFOMailBox_TypeDef *to_send); +int safety_rx_hook(CAN_FIFOMailBox_TypeDef *to_send); int safety_tx_hook(CAN_FIFOMailBox_TypeDef *to_push); int safety_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd); int set_safety_hooks(uint16_t mode, int16_t param); diff --git a/tests/safety/test_honda.py b/tests/safety/test_honda.py index 907ae63..e32655d 100755 --- a/tests/safety/test_honda.py +++ b/tests/safety/test_honda.py @@ -3,7 +3,9 @@ import unittest import numpy as np from panda import Panda from panda.tests.safety import libpandasafety_py -from panda.tests.safety.common import test_relay_malfunction, make_msg, test_manually_enable_controls_allowed, test_spam_can_buses +from panda.tests.safety.common import test_relay_malfunction, make_msg, \ + test_manually_enable_controls_allowed, \ + test_spam_can_buses, MAX_WRONG_COUNTERS MAX_BRAKE = 255 @@ -14,28 +16,53 @@ HONDA_N_HW = 0 HONDA_BG_HW = 1 HONDA_BH_HW = 2 +def honda_checksum(msg, addr, len_msg): + checksum = 0 + while addr > 0: + checksum += addr + addr >>= 4 + for i in range (0, 2*len_msg): + if i < 8: + checksum += (msg.RDLR >> (4 * i)) + else: + checksum += (msg.RDHR >> (4 * (i - 8))) + return (8 - checksum) & 0xF + + class TestHondaSafety(unittest.TestCase): @classmethod def setUp(cls): cls.safety = libpandasafety_py.libpandasafety cls.safety.set_safety_hooks(Panda.SAFETY_HONDA_NIDEC, 0) cls.safety.init_tests_honda() + cls.cnt_speed = 0 + cls.cnt_gas = 0 + cls.cnt_button = 0 def _speed_msg(self, speed): to_send = make_msg(0, 0x158) to_send[0].RDLR = speed + to_send[0].RDHR |= (self.cnt_speed % 4) << 28 + to_send[0].RDHR |= honda_checksum(to_send[0], 0x158, 8) << 24 + self.cnt_speed += 1 return to_send - def _button_msg(self, buttons, msg): + def _button_msg(self, buttons, addr): honda_hw = self.safety.get_honda_hw() bus = 1 if honda_hw == HONDA_BH_HW else 0 - to_send = make_msg(bus, msg) + to_send = make_msg(bus, addr) to_send[0].RDLR = buttons << 5 + to_send[0].RDHR |= (self.cnt_button % 4) << 28 + to_send[0].RDHR |= honda_checksum(to_send[0], addr, 8) << 24 + self.cnt_button += 1 return to_send def _brake_msg(self, brake): to_send = make_msg(0, 0x17C) to_send[0].RDHR = 0x200000 if brake else 0 + to_send[0].RDHR |= (self.cnt_gas % 4) << 28 + to_send[0].RDHR |= honda_checksum(to_send[0], 0x17C, 8) << 24 + self.cnt_gas += 1 return to_send def _alt_brake_msg(self, brake): @@ -46,6 +73,9 @@ class TestHondaSafety(unittest.TestCase): def _gas_msg(self, gas): to_send = make_msg(0, 0x17C) to_send[0].RDLR = 1 if gas else 0 + to_send[0].RDHR |= (self.cnt_gas % 4) << 28 + to_send[0].RDHR |= honda_checksum(to_send[0], 0x17C, 8) << 24 + self.cnt_gas += 1 return to_send def _send_brake_msg(self, brake): @@ -229,6 +259,51 @@ class TestHondaSafety(unittest.TestCase): self.safety.set_controls_allowed(1) self.assertTrue(self.safety.safety_tx_hook(self._button_msg(RESUME_BTN, BUTTON_MSG))) + def test_rx_hook(self): + # checksum checks + SET_BTN = 3 + for msg in ["btn1", "btn2", "gas", "speed"]: + self.safety.set_controls_allowed(1) + if msg == "btn1": + to_push = self._button_msg(SET_BTN, 0x1A6) + if msg == "btn2": + to_push = self._button_msg(SET_BTN, 0x296) + if msg == "gas": + to_push = self._gas_msg(0) + if msg == "speed": + to_push = self._speed_msg(0) + self.assertTrue(self.safety.safety_rx_hook(to_push)) + to_push[0].RDHR = 0 + self.assertFalse(self.safety.safety_rx_hook(to_push)) + self.assertFalse(self.safety.get_controls_allowed()) + + # counter + # reset wrong_counters to zero by sending valid messages + for i in range(MAX_WRONG_COUNTERS + 1): + self.cnt_speed = 0 + self.cnt_gas = 0 + self.cnt_button = 0 + if i < MAX_WRONG_COUNTERS: + self.safety.set_controls_allowed(1) + self.safety.safety_rx_hook(self._button_msg(SET_BTN, 0x1A6)) + self.safety.safety_rx_hook(self._speed_msg(0)) + self.safety.safety_rx_hook(self._gas_msg(0)) + else: + self.assertFalse(self.safety.safety_rx_hook(self._button_msg(SET_BTN, 0x1A6))) + self.assertFalse(self.safety.safety_rx_hook(self._speed_msg(0))) + self.assertFalse(self.safety.safety_rx_hook(self._gas_msg(0))) + self.assertFalse(self.safety.get_controls_allowed()) + + # restore counters for future tests with a couple of good messages + for i in range(2): + self.safety.set_controls_allowed(1) + self.safety.safety_rx_hook(self._button_msg(SET_BTN, 0x1A6)) + self.safety.safety_rx_hook(self._speed_msg(0)) + self.safety.safety_rx_hook(self._gas_msg(0)) + self.safety.safety_rx_hook(self._button_msg(SET_BTN, 0x1A6)) + self.assertTrue(self.safety.get_controls_allowed()) + + def test_fwd_hook(self): buss = list(range(0x0, 0x3)) msgs = list(range(0x1, 0x800)) diff --git a/tests/safety/test_toyota.py b/tests/safety/test_toyota.py index c322f3e..1018162 100644 --- a/tests/safety/test_toyota.py +++ b/tests/safety/test_toyota.py @@ -36,6 +36,16 @@ def sign(a): else: return -1 +def toyota_checksum(msg, addr, len_msg): + checksum = (len_msg + 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 TestToyotaSafety(unittest.TestCase): @classmethod def setUp(cls): @@ -51,7 +61,8 @@ class TestToyotaSafety(unittest.TestCase): def _torque_meas_msg(self, torque): t = twos_comp(torque, 16) to_send = make_msg(0, 0x260) - to_send[0].RDHR = t | ((t & 0xFF) << 16) + to_send[0].RDHR = (t & 0xff00) | ((t & 0xFF) << 16) + to_send[0].RDHR = to_send[0].RDHR | (toyota_checksum(to_send[0], 0x260, 8) << 24) return to_send def _torque_msg(self, torque): @@ -81,6 +92,7 @@ class TestToyotaSafety(unittest.TestCase): def _pcm_cruise_msg(self, cruise_on): to_send = make_msg(0, 0x1D2) to_send[0].RDLR = cruise_on << 5 + to_send[0].RDHR = to_send[0].RDHR | (toyota_checksum(to_send[0], 0x1D2, 8) << 24) return to_send def test_spam_can_buses(self): @@ -259,6 +271,19 @@ class TestToyotaSafety(unittest.TestCase): self.safety.set_controls_allowed(1) self.assertTrue(self.safety.safety_tx_hook(self._send_interceptor_msg(0x1000, 0x200))) + def test_rx_hook(self): + # checksum checks + for msg in ["trq", "pcm"]: + self.safety.set_controls_allowed(1) + if msg == "trq": + to_push = self._torque_meas_msg(0) + if msg == "pcm": + to_push = self._pcm_cruise_msg(1) + self.assertTrue(self.safety.safety_rx_hook(to_push)) + to_push[0].RDHR = 0 + self.assertFalse(self.safety.safety_rx_hook(to_push)) + self.assertFalse(self.safety.get_controls_allowed()) + def test_fwd_hook(self): buss = list(range(0x0, 0x3)) diff --git a/tests/safety/test_toyota_ipas.py b/tests/safety/test_toyota_ipas.py index 680fc14..3fcca95 100644 --- a/tests/safety/test_toyota_ipas.py +++ b/tests/safety/test_toyota_ipas.py @@ -3,7 +3,8 @@ import unittest import numpy as np from panda import Panda from panda.tests.safety import libpandasafety_py -from panda.tests. safety.common import make_msg +from panda.tests.safety.common import make_msg +from panda.tests.safety.test_toyota import toyota_checksum IPAS_OVERRIDE_THRESHOLD = 200 @@ -23,6 +24,7 @@ def sign(a): else: return -1 + class TestToyotaSafety(unittest.TestCase): @classmethod def setUp(cls): @@ -34,6 +36,7 @@ class TestToyotaSafety(unittest.TestCase): to_send = make_msg(0, 0x260) t = twos_comp(torque, 16) to_send[0].RDLR = t | ((t & 0xFF) << 16) + to_send[0].RDHR = to_send[0].RDHR | (toyota_checksum(to_send[0], 0x260, 8) << 24) return to_send def _torque_driver_msg_array(self, torque): diff --git a/tests/safety_replay/replay_drive.py b/tests/safety_replay/replay_drive.py index 09c677c..456723c 100755 --- a/tests/safety_replay/replay_drive.py +++ b/tests/safety_replay/replay_drive.py @@ -16,8 +16,9 @@ def replay_drive(lr, safety_mode, param): if "SEGMENT" in os.environ: init_segment(safety, lr, mode) - tx_tot, tx_blocked, tx_controls, tx_controls_blocked = 0, 0, 0, 0 + rx_tot, rx_invalid, tx_tot, tx_blocked, tx_controls, tx_controls_blocked = 0, 0, 0, 0, 0, 0 blocked_addrs = set() + invalid_addrs = set() start_t = None for msg in lr: @@ -44,15 +45,24 @@ def replay_drive(lr, safety_mode, param): if canmsg.src >= 128: continue to_push = package_can_msg(canmsg) - safety.safety_rx_hook(to_push) + recv = safety.safety_rx_hook(to_push) + if not recv: + rx_invalid += 1 + invalid_addrs.add(canmsg.address) + rx_tot += 1 + print("\nRX") + print("total rx msgs:", rx_tot) + print("invalid rx msgs:", rx_invalid) + print("invalid addrs:", invalid_addrs) + print("\nTX") print("total openpilot msgs:", tx_tot) print("total msgs with controls allowed:", tx_controls) print("blocked msgs:", tx_blocked) print("blocked with controls allowed:", tx_controls_blocked) print("blocked addrs:", blocked_addrs) - return tx_controls_blocked == 0 + return tx_controls_blocked == 0 and rx_invalid == 0 if __name__ == "__main__": mode = int(sys.argv[2])