From ace4a224c8efbe4399b21ca1b12f0eaa26abe627 Mon Sep 17 00:00:00 2001
From: rbiasini <riccardo.biasini@gmail.com>
Date: Wed, 2 May 2018 22:45:23 -0700
Subject: [PATCH] Ford safety (#115)

* added initial ford safety header

* added ford ok safety. Missing brake check

* added force cancel check in ford safety

* added brake check to fusion

* fixed nomenclature
---
 board/safety.h             |   9 ++-
 board/safety/safety_ford.h | 110 +++++++++++++++++++++++++++++++++++++
 2 files changed, 116 insertions(+), 3 deletions(-)
 create mode 100644 board/safety/safety_ford.h

diff --git a/board/safety.h b/board/safety.h
index c0bd303ec9b7ef..89b9e090ead5df 100644
--- a/board/safety.h
+++ b/board/safety.h
@@ -30,6 +30,7 @@ int controls_allowed = 0;
 #include "safety/safety_toyota_ipas.h"
 #endif
 #include "safety/safety_gm.h"
+#include "safety/safety_ford.h"
 #include "safety/safety_elm327.h"
 
 const safety_hooks *current_hooks = &nooutput_hooks;
@@ -64,10 +65,11 @@ typedef struct {
 #define SAFETY_NOOUTPUT 0
 #define SAFETY_HONDA 1
 #define SAFETY_TOYOTA 2
-#define SAFETY_TOYOTA_IPAS 0x1335
-#define SAFETY_TOYOTA_NOLIMITS 0x1336
 #define SAFETY_GM 3
 #define SAFETY_HONDA_BOSCH 4
+#define SAFETY_FORD 5
+#define SAFETY_TOYOTA_IPAS 0x1335
+#define SAFETY_TOYOTA_NOLIMITS 0x1336
 #define SAFETY_ALLOUTPUT 0x1337
 #define SAFETY_ELM327 0xE327
 
@@ -76,11 +78,12 @@ const safety_hook_config safety_hook_registry[] = {
   {SAFETY_HONDA, &honda_hooks},
   {SAFETY_HONDA_BOSCH, &honda_bosch_hooks},
   {SAFETY_TOYOTA, &toyota_hooks},
+  {SAFETY_GM, &gm_hooks},
+  {SAFETY_FORD, &ford_hooks},
   {SAFETY_TOYOTA_NOLIMITS, &toyota_nolimits_hooks},
 #ifdef PANDA
   {SAFETY_TOYOTA_IPAS, &toyota_ipas_hooks},
 #endif
-  {SAFETY_GM, &gm_hooks},
   {SAFETY_ALLOUTPUT, &alloutput_hooks},
   {SAFETY_ELM327, &elm327_hooks},
 };
diff --git a/board/safety/safety_ford.h b/board/safety/safety_ford.h
new file mode 100644
index 00000000000000..55b9900387b219
--- /dev/null
+++ b/board/safety/safety_ford.h
@@ -0,0 +1,110 @@
+// board enforces
+//   in-state
+//      accel set/resume
+//   out-state
+//      cancel button
+//      accel rising edge
+//      brake rising edge
+//      brake > 0mph
+
+int ford_brake_prev = 0;
+int ford_gas_prev = 0;
+int ford_is_moving = 0;
+
+static void ford_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
+
+  if ((to_push->RIR>>21) == 0x217) {
+    // wheel speeds are 14 bits every 16
+    ford_is_moving = 0xFCFF & (to_push->RDLR | (to_push->RDLR >> 16) |
+                               to_push->RDHR | (to_push->RDHR >> 16));
+  }
+
+  // state machine to enter and exit controls
+  if ((to_push->RIR>>21) == 0x83) {
+    int cancel = ((to_push->RDLR >> 8) & 0x1);
+    int set_or_resume = (to_push->RDLR >> 28) & 0x3;
+    if (cancel) {
+      controls_allowed = 0;
+    } else if (set_or_resume) {
+      controls_allowed = 1;
+    }
+  }
+
+  // exit controls on rising edge of brake press or on brake press when
+  // speed > 0
+  if ((to_push->RIR>>21) == 0x165) {
+    int brake = to_push->RDLR & 0x20;
+    if (brake && (!(ford_brake_prev) || ford_is_moving)) {
+      controls_allowed = 0;
+    }
+    ford_brake_prev = brake;
+  }
+
+  // exit controls on rising edge of gas press
+  if ((to_push->RIR>>21) == 0x204) {
+    int gas = to_push->RDLR & 0xFF03;
+    if (gas && !(ford_gas_prev)) {
+      controls_allowed = 0;
+    }
+    ford_gas_prev = gas;
+  }
+}
+
+// all commands: just steering
+// if controls_allowed and no pedals pressed
+//     allow all commands up to limit
+// else
+//     block all commands that produce actuation
+
+static int ford_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
+
+  // disallow actuator commands if gas or brake (with vehicle moving) are pressed
+  // and the the latching controls_allowed flag is True
+  int pedal_pressed = ford_gas_prev || (ford_brake_prev && ford_is_moving);
+  int current_controls_allowed = controls_allowed && !(pedal_pressed);
+
+  // STEER: safety check
+  if ((to_send->RIR>>21) == 0x3CA) {
+    if (current_controls_allowed) {
+      // all messages are fine here
+    } else {
+      // bits 7-4 need to be 0xF to disallow lkas commands
+      if (((to_send->RDLR >> 4) & 0xF) != 0xF) return 0;
+    }
+  }
+
+  // FORCE CANCEL: safety check only relevant when spamming the cancel button
+  // ensuring that set and resume aren't sent
+  if ((to_send->RIR>>21) == 0x83) {
+    if ((to_send->RDLR >> 28) & 0x3) return 0;
+  }
+
+  // 1 allows the message through
+  return true;
+}
+
+static int ford_tx_lin_hook(int lin_num, uint8_t *data, int len) {
+  // TODO: add safety if using LIN
+  return true;
+}
+
+static void ford_init(int16_t param) {
+  controls_allowed = 0;
+}
+
+static int ford_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
+  return -1;
+}
+
+static int ford_ign_hook() {
+  return -1;
+}
+
+const safety_hooks ford_hooks = {
+  .init = ford_init,
+  .rx = ford_rx_hook,
+  .tx = ford_tx_hook,
+  .tx_lin = ford_tx_lin_hook,
+  .ignition = ford_ign_hook,
+  .fwd = ford_fwd_hook,
+};