Deep sleep(stop) mode for pandas (commaai#832)

* Add RTC with LSI for BP, GP, WP * disable jenkins temporarily, REVERT! * experiments * cleanup is still needed * cppcheck unused suppress * raise deepsleep limit timeout to 120sec * more experiments on usb_enumerated * continue * soft_disconnect * almost done * not enough * no ignition * still don't like it.. * rename to has_rtc_battery * clock_source on the way!? * delay 3 sec * works on C3, needs test on C2 * And this is for C2 * disable bootkick * misra 10.4 * .. * .. * set power state the right way * change that * seems don't need that? check on C2/C3 * partially works on RP... * comments (will work after rebase) * change fault name * switch to manual activation mode * Revert "disable jenkins temporarily, REVERT!" This reverts commit 578d425. * my dear pedal!
budney · Mar 7, 2022 · de7e1e7 · de7e1e7
1 parent e8e9d84
commit de7e1e7
Show file tree

Hide file tree

Showing 13 changed files with 255 additions and 0 deletions.
diff --git a/board/config.h b/board/config.h
@@ -7,6 +7,8 @@
 //#define DEBUG_SPI
 //#define DEBUG_FAULTS
 
+#define DEEPSLEEP_WAKEUP_DELAY 3U
+
 #define NULL ((void*)0)
 #define COMPILE_TIME_ASSERT(pred) ((void)sizeof(char[1 - (2 * ((int)(!(pred))))]))
 

diff --git a/board/drivers/usb.h b/board/drivers/usb.h
@@ -942,3 +942,11 @@ void usb_outep3_resume_if_paused(void) {
   }
   EXIT_CRITICAL();
 }
+
+void usb_soft_disconnect(bool enable) {
+  if (enable) {
+    USBx_DEVICE->DCTL |= USB_OTG_DCTL_SDIS;
+  } else {
+    USBx_DEVICE->DCTL &= ~USB_OTG_DCTL_SDIS;
+  }
+}
diff --git a/board/faults.h b/board/faults.h
@@ -25,6 +25,7 @@
 #define FAULT_INTERRUPT_RATE_KLINE_INIT     (1U << 19)
 #define FAULT_INTERRUPT_RATE_CLOCK_SOURCE   (1U << 20)
 #define FAULT_INTERRUPT_RATE_TICK           (1U << 21)
+#define FAULT_INTERRUPT_RATE_EXTI           (1U << 22)
 
 // Permanent faults
 #define PERMANENT_FAULTS 0U

diff --git a/board/main.c b/board/main.c
@@ -276,6 +276,38 @@ void tick_handler(void) {
   TICK_TIMER->SR = 0;
 }
 
+void EXTI_IRQ_Handler(void) {
+  if (check_exti_irq()) {
+    exti_irq_clear();
+    clock_init();
+
+    current_board->set_usb_power_mode(USB_POWER_CDP);
+    set_power_save_state(POWER_SAVE_STATUS_DISABLED);
+    deepsleep_requested = false;
+    heartbeat_counter = 0U;
+    usb_soft_disconnect(false);
+
+    NVIC_EnableIRQ(TICK_TIMER_IRQ);
+  }
+}
+
+uint8_t rtc_counter = 0;
+void RTC_WKUP_IRQ_Handler(void) {
+  exti_irq_clear();
+  clock_init();
+
+  rtc_counter++;
+  if ((rtc_counter % 2U) == 0U) {
+    current_board->set_led(LED_BLUE, false);
+  } else {
+    current_board->set_led(LED_BLUE, true);
+  }
+
+  if (rtc_counter == __UINT8_MAX__) {
+    rtc_counter = 1U;
+  }
+}
+
 
 int main(void) {
   // Init interrupt table
@@ -387,7 +419,25 @@ int main(void) {
         }
       #endif
     } else {
+      if (deepsleep_requested && !usb_enumerated && !check_started()) {
+        usb_soft_disconnect(true);
+        current_board->set_fan_power(0U);
+        current_board->set_usb_power_mode(USB_POWER_CLIENT);
+        NVIC_DisableIRQ(TICK_TIMER_IRQ);
+        delay(512000U);
+
+        // Init IRQs for CAN transceiver and ignition line
+        exti_irq_init();
+
+        // Init RTC Wakeup event on EXTI22
+        REGISTER_INTERRUPT(RTC_WKUP_IRQn, RTC_WKUP_IRQ_Handler, 10U, FAULT_INTERRUPT_RATE_EXTI)
+        rtc_wakeup_init();
+
+        // STOP mode
+        SCB->SCR |= SCB_SCR_SLEEPDEEP_Msk;
+      }
       __WFI();
+      SCB->SCR &= ~SCB_SCR_SLEEPDEEP_Msk;
     }
   }
 

diff --git a/board/main_declarations.h b/board/main_declarations.h
@@ -23,6 +23,9 @@ bool heartbeat_disabled = false;            // set over USB
 bool heartbeat_engaged = false;             // openpilot enabled, passed in heartbeat USB command
 uint32_t heartbeat_engaged_mismatches = 0;  // count of mismatches between heartbeat_engaged and controls_allowed
 
+// Enter deep sleep mode
+bool deepsleep_requested = false;
+
 // siren state
 bool siren_enabled = false;
 uint32_t siren_countdown = 0; // siren plays while countdown > 0

diff --git a/board/stm32fx/llexti.h b/board/stm32fx/llexti.h
@@ -0,0 +1,56 @@
+void EXTI_IRQ_Handler(void);
+
+void exti_irq_init(void) {
+  SYSCFG->EXTICR[2] &= ~(SYSCFG_EXTICR3_EXTI8_Msk);
+  if (car_harness_status == HARNESS_STATUS_FLIPPED) {
+    // CAN2_RX
+    current_board->enable_can_transceiver(3U, false);
+    SYSCFG->EXTICR[2] |=  (SYSCFG_EXTICR3_EXTI8_PA);
+
+    // IRQ on falling edge for PC3 (SBU2, EXTI3)
+    SYSCFG->EXTICR[0] &= ~(SYSCFG_EXTICR1_EXTI3_Msk);
+    SYSCFG->EXTICR[0] |=  (SYSCFG_EXTICR1_EXTI3_PC);
+    EXTI->IMR  |=  EXTI_IMR_MR3;
+    EXTI->RTSR &=  ~EXTI_RTSR_TR3; // rising edge
+    EXTI->FTSR |=  EXTI_FTSR_TR3; // falling edge
+    REGISTER_INTERRUPT(EXTI3_IRQn, EXTI_IRQ_Handler, 100U, FAULT_INTERRUPT_RATE_EXTI)
+    NVIC_EnableIRQ(EXTI3_IRQn);
+  } else {
+    // CAN0_RX
+    current_board->enable_can_transceiver(1U, false);
+    SYSCFG->EXTICR[2] |=  (SYSCFG_EXTICR3_EXTI8_PB);
+
+    // IRQ on falling edge for PC0 (SBU1, EXTI0)
+    SYSCFG->EXTICR[0] &= ~(SYSCFG_EXTICR1_EXTI0_Msk);
+    SYSCFG->EXTICR[0] |=  (SYSCFG_EXTICR1_EXTI0_PC);
+    EXTI->IMR  |=  EXTI_IMR_MR0;
+    EXTI->RTSR &=  ~EXTI_RTSR_TR0; // rising edge
+    EXTI->FTSR |=  EXTI_FTSR_TR0; // falling edge
+    REGISTER_INTERRUPT(EXTI0_IRQn, EXTI_IRQ_Handler, 100U, FAULT_INTERRUPT_RATE_EXTI)
+    NVIC_EnableIRQ(EXTI0_IRQn);
+  }
+  // CAN0 or CAN2 IRQ on falling edge (EXTI8)
+  EXTI->IMR  |=  EXTI_IMR_MR8;
+  EXTI->RTSR &=  ~EXTI_RTSR_TR8; // rising edge
+  EXTI->FTSR |=  EXTI_FTSR_TR8; // falling edge
+  REGISTER_INTERRUPT(EXTI9_5_IRQn, EXTI_IRQ_Handler, 100U, FAULT_INTERRUPT_RATE_EXTI)
+  NVIC_EnableIRQ(EXTI9_5_IRQn);
+}
+
+bool check_exti_irq(void) {
+  return ((EXTI->PR & EXTI_PR_PR8) || (EXTI->PR & EXTI_PR_PR3) || (EXTI->PR & EXTI_PR_PR0));
+}
+
+void exti_irq_clear(void) {
+  // Clear pending bits
+  EXTI->PR |= EXTI_PR_PR8;
+  EXTI->PR |= EXTI_PR_PR0;
+  EXTI->PR |= EXTI_PR_PR3;
+  EXTI->PR |= EXTI_PR_PR22;
+
+  // Disable all active EXTI IRQs
+  EXTI->IMR &= ~EXTI_IMR_MR8;
+  EXTI->IMR &= ~EXTI_IMR_MR0;
+  EXTI->IMR &= ~EXTI_IMR_MR3;
+  EXTI->IMR &= ~EXTI_IMR_MR22;
+}
diff --git a/board/stm32fx/llrtc.h b/board/stm32fx/llrtc.h
@@ -8,3 +8,33 @@ void enable_bdomain_protection(void) {
 void disable_bdomain_protection(void) {
   register_set_bits(&(PWR->CR), PWR_CR_DBP);
 }
+
+void rtc_wakeup_init(void) {
+  EXTI->IMR  |=  EXTI_IMR_MR22;
+  EXTI->RTSR |=  EXTI_RTSR_TR22; // rising edge
+  EXTI->FTSR &=  ~EXTI_FTSR_TR22; // falling edge
+
+  NVIC_DisableIRQ(RTC_WKUP_IRQn);
+
+  // Disable write protection
+  disable_bdomain_protection();
+  RTC->WPR = 0xCA;
+  RTC->WPR = 0x53;
+
+  RTC->CR &= ~RTC_CR_WUTE;
+  while((RTC->ISR & RTC_ISR_WUTWF) == 0){}
+
+  RTC->CR &= ~RTC_CR_WUTIE;
+  RTC->ISR &= ~RTC_ISR_WUTF;
+  //PWR->CR |= PWR_CR_CWUF;
+
+  RTC->WUTR = DEEPSLEEP_WAKEUP_DELAY;
+  // Wakeup timer interrupt enable, wakeup timer enable, select 1Hz rate
+  RTC->CR |= RTC_CR_WUTE | RTC_CR_WUTIE | RTC_CR_WUCKSEL_2;
+
+  // Re-enable write protection
+  RTC->WPR = 0x00;
+  enable_bdomain_protection();
+
+  NVIC_EnableIRQ(RTC_WKUP_IRQn);
+}
diff --git a/board/stm32fx/stm32fx_config.h b/board/stm32fx/stm32fx_config.h
@@ -69,6 +69,10 @@
   #include "stm32fx/lluart.h"
 #endif
 
+#if !defined(PEDAL_USB) && !defined(PEDAL) && !defined(BOOTSTUB)
+  #include "stm32fx/llexti.h"
+#endif
+
 #ifdef BOOTSTUB
   #include "stm32fx/llflash.h"
 #else

diff --git a/board/stm32h7/llexti.h b/board/stm32h7/llexti.h
@@ -0,0 +1,63 @@
+void EXTI_IRQ_Handler(void);
+
+void exti_irq_init(void) {
+  if (car_harness_status == HARNESS_STATUS_FLIPPED) {
+    // CAN2_RX IRQ on falling edge (EXTI10)
+    current_board->enable_can_transceiver(3U, false);
+    SYSCFG->EXTICR[2] &= ~(SYSCFG_EXTICR3_EXTI10_Msk);
+    SYSCFG->EXTICR[2] |=  (SYSCFG_EXTICR3_EXTI10_PG);
+    EXTI->IMR1  |=  EXTI_IMR1_IM10;
+    EXTI->RTSR1 &=  ~EXTI_RTSR1_TR10; // rising edge
+    EXTI->FTSR1 |=  EXTI_FTSR1_TR10; // falling edge
+
+    // IRQ on falling edge for PA1 (SBU2, EXTI1)
+    SYSCFG->EXTICR[0] &= ~(SYSCFG_EXTICR1_EXTI1_Msk);
+    SYSCFG->EXTICR[0] |=  (SYSCFG_EXTICR1_EXTI1_PA);
+    EXTI->IMR1  |=  EXTI_IMR1_IM1;
+    EXTI->RTSR1 &=  ~EXTI_RTSR1_TR1; // rising edge
+    EXTI->FTSR1 |=  EXTI_FTSR1_TR1; // falling edge
+    REGISTER_INTERRUPT(EXTI1_IRQn, EXTI_IRQ_Handler, 100U, FAULT_INTERRUPT_RATE_EXTI)
+    NVIC_EnableIRQ(EXTI1_IRQn);
+    REGISTER_INTERRUPT(EXTI15_10_IRQn, EXTI_IRQ_Handler, 100U, FAULT_INTERRUPT_RATE_EXTI)
+    NVIC_EnableIRQ(EXTI15_10_IRQn);
+  } else {
+    // CAN0_RX IRQ on falling edge (EXTI8)
+    current_board->enable_can_transceiver(1U, false);
+    SYSCFG->EXTICR[2] &= ~(SYSCFG_EXTICR3_EXTI8_Msk);
+    SYSCFG->EXTICR[2] |=  (SYSCFG_EXTICR3_EXTI8_PB);
+    EXTI->IMR1  |=  EXTI_IMR1_IM8;
+    EXTI->RTSR1 &=  ~EXTI_RTSR1_TR8; // rising edge
+    EXTI->FTSR1 |=  EXTI_FTSR1_TR8; // falling edge
+
+    // IRQ on falling edge for PC4 (SBU1, EXTI4)
+    SYSCFG->EXTICR[1] &= ~(SYSCFG_EXTICR2_EXTI4_Msk);
+    SYSCFG->EXTICR[1] |=  (SYSCFG_EXTICR2_EXTI4_PC);
+    EXTI->IMR1  |=  EXTI_IMR1_IM4;
+    EXTI->RTSR1 &=  ~EXTI_RTSR1_TR4; // rising edge
+    EXTI->FTSR1 |=  EXTI_FTSR1_TR4; // falling edge
+    REGISTER_INTERRUPT(EXTI4_IRQn, EXTI_IRQ_Handler, 100U, FAULT_INTERRUPT_RATE_EXTI)
+    NVIC_EnableIRQ(EXTI4_IRQn);
+    REGISTER_INTERRUPT(EXTI9_5_IRQn, EXTI_IRQ_Handler, 100U, FAULT_INTERRUPT_RATE_EXTI)
+    NVIC_EnableIRQ(EXTI9_5_IRQn);
+  }
+}
+
+bool check_exti_irq(void) {
+  return ((EXTI->PR1 & EXTI_PR1_PR8) || (EXTI->PR1 & EXTI_PR1_PR10) || (EXTI->PR1 & EXTI_PR1_PR1) || (EXTI->PR1 & EXTI_PR1_PR4));
+}
+
+void exti_irq_clear(void) {
+  // Clear pending bits
+  EXTI->PR1 |= EXTI_PR1_PR8;
+  EXTI->PR1 |= EXTI_PR1_PR10;
+  EXTI->PR1 |= EXTI_PR1_PR4;
+  EXTI->PR1 |= EXTI_PR1_PR1; // works
+  EXTI->PR1 |= EXTI_PR1_PR19; // works
+
+  // Disable all active EXTI IRQs
+  EXTI->IMR1 &= ~EXTI_IMR1_IM8;
+  EXTI->IMR1 &= ~EXTI_IMR1_IM10;
+  EXTI->IMR1 &= ~EXTI_IMR1_IM4;
+  EXTI->IMR1 &= ~EXTI_IMR1_IM1;
+  EXTI->IMR1 &= ~EXTI_IMR1_IM19;
+}
diff --git a/board/stm32h7/llrtc.h b/board/stm32h7/llrtc.h
@@ -8,3 +8,33 @@ void enable_bdomain_protection(void) {
 void disable_bdomain_protection(void) {
   register_set_bits(&(PWR->CR1), PWR_CR1_DBP);
 }
+
+void rtc_wakeup_init(void) {
+  EXTI->IMR1  |=  EXTI_IMR1_IM19;
+  EXTI->RTSR1 |=  EXTI_RTSR1_TR19; // rising edge
+  EXTI->FTSR1 &=  ~EXTI_FTSR1_TR19; // falling edge
+
+  NVIC_DisableIRQ(RTC_WKUP_IRQn);
+
+  // Disable write protection
+  disable_bdomain_protection();
+  RTC->WPR = 0xCA;
+  RTC->WPR = 0x53;
+
+  RTC->CR &= ~RTC_CR_WUTE;
+  while((RTC->ISR & RTC_ISR_WUTWF) == 0){}
+
+  RTC->CR &= ~RTC_CR_WUTIE;
+  RTC->ISR &= ~RTC_ISR_WUTF;
+  //PWR->CR1 |= PWR_CR1_CWUF;
+
+  RTC->WUTR = DEEPSLEEP_WAKEUP_DELAY;
+  // Wakeup timer interrupt enable, wakeup timer enable, select 1Hz rate
+  RTC->CR |= RTC_CR_WUTE | RTC_CR_WUTIE | RTC_CR_WUCKSEL_2;
+
+  // Re-enable write protection
+  RTC->WPR = 0x00;
+  enable_bdomain_protection();
+
+  NVIC_EnableIRQ(RTC_WKUP_IRQn);
+}
diff --git a/board/stm32h7/stm32h7_config.h b/board/stm32h7/stm32h7_config.h
@@ -59,6 +59,7 @@
 #if !defined(BOOTSTUB) && defined(PANDA)
   #include "drivers/uart.h"
   #include "stm32h7/lluart.h"
+  #include "stm32h7/llexti.h"
 #endif
 
 #ifdef BOOTSTUB

diff --git a/board/usb_comms.h b/board/usb_comms.h
@@ -456,6 +456,10 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp) {
         resp_len = 2;
       }
       break;
+    // **** 0xfb: enter deep sleep(stop) mode
+    case 0xfb:
+      deepsleep_requested = true;
+      break;
     default:
       puts("NO HANDLER ");
       puth(setup->b.bRequest);

diff --git a/python/__init__.py b/python/__init__.py
@@ -473,6 +473,9 @@ def set_usb_power(self, on):
   def set_power_save(self, power_save_enabled=0):
     self._handle.controlWrite(Panda.REQUEST_OUT, 0xe7, int(power_save_enabled), 0, b'')
 
+  def set_deepsleep(self):
+    self._handle.controlWrite(Panda.REQUEST_OUT, 0xfb, 0, 0, b'')
+
   def set_esp_power(self, on):
     self._handle.controlWrite(Panda.REQUEST_OUT, 0xd9, int(on), 0, b'')