Skip to content

Commit

Permalink
gen2 long safety
Browse files Browse the repository at this point in the history
  • Loading branch information
@jnewb1
jnewb1 committed Aug 31, 2023
1 parent ef1a933 commit 21b6275
Show file tree
Hide file tree
Showing 3 changed files with 122 additions and 44 deletions.
59 changes: 45 additions & 14 deletions board/safety/safety_subaru.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,20 +39,32 @@ const LongitudinalLimits SUBARU_LONG_LIMITS = {
#define MSG_SUBARU_ES_LKAS_State 0x322
#define MSG_SUBARU_ES_Infotainment 0x323

#define MSG_SUBARU_ES_UDS_Request 0x787

#define MSG_SUBARU_ES_HighBeamAssist 0x121
#define MSG_SUBARU_ES_STATIC_1 0x22a
#define MSG_SUBARU_ES_STATIC_2 0x325

#define SUBARU_MAIN_BUS 0
#define SUBARU_ALT_BUS 1
#define SUBARU_CAM_BUS 2

#define SUBARU_COMMON_TX_MSGS(alt_bus, lkas_msg) \
{MSG_SUBARU_ES_LKAS, SUBARU_MAIN_BUS, 8}, \
{MSG_SUBARU_ES_Distance, alt_bus, 8}, \
{MSG_SUBARU_ES_DashStatus, SUBARU_MAIN_BUS, 8}, \
{MSG_SUBARU_ES_LKAS_State, SUBARU_MAIN_BUS, 8}, \
{MSG_SUBARU_ES_Infotainment, SUBARU_MAIN_BUS, 8}, \
#define SUBARU_COMMON_TX_MSGS(alt_bus, lkas_msg) \
{lkas_msg, SUBARU_MAIN_BUS, 8}, \
{MSG_SUBARU_ES_Distance, alt_bus, 8}, \
{MSG_SUBARU_ES_DashStatus, SUBARU_MAIN_BUS, 8}, \
{MSG_SUBARU_ES_LKAS_State, SUBARU_MAIN_BUS, 8}, \
{MSG_SUBARU_ES_Infotainment, SUBARU_MAIN_BUS, 8}, \

#define SUBARU_COMMON_LONG_TX_MSGS(alt_bus) \
{MSG_SUBARU_ES_Brake, alt_bus, 8}, \
{MSG_SUBARU_ES_Status, alt_bus, 8}, \

#define SUBARU_COMMON_LONG_TX_MSGS(alt_bus) \
{MSG_SUBARU_ES_Brake, SUBARU_MAIN_BUS, 8}, \
{MSG_SUBARU_ES_Status, SUBARU_MAIN_BUS, 8}, \
#define SUBARU_GEN2_LONG_ADDITIONAL_TX_MSGS() \
{MSG_SUBARU_ES_UDS_Request, SUBARU_CAM_BUS, 8}, \
{MSG_SUBARU_ES_HighBeamAssist, SUBARU_MAIN_BUS, 8}, \
{MSG_SUBARU_ES_STATIC_1, SUBARU_MAIN_BUS, 8}, \
{MSG_SUBARU_ES_STATIC_2, SUBARU_MAIN_BUS, 8}, \

#define SUBARU_COMMON_ADDR_CHECKS(alt_bus) \
{.msg = {{MSG_SUBARU_Throttle, SUBARU_MAIN_BUS, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 10000U}, { 0 }, { 0 }}}, \
Expand All @@ -77,6 +89,13 @@ const CanMsg SUBARU_GEN2_TX_MSGS[] = {
};
#define SUBARU_GEN2_TX_MSGS_LEN (sizeof(SUBARU_GEN2_TX_MSGS) / sizeof(SUBARU_GEN2_TX_MSGS[0]))

const CanMsg SUBARU_GEN2_LONG_TX_MSGS[] = {
SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS, MSG_SUBARU_ES_LKAS)
SUBARU_COMMON_LONG_TX_MSGS(SUBARU_ALT_BUS)
SUBARU_GEN2_LONG_ADDITIONAL_TX_MSGS()
};
#define SUBARU_GEN2_LONG_TX_MSGS_LEN (sizeof(SUBARU_GEN2_LONG_TX_MSGS) / sizeof(SUBARU_GEN2_LONG_TX_MSGS[0]))

AddrCheckStruct subaru_addr_checks[] = {
SUBARU_COMMON_ADDR_CHECKS(SUBARU_MAIN_BUS)
};
Expand Down Expand Up @@ -175,12 +194,14 @@ static int subaru_tx_hook(CANPacket_t *to_send) {
int addr = GET_ADDR(to_send);
bool violation = false;

if (subaru_gen2) {
tx = msg_allowed(to_send, SUBARU_GEN2_TX_MSGS, SUBARU_GEN2_TX_MSGS_LEN);
if (subaru_gen2 && subaru_longitudinal) {
tx = msg_allowed(to_send, SUBARU_GEN2_LONG_TX_MSGS, SUBARU_GEN2_LONG_TX_MSGS_LEN);
} else if (subaru_gen2) {
tx = msg_allowed(to_send, SUBARU_GEN2_TX_MSGS, SUBARU_GEN2_TX_MSGS_LEN);
} else if (subaru_longitudinal) {
tx = msg_allowed(to_send, SUBARU_LONG_TX_MSGS, SUBARU_LONG_TX_MSGS_LEN);
tx = msg_allowed(to_send, SUBARU_LONG_TX_MSGS, SUBARU_LONG_TX_MSGS_LEN);
} else {
tx = msg_allowed(to_send, SUBARU_TX_MSGS, SUBARU_TX_MSGS_LEN);
tx = msg_allowed(to_send, SUBARU_TX_MSGS, SUBARU_TX_MSGS_LEN);
}

// steer cmd checks
Expand Down Expand Up @@ -221,6 +242,16 @@ static int subaru_tx_hook(CANPacket_t *to_send) {
violation |= longitudinal_transmission_rpm_checks(transmission_rpm, SUBARU_LONG_LIMITS);
}

if (addr == MSG_SUBARU_ES_UDS_Request) {
// tester present ('\x02\x3E\x80\x00\x00\x00\x00\x00') is allowed for gen2 longitudinal to keep eyesight disabled
bool is_tester_present = (GET_BYTES(to_send, 0, 4) == 0x00803E02U) && (GET_BYTES(to_send, 4, 4) == 0x0U);

// reading ES button data by identifier (b'\x03\x22\x11\x30\x00\x00\x00\x00') is also allowed (DID 0x1130)
bool is_button_rdbi = (GET_BYTES(to_send, 0, 4) == 0x30112203U) && (GET_BYTES(to_send, 4, 4) == 0x0U);

violation |= !(is_tester_present || is_button_rdbi);
}

if (violation){
tx = 0;
}
Expand Down Expand Up @@ -258,7 +289,7 @@ static const addr_checks* subaru_init(uint16_t param) {
subaru_gen2 = GET_FLAG(param, SUBARU_PARAM_GEN2);

#ifdef ALLOW_DEBUG
subaru_longitudinal = GET_FLAG(param, SUBARU_PARAM_LONGITUDINAL) && !subaru_gen2;
subaru_longitudinal = GET_FLAG(param, SUBARU_PARAM_LONGITUDINAL);
#endif

if (subaru_gen2) {
Expand Down
2 changes: 1 addition & 1 deletion tests/safety/common.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -955,7 +955,7 @@ def test_tx_hook_on_wrong_safety_mode(self):
if attr.startswith('TestToyota') and current_test.startswith('TestToyota'):
continue
if {attr, current_test}.issubset({'TestSubaruGen1TorqueStockLongitudinalSafety', 'TestSubaruGen2TorqueStockLongitudinalSafety',
'TestSubaruGen1LongitudinalSafety'}):
'TestSubaruGen1LongitudinalSafety', 'TestSubaruGen2LongitudinalSafety'}):
continue
if {attr, current_test}.issubset({'TestVolkswagenPqSafety', 'TestVolkswagenPqStockSafety', 'TestVolkswagenPqLongSafety'}):
continue
Expand Down
105 changes: 76 additions & 29 deletions tests/safety/test_subaru.py
View file
Original file line number Diff line number Diff line change
@@ -1,49 +1,61 @@
#!/usr/bin/env python3
import enum
import unittest
from panda import Panda
from panda.tests.libpanda import libpanda_py
import panda.tests.safety.common as common
from panda.tests.safety.common import CANPackerPanda, MeasurementSafetyTest
from functools import partial

class SubaruMsg(enum.IntEnum):
Brake_Status = 0x13c
CruiseControl = 0x240
Throttle = 0x40
Steering_Torque = 0x119
Wheel_Speeds = 0x13a
ES_LKAS = 0x122
ES_LKAS_ANGLE = 0x124
ES_Brake = 0x220
ES_Distance = 0x221
ES_Status = 0x222
ES_DashStatus = 0x321
ES_LKAS_State = 0x322
ES_Infotainment = 0x323
ES_UDS_Request = 0x787
ES_HighBeamAssist = 0x121
ES_STATIC_1 = 0x22a
ES_STATIC_2 = 0x325

MSG_SUBARU_Brake_Status = 0x13c
MSG_SUBARU_CruiseControl = 0x240
MSG_SUBARU_Throttle = 0x40
MSG_SUBARU_Steering_Torque = 0x119
MSG_SUBARU_Wheel_Speeds = 0x13a
MSG_SUBARU_ES_LKAS = 0x122
MSG_SUBARU_ES_LKAS_ANGLE = 0x124
MSG_SUBARU_ES_Brake = 0x220
MSG_SUBARU_ES_Distance = 0x221
MSG_SUBARU_ES_Status = 0x222
MSG_SUBARU_ES_DashStatus = 0x321
MSG_SUBARU_ES_LKAS_State = 0x322
MSG_SUBARU_ES_Infotainment = 0x323

SUBARU_MAIN_BUS = 0
SUBARU_ALT_BUS = 1
SUBARU_CAM_BUS = 2


def lkas_tx_msgs(alt_bus, lkas_msg=MSG_SUBARU_ES_LKAS):
def lkas_tx_msgs(alt_bus, lkas_msg=SubaruMsg.ES_LKAS):
return [[lkas_msg, SUBARU_MAIN_BUS],
[MSG_SUBARU_ES_Distance, alt_bus],
[MSG_SUBARU_ES_DashStatus, SUBARU_MAIN_BUS],
[MSG_SUBARU_ES_LKAS_State, SUBARU_MAIN_BUS],
[MSG_SUBARU_ES_Infotainment, SUBARU_MAIN_BUS]]
[SubaruMsg.ES_Distance, alt_bus],
[SubaruMsg.ES_DashStatus, SUBARU_MAIN_BUS],
[SubaruMsg.ES_LKAS_State, SUBARU_MAIN_BUS],
[SubaruMsg.ES_Infotainment, SUBARU_MAIN_BUS]]

def long_tx_msgs():
return [[MSG_SUBARU_ES_Brake, SUBARU_MAIN_BUS],
[MSG_SUBARU_ES_Status, SUBARU_MAIN_BUS]]
def long_tx_msgs(alt_bus):
return [[SubaruMsg.ES_Brake, alt_bus],
[SubaruMsg.ES_Status, alt_bus]]

def fwd_blacklisted_addr(lkas_msg=MSG_SUBARU_ES_LKAS):
return {SUBARU_CAM_BUS: [lkas_msg, MSG_SUBARU_ES_DashStatus, MSG_SUBARU_ES_LKAS_State, MSG_SUBARU_ES_Infotainment]}
def gen2_long_additional_tx_msgs():
return [[SubaruMsg.ES_UDS_Request, SUBARU_CAM_BUS],
[SubaruMsg.ES_HighBeamAssist, SUBARU_MAIN_BUS],
[SubaruMsg.ES_STATIC_1, SUBARU_MAIN_BUS],
[SubaruMsg.ES_STATIC_2, SUBARU_MAIN_BUS]]

def fwd_blacklisted_addr(lkas_msg=SubaruMsg.ES_LKAS):
return {SUBARU_CAM_BUS: [lkas_msg, SubaruMsg.ES_DashStatus, SubaruMsg.ES_LKAS_State, SubaruMsg.ES_Infotainment]}

class TestSubaruSafetyBase(common.PandaSafetyTest, MeasurementSafetyTest):
FLAGS = 0
STANDSTILL_THRESHOLD = 0 # kph
RELAY_MALFUNCTION_ADDR = MSG_SUBARU_ES_LKAS
RELAY_MALFUNCTION_ADDR = SubaruMsg.ES_LKAS
RELAY_MALFUNCTION_BUS = SUBARU_MAIN_BUS
FWD_BUS_LOOKUP = {SUBARU_MAIN_BUS: SUBARU_CAM_BUS, SUBARU_CAM_BUS: SUBARU_MAIN_BUS}
FWD_BLACKLISTED_ADDRS = fwd_blacklisted_addr()
Expand Down Expand Up @@ -122,9 +134,9 @@ class TestSubaruLongitudinalSafetyBase(TestSubaruSafetyBase, common.Longitudinal
MAX_RPM = 2400
MAX_POSSIBLE_RPM = 2**12

FWD_BLACKLISTED_ADDRS = {2: [MSG_SUBARU_ES_LKAS, MSG_SUBARU_ES_Brake, MSG_SUBARU_ES_Distance,
MSG_SUBARU_ES_Status, MSG_SUBARU_ES_DashStatus,
MSG_SUBARU_ES_LKAS_State, MSG_SUBARU_ES_Infotainment]}
FWD_BLACKLISTED_ADDRS = {2: [SubaruMsg.ES_LKAS, SubaruMsg.ES_Brake, SubaruMsg.ES_Distance,
SubaruMsg.ES_Status, SubaruMsg.ES_DashStatus,
SubaruMsg.ES_LKAS_State, SubaruMsg.ES_Infotainment]}

def test_rpm_safety_check(self):
self._generic_limit_safety_check(self._send_rpm_msg, self.MIN_RPM, self.MAX_RPM, 0, self.MAX_POSSIBLE_RPM, 1)
Expand Down Expand Up @@ -157,21 +169,56 @@ class TestSubaruGen1TorqueStockLongitudinalSafety(TestSubaruStockLongitudinalSaf
TX_MSGS = lkas_tx_msgs(SUBARU_MAIN_BUS)


class TestSubaruGen2TorqueStockLongitudinalSafety(TestSubaruStockLongitudinalSafetyBase, TestSubaruTorqueSafetyBase):
class TestSubaruGen2TorqueSafetyBase(TestSubaruTorqueSafetyBase):
ALT_MAIN_BUS = SUBARU_ALT_BUS
ALT_CAM_BUS = SUBARU_ALT_BUS

MAX_RATE_UP = 40
MAX_RATE_DOWN = 40
MAX_TORQUE = 1000


class TestSubaruGen2TorqueStockLongitudinalSafety(TestSubaruStockLongitudinalSafetyBase, TestSubaruGen2TorqueSafetyBase):
FLAGS = Panda.FLAG_SUBARU_GEN2
TX_MSGS = lkas_tx_msgs(SUBARU_ALT_BUS)


class TestSubaruGen1LongitudinalSafety(TestSubaruLongitudinalSafetyBase, TestSubaruTorqueSafetyBase):
FLAGS = Panda.FLAG_SUBARU_LONG
TX_MSGS = lkas_tx_msgs(SUBARU_MAIN_BUS) + long_tx_msgs()
TX_MSGS = lkas_tx_msgs(SUBARU_MAIN_BUS) + long_tx_msgs(SUBARU_MAIN_BUS)


class TestSubaruGen2LongitudinalSafety(TestSubaruLongitudinalSafetyBase, TestSubaruGen2TorqueSafetyBase):
FLAGS = Panda.FLAG_SUBARU_LONG | Panda.FLAG_SUBARU_GEN2
TX_MSGS = lkas_tx_msgs(SUBARU_ALT_BUS) + long_tx_msgs(SUBARU_ALT_BUS) + gen2_long_additional_tx_msgs()

def _rdbi_msg(self, did: int):
return b'\x03\x22' + did.to_bytes(2) + b'\x00\x00\x00\x00'

def _es_uds_msg(self, msg: bytes):
return libpanda_py.make_CANPacket(SubaruMsg.ES_UDS_Request, 2, msg)

def test_es_uds_message(self):
tester_present = b'\x02\x3E\x80\x00\x00\x00\x00\x00'
not_tester_present = b"\x03\xAA\xAA\x00\x00\x00\x00\x00"

button_did = 0x1130

# Tester present is allowed for gen2 long to keep eyesight disabled
self.assertTrue(self._tx(self._es_uds_msg(tester_present)))

# Non-Tester present is not allowed
self.assertFalse(self._tx(self._es_uds_msg(not_tester_present)))

# Only button_did is allowed to be read via UDS
for did in range(0xFFFF):
should_tx = (did == button_did)
self.assertEqual(self._tx(self._es_uds_msg(self._rdbi_msg(did))), should_tx)

# any other msg is not allowed
for sid in range(0xFF):
msg = b'\x03' + sid.to_bytes(1) + b'\x00' * 6
self.assertFalse(self._tx(self._es_uds_msg(msg)))


if __name__ == "__main__":
Expand Down

0 comments on commit 21b6275

Please sign in to comment.