From 5541300cfec663ae16a2828d09bf5a8555fb5f55 Mon Sep 17 00:00:00 2001
From: Raf <rafcabezas@gmail.com>
Date: Sun, 20 Dec 2020 20:45:41 -0500
Subject: [PATCH] Fix panda code merging issue (p3)

---
 panda/board/main.c         |  8 +++++++-
 panda/board/power_saving.h |  2 +-
 panda/board/safety.h       | 13 ++++++++-----
 3 files changed, 16 insertions(+), 7 deletions(-)

diff --git a/panda/board/main.c b/panda/board/main.c
index 15a83143811c44..4893c15c8dd4f2 100644
--- a/panda/board/main.c
+++ b/panda/board/main.c
@@ -485,6 +485,8 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired)
     case 0xde:
       if (setup->b.wValue.w < BUS_MAX) {
         can_speed[setup->b.wValue.w] = setup->b.wIndex.w;
+        can_init(CAN_NUM_FROM_BUS_NUM(setup->b.wValue.w));
+        /* Tesla branch commented out:
         bool ret = can_init(CAN_NUM_FROM_BUS_NUM(setup->b.wValue.w));
         UNUSED(ret);
       }
@@ -496,6 +498,7 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired)
           (current_safety_mode == SAFETY_NOOUTPUT) ||
           (current_safety_mode == SAFETY_ELM327)) {
         unsafe_mode = setup->b.wValue.w;
+      */
       }
       break;
     // **** 0xe0: uart read
@@ -725,6 +728,8 @@ void TIM1_BRK_TIM9_IRQ_Handler(void) {
         heartbeat_counter += 1U;
       }
 
+    //BB we do not want to disable safety mode when on tesla
+    /*
       #ifdef EON
       // check heartbeat counter if we are running EON code.
       // if the heartbeat has been gone for a while, go to SILENT safety mode and enter power save
@@ -755,6 +760,7 @@ void TIM1_BRK_TIM9_IRQ_Handler(void) {
         current_board->set_usb_power_mode(USB_POWER_CDP);
       }
       #endif
+    */
 
       // check registers
       check_registers();
@@ -848,7 +854,7 @@ int main(void) {
 
   // init to SILENT and can silent
   //set_safety_mode(SAFETY_SILENT, 0);
-  set_safety_mode2(SAFETY_TESLA,0 );
+  set_safety_mode2(SAFETY_TESLA, 0);
 
   // enable CAN TXs
   current_board->enable_can_transceivers(true);
diff --git a/panda/board/power_saving.h b/panda/board/power_saving.h
index 2cb79cb61820ac..9805411470ec69 100644
--- a/panda/board/power_saving.h
+++ b/panda/board/power_saving.h
@@ -1,7 +1,7 @@
 // WARNING: To stay in compliance with the SIL2 rules laid out in STM UM1840, we should never implement any of the available hardware low power modes.
 // See rule: CoU_3
 
-#define POWER_SAVE_STATUS_DISABLED 0
+#define POWER_SAVE_STATUS_DISABLED 1
 #define POWER_SAVE_STATUS_ENABLED 1
 
 int power_save_status = POWER_SAVE_STATUS_DISABLED;
diff --git a/panda/board/safety.h b/panda/board/safety.h
index 3883fb6b51207d..5ec8a639cdecd5 100644
--- a/panda/board/safety.h
+++ b/panda/board/safety.h
@@ -271,7 +271,13 @@ int set_safety_hooks(uint16_t mode, int16_t param) {
 int set_safety_hooks2(uint16_t mode, int16_t param) {
   // reset state set by safety mode
   safety_mode_cnt = 0U;  // reset safety mode timer
-    relay_malfunction = false;
+
+  //BB prevent resetting if already in the correct mode
+  if ((mode > 0) && (current_safety == mode)) {
+    return 1;
+  }
+
+  relay_malfunction = false;
   gas_interceptor_detected = false;
   gas_interceptor_prev = 0;
   gas_pressed = false;
@@ -296,10 +302,7 @@ int set_safety_hooks2(uint16_t mode, int16_t param) {
 
   int set_status = -1;  // not set
   int hook_config_count = sizeof(safety_hook_registry) / sizeof(safety_hook_config);
-  //BB prevent resetting if already in the correct mode
-  if ((mode > 0) && (current_safety == mode)) {
-    return 1;
-  }
+
   for (int i = 0; i < hook_config_count; i++) {
     if (safety_hook_registry[i].id == mode) {
       current_hooks = safety_hook_registry[i].hooks;