Merge pull request #23 from commaai/redux/modular_safety

Modularize safety modes to encourage 3rd party safety code contribution.

board/main.c

@@ -1,5 +1,6 @@
 #include "config.h"
 #include "early.h"
+#include <stdbool.h>
 
 #define NULL ((void*)0)
 
@@ -13,8 +14,8 @@
 // can_num_lookup: Translates from 'bus number' to 'can number'.
 // can_forwarding: Given a bus num, lookup bus num to forward to. -1 means no forward.
 
-// old:         CAN1 = 1   CAN2 = 0
-// panda:       CAN1 = 0   CAN2 = 1   CAN3 = 4
+// NEO:         Bus 1=CAN1   Bus 2=CAN2
+// Panda:       Bus 0=CAN1   Bus 1=CAN2   Bus 3=CAN3
 #ifdef PANDA
   CAN_TypeDef *cans[] = {CAN1, CAN2, CAN3};
   uint8_t bus_lookup[] = {0,1,2};
@@ -333,16 +334,7 @@ int putc(uart_ring *q, char elem) {
 #include "usb.h"
 #include "can.h"
 #include "spi.h"
-
-void safety_rx_hook(CAN_FIFOMailBox_TypeDef *to_push);
-int safety_tx_hook(CAN_FIFOMailBox_TypeDef *to_send, int hardwired);
-int safety_tx_lin_hook(int lin_num, uint8_t *data, int len, int hardwired);
-
-#ifdef PANDA_SAFETY
-#include "panda_safety.h"
-#else
-#include "honda_safety.h"
-#endif
+#include "safety.h"
 
 // ***************************** CAN *****************************
 
@@ -643,8 +635,7 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, int hardwired) {
       }
       break;
     case 0xdc: // set controls allowed
-      controls_allowed = setup->b.wValue.w == 0x1337;
-      // take CAN out of SILM, careful with speed!
+      set_safety_mode(setup->b.wValue.w);
       for(i=0; i < CAN_MAX; i++)
         can_init(i, 0);
       break;

board/safety.h

@@ -0,0 +1,99 @@
+void safety_rx_hook(CAN_FIFOMailBox_TypeDef *to_push);
+int safety_tx_hook(CAN_FIFOMailBox_TypeDef *to_send, int hardwired);
+int safety_tx_lin_hook(int lin_num, uint8_t *data, int len, int hardwired);
+
+typedef void (*safety_hook_init)();
+typedef void (*rx_hook)(CAN_FIFOMailBox_TypeDef *to_push);
+typedef int (*tx_hook)(CAN_FIFOMailBox_TypeDef *to_send, int hardwired);
+typedef int (*tx_lin_hook)(int lin_num, uint8_t *data, int len, int hardwired);
+
+typedef struct {
+  safety_hook_init init;
+  rx_hook rx;
+  tx_hook tx;
+  tx_lin_hook tx_lin;
+} safety_hooks;
+
+// Include the actual safety policies.
+#include "safety_honda.h"
+
+void default__rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {}
+
+void nooutput__init() {
+  controls_allowed = false;
+}
+
+int nooutput__tx_hook(CAN_FIFOMailBox_TypeDef *to_send, int hardwired) {
+  return false;
+}
+
+int nooutput__tx_lin_hook(int lin_num, uint8_t *data, int len, int hardwired) {
+  return false;
+}
+
+void alloutput__init() {
+  controls_allowed = true;
+}
+
+int alloutput__tx_hook(CAN_FIFOMailBox_TypeDef *to_send, int hardwired) {
+  return hardwired;
+}
+
+int alloutput__tx_lin_hook(int lin_num, uint8_t *data, int len, int hardwired) {
+  return hardwired;
+}
+
+const safety_hooks nooutput_hooks = {
+  .init = nooutput__init,
+  .rx = default__rx_hook,
+  .tx = alloutput__tx_hook,
+  .tx_lin = alloutput__tx_lin_hook,
+};
+
+const safety_hooks alloutput_hooks = {
+  .init = alloutput__init,
+  .rx = default__rx_hook,
+  .tx = alloutput__tx_hook,
+  .tx_lin = alloutput__tx_lin_hook,
+};
+
+const safety_hooks *current_hooks = &nooutput_hooks;
+
+void safety_rx_hook(CAN_FIFOMailBox_TypeDef *to_push){
+  current_hooks->rx(to_push);
+}
+
+int safety_tx_hook(CAN_FIFOMailBox_TypeDef *to_send, int hardwired){
+  return current_hooks->tx(to_send, hardwired);
+}
+
+int safety_tx_lin_hook(int lin_num, uint8_t *data, int len, int hardwired){
+  return current_hooks->tx_lin(lin_num, data, len, hardwired);
+}
+
+
+typedef struct {
+  uint16_t id;
+  const safety_hooks *hooks;
+} safety_hook_config;
+
+const safety_hook_config safety_hook_registry[] = {
+  {0x0000, &nooutput_hooks},
+  {0x0001, &honda_hooks},
+  {0x1337, &alloutput_hooks},
+};
+
+#define HOOK_CONFIG_COUNT (sizeof(safety_hook_registry)/sizeof(safety_hook_config))
+
+int set_safety_mode(uint16_t mode){
+  int i;
+  for(i = 0; i < HOOK_CONFIG_COUNT; i++){
+    if(safety_hook_registry[i].id == mode){
+      current_hooks = safety_hook_registry[i].hooks;
+      current_hooks->init();
+      return 0;
+    }
+  }
+
+  return -1;
+}

board/safety_honda.h

@@ -11,7 +11,7 @@
 // else
 //     block all commands that produce actuation
 
-void safety_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
+void honda__rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
   // state machine to enter and exit controls
   // 0x1A6 for the ILX, 0x296 for the Civic Touring
   if ((to_push->RIR>>21) == 0x1A6 || (to_push->RIR>>21) == 0x296) {
@@ -50,7 +50,7 @@ void safety_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
   }
 }
 
-int safety_tx_hook(CAN_FIFOMailBox_TypeDef *to_send, int hardwired) {
+int honda__tx_hook(CAN_FIFOMailBox_TypeDef *to_send, int hardwired) {
   // BRAKE: safety check
   if ((to_send->RIR>>21) == 0x1FA) {
     if (controls_allowed) {
@@ -67,7 +67,7 @@ int safety_tx_hook(CAN_FIFOMailBox_TypeDef *to_send, int hardwired) {
     } else {
       to_send->RDLR &= 0xFFFF0000;
     }
-  } 
+  }
 
   // GAS: safety check
   if ((to_send->RIR>>21) == 0x200) {
@@ -76,13 +76,23 @@ int safety_tx_hook(CAN_FIFOMailBox_TypeDef *to_send, int hardwired) {
     } else {
       to_send->RDLR &= 0xFFFF0000;
     }
-  } 
+  }
 
   // 1 allows the message through
   return hardwired;
 }
 
-int safety_tx_lin_hook(int lin_num, uint8_t *data, int len, int hardwired) {
+int honda__tx_lin_hook(int lin_num, uint8_t *data, int len, int hardwired) {
   return hardwired;
 }
 
+void honda__init() {
+  controls_allowed = true;
+}
+
+const safety_hooks honda_hooks = {
+  .init = honda__init,
+  .rx = honda__rx_hook,
+  .tx = honda__tx_hook,
+  .tx_lin = honda__tx_lin_hook,
+};

panda/__init__.py

@@ -77,6 +77,10 @@ class WifiHandle(object):
   def close(self):
     self.sock.close()
 
+SAFETY_NOOUTPUT = 0
+SAFETY_HONDA = 1
+SAFETY_ALLOUTPUT = 0x1337
+
 class Panda(object):
   REQUEST_TYPE = usb1.TYPE_VENDOR | usb1.RECIPIENT_DEVICE
 
@@ -147,6 +151,12 @@ class Panda(object):
 
   # ******************* configuration *******************
 
+  def set_controls_mode(self, mode=SAFETY_ALLOUTPUT):
+    self._handle.controlWrite(Panda.REQUEST_TYPE, 0xdc, mode, 0, b'')
+
+  def set_controls_allowed(self, on):
+    self.set_controls_mode(SAFETY_ALLOUTPUT if on else SAFETY_NOOUTPUT)
+
   def set_controls_allowed(self, on):
     self._handle.controlWrite(Panda.REQUEST_TYPE, 0xdc, (0x1337 if on else 0), 0, b'')