Toyota: added safety_toyota

board/safety.h

@@ -20,6 +20,7 @@ int controls_allowed = 0;
 // Include the actual safety policies.
 #include "safety/safety_defaults.h"
 #include "safety/safety_honda.h"
+#include "safety/safety_toyota.h"
 #include "safety/safety_elm327.h"
 
 const safety_hooks *current_hooks = &nooutput_hooks;
@@ -43,12 +44,14 @@ typedef struct {
 
 #define SAFETY_NOOUTPUT 0
 #define SAFETY_HONDA 1
+#define SAFETY_TOYOTA 2
 #define SAFETY_ALLOUTPUT 0x1337
 #define SAFETY_ELM327 0xE327
 
 const safety_hook_config safety_hook_registry[] = {
   {SAFETY_NOOUTPUT, &nooutput_hooks},
   {SAFETY_HONDA, &honda_hooks},
+  {SAFETY_TOYOTA, &toyota_hooks},
   {SAFETY_ALLOUTPUT, &alloutput_hooks},
   {SAFETY_ELM327, &elm327_hooks},
 };

board/safety/safety_toyota.h

@@ -0,0 +1,59 @@
+// board enforces
+//   in-state
+//      accel set/resume
+//   out-state
+//      cancel button
+
+// these are set in the toyota safety hooks...this is the wrong place
+int gas_interceptor_detected = 0;
+
+// all commands: brake and steering
+// if controls_allowed
+//     allow all commands up to limit
+// else
+//     block all commands that produce actuation
+
+static void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
+
+  // exit controls on ACC off
+  if ((to_push->RIR>>21) == 0x1D2) {
+    // 4 bits: 55-52
+    if (to_push->RDHR & 0xF00000) {
+      controls_allowed = 1;
+    else {
+      controls_allowed = 0;
+    }
+  }
+}
+
+static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
+
+  // STEER: safety check on bytes 2-3
+  if ((to_send->RIR>>21) == 0x2E4) {
+    if (controls_allowed) {
+      // all messages are fine here
+    } else {
+      if ((to_send->RDLR & 0xFFFF00) != to_send->RDLR) return 0;
+    }
+  }
+
+  // 1 allows the message through
+  return true;
+}
+
+static int toyota_tx_lin_hook(int lin_num, uint8_t *data, int len) {
+  // TODO: add safety if using LIN
+  return true;
+}
+
+static void toyota_init() {
+  controls_allowed = 0;
+}
+
+const safety_hooks toyota_hooks = {
+  .init = toyota_init,
+  .rx = toyota_rx_hook,
+  .tx = toyota_tx_hook,
+  .tx_lin = toyota_tx_lin_hook,
+};
+