From 5c7ef9ee43d38a3b22bef84a95426d916f48054d Mon Sep 17 00:00:00 2001
From: gregjhogan <gregjhogan@gmail.com>
Date: Wed, 28 Feb 2018 01:27:47 -0600
Subject: [PATCH] added bosch safety hooks and forwarding

---
 board/drivers/can.h            |  5 +++--
 board/safety.h                 |  8 ++++++++
 board/safety/safety_defaults.h | 10 +++++++++
 board/safety/safety_elm327.h   |  5 +++++
 board/safety/safety_gm.h       |  5 +++++
 board/safety/safety_honda.h    | 37 ++++++++++++++++++++++++++++------
 board/safety/safety_toyota.h   |  6 ++++++
 python/__init__.py             |  1 +
 8 files changed, 69 insertions(+), 8 deletions(-)

diff --git a/board/drivers/can.h b/board/drivers/can.h
index da1226f..b094d2b 100644
--- a/board/drivers/can.h
+++ b/board/drivers/can.h
@@ -348,13 +348,14 @@ void can_rx(uint8_t can_number) {
 
     // forwarding (panda only)
     #ifdef PANDA
-      if (can_forwarding[bus_number] != -1) {
+      int bus_fwd_num = can_forwarding[bus_number] != -1 ? can_forwarding[bus_number] : safety_fwd_hook(bus_number, &to_push);
+      if (bus_fwd_num != -1) {
         CAN_FIFOMailBox_TypeDef to_send;
         to_send.RIR = to_push.RIR | 1; // TXRQ
         to_send.RDTR = to_push.RDTR;
         to_send.RDLR = to_push.RDLR;
         to_send.RDHR = to_push.RDHR;
-        can_send(&to_send, can_forwarding[bus_number]);
+        can_send(&to_send, bus_fwd_num);
       }
     #endif
 
diff --git a/board/safety.h b/board/safety.h
index dc824ba..3e0b0cf 100644
--- a/board/safety.h
+++ b/board/safety.h
@@ -6,12 +6,14 @@ typedef void (*safety_hook_init)(int16_t param);
 typedef void (*rx_hook)(CAN_FIFOMailBox_TypeDef *to_push);
 typedef int (*tx_hook)(CAN_FIFOMailBox_TypeDef *to_send);
 typedef int (*tx_lin_hook)(int lin_num, uint8_t *data, int len);
+typedef int (*fwd_hook)(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd);
 
 typedef struct {
   safety_hook_init init;
   rx_hook rx;
   tx_hook tx;
   tx_lin_hook tx_lin;
+  fwd_hook fwd;
 } safety_hooks;
 
 // This can be set by the safety hooks.
@@ -38,6 +40,10 @@ int safety_tx_lin_hook(int lin_num, uint8_t *data, int len){
   return current_hooks->tx_lin(lin_num, data, len);
 }
 
+int safety_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
+  return current_hooks->fwd(bus_num, to_fwd);
+}
+
 typedef struct {
   uint16_t id;
   const safety_hooks *hooks;
@@ -48,12 +54,14 @@ typedef struct {
 #define SAFETY_TOYOTA 2
 #define SAFETY_TOYOTA_NOLIMITS 0x1336
 #define SAFETY_GM 3
+#define SAFETY_HONDA_BOSCH 4
 #define SAFETY_ALLOUTPUT 0x1337
 #define SAFETY_ELM327 0xE327
 
 const safety_hook_config safety_hook_registry[] = {
   {SAFETY_NOOUTPUT, &nooutput_hooks},
   {SAFETY_HONDA, &honda_hooks},
+  {SAFETY_HONDA_BOSCH, &honda_bosch_hooks},
   {SAFETY_TOYOTA, &toyota_hooks},
   {SAFETY_TOYOTA_NOLIMITS, &toyota_nolimits_hooks},
   {SAFETY_GM, &gm_hooks},
diff --git a/board/safety/safety_defaults.h b/board/safety/safety_defaults.h
index b7b4d37..2411a41 100644
--- a/board/safety/safety_defaults.h
+++ b/board/safety/safety_defaults.h
@@ -14,11 +14,16 @@ static int nooutput_tx_lin_hook(int lin_num, uint8_t *data, int len) {
   return false;
 }
 
+static int nooutput_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
+  return -1;
+}
+
 const safety_hooks nooutput_hooks = {
   .init = nooutput_init,
   .rx = default_rx_hook,
   .tx = nooutput_tx_hook,
   .tx_lin = nooutput_tx_lin_hook,
+  .fwd = nooutput_fwd_hook,
 };
 
 // *** all output safety mode ***
@@ -35,10 +40,15 @@ static int alloutput_tx_lin_hook(int lin_num, uint8_t *data, int len) {
   return true;
 }
 
+static int alloutput_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
+  return -1;
+}
+
 const safety_hooks alloutput_hooks = {
   .init = alloutput_init,
   .rx = default_rx_hook,
   .tx = alloutput_tx_hook,
   .tx_lin = alloutput_tx_lin_hook,
+  .fwd = alloutput_fwd_hook,
 };
 
diff --git a/board/safety/safety_elm327.h b/board/safety/safety_elm327.h
index b9af35e..3a0efe6 100644
--- a/board/safety/safety_elm327.h
+++ b/board/safety/safety_elm327.h
@@ -31,9 +31,14 @@ static void elm327_init(int16_t param) {
   controls_allowed = 1;
 }
 
+static int elm327_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
+  return -1;
+}
+
 const safety_hooks elm327_hooks = {
   .init = elm327_init,
   .rx = elm327_rx_hook,
   .tx = elm327_tx_hook,
   .tx_lin = elm327_tx_lin_hook,
+  .fwd = elm327_fwd_hook,
 };
diff --git a/board/safety/safety_gm.h b/board/safety/safety_gm.h
index 661d8a7..99461b3 100644
--- a/board/safety/safety_gm.h
+++ b/board/safety/safety_gm.h
@@ -172,10 +172,15 @@ static void gm_init(int16_t param) {
   controls_allowed = 0;
 }
 
+static int gm_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
+  return -1;
+}
+
 const safety_hooks gm_hooks = {
   .init = gm_init,
   .rx = gm_rx_hook,
   .tx = gm_tx_hook,
   .tx_lin = gm_tx_lin_hook,
+  .fwd = gm_fwd_hook,
 };
 
diff --git a/board/safety/safety_honda.h b/board/safety/safety_honda.h
index bf30d84..a9567ff 100644
--- a/board/safety/safety_honda.h
+++ b/board/safety/safety_honda.h
@@ -14,6 +14,9 @@ int gas_prev = 0;
 int gas_interceptor_prev = 0;
 int ego_speed = 0;
 
+// TODO: auto-detect bosch hardware based on CAN messages?
+bool bosch_hardware = false;
+
 static void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
 
   // sample speed
@@ -35,7 +38,7 @@ static void honda_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) == 0x17C) {
+  if (!bosch_hardware && (to_push->RIR>>21) == 0x17C) {
     // bit 53
     int brake = to_push->RDHR & 0x200000;
     if (brake && (!(brake_prev) || ego_speed)) {
@@ -45,7 +48,7 @@ static void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
   }
 
   // exit controls on rising edge of gas press if interceptor
-  if ((to_push->RIR>>21) == 0x201) {
+  if (!bosch_hardware && (to_push->RIR>>21) == 0x201) {
     gas_interceptor_detected = 1;
     int gas_interceptor = ((to_push->RDLR & 0xFF) << 8) | ((to_push->RDLR & 0xFF00) >> 8);
     if ((gas_interceptor > 328) && (gas_interceptor_prev <= 328)) {
@@ -55,7 +58,7 @@ static void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
   }
 
   // exit controls on rising edge of gas press if no interceptor
-  if (!gas_interceptor_detected) {
+  if (!bosch_hardware && !gas_interceptor_detected) {
     if ((to_push->RIR>>21) == 0x17C) {
       int gas = to_push->RDLR & 0xFF;
       if (gas && !(gas_prev)) {
@@ -80,7 +83,7 @@ static int honda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
   int current_controls_allowed = controls_allowed && !(pedal_pressed);
 
   // BRAKE: safety check
-  if ((to_send->RIR>>21) == 0x1FA) {
+  if (!bosch_hardware && (to_send->RIR>>21) == 0x1FA) {
     if (current_controls_allowed) {
       if ((to_send->RDLR & 0xFFFFFF3F) != to_send->RDLR) return 0;
     } else {
@@ -89,7 +92,7 @@ static int honda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
   }
 
   // STEER: safety check
-  if ((to_send->RIR>>21) == 0xE4 || (to_send->RIR>>21) == 0x194) {
+  if ((to_send->RIR>>21) == 0xE4 || (!bosch_hardware && (to_send->RIR>>21) == 0x194)) {
     if (current_controls_allowed) {
       // all messages are fine here
     } else {
@@ -98,7 +101,7 @@ static int honda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
   }
 
   // GAS: safety check
-  if ((to_send->RIR>>21) == 0x200) {
+  if (!bosch_hardware && (to_send->RIR>>21) == 0x200) {
     if (current_controls_allowed) {
       // all messages are fine here
     } else {
@@ -119,10 +122,32 @@ static void honda_init(int16_t param) {
   controls_allowed = 0;
 }
 
+static int honda_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
+  int bus_fwd_num = -1;
+  if (bosch_hardware && (bus_num == 1 || bus_num == 2)) {
+    int addr = to_fwd->RIR>>21;
+    bus_fwd_num = addr != 0xE4 && addr != 0x33D && addr < 0x1000 ? (uint8_t)(~bus_num & 0x3) : -1;
+  }
+  return bus_fwd_num;
+}
+
 const safety_hooks honda_hooks = {
   .init = honda_init,
   .rx = honda_rx_hook,
   .tx = honda_tx_hook,
   .tx_lin = honda_tx_lin_hook,
+  .fwd = honda_fwd_hook,
 };
 
+static void honda_bosch_init(int16_t param) {
+  controls_allowed = 0;
+  bosch_hardware = true;
+}
+
+const safety_hooks honda_bosch_hooks = {
+  .init = honda_bosch_init,
+  .rx = honda_rx_hook,
+  .tx = honda_tx_hook,
+  .tx_lin = honda_tx_lin_hook,
+  .fwd = honda_fwd_hook,
+};
diff --git a/board/safety/safety_toyota.h b/board/safety/safety_toyota.h
index 9ddaf17..34af1b0 100644
--- a/board/safety/safety_toyota.h
+++ b/board/safety/safety_toyota.h
@@ -161,11 +161,16 @@ static void toyota_init(int16_t param) {
   dbc_eps_torque_factor = param;
 }
 
+static int toyota_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
+  return -1;
+}
+
 const safety_hooks toyota_hooks = {
   .init = toyota_init,
   .rx = toyota_rx_hook,
   .tx = toyota_tx_hook,
   .tx_lin = toyota_tx_lin_hook,
+  .fwd = toyota_fwd_hook,
 };
 
 static void toyota_nolimits_init(int16_t param) {
@@ -179,4 +184,5 @@ const safety_hooks toyota_nolimits_hooks = {
   .rx = toyota_rx_hook,
   .tx = toyota_tx_hook,
   .tx_lin = toyota_tx_lin_hook,
+  .fwd = toyota_fwd_hook,
 };
diff --git a/python/__init__.py b/python/__init__.py
index 486f94c..35ce524 100644
--- a/python/__init__.py
+++ b/python/__init__.py
@@ -104,6 +104,7 @@ class Panda(object):
   SAFETY_NOOUTPUT = 0
   SAFETY_HONDA = 1
   SAFETY_TOYOTA = 2
+  SAFETY_HONDA_BOSCH = 4
   SAFETY_TOYOTA_NOLIMITS = 0x1336
   SAFETY_ALLOUTPUT = 0x1337
   SAFETY_ELM327 = 0xE327