From 0a1ec8580ecbde1f73b424f63b9fb8fbe29ddf09 Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Thu, 15 Feb 2024 01:22:06 -0600
Subject: [PATCH] Toyota: radar disable safety (#1611)

* toyota: add safety for tester present

* block ACC_HUD

* check stock ECU

* add test

* and now pass test

* check ACC_CONTROL for relay malfunction

* not needed

* split tx msgs, test_panda_safety_tx_cases was really helpful!

* now don't need this

* 0x343 is expected to be on bus 0 if stock long

* better as cases

* swap

* always block pre_collision if stock (used for dsu removal)

* do tx msgs

* fix comment
---
 board/safety/safety_toyota.h | 10 ++++++++++
 tests/safety/test_toyota.py  | 13 ++++++++++++-
 2 files changed, 22 insertions(+), 1 deletion(-)

diff --git a/board/safety/safety_toyota.h b/board/safety/safety_toyota.h
index 9368f4ed..4256f125 100644
--- a/board/safety/safety_toyota.h
+++ b/board/safety/safety_toyota.h
@@ -52,6 +52,7 @@ const int TOYOTA_GAS_INTERCEPTOR_THRSLD = 805;
   {0x283, 0, 7}, {0x2E6, 0, 8}, {0x2E7, 0, 8}, {0x33E, 0, 7}, {0x344, 0, 8}, {0x365, 0, 7}, {0x366, 0, 7}, {0x4CB, 0, 8},  /* DSU bus 0 */  \
   {0x128, 1, 6}, {0x141, 1, 4}, {0x160, 1, 8}, {0x161, 1, 7}, {0x470, 1, 4},  /* DSU bus 1 */                                               \
   {0x411, 0, 8},  /* PCS_HUD */                                                                                                             \
+  {0x750, 0, 8},  /* radar diagnostic address */                                                                                            \
 
 const CanMsg TOYOTA_TX_MSGS[] = {
   TOYOTA_COMMON_TX_MSGS
@@ -340,6 +341,15 @@ static bool toyota_tx_hook(const CANPacket_t *to_send) {
     }
   }
 
+  // UDS: Only tester present ("\x0F\x02\x3E\x00\x00\x00\x00\x00") allowed on diagnostics address
+  if (addr == 0x750) {
+    // this address is sub-addressed. only allow tester present to radar (0xF)
+    bool invalid_uds_msg = (GET_BYTES(to_send, 0, 4) != 0x003E020FU) || (GET_BYTES(to_send, 4, 4) != 0x0U);
+    if (invalid_uds_msg) {
+      tx = 0;
+    }
+  }
+
   return tx;
 }
 
diff --git a/tests/safety/test_toyota.py b/tests/safety/test_toyota.py
index c9fefdea..0743c67e 100755
--- a/tests/safety/test_toyota.py
+++ b/tests/safety/test_toyota.py
@@ -12,7 +12,8 @@ from panda.tests.safety.common import CANPackerPanda
 TOYOTA_COMMON_TX_MSGS = [[0x2E4, 0], [0x191, 0], [0x412, 0], [0x343, 0], [0x1D2, 0]]  # LKAS + LTA + ACC & PCM cancel cmds
 TOYOTA_COMMON_LONG_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
-                              [0x411, 0]]  # PCS_HUD
+                              [0x411, 0],  # PCS_HUD
+                              [0x750, 0]]  # radar diagnostic address
 GAS_INTERCEPTOR_TX_MSGS = [[0x200, 0]]
 
 
@@ -80,6 +81,13 @@ class TestToyotaSafetyBase(common.PandaCarSafetyTest, common.LongitudinalAccelSa
     values = {"CRUISE_ACTIVE": enable}
     return self.packer.make_can_msg_panda("PCM_CRUISE", 0, values)
 
+  def test_diagnostics(self, stock_longitudinal: bool = False):
+    for should_tx, msg in ((False, b"\x6D\x02\x3E\x00\x00\x00\x00\x00"),  # fwdCamera tester present
+                           (False, b"\x0F\x03\xAA\xAA\x00\x00\x00\x00"),  # non-tester present
+                           (True, b"\x0F\x02\x3E\x00\x00\x00\x00\x00")):
+      tester_present = libpanda_py.make_CANPacket(0x750, 0, msg)
+      self.assertEqual(should_tx and not stock_longitudinal, self._tx(tester_present))
+
   def test_block_aeb(self, stock_longitudinal: bool = False):
     for controls_allowed in (True, False):
       for bad in (True, False):
@@ -316,6 +324,9 @@ class TestToyotaStockLongitudinalBase(TestToyotaSafetyBase):
   RELAY_MALFUNCTION_ADDRS = {0: (0x2E4,)}
   FWD_BLACKLISTED_ADDRS = {2: [0x2E4, 0x412, 0x191]}
 
+  def test_diagnostics(self, stock_longitudinal: bool = True):
+    super().test_diagnostics(stock_longitudinal=stock_longitudinal)
+
   def test_block_aeb(self, stock_longitudinal: bool = True):
     super().test_block_aeb(stock_longitudinal=stock_longitudinal)