From bacfcd0fd0c022818b4d01fd762b41aa81b32b2d Mon Sep 17 00:00:00 2001 From: rbiasini Date: Fri, 14 Jun 2019 19:13:05 -0700 Subject: [PATCH] Make macros to get msg attributes (#218) * made GET_ADDR, GET_BUS and GET_LEN macros * for simplicity, addr is int instead of uint32_t as it can only use up to 29 bits --- board/safety/safety_cadillac.h | 20 +++++++------- board/safety/safety_chrysler.h | 33 ++++++---------------- board/safety/safety_elm327.h | 24 +++++++--------- board/safety/safety_ford.h | 12 ++++---- board/safety/safety_gm.h | 46 ++++++++++--------------------- board/safety/safety_gm_ascm.h | 11 ++++---- board/safety/safety_honda.h | 14 ++++++---- board/safety/safety_hyundai.h | 40 ++++++++------------------- board/safety/safety_subaru.h | 14 +++++----- board/safety/safety_tesla.h | 28 ++++++------------- board/safety/safety_toyota.h | 10 +++---- board/safety/safety_toyota_ipas.h | 7 +++-- board/safety_declarations.h | 4 +++ 13 files changed, 105 insertions(+), 158 deletions(-) diff --git a/board/safety/safety_cadillac.h b/board/safety/safety_cadillac.h index 87646ab..5cfcb75 100644 --- a/board/safety/safety_cadillac.h +++ b/board/safety/safety_cadillac.h @@ -19,15 +19,15 @@ uint32_t cadillac_ts_last = 0; int cadillac_supercruise_on = 0; struct sample_t cadillac_torque_driver; // last few driver torques measured -int cadillac_get_torque_idx(uint32_t addr, int array_size) { - return min(max(addr - 0x151U, 0), array_size); // 0x151 is id 0, 0x152 is id 1 and so on... +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) { - int bus_number = (to_push->RDTR >> 4) & 0xFF; - uint32_t addr = to_push->RIR >> 21; + int bus = GET_BUS(to_push); + int addr = GET_ADDR(to_push); - if (addr == 356U) { + if (addr == 356) { int torque_driver_new = ((to_push->RDLR & 0x7) << 8) | ((to_push->RDLR >> 8) & 0xFF); torque_driver_new = to_signed(torque_driver_new, 11); // update array of samples @@ -35,12 +35,12 @@ static void cadillac_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { } // this message isn't all zeros when ignition is on - if ((addr == 0x160U) && (bus_number == 0)) { + if ((addr == 0x160) && (bus == 0)) { cadillac_ign = to_push->RDLR > 0; } // enter controls on rising edge of ACC, exit controls on ACC off - if ((addr == 0x370U) && (bus_number == 0)) { + if ((addr == 0x370) && (bus == 0)) { int cruise_engaged = to_push->RDLR & 0x800000; // bit 23 if (cruise_engaged && !cadillac_cruise_engaged_last) { controls_allowed = 1; @@ -51,17 +51,17 @@ static void cadillac_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { } // know supercruise mode and block openpilot msgs if on - if ((addr == 0x152U) || (addr == 0x154U)) { + if ((addr == 0x152) || (addr == 0x154)) { cadillac_supercruise_on = (to_push->RDHR>>4) & 0x1; } } static int cadillac_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { int tx = 1; - uint32_t addr = to_send->RIR >> 21; + int addr = GET_ADDR(to_send); // steer cmd checks - if ((addr == 0x151U) || (addr == 0x152U) || (addr == 0x153U) || (addr == 0x154U)) { + if ((addr == 0x151) || (addr == 0x152) || (addr == 0x153) || (addr == 0x154)) { int desired_torque = ((to_send->RDLR & 0x3f) << 8) + ((to_send->RDLR & 0xff00) >> 8); int violation = 0; uint32_t ts = TIM2->CNT; diff --git a/board/safety/safety_chrysler.h b/board/safety/safety_chrysler.h index 6810cab..5807596 100644 --- a/board/safety/safety_chrysler.h +++ b/board/safety/safety_chrysler.h @@ -13,20 +13,11 @@ 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) { - int bus = (to_push->RDTR >> 4) & 0xFF; - uint32_t addr; - if ((to_push->RIR & 4) != 0) { - // Extended - // Not looked at, but have to be separated - // to avoid address collision - addr = to_push->RIR >> 3; - } else { - // Normal - addr = to_push->RIR >> 21; - } + int bus = GET_BUS(to_push); + int addr = GET_ADDR(to_push); // Measured eps torque - if (addr == 544U) { + if (addr == 544) { uint32_t rdhr = to_push->RDHR; int torque_meas_new = ((rdhr & 0x7U) << 8) + ((rdhr & 0xFF00U) >> 8) - 1024U; @@ -35,7 +26,7 @@ static void chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { } // enter controls on rising edge of ACC, exit controls on ACC off - if (addr == 0x1F4U) { + if (addr == 0x1F4) { int cruise_engaged = ((to_push->RDLR & 0x380000) >> 19) == 7; if (cruise_engaged && !chrysler_cruise_engaged_last) { controls_allowed = 1; @@ -46,7 +37,7 @@ static void chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { } // check if stock camera ECU is still online - if ((bus == 0) && (addr == 0x292U)) { + if ((bus == 0) && (addr == 0x292)) { chrysler_camera_detected = 1; controls_allowed = 0; } @@ -61,18 +52,10 @@ static int chrysler_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { tx = 0; } - uint32_t addr; - if ((to_send->RIR & 4) != 0) { - // Extended - addr = to_send->RIR >> 3; - } else { - // Normal - addr = to_send->RIR >> 21; - } - + int addr = GET_ADDR(to_send); // LKA STEER - if (addr == 0x292U) { + if (addr == 0x292) { uint32_t rdlr = to_send->RDLR; int desired_torque = ((rdlr & 0x7U) << 8) + ((rdlr & 0xFF00U) >> 8) - 1024U; uint32_t ts = TIM2->CNT; @@ -135,7 +118,7 @@ static void chrysler_init(int16_t param) { static int chrysler_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { int bus_fwd = -1; - int32_t addr = to_fwd->RIR >> 21; + int addr = GET_ADDR(to_fwd); // forward CAN 0 -> 2 so stock LKAS camera sees messages if ((bus_num == 0) && !chrysler_camera_detected) { bus_fwd = 2; diff --git a/board/safety/safety_elm327.h b/board/safety/safety_elm327.h index ea57766..1f44e99 100644 --- a/board/safety/safety_elm327.h +++ b/board/safety/safety_elm327.h @@ -1,29 +1,25 @@ static int elm327_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { int tx = 1; + int bus = GET_BUS(to_send); + int addr = GET_ADDR(to_send); + int len = GET_LEN(to_send); //All ELM traffic must appear on CAN0 - if (((to_send->RDTR >> 4) & 0xf) != 0) { + if (bus != 0) { tx = 0; } //All ISO 15765-4 messages must be 8 bytes long - if ((to_send->RDTR & 0xf) != 8) { + if (len != 8) { tx = 0; } - if ((to_send->RIR & 4) != 0) { - uint32_t addr = to_send->RIR >> 3; - //Check valid 29 bit send addresses for ISO 15765-4 - if (!((addr == 0x18DB33F1U) || ((addr & 0x1FFF00FFU) == 0x18DA00F1U))) { - tx = 0; - } - } else { - uint32_t addr = to_send->RIR >> 21; - //Check valid 11 bit send addresses for ISO 15765-4 - if (!((addr == 0x7DFU) || ((addr & 0x7F8U) == 0x7E0U))) { - tx = 0; - } + //Check valid 29 bit send addresses for ISO 15765-4 + //Check valid 11 bit send addresses for ISO 15765-4 + if ((addr != 0x18DB33F1) && ((addr & 0x1FFF00FF) != 0x18DA00F1) && + ((addr != 0x7DF) && ((addr & 0x7F8) != 0x7E0))) { + tx = 0; } return tx; } diff --git a/board/safety/safety_ford.h b/board/safety/safety_ford.h index c5b41ef..00ab6b3 100644 --- a/board/safety/safety_ford.h +++ b/board/safety/safety_ford.h @@ -13,14 +13,16 @@ int ford_is_moving = 0; static void ford_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { - if ((to_push->RIR>>21) == 0x217) { + int addr = GET_ADDR(to_push); + + if (addr == 0x217) { // wheel speeds are 14 bits every 16 ford_is_moving = 0xFCFF & (to_push->RDLR | (to_push->RDLR >> 16) | to_push->RDHR | (to_push->RDHR >> 16)); } // state machine to enter and exit controls - if ((to_push->RIR>>21) == 0x83) { + if (addr == 0x83) { bool cancel = (to_push->RDLR >> 8) & 0x1; bool set_or_resume = (to_push->RDLR >> 28) & 0x3; if (cancel) { @@ -32,7 +34,7 @@ static void ford_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // exit controls on rising edge of brake press or on brake press when // speed > 0 - if ((to_push->RIR>>21) == 0x165) { + if (addr == 0x165) { int brake = to_push->RDLR & 0x20; if (brake && (!(ford_brake_prev) || ford_is_moving)) { controls_allowed = 0; @@ -41,7 +43,7 @@ static void ford_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { } // exit controls on rising edge of gas press - if ((to_push->RIR>>21) == 0x204) { + if (addr == 0x204) { int gas = to_push->RDLR & 0xFF03; if (gas && !(ford_gas_prev)) { controls_allowed = 0; @@ -63,7 +65,7 @@ static int ford_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { // and the the latching controls_allowed flag is True int pedal_pressed = ford_gas_prev || (ford_brake_prev && ford_is_moving); bool current_controls_allowed = controls_allowed && !(pedal_pressed); - int addr = to_send->RIR >> 21; + int addr = GET_ADDR(to_send); // STEER: safety check if (addr == 0x3CA) { diff --git a/board/safety/safety_gm.h b/board/safety/safety_gm.h index 8a251b5..00f01b4 100644 --- a/board/safety/safety_gm.h +++ b/board/safety/safety_gm.h @@ -31,26 +31,17 @@ 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) { - int bus_number = (to_push->RDTR >> 4) & 0xFF; - uint32_t addr; - if ((to_push->RIR & 4) != 0) { - // Extended - // Not looked at, but have to be separated - // to avoid address collision - addr = to_push->RIR >> 3; - } else { - // Normal - addr = to_push->RIR >> 21; - } + int bus_number = GET_BUS(to_push); + int addr = GET_ADDR(to_push); - if (addr == 388U) { + if (addr == 388) { int torque_driver_new = (((to_push->RDHR >> 16) & 0x7) << 8) | ((to_push->RDHR >> 24) & 0xFF); torque_driver_new = to_signed(torque_driver_new, 11); // update array of samples update_sample(&gm_torque_driver, torque_driver_new); } - if ((addr == 0x1F1U) && (bus_number == 0)) { + if ((addr == 0x1F1) && (bus_number == 0)) { //Bit 5 should be ignition "on" //Backup plan is Bit 2 (accessory power) bool ign = ((to_push->RDLR) & 0x20) != 0; @@ -59,7 +50,7 @@ static void gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // sample speed, really only care if car is moving or not // rear left wheel speed - if (addr == 842U) { + if (addr == 842) { gm_speed = to_push->RDLR & 0xFFFF; } @@ -67,13 +58,13 @@ static void gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // on powertrain bus. // 384 = ASCMLKASteeringCmd // 715 = ASCMGasRegenCmd - if ((bus_number == 0) && ((addr == 384U) || (addr == 715U))) { + if ((bus_number == 0) && ((addr == 384) || (addr == 715))) { gm_ascm_detected = 1; controls_allowed = 0; } // ACC steering wheel buttons - if (addr == 481U) { + if (addr == 481) { int buttons = (to_push->RDHR >> 12) & 0x7; // res/set - enable, cancel button - disable if ((buttons == 2) || (buttons == 3)) { @@ -85,7 +76,7 @@ static void gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // exit controls on rising edge of brake press or on brake press when // speed > 0 - if (addr == 241U) { + if (addr == 241) { int brake = (to_push->RDLR & 0xFF00) >> 8; // Brake pedal's potentiometer returns near-zero reading // even when pedal is not pressed @@ -99,7 +90,7 @@ static void gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { } // exit controls on rising edge of gas press - if (addr == 417U) { + if (addr == 417) { int gas = to_push->RDHR & 0xFF0000; if (gas && !gm_gas_prev && long_controls_allowed) { controls_allowed = 0; @@ -108,7 +99,7 @@ static void gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { } // exit controls on regen paddle - if (addr == 189U) { + if (addr == 189) { bool regen = to_push->RDLR & 0x20; if (regen) { controls_allowed = 0; @@ -136,17 +127,10 @@ static int gm_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { int pedal_pressed = gm_gas_prev || (gm_brake_prev && gm_speed); bool current_controls_allowed = controls_allowed && !pedal_pressed; - uint32_t addr; - if ((to_send->RIR & 4) != 0) { - // Extended - addr = to_send->RIR >> 3; - } else { - // Normal - addr = to_send->RIR >> 21; - } + int addr = GET_ADDR(to_send); // BRAKE: safety check - if (addr == 789U) { + if (addr == 789) { uint32_t rdlr = to_send->RDLR; int brake = ((rdlr & 0xFU) << 8) + ((rdlr & 0xFF00U) >> 8); brake = (0x1000 - brake) & 0xFFF; @@ -162,7 +146,7 @@ static int gm_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { } // LKA STEER: safety check - if (addr == 384U) { + if (addr == 384) { uint32_t rdlr = to_send->RDLR; int desired_torque = ((rdlr & 0x7U) << 8) + ((rdlr & 0xFF00U) >> 8); uint32_t ts = TIM2->CNT; @@ -211,12 +195,12 @@ static int gm_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { } // PARK ASSIST STEER: unlimited torque, no thanks - if (addr == 823U) { + if (addr == 823) { tx = 0; } // GAS/REGEN: safety check - if (addr == 715U) { + if (addr == 715) { uint32_t rdlr = to_send->RDLR; int gas_regen = ((rdlr & 0x7F0000U) >> 11) + ((rdlr & 0xF8000000U) >> 27); bool apply = (rdlr & 1U) != 0U; diff --git a/board/safety/safety_gm_ascm.h b/board/safety/safety_gm_ascm.h index e2b526b..4b436e1 100644 --- a/board/safety/safety_gm_ascm.h +++ b/board/safety/safety_gm_ascm.h @@ -6,22 +6,23 @@ static int gm_ascm_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { int bus_fwd = -1; if (bus_num == 0) { - uint32_t addr = to_fwd->RIR >> 21; + int addr = GET_ADDR(to_fwd); bus_fwd = 2; // do not propagate lkas messages from ascm to actuators, unless supercruise is on // block 0x152 and 0x154, which are the lkas command from ASCM1 and ASCM2 // block 0x315 and 0x2cb, which are the brake and accel commands from ASCM1 //if ((addr == 0x152) || (addr == 0x154) || (addr == 0x315) || (addr == 0x2cb)) { - if ((addr == 0x152U) || (addr == 0x154U)) { - int supercruise_on = (to_fwd->RDHR>>4) & 0x1; // bit 36 + if ((addr == 0x152) || (addr == 0x154)) { + int supercruise_on = (to_fwd->RDHR >> 4) & 0x1; // bit 36 if (!supercruise_on) { bus_fwd = -1; } - } else if ((addr == 0x151U) || (addr == 0x153U) || (addr == 0x314U)) { + } else if ((addr == 0x151) || (addr == 0x153) || (addr == 0x314)) { // on the chassis bus, the OBDII port is on the module side, so we need to read // the lkas messages sent by openpilot (put on unused 0x151 ane 0x153 addrs) and send it to // the actuator as 0x152 and 0x154 - to_fwd->RIR = ((addr + 1U) << 21) | (to_fwd->RIR & 0x1fffff); + uint32_t fwd_addr = addr + 1; + to_fwd->RIR = (fwd_addr << 21) | (to_fwd->RIR & 0x1fffff); } } diff --git a/board/safety/safety_honda.h b/board/safety/safety_honda.h index 68d581c..988b381 100644 --- a/board/safety/safety_honda.h +++ b/board/safety/safety_honda.h @@ -16,7 +16,8 @@ bool honda_alt_brake_msg = false; static void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { - int addr = to_push->RIR >> 21; + int addr = GET_ADDR(to_push); + int len = GET_LEN(to_push); // sample speed if (addr == 0x158) { @@ -56,7 +57,7 @@ static void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // 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) && ((to_push->RDTR & 0xf) == 6)) { + if ((addr == 0x201) && (len == 6)) { gas_interceptor_detected = 1; int gas_interceptor = ((to_push->RDLR & 0xFF) << 8) | ((to_push->RDLR & 0xFF00) >> 8); if ((gas_interceptor > HONDA_GAS_INTERCEPTOR_THRESHOLD) && @@ -87,8 +88,9 @@ static void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { static int honda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { - int addr = to_send->RIR >> 21; int tx = 1; + int addr = GET_ADDR(to_send); + int bus = GET_BUS(to_send); // disallow actuator commands if gas or brake (with vehicle moving) are pressed // and the the latching controls_allowed flag is True @@ -135,7 +137,7 @@ static int honda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { // ensuring that only the cancel button press is sent (VAL 2) when controls are off. // This avoids unintended engagements while still allowing resume spam if ((addr == 0x296) && honda_bosch_hardware && - !current_controls_allowed && ((to_send->RDTR >> 4) & 0xFF) == 0) { + !current_controls_allowed && (bus == 0)) { if (((to_send->RDLR >> 5) & 0x7) != 2) { tx = 0; } @@ -169,7 +171,7 @@ static int honda_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { bus_fwd = 2; } else if (bus_num == 2) { // block stock lkas messages and stock acc messages (if OP is doing ACC) - int addr = to_fwd->RIR>>21; + int addr = GET_ADDR(to_fwd); int is_lkas_msg = (addr == 0xE4) || (addr == 0x194) || (addr == 0x33D); int is_acc_msg = (addr == 0x1FA) || (addr == 0x30C) || (addr == 0x39F); int block_fwd = is_lkas_msg || (is_acc_msg && long_controls_allowed); @@ -186,7 +188,7 @@ static int honda_bosch_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { if (bus_num == 2) { bus_fwd = 1; } else if (bus_num == 1) { - int addr = to_fwd->RIR >> 21; + int addr = GET_ADDR(to_fwd); int is_lkas_msg = (addr == 0xE4) || (addr == 0x33D); if (!is_lkas_msg) { bus_fwd = 2; diff --git a/board/safety/safety_hyundai.h b/board/safety/safety_hyundai.h index bbd7e48..f98fe07 100644 --- a/board/safety/safety_hyundai.h +++ b/board/safety/safety_hyundai.h @@ -16,37 +16,28 @@ 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) { - int bus = (to_push->RDTR >> 4) & 0xFF; - uint32_t addr; - if ((to_push->RIR & 4) != 0) { - // Extended - // Not looked at, but have to be separated - // to avoid address collision - addr = to_push->RIR >> 3; - } else { - // Normal - addr = to_push->RIR >> 21; - } + int bus = GET_BUS(to_push); + int addr = GET_ADDR(to_push); - if (addr == 897U) { + if (addr == 897) { int torque_driver_new = ((to_push->RDLR >> 11) & 0xfff) - 2048; // update array of samples update_sample(&hyundai_torque_driver, torque_driver_new); } // check if stock camera ECU is still online - if ((bus == 0) && (addr == 832U)) { + if ((bus == 0) && (addr == 832)) { hyundai_camera_detected = 1; controls_allowed = 0; } // Find out which bus the camera is on - if (addr == 832U) { + if (addr == 832) { hyundai_camera_bus = bus; } // enter controls on rising edge of ACC, exit controls on ACC off - if (addr == 1057U) { + if (addr == 1057) { // 2 bits: 13-14 int cruise_engaged = (to_push->RDLR >> 13) & 0x3; if (cruise_engaged && !hyundai_cruise_engaged_last) { @@ -58,7 +49,7 @@ static void hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { } // 832 is lkas cmd. If it is on camera bus, then giraffe switch 2 is high - if ((addr == 832U) && (bus == hyundai_camera_bus) && (hyundai_camera_bus != 0)) { + if ((addr == 832) && (bus == hyundai_camera_bus) && (hyundai_camera_bus != 0)) { hyundai_giraffe_switch_2 = 1; } } @@ -66,23 +57,15 @@ static void hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { static int hyundai_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { int tx = 1; + int addr = GET_ADDR(to_send); // There can be only one! (camera) if (hyundai_camera_detected) { tx = 0; } - uint32_t addr; - if ((to_send->RIR & 4) != 0) { - // Extended - addr = to_send->RIR >> 3; - } else { - // Normal - addr = to_send->RIR >> 21; - } - // LKA STEER: safety check - if (addr == 832U) { + if (addr == 832) { int desired_torque = ((to_send->RDLR >> 16) & 0x7ff) - 1024; uint32_t ts = TIM2->CNT; bool violation = 0; @@ -132,7 +115,7 @@ static int hyundai_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { // ensuring that only the cancel button press is sent (VAL 4) when controls are off. // This avoids unintended engagements while still allowing resume spam // TODO: fix bug preventing the button msg to be fwd'd on bus 2 - //if (((to_send->RIR>>21) == 1265) && !controls_allowed && ((to_send->RDTR >> 4) & 0xFF) == 0) { + //if ((addr == 1265) && !controls_allowed && (bus == 0) { // if ((to_send->RDLR & 0x7) != 4) { // tx = 0; // } @@ -150,7 +133,8 @@ static int hyundai_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { if (bus_num == 0) { bus_fwd = hyundai_camera_bus; } else if (bus_num == hyundai_camera_bus) { - if ((to_fwd->RIR>>21) != 832) { + int addr = GET_ADDR(to_fwd); + if (addr != 832) { bus_fwd = 0; } } diff --git a/board/safety/safety_subaru.h b/board/safety/safety_subaru.h index eca95ac..ba05178 100644 --- a/board/safety/safety_subaru.h +++ b/board/safety/safety_subaru.h @@ -16,10 +16,10 @@ struct sample_t subaru_torque_driver; // last few driver torques measure static void subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { - int bus_number = (to_push->RDTR >> 4) & 0xFF; - uint32_t addr = to_push->RIR >> 21; + int bus = GET_BUS(to_push); + int addr = GET_ADDR(to_push); - if ((addr == 0x119U) && (bus_number == 0)){ + if ((addr == 0x119) && (bus == 0)){ int torque_driver_new = ((to_push->RDLR >> 16) & 0x7FF); torque_driver_new = to_signed(torque_driver_new, 11); // update array of samples @@ -27,7 +27,7 @@ static void subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { } // enter controls on rising edge of ACC, exit controls on ACC off - if ((addr == 0x240U) && (bus_number == 0)) { + if ((addr == 0x240) && (bus == 0)) { int cruise_engaged = (to_push->RDHR >> 9) & 1; if (cruise_engaged && !subaru_cruise_engaged_last) { controls_allowed = 1; @@ -40,10 +40,10 @@ static void subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { static int subaru_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { int tx = 1; - uint32_t addr = to_send->RIR >> 21; + int addr = GET_ADDR(to_send); // steer cmd checks - if (addr == 0x122U) { + if (addr == 0x122) { int desired_torque = ((to_send->RDLR >> 16) & 0x1FFF); bool violation = 0; uint32_t ts = TIM2->CNT; @@ -104,7 +104,7 @@ static int subaru_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { // 356 is LKAS for Global Platform // 545 is ES_Distance // 802 is ES_LKAS - int32_t addr = to_fwd->RIR >> 21; + int addr = GET_ADDR(to_fwd); int block_msg = (addr == 290) || (addr == 356) || (addr == 545) || (addr == 802); if (!block_msg) { bus_fwd = 0; // Main CAN diff --git a/board/safety/safety_tesla.h b/board/safety/safety_tesla.h index 515d3e2..7fe359e 100644 --- a/board/safety/safety_tesla.h +++ b/board/safety/safety_tesla.h @@ -51,19 +51,9 @@ static void 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 - //int bus_number = (to_push->RDTR >> 4) & 0xFF; - uint32_t addr; - if ((to_push->RIR & 4) != 0) { - // Extended - // Not looked at, but have to be separated - // to avoid address collision - addr = to_push->RIR >> 3; - } else { - // Normal - addr = to_push->RIR >> 21; - } + int addr = GET_ADDR(to_push); - if (addr == 0x45U) { + if (addr == 0x45) { // 6 bits starting at position 0 int lever_position = (to_push->RDLR & 0x3F); if (lever_position == 2) { // pull forward @@ -76,7 +66,7 @@ static void tesla_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { } // Detect drive rail on (ignition) (start recording) - if (addr == 0x348U) { + if (addr == 0x348) { // GTW_status int drive_rail_on = (to_push->RDLR & 0x0001); tesla_ignition_started = drive_rail_on == 1; @@ -84,7 +74,7 @@ static void tesla_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // exit controls on brake press // DI_torque2::DI_brakePedal 0x118 - if (addr == 0x118U) { + if (addr == 0x118) { // 1 bit at position 16 if ((((to_push->RDLR & 0x8000)) >> 15) == 1) { // disable break cancel by commenting line below @@ -99,7 +89,7 @@ static void tesla_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // exit controls on EPAS error // EPAS_sysStatus::EPAS_eacStatus 0x370 - if (addr == 0x370U) { + if (addr == 0x370) { // if EPAS_eacStatus is not 1 or 2, disable control eac_status = ((to_push->RDHR >> 21)) & 0x7; // For human steering override we must not disable controls when eac_status == 0 @@ -110,7 +100,7 @@ static void tesla_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { } } //get latest steering wheel angle - if (addr == 0x00EU) { + if (addr == 0x00E) { float angle_meas_now = (int)(((((to_push->RDLR & 0x3F) << 8) + ((to_push->RDLR >> 8) & 0xFF)) * 0.1) - 819.2); uint32_t ts = TIM2->CNT; uint32_t ts_elapsed = get_ts_elapsed(ts, tesla_ts_angle_last); @@ -150,11 +140,11 @@ static void tesla_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { static int tesla_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { int tx = 1; - uint32_t addr = to_send->RIR >> 21; + int addr = GET_ADDR(to_send); // do not transmit CAN message if steering angle too high // DAS_steeringControl::DAS_steeringAngleRequest - if (addr == 0x488U) { + if (addr == 0x488) { float angle_raw = ((to_send->RDLR & 0x7F) << 8) + ((to_send->RDLR & 0xFF00) >> 8); float desired_angle = (angle_raw * 0.1) - 1638.35; bool violation = 0; @@ -202,7 +192,7 @@ static int tesla_ign_hook() { static int tesla_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { int bus_fwd = -1; - int32_t addr = to_fwd->RIR >> 21; + int addr = GET_ADDR(to_fwd); if (bus_num == 0) { // change inhibit of GTW_epasControl diff --git a/board/safety/safety_toyota.h b/board/safety/safety_toyota.h index 10b3030..21e05ee 100644 --- a/board/safety/safety_toyota.h +++ b/board/safety/safety_toyota.h @@ -34,7 +34,8 @@ struct sample_t toyota_torque_meas; // last 3 motor torques produced by th static void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { - int addr = to_push->RIR >> 21; + int bus = GET_BUS(to_push); + int addr = GET_ADDR(to_push); // get eps motor torque (0.66 factor in dbc) if (addr == 0x260) { @@ -85,7 +86,6 @@ static void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { toyota_gas_prev = gas; } - int bus = (to_push->RDTR >> 4) & 0xF; // msgs are only on bus 2 if panda is connected to frc if (bus == 2) { toyota_camera_forwarded = 1; @@ -100,8 +100,8 @@ static void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { int tx = 1; - int addr = to_send->RIR >> 21; - int bus = (to_send->RDTR >> 4) & 0xF; + int addr = GET_ADDR(to_send); + int bus = GET_BUS(to_send); // Check if msg is sent on BUS 0 if (bus == 0) { @@ -203,7 +203,7 @@ static int toyota_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { if (bus_num == 0) { bus_fwd = 2; } else if (bus_num == 2) { - int addr = to_fwd->RIR>>21; + int addr = GET_ADDR(to_fwd); // block stock lkas messages and stock acc messages (if OP is doing ACC) int is_lkas_msg = ((addr == 0x2E4) || (addr == 0x412)); // in TSSP 2.0 the camera does ACC as well, so filter 0x343 diff --git a/board/safety/safety_toyota_ipas.h b/board/safety/safety_toyota_ipas.h index 86e3382..99e6dae 100644 --- a/board/safety/safety_toyota_ipas.h +++ b/board/safety/safety_toyota_ipas.h @@ -35,7 +35,7 @@ static void toyota_ipas_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // check standard toyota stuff as well toyota_rx_hook(to_push); - int addr = to_push->RIR >> 21; + int addr = GET_ADDR(to_push); if (addr == 0x260) { // get driver steering torque @@ -101,10 +101,11 @@ static int toyota_ipas_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { int tx = 1; int bypass_standard_tx_hook = 0; - int addr = to_send->RIR >> 21; + int bus = GET_BUS(to_send); + int addr = GET_ADDR(to_send); // Check if msg is sent on BUS 0 - if (((to_send->RDTR >> 4) & 0xF) == 0) { + if (bus == 0) { // STEER ANGLE if ((addr == 0x266) || (addr == 0x167)) { diff --git a/board/safety_declarations.h b/board/safety_declarations.h index c5c8f35..8f02f34 100644 --- a/board/safety_declarations.h +++ b/board/safety_declarations.h @@ -1,3 +1,7 @@ +#define GET_BUS(msg) (((msg)->RDTR >> 4) & 0xFF) +#define GET_LEN(msg) ((msg)->RDTR & 0xf) +#define GET_ADDR(msg) ((((msg)->RIR & 4) != 0) ? ((msg)->RIR >> 3) : ((msg)->RIR >> 21)) + // sample struct that keeps 3 samples in memory struct sample_t { int values[6];