From 5b1a8dc873a40c31f933465a070b2fcfc9086d11 Mon Sep 17 00:00:00 2001
From: Andre Volmensky <andre@volmens.ky>
Date: Wed, 11 Mar 2020 16:00:33 +0900
Subject: [PATCH] Filtering LKAS HUD messages (#468)

* Filtering LKAS HUD messages

* Added nissan test_fwd_hook()

* Added Nissan TX_MSGS safety test

Co-authored-by: rbiasini <riccardo.biasini@gmail.com>
---
 board/safety/safety_nissan.h |  6 +++---
 tests/safety/test_nissan.py  | 20 +++++++++++++++++++-
 2 files changed, 22 insertions(+), 4 deletions(-)

diff --git a/board/safety/safety_nissan.h b/board/safety/safety_nissan.h
index 7e32963891a0b5..baa82b8d4aa319 100644
--- a/board/safety/safety_nissan.h
+++ b/board/safety/safety_nissan.h
@@ -15,7 +15,7 @@ const struct lookup_t NISSAN_LOOKUP_MAX_ANGLE = {
 
 const int NISSAN_DEG_TO_CAN = 100;
 
-const AddrBus NISSAN_TX_MSGS[] = {{0x169, 0}, {0x20b, 2}};
+const AddrBus NISSAN_TX_MSGS[] = {{0x169, 0}, {0x2b1, 0}, {0x4cc, 0}, {0x20b, 2}};
 
 AddrCheckStruct nissan_rx_checks[] = {
   {.addr = {0x2}, .bus = 0, .expected_timestep = 10000U},
@@ -187,8 +187,8 @@ static int nissan_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
   }
 
   if (bus_num == 2) {
-    // 0x169 is LKAS
-    int block_msg = (addr == 0x169);
+    // 0x169 is LKAS, 0x2b1 LKAS_HUD, 0x4cc LKAS_HUD_INFO_MSG
+    int block_msg = ((addr == 0x169) || (addr == 0x2b1) || (addr == 0x4cc));
     if (!block_msg) {
       bus_fwd = 0;  // V-CAN
     }
diff --git a/tests/safety/test_nissan.py b/tests/safety/test_nissan.py
index 9d4145699887a3..ab826fffbd2a03 100644
--- a/tests/safety/test_nissan.py
+++ b/tests/safety/test_nissan.py
@@ -11,7 +11,7 @@
 ANGLE_DELTA_V = [5., .8, .15]     # windup limit
 ANGLE_DELTA_VU = [5., 3.5, 0.4]   # unwind limit
 
-TX_MSGS = [[0x169, 0], [0x20b, 2]]
+TX_MSGS = [[0x169, 0], [0x2b1, 0], [0x4cc, 0], [0x20b, 2]]
 
 def twos_comp(val, bits):
   if val >= 0:
@@ -176,5 +176,23 @@ def test_acc_buttons(self):
   def test_relay_malfunction(self):
     StdTest.test_relay_malfunction(self, 0x169)
 
+  def test_fwd_hook(self):
+
+    buss = list(range(0x0, 0x3))
+    msgs = list(range(0x1, 0x800))
+
+    blocked_msgs = [0x169,0x2b1,0x4cc]
+    for b in buss:
+      for m in msgs:
+        if b == 0:
+          fwd_bus = 2
+        elif b == 1:
+          fwd_bus = -1
+        elif b == 2:
+          fwd_bus = -1 if m in blocked_msgs else 0
+
+        # assume len 8
+        self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, make_msg(b, m, 8)))
+
 if __name__ == "__main__":
   unittest.main()