From de7e1e757924ffcdc32377c80f20e57d542b210a Mon Sep 17 00:00:00 2001 From: Igor Biletskyy Date: Mon, 7 Mar 2022 14:24:04 -0800 Subject: [PATCH] Deep sleep(stop) mode for pandas (#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 578d425fa7ba256f513c1c6ca54a00d69d78f53e. * my dear pedal! --- board/config.h | 2 ++ board/drivers/usb.h | 8 +++++ board/faults.h | 1 + board/main.c | 50 +++++++++++++++++++++++++++ board/main_declarations.h | 3 ++ board/stm32fx/llexti.h | 56 ++++++++++++++++++++++++++++++ board/stm32fx/llrtc.h | 30 ++++++++++++++++ board/stm32fx/stm32fx_config.h | 4 +++ board/stm32h7/llexti.h | 63 ++++++++++++++++++++++++++++++++++ board/stm32h7/llrtc.h | 30 ++++++++++++++++ board/stm32h7/stm32h7_config.h | 1 + board/usb_comms.h | 4 +++ python/__init__.py | 3 ++ 13 files changed, 255 insertions(+) create mode 100644 board/stm32fx/llexti.h create mode 100644 board/stm32h7/llexti.h diff --git a/board/config.h b/board/config.h index 757c80e6..6a25a398 100644 --- 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 index f5903e95..23a798f7 100644 --- 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 index 2b152fc1..8f6ee28f 100644 --- 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 index 3682d1b7..3dbbadc0 100644 --- 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 index 1a8be2e9..27890cdf 100644 --- 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 new file mode 100644 index 00000000..d4362cd1 --- /dev/null +++ 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 index 39a0aef3..b286ce72 100644 --- 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 index fb193492..cbccadab 100644 --- 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 new file mode 100644 index 00000000..7b7c260d --- /dev/null +++ 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 index 54ef3017..d78a514b 100644 --- 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 index 61e96e3c..02be1100 100644 --- 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 index c86fbe4d..ea9a18aa 100644 --- 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 index 68bcc684..af3beba7 100644 --- a/python/__init__.py +++ b/python/__init__.py @@ -473,6 +473,9 @@ class Panda(object): 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'')