Toyota: check STEER_REQUEST bit (#940)

* check steer_req

* think this is clearer on first glance

* add test

* also test recovery

* Update tests/safety/test_toyota.py

* Update tests/safety/test_toyota.py

* move

board/safety/safety_toyota.h

@@ -215,6 +215,7 @@ static int toyota_tx_hook(CANPacket_t *to_send, bool longitudinal_allowed) {
     if (addr == 0x2E4) {
       int desired_torque = (GET_BYTE(to_send, 1) << 8) | GET_BYTE(to_send, 2);
       desired_torque = to_signed(desired_torque, 16);
+      bool steer_req = GET_BIT(to_send, 0U) != 0U;
       bool violation = 0;
 
       uint32_t ts = microsecond_timer_get();
@@ -242,8 +243,8 @@ static int toyota_tx_hook(CANPacket_t *to_send, bool longitudinal_allowed) {
         }
       }
 
-      // no torque if controls is not allowed
-      if (!controls_allowed && (desired_torque != 0)) {
+      // no torque if controls is not allowed or mismatch with STEER_REQUEST bit
+      if ((!controls_allowed || !steer_req) && (desired_torque != 0)) {
         violation = 1;
       }
 

tests/safety/common.py

@@ -131,7 +131,7 @@ class TorqueSteeringSafetyTest(PandaSafetyTestBase):
     pass
 
   @abc.abstractmethod
-  def _torque_msg(self, torque):
+  def _torque_msg(self, torque, steer_req=1):
     pass
 
   def _set_prev_torque(self, t):

tests/safety/test_chrysler.py

@@ -61,7 +61,7 @@ class TestChryslerSafety(common.PandaSafetyTest, common.TorqueSteeringSafetyTest
     self.__class__.cnt_torque_meas += 1
     return self.packer.make_can_msg_panda("EPS_STATUS", 0, values)
 
-  def _torque_msg(self, torque):
+  def _torque_msg(self, torque, steer_req=1):
     values = {"LKAS_STEERING_TORQUE": torque}
     return self.packer.make_can_msg_panda("LKAS_COMMAND", 0, values)
 

tests/safety/test_gm.py

@@ -89,7 +89,7 @@ class TestGmSafety(common.PandaSafetyTest):
     values = {"LKADriverAppldTrq": torque}
     return self.packer.make_can_msg_panda("PSCMStatus", 0, values)
 
-  def _torque_msg(self, torque):
+  def _torque_msg(self, torque, steer_req=1):
     values = {"LKASteeringCmd": torque}
     return self.packer.make_can_msg_panda("ASCMLKASteeringCmd", 0, values)
 

tests/safety/test_hyundai.py

@@ -119,7 +119,7 @@ class TestHyundaiSafety(common.PandaSafetyTest):
     values = {"CR_Mdps_StrColTq": torque}
     return self.packer.make_can_msg_panda("MDPS12", 0, values)
 
-  def _torque_msg(self, torque):
+  def _torque_msg(self, torque, steer_req=1):
     values = {"CR_Lkas_StrToqReq": torque}
     return self.packer.make_can_msg_panda("LKAS11", 0, values)
 

tests/safety/test_mazda.py

@@ -39,7 +39,7 @@ class TestMazdaSafety(common.PandaSafetyTest):
 #    values = {"STEER_TORQUE_DRIVER": torque}
 #    return self.packer.make_can_msg_panda("STEER_TORQUE", 0, values)
 
-  def _torque_msg(self, torque):
+  def _torque_msg(self, torque, steer_req=1):
     values = {"LKAS_REQUEST": torque}
     return self.packer.make_can_msg_panda("CAM_LKAS", 0, values)
 

tests/safety/test_subaru.py

@@ -58,7 +58,7 @@ class TestSubaruSafety(common.PandaSafetyTest):
     self.__class__.cnt_brake += 1
     return self.packer.make_can_msg_panda("Brake_Status", 0, values)
 
-  def _torque_msg(self, torque):
+  def _torque_msg(self, torque, steer_req=1):
     values = {"LKAS_Output": torque}
     return self.packer.make_can_msg_panda("ES_LKAS", 0, values)
 

tests/safety/test_subaru_legacy.py

@@ -50,7 +50,7 @@ class TestSubaruLegacySafety(common.PandaSafetyTest):
     values = {"Brake_Pedal": brake}
     return self.packer.make_can_msg_panda("Brake_Pedal", 0, values)
 
-  def _torque_msg(self, torque):
+  def _torque_msg(self, torque, steer_req=1):
     values = {"LKAS_Command": torque}
     return self.packer.make_can_msg_panda("ES_LKAS", 0, values)
 

tests/safety/test_toyota.py

@@ -54,8 +54,8 @@ class TestToyotaSafety(common.PandaSafetyTest, common.InterceptorSafetyTest,
     values = {"STEER_TORQUE_EPS": (torque / self.EPS_SCALE) * 100.}
     return self.packer.make_can_msg_panda("STEER_TORQUE_SENSOR", 0, values)
 
-  def _torque_msg(self, torque):
-    values = {"STEER_TORQUE_CMD": torque}
+  def _torque_msg(self, torque, steer_req=1):
+    values = {"STEER_TORQUE_CMD": torque, "STEER_REQUEST": steer_req}
     return self.packer.make_can_msg_panda("STEERING_LKA", 0, values)
 
   def _lta_msg(self, req, req2, angle_cmd):
@@ -137,6 +137,22 @@ class TestToyotaSafety(common.PandaSafetyTest, common.InterceptorSafetyTest,
         should_tx = not req and not req2 and angle == 0
         self.assertEqual(should_tx, self._tx(self._lta_msg(req, req2, angle)))
 
+  def test_steer_req_bit(self):
+    """
+      On Toyota, you can ramp up torque and then set the STEER_REQUEST bit and the
+      EPS will ramp up faster than the effective panda safety limits. This tests:
+        - Nothing is sent when cutting torque
+        - Nothing is blocked when sending torque normally
+    """
+    self.safety.set_controls_allowed(True)
+    for _ in range(100):
+      self._set_prev_torque(self.MAX_TORQUE)
+      self.assertFalse(self._tx(self._torque_msg(self.MAX_TORQUE, steer_req=0)))
+
+    self._set_prev_torque(self.MAX_TORQUE)
+    for _ in range(100):
+      self.assertTrue(self._tx(self._torque_msg(self.MAX_TORQUE, steer_req=1)))
+
   def test_rx_hook(self):
     # checksum checks
     for msg in ["trq", "pcm"]: