safety: fwd hook config

opendbc/safety/safety.h

@@ -238,6 +238,20 @@ static bool tx_msg_safety_check(const CANPacket_t *to_send, const CanMsg msg_lis
   return allowed;
 }
 
+int get_fwd_bus(int bus_num, const safety_config *cfg) {
+  const FwdBus *fwd_buses = (cfg->fwd_buses == NULL) ? DEFAULT_FWD_BUS_LOOKUP : cfg->fwd_buses;
+  const int fwd_buses_len = (cfg->fwd_buses == NULL) ? DEFAULT_FWD_BUS_LEN : cfg->fwd_buses_len;
+
+  int destination_bus = -1;
+  for (int i = 0; i < fwd_buses_len; i++) {
+    if (fwd_buses[i].source_bus == bus_num) {
+      destination_bus = fwd_buses[i].destination_bus;
+      break;
+    }
+  }
+  return destination_bus;
+}
+
 bool safety_tx_hook(CANPacket_t *to_send) {
   bool allowed = tx_msg_safety_check(to_send, current_safety_config.tx_msgs, current_safety_config.tx_msgs_len);
   if ((current_safety_mode == SAFETY_ALLOUTPUT) || (current_safety_mode == SAFETY_ELM327)) {
@@ -253,7 +267,20 @@ bool safety_tx_hook(CANPacket_t *to_send) {
 }
 
 int safety_fwd_hook(int bus_num, int addr) {
-  return (relay_malfunction ? -1 : current_hooks->fwd(bus_num, addr));
+  const int destination_bus = get_fwd_bus(bus_num, &current_safety_config);
+
+  bool blocked = false;
+  for (int i = 0; i < current_safety_config.tx_msgs_len; i++) {
+    const CanMsg *m = &current_safety_config.tx_msgs[i];
+    if ((m->addr == addr) && m->blocked) {
+      if (m->bus == destination_bus) {
+        blocked = true;
+        break;
+      }
+    }
+  }
+
+  return (relay_malfunction || blocked) ? -1 : destination_bus;
 }
 
 bool get_longitudinal_allowed(void) {
@@ -431,6 +458,8 @@ int set_safety_hooks(uint16_t mode, uint16_t param) {
   current_safety_config.rx_checks_len = 0;
   current_safety_config.tx_msgs = NULL;
   current_safety_config.tx_msgs_len = 0;
+  current_safety_config.fwd_buses = NULL;
+  current_safety_config.fwd_buses_len = 0;
 
   int set_status = -1;  // not set
   int hook_config_count = sizeof(safety_hook_registry) / sizeof(safety_hook_config);
@@ -448,6 +477,8 @@ int set_safety_hooks(uint16_t mode, uint16_t param) {
     current_safety_config.rx_checks_len = cfg.rx_checks_len;
     current_safety_config.tx_msgs = cfg.tx_msgs;
     current_safety_config.tx_msgs_len = cfg.tx_msgs_len;
+    current_safety_config.fwd_buses = cfg.fwd_buses;
+    current_safety_config.fwd_buses_len = cfg.fwd_buses_len;
     // reset all dynamic fields in addr struct
     for (int j = 0; j < current_safety_config.rx_checks_len; j++) {
       current_safety_config.rx_checks[j].status = (RxStatus){0};

opendbc/safety/safety/safety_rivian.h

@@ -91,50 +91,13 @@ static bool rivian_tx_hook(const CANPacket_t *to_send) {
   return tx;
 }
 
-static int rivian_fwd_hook(int bus, int addr) {
-  int bus_fwd = -1;
-  bool block_msg = false;
-
-  if (bus == 0) {
-    // SCCM_WheelTouch: for hiding hold wheel alert
-    if (addr == 0x321) {
-      block_msg = true;
-    }
-
-    // VDM_AdasSts: for canceling stock ACC
-    if ((addr == 0x162) && !rivian_longitudinal) {
-      block_msg = true;
-    }
-
-    if (!block_msg) {
-      bus_fwd = 2;
-    }
-  }
-
-  if (bus == 2) {
-    // ACM_lkaHbaCmd: lateral control message
-    if (addr == 0x120) {
-      block_msg = true;
-    }
-
-    // ACM_longitudinalRequest: longitudinal control message
-    if (rivian_longitudinal && (addr == 0x160)) {
-      block_msg = true;
-    }
-
-    if (!block_msg) {
-      bus_fwd = 0;
-    }
-  }
-
-  return bus_fwd;
-}
-
 static safety_config rivian_init(uint16_t param) {
+  // Block SCCM_WheelTouch: for hiding hold wheel alert
+  // Block VDM_AdasSts: for canceling stock ACC
   // 0x120 = ACM_lkaHbaCmd, 0x321 = SCCM_WheelTouch, 0x162 = VDM_AdasSts
-  static const CanMsg RIVIAN_TX_MSGS[] = {{0x120, 0, 8}, {0x321, 2, 7}, {0x162, 2, 8}};
+  static const CanMsg RIVIAN_TX_MSGS[] = {{0x120, 0, 8, .blocked = true}, {0x321, 2, 7, .blocked = true}, {0x162, 2, 8, .blocked = true}};
   // 0x160 = ACM_longitudinalRequest
-  static const CanMsg RIVIAN_LONG_TX_MSGS[] = {{0x120, 0, 8}, {0x321, 2, 7}, {0x160, 0, 5}};
+  static const CanMsg RIVIAN_LONG_TX_MSGS[] = {{0x120, 0, 8, .blocked = true}, {0x321, 2, 7, .blocked = true}, {0x160, 0, 5, .blocked = true}};
 
   static RxCheck rivian_rx_checks[] = {
     {.msg = {{0x208, 0, 8, .frequency = 50U, .ignore_checksum = true, .ignore_counter = true}, { 0 }, { 0 }}},   // ESP_Status (speed)
@@ -158,5 +121,4 @@ const safety_hooks rivian_hooks = {
   .init = rivian_init,
   .rx = rivian_rx_hook,
   .tx = rivian_tx_hook,
-  .fwd = rivian_fwd_hook,
 };

opendbc/safety/safety/safety_toyota.h

@@ -3,19 +3,27 @@
 #include "safety_declarations.h"
 
 // Stock longitudinal
+// block stock lkas messages and stock acc messages (if OP is doing ACC)
+// in TSS2, 0x191 is LTA which we need to block to avoid controls collision
 #define TOYOTA_BASE_TX_MSGS \
-  {0x191, 0, 8}, {0x412, 0, 8}, {0x343, 0, 8}, {0x1D2, 0, 8},  /* LKAS + LTA + ACC & PCM cancel cmds */  \
+  {0x191, 0, 8, .blocked = true}, {0x412, 0, 8, .blocked = true}, {0x1D2, 0, 8},  /* LKAS + LTA + PCM cancel cmds */  \
 
 #define TOYOTA_COMMON_TX_MSGS \
   TOYOTA_BASE_TX_MSGS \
-  {0x2E4, 0, 5}, \
+  {0x343, 0, 8},  /* ACC cancel cmd */ \
+  {0x2E4, 0, 5, .blocked = true},  /* LKAS */ \
 
+// on SecOC cars 0x131 is also LTA
 #define TOYOTA_COMMON_SECOC_TX_MSGS \
   TOYOTA_BASE_TX_MSGS \
-  {0x2E4, 0, 8}, {0x131, 0, 8}, \
+  {0x343, 0, 8}, /* ACC cancel cmd */ \
+  {0x2E4, 0, 8, .blocked = true}, {0x131, 0, 8, .blocked = true}, \
 
+// in TSS2 the camera does ACC as well, so filter 0x343
 #define TOYOTA_COMMON_LONG_TX_MSGS                                                                                                          \
-  TOYOTA_COMMON_TX_MSGS                                                                                                                     \
+  TOYOTA_BASE_TX_MSGS                                                                                                                       \
+  {0x343, 0, 8, .blocked = true},  /* ACC cancel cmd */                                                                                     \
+  {0x2E4, 0, 5, .blocked = true},  /* LKAS */                                                                                               \
   {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 */                                                                                                             \
@@ -391,36 +399,10 @@ static safety_config toyota_init(uint16_t param) {
   return ret;
 }
 
-static int toyota_fwd_hook(int bus_num, int addr) {
-
-  int bus_fwd = -1;
-
-  if (bus_num == 0) {
-    bus_fwd = 2;
-  }
-
-  if (bus_num == 2) {
-    // block stock lkas messages and stock acc messages (if OP is doing ACC)
-    // in TSS2, 0x191 is LTA which we need to block to avoid controls collision
-    bool is_lkas_msg = ((addr == 0x2E4) || (addr == 0x412) || (addr == 0x191));
-    // on SecOC cars 0x131 is also LTA
-    is_lkas_msg |= toyota_secoc && (addr == 0x131);
-    // in TSS2 the camera does ACC as well, so filter 0x343
-    bool is_acc_msg = (addr == 0x343);
-    bool block_msg = is_lkas_msg || (is_acc_msg && !toyota_stock_longitudinal);
-    if (!block_msg) {
-      bus_fwd = 0;
-    }
-  }
-
-  return bus_fwd;
-}
-
 const safety_hooks toyota_hooks = {
   .init = toyota_init,
   .rx = toyota_rx_hook,
   .tx = toyota_tx_hook,
-  .fwd = toyota_fwd_hook,
   .get_checksum = toyota_get_checksum,
   .compute_checksum = toyota_compute_checksum,
   .get_quality_flag_valid = toyota_get_quality_flag_valid,

opendbc/safety/safety_declarations.h

@@ -13,12 +13,14 @@
   do { \
     (config).rx_checks = (rx); \
     (config).rx_checks_len = sizeof((rx)) / sizeof((rx)[0]); \
+    (config).fwd_buses = NULL; \
   } while (0);
 
 #define SET_TX_MSGS(tx, config) \
   do { \
     (config).tx_msgs = (tx); \
     (config).tx_msgs_len = sizeof((tx)) / sizeof((tx)[0]); \
+    (config).fwd_buses = NULL; \
   } while(0);
 
 #define UPDATE_VEHICLE_SPEED(val_ms) (update_sample(&vehicle_speed, ROUND((val_ms) * VEHICLE_SPEED_FACTOR)))
@@ -27,6 +29,7 @@ uint32_t GET_BYTES(const CANPacket_t *msg, int start, int len);
 
 extern const int MAX_WRONG_COUNTERS;
 #define MAX_ADDR_CHECK_MSGS 3U
+#define DEFAULT_FWD_BUS_LEN 2U
 #define MAX_SAMPLE_VALS 6
 // used to represent floating point vehicle speed in a sample_t
 #define VEHICLE_SPEED_FACTOR 1000.0
@@ -49,8 +52,19 @@ typedef struct {
   int addr;
   int bus;
   int len;
+  bool blocked;
 } CanMsg;
 
+typedef struct {
+  int source_bus;
+  int destination_bus;
+} FwdBus;
+
+const FwdBus DEFAULT_FWD_BUS_LOOKUP[DEFAULT_FWD_BUS_LEN] = {
+  {0, 2},
+  {2, 0},
+};
+
 typedef enum {
   TorqueMotorLimited,   // torque steering command, limited by EPS output torque
   TorqueDriverLimited,  // torque steering command, limited by driver's input torque
@@ -151,6 +165,8 @@ typedef struct {
   int rx_checks_len;
   const CanMsg *tx_msgs;
   int tx_msgs_len;
+  const FwdBus *fwd_buses;
+  int fwd_buses_len;
 } safety_config;
 
 typedef uint32_t (*get_checksum_t)(const CANPacket_t *to_push);