From f67ec28e3fd8b3239542bada8259982061b89442 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Sat, 18 Jan 2020 13:26:34 -0800 Subject: [PATCH] Fix msg checks for non Honda and Toyota (#426) --- board/safety.h | 24 +++--- board/safety/safety_chrysler.h | 50 ++++++------ board/safety/safety_gm.h | 130 ++++++++++++++++--------------- board/safety/safety_hyundai.h | 50 ++++++------ board/safety/safety_subaru.h | 52 +++++++------ board/safety/safety_volkswagen.h | 72 +++++++++-------- 6 files changed, 204 insertions(+), 174 deletions(-) diff --git a/board/safety.h b/board/safety.h index 4588eec..7fd057b 100644 --- a/board/safety.h +++ b/board/safety.h @@ -148,20 +148,20 @@ bool addr_safety_check(CAN_FIFOMailBox_TypeDef *to_push, 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; - } + if ((get_checksum != NULL) && (compute_checksum != NULL) && 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; + } else { + rx_checks[index].valid_checksum = true; } - // 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); - } + // counter check (max_counter == 0 means skip check) + if ((get_counter != NULL) && (rx_checks[index].max_counter > 0U)) { + uint8_t counter = get_counter(to_push); + update_counter(rx_checks, index, counter); + } else { + rx_checks[index].wrong_counters = 0U; } } return is_msg_valid(rx_checks, index); diff --git a/board/safety/safety_chrysler.h b/board/safety/safety_chrysler.h index 8c201a3..293839b 100644 --- a/board/safety/safety_chrysler.h +++ b/board/safety/safety_chrysler.h @@ -20,36 +20,42 @@ uint32_t chrysler_ts_last = 0; struct sample_t chrysler_torque_meas; // last few torques measured static int chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { - int bus = GET_BUS(to_push); - int addr = GET_ADDR(to_push); - // Measured eps torque - if (addr == 544) { - int torque_meas_new = ((GET_BYTE(to_push, 4) & 0x7U) << 8) + GET_BYTE(to_push, 5) - 1024U; + bool valid = addr_safety_check(to_push, chrysler_rx_checks, CHRYSLER_RX_CHECK_LEN, + NULL, NULL, NULL); - // update array of samples - update_sample(&chrysler_torque_meas, torque_meas_new); - } + if (valid) { + int bus = GET_BUS(to_push); + int addr = GET_ADDR(to_push); - // enter controls on rising edge of ACC, exit controls on ACC off - if (addr == 0x1F4) { - int cruise_engaged = ((GET_BYTE(to_push, 2) & 0x38) >> 3) == 7; - if (cruise_engaged && !chrysler_cruise_engaged_last) { - controls_allowed = 1; + // Measured eps torque + if (addr == 544) { + int torque_meas_new = ((GET_BYTE(to_push, 4) & 0x7U) << 8) + GET_BYTE(to_push, 5) - 1024U; + + // update array of samples + update_sample(&chrysler_torque_meas, torque_meas_new); } - if (!cruise_engaged) { - controls_allowed = 0; + + // enter controls on rising edge of ACC, exit controls on ACC off + if (addr == 0x1F4) { + int cruise_engaged = ((GET_BYTE(to_push, 2) & 0x38) >> 3) == 7; + if (cruise_engaged && !chrysler_cruise_engaged_last) { + controls_allowed = 1; + } + if (!cruise_engaged) { + controls_allowed = 0; + } + chrysler_cruise_engaged_last = cruise_engaged; } - chrysler_cruise_engaged_last = cruise_engaged; - } - // TODO: add gas pressed check + // 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; + // 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; + return valid; } static int chrysler_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { diff --git a/board/safety/safety_gm.h b/board/safety/safety_gm.h index 5356a11..8151290 100644 --- a/board/safety/safety_gm.h +++ b/board/safety/safety_gm.h @@ -42,78 +42,84 @@ uint32_t gm_ts_last = 0; struct sample_t gm_torque_driver; // last few driver torques measured static int gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { - int bus = GET_BUS(to_push); - int addr = GET_ADDR(to_push); - if (addr == 388) { - int torque_driver_new = ((GET_BYTE(to_push, 6) & 0x7) << 8) | GET_BYTE(to_push, 7); - torque_driver_new = to_signed(torque_driver_new, 11); - // update array of samples - update_sample(&gm_torque_driver, torque_driver_new); - } + bool valid = addr_safety_check(to_push, gm_rx_checks, GM_RX_CHECK_LEN, + NULL, NULL, NULL); - // sample speed, really only care if car is moving or not - // rear left wheel speed - if (addr == 842) { - gm_moving = GET_BYTE(to_push, 0) | GET_BYTE(to_push, 1); - } + if (valid) { + int bus = GET_BUS(to_push); + int addr = GET_ADDR(to_push); - // ACC steering wheel buttons - if (addr == 481) { - int button = (GET_BYTE(to_push, 5) & 0x70) >> 4; - switch (button) { - case 2: // resume - case 3: // set - controls_allowed = 1; - break; - case 6: // cancel + if (addr == 388) { + int torque_driver_new = ((GET_BYTE(to_push, 6) & 0x7) << 8) | GET_BYTE(to_push, 7); + torque_driver_new = to_signed(torque_driver_new, 11); + // update array of samples + update_sample(&gm_torque_driver, torque_driver_new); + } + + // sample speed, really only care if car is moving or not + // rear left wheel speed + if (addr == 842) { + gm_moving = GET_BYTE(to_push, 0) | GET_BYTE(to_push, 1); + } + + // ACC steering wheel buttons + if (addr == 481) { + int button = (GET_BYTE(to_push, 5) & 0x70) >> 4; + switch (button) { + case 2: // resume + case 3: // set + controls_allowed = 1; + break; + case 6: // cancel + controls_allowed = 0; + break; + default: + break; // any other button is irrelevant + } + } + + // exit controls on rising edge of brake press or on brake press when + // speed > 0 + if (addr == 241) { + int brake = GET_BYTE(to_push, 1); + // Brake pedal's potentiometer returns near-zero reading + // even when pedal is not pressed + if (brake < 10) { + brake = 0; + } + if (brake && (!gm_brake_prev || gm_moving)) { + controls_allowed = 0; + } + gm_brake_prev = brake; + } + + // exit controls on rising edge of gas press + if (addr == 417) { + int gas = GET_BYTE(to_push, 6); + if (gas && !gm_gas_prev) { controls_allowed = 0; - break; - default: - break; // any other button is irrelevant + } + gm_gas_prev = gas; } - } - // exit controls on rising edge of brake press or on brake press when - // speed > 0 - if (addr == 241) { - int brake = GET_BYTE(to_push, 1); - // Brake pedal's potentiometer returns near-zero reading - // even when pedal is not pressed - if (brake < 10) { - brake = 0; + // exit controls on regen paddle + if (addr == 189) { + bool regen = GET_BYTE(to_push, 0) & 0x20; + if (regen) { + controls_allowed = 0; + } } - if (brake && (!gm_brake_prev || gm_moving)) { - controls_allowed = 0; - } - gm_brake_prev = brake; - } - // exit controls on rising edge of gas press - if (addr == 417) { - int gas = GET_BYTE(to_push, 6); - if (gas && !gm_gas_prev) { - controls_allowed = 0; - } - gm_gas_prev = gas; - } - - // exit controls on regen paddle - if (addr == 189) { - bool regen = GET_BYTE(to_push, 0) & 0x20; - if (regen) { - controls_allowed = 0; + // Check if ASCM or LKA camera are online + // on powertrain bus. + // 384 = ASCMLKASteeringCmd + // 715 = ASCMGasRegenCmd + if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && ((addr == 384) || (addr == 715))) { + relay_malfunction = true; } } - - // Check if ASCM or LKA camera are online - // on powertrain bus. - // 384 = ASCMLKASteeringCmd - // 715 = ASCMGasRegenCmd - if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && ((addr == 384) || (addr == 715))) { - relay_malfunction = true; - } - return 1; + return valid; } // all commands: gas/regen, friction brake and steering diff --git a/board/safety/safety_hyundai.h b/board/safety/safety_hyundai.h index fcf112a..33a670f 100644 --- a/board/safety/safety_hyundai.h +++ b/board/safety/safety_hyundai.h @@ -21,35 +21,41 @@ uint32_t hyundai_ts_last = 0; struct sample_t hyundai_torque_driver; // last few driver torques measured static int hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { - int bus = GET_BUS(to_push); - int addr = GET_ADDR(to_push); - if (addr == 897) { - int torque_driver_new = ((GET_BYTES_04(to_push) >> 11) & 0xfff) - 2048; - // update array of samples - update_sample(&hyundai_torque_driver, torque_driver_new); - } + bool valid = addr_safety_check(to_push, hyundai_rx_checks, HYUNDAI_RX_CHECK_LEN, + NULL, NULL, NULL); - // enter controls on rising edge of ACC, exit controls on ACC off - if (addr == 1057) { - // 2 bits: 13-14 - int cruise_engaged = (GET_BYTES_04(to_push) >> 13) & 0x3; - if (cruise_engaged && !hyundai_cruise_engaged_last) { - controls_allowed = 1; + if (valid) { + int bus = GET_BUS(to_push); + int addr = GET_ADDR(to_push); + + if (addr == 897) { + int torque_driver_new = ((GET_BYTES_04(to_push) >> 11) & 0xfff) - 2048; + // update array of samples + update_sample(&hyundai_torque_driver, torque_driver_new); } - if (!cruise_engaged) { - controls_allowed = 0; + + // enter controls on rising edge of ACC, exit controls on ACC off + if (addr == 1057) { + // 2 bits: 13-14 + int cruise_engaged = (GET_BYTES_04(to_push) >> 13) & 0x3; + if (cruise_engaged && !hyundai_cruise_engaged_last) { + controls_allowed = 1; + } + if (!cruise_engaged) { + controls_allowed = 0; + } + hyundai_cruise_engaged_last = cruise_engaged; } - hyundai_cruise_engaged_last = cruise_engaged; - } - // TODO: check gas pressed + // 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; + // 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; + return valid; } static int hyundai_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { diff --git a/board/safety/safety_subaru.h b/board/safety/safety_subaru.h index b4ff5f2..885e273 100644 --- a/board/safety/safety_subaru.h +++ b/board/safety/safety_subaru.h @@ -24,36 +24,42 @@ uint32_t subaru_ts_last = 0; struct sample_t subaru_torque_driver; // last few driver torques measured static int subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { - int bus = GET_BUS(to_push); - int addr = GET_ADDR(to_push); - if (((addr == 0x119) || (addr == 0x371)) && (bus == 0)){ - int bit_shift = (addr == 0x119) ? 16 : 29; - int torque_driver_new = ((GET_BYTES_04(to_push) >> bit_shift) & 0x7FF); - torque_driver_new = to_signed(torque_driver_new, 11); - // update array of samples - update_sample(&subaru_torque_driver, torque_driver_new); - } + bool valid = addr_safety_check(to_push, subaru_rx_checks, SUBARU_RX_CHECK_LEN, + NULL, NULL, NULL); - // enter controls on rising edge of ACC, exit controls on ACC off - if (((addr == 0x240) || (addr == 0x144)) && (bus == 0)) { - int bit_shift = (addr == 0x240) ? 9 : 17; - int cruise_engaged = ((GET_BYTES_48(to_push) >> bit_shift) & 1); - if (cruise_engaged && !subaru_cruise_engaged_last) { - controls_allowed = 1; + if (valid) { + int bus = GET_BUS(to_push); + int addr = GET_ADDR(to_push); + + if (((addr == 0x119) || (addr == 0x371)) && (bus == 0)){ + int bit_shift = (addr == 0x119) ? 16 : 29; + int torque_driver_new = ((GET_BYTES_04(to_push) >> bit_shift) & 0x7FF); + torque_driver_new = to_signed(torque_driver_new, 11); + // update array of samples + update_sample(&subaru_torque_driver, torque_driver_new); } - if (!cruise_engaged) { - controls_allowed = 0; + + // enter controls on rising edge of ACC, exit controls on ACC off + if (((addr == 0x240) || (addr == 0x144)) && (bus == 0)) { + int bit_shift = (addr == 0x240) ? 9 : 17; + int cruise_engaged = ((GET_BYTES_48(to_push) >> bit_shift) & 1); + if (cruise_engaged && !subaru_cruise_engaged_last) { + controls_allowed = 1; + } + if (!cruise_engaged) { + controls_allowed = 0; + } + subaru_cruise_engaged_last = cruise_engaged; } - subaru_cruise_engaged_last = cruise_engaged; - } - // TODO: enforce cancellation on gas pressed + // TODO: enforce cancellation on gas pressed - if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && ((addr == 0x122) || (addr == 0x164))) { - relay_malfunction = true; + if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && ((addr == 0x122) || (addr == 0x164))) { + relay_malfunction = true; + } } - return 1; + return valid; } static int subaru_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { diff --git a/board/safety/safety_volkswagen.h b/board/safety/safety_volkswagen.h index 4b01fd4..102cb22 100644 --- a/board/safety/safety_volkswagen.h +++ b/board/safety/safety_volkswagen.h @@ -33,42 +33,48 @@ uint32_t volkswagen_ts_last = 0; int volkswagen_gas_prev = 0; static int volkswagen_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { - int bus = GET_BUS(to_push); - int addr = GET_ADDR(to_push); - // Update driver input torque samples from EPS_01.Driver_Strain for absolute torque, and EPS_01.Driver_Strain_VZ - // for the direction. - if ((bus == 0) && (addr == MSG_EPS_01)) { - int torque_driver_new = GET_BYTE(to_push, 5) | ((GET_BYTE(to_push, 6) & 0x1F) << 8); - int sign = (GET_BYTE(to_push, 6) & 0x80) >> 7; - if (sign == 1) { - torque_driver_new *= -1; - } + bool valid = addr_safety_check(to_push, volkswagen_rx_checks, VOLKSWAGEN_RX_CHECK_LEN, + NULL, NULL, NULL); - update_sample(&volkswagen_torque_driver, torque_driver_new); + if (valid) { + int bus = GET_BUS(to_push); + int addr = GET_ADDR(to_push); + + // Update driver input torque samples from EPS_01.Driver_Strain for absolute torque, and EPS_01.Driver_Strain_VZ + // for the direction. + if ((bus == 0) && (addr == MSG_EPS_01)) { + int torque_driver_new = GET_BYTE(to_push, 5) | ((GET_BYTE(to_push, 6) & 0x1F) << 8); + int sign = (GET_BYTE(to_push, 6) & 0x80) >> 7; + if (sign == 1) { + torque_driver_new *= -1; + } + + update_sample(&volkswagen_torque_driver, torque_driver_new); + } + + // Monitor ACC_06.ACC_Status_ACC for stock ACC status. Because the current MQB port is lateral-only, OP's control + // allowed state is directly driven by stock ACC engagement. Permit the ACC message to come from either bus, in + // order to accommodate future camera-side integrations if needed. + if (addr == MSG_ACC_06) { + int acc_status = (GET_BYTE(to_push, 7) & 0x70) >> 4; + controls_allowed = ((acc_status == 3) || (acc_status == 4) || (acc_status == 5)) ? 1 : 0; + } + + // exit controls on rising edge of gas press. Bits [12-20) + if (addr == MSG_MOTOR_20) { + int gas = (GET_BYTES_04(to_push) >> 12) & 0xFF; + if ((gas > 0) && (volkswagen_gas_prev == 0)) { + controls_allowed = 0; + } + volkswagen_gas_prev = gas; + } + + if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && (addr == MSG_HCA_01)) { + relay_malfunction = true; + } } - - // Monitor ACC_06.ACC_Status_ACC for stock ACC status. Because the current MQB port is lateral-only, OP's control - // allowed state is directly driven by stock ACC engagement. Permit the ACC message to come from either bus, in - // order to accommodate future camera-side integrations if needed. - if (addr == MSG_ACC_06) { - int acc_status = (GET_BYTE(to_push, 7) & 0x70) >> 4; - controls_allowed = ((acc_status == 3) || (acc_status == 4) || (acc_status == 5)) ? 1 : 0; - } - - // exit controls on rising edge of gas press. Bits [12-20) - if (addr == MSG_MOTOR_20) { - int gas = (GET_BYTES_04(to_push) >> 12) & 0xFF; - if ((gas > 0) && (volkswagen_gas_prev == 0)) { - controls_allowed = 0; - } - volkswagen_gas_prev = gas; - } - - if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && (addr == MSG_HCA_01)) { - relay_malfunction = true; - } - return 1; + return valid; } static int volkswagen_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {