1 files changed, 125 insertions, 0 deletions
diff --git a/ports/stm32/powerctrl.c b/ports/stm32/powerctrl.c
index 4199cf690..45e886e13 100644
--- a/ports/stm32/powerctrl.c
+++ b/ports/stm32/powerctrl.c
@@ -27,6 +27,7 @@
 #include "py/mperrno.h"
 #include "py/mphal.h"
 #include "powerctrl.h"
+#include "rtc.h"
 #include "genhdr/pllfreqtable.h"
 
 #if !defined(STM32F0)
@@ -257,3 +258,127 @@ set_clk:
 }
 
 #endif
+
+void powerctrl_enter_stop_mode(void) {
+    #if defined(STM32L4)
+    // Configure the MSI as the clock source after waking up
+    __HAL_RCC_WAKEUPSTOP_CLK_CONFIG(RCC_STOP_WAKEUPCLOCK_MSI);
+    #endif
+
+    #if !defined(STM32F0) && !defined(STM32L4)
+    // takes longer to wake but reduces stop current
+    HAL_PWREx_EnableFlashPowerDown();
+    #endif
+
+    # if defined(STM32F7)
+    HAL_PWR_EnterSTOPMode((PWR_CR1_LPDS | PWR_CR1_LPUDS | PWR_CR1_FPDS | PWR_CR1_UDEN), PWR_STOPENTRY_WFI);
+    # else
+    HAL_PWR_EnterSTOPMode(PWR_LOWPOWERREGULATOR_ON, PWR_STOPENTRY_WFI);
+    #endif
+
+    // reconfigure the system clock after waking up
+
+    #if defined(STM32F0)
+
+    // Enable HSI48
+    __HAL_RCC_HSI48_ENABLE();
+    while (!__HAL_RCC_GET_FLAG(RCC_FLAG_HSI48RDY)) {
+    }
+
+    // Select HSI48 as system clock source
+    MODIFY_REG(RCC->CFGR, RCC_CFGR_SW, RCC_SYSCLKSOURCE_HSI48);
+    while (__HAL_RCC_GET_SYSCLK_SOURCE() != RCC_CFGR_SWS_HSI48) {
+    }
+
+    #else
+
+    #if !defined(STM32L4)
+    // enable HSE
+    __HAL_RCC_HSE_CONFIG(MICROPY_HW_CLK_HSE_STATE);
+    while (!__HAL_RCC_GET_FLAG(RCC_FLAG_HSERDY)) {
+    }
+    #endif
+
+    // enable PLL
+    __HAL_RCC_PLL_ENABLE();
+    while (!__HAL_RCC_GET_FLAG(RCC_FLAG_PLLRDY)) {
+    }
+
+    // select PLL as system clock source
+    MODIFY_REG(RCC->CFGR, RCC_CFGR_SW, RCC_SYSCLKSOURCE_PLLCLK);
+    #if defined(STM32H7)
+    while (__HAL_RCC_GET_SYSCLK_SOURCE() != RCC_CFGR_SWS_PLL1) {
+    }
+    #else
+    while (__HAL_RCC_GET_SYSCLK_SOURCE() != RCC_CFGR_SWS_PLL) {
+    }
+    #endif
+
+    #if defined(STM32F7)
+    if (RCC->DCKCFGR2 & RCC_DCKCFGR2_CK48MSEL) {
+        // Enable PLLSAI if it is selected as 48MHz source
+        RCC->CR |= RCC_CR_PLLSAION;
+        while (!(RCC->CR & RCC_CR_PLLSAIRDY)) {
+        }
+    }
+    #endif
+
+    #if defined(STM32L4)
+    // Enable PLLSAI1 for peripherals that use it
+    RCC->CR |= RCC_CR_PLLSAI1ON;
+    while (!(RCC->CR & RCC_CR_PLLSAI1RDY)) {
+    }
+    #endif
+
+    #endif
+}
+
+#if !defined(STM32L4)
+void powerctrl_enter_standby_mode(void) {
+    rtc_init_finalise();
+
+    // We need to clear the PWR wake-up-flag before entering standby, since
+    // the flag may have been set by a previous wake-up event.  Furthermore,
+    // we need to disable the wake-up sources while clearing this flag, so
+    // that if a source is active it does actually wake the device.
+    // See section 5.3.7 of RM0090.
+
+    // Note: we only support RTC ALRA, ALRB, WUT and TS.
+    // TODO support TAMP and WKUP (PA0 external pin).
+    #if defined(STM32F0)
+    #define CR_BITS (RTC_CR_ALRAIE | RTC_CR_WUTIE | RTC_CR_TSIE)
+    #define ISR_BITS (RTC_ISR_ALRAF | RTC_ISR_WUTF | RTC_ISR_TSF)
+    #else
+    #define CR_BITS (RTC_CR_ALRAIE | RTC_CR_ALRBIE | RTC_CR_WUTIE | RTC_CR_TSIE)
+    #define ISR_BITS (RTC_ISR_ALRAF | RTC_ISR_ALRBF | RTC_ISR_WUTF | RTC_ISR_TSF)
+    #endif
+
+    // save RTC interrupts
+    uint32_t save_irq_bits = RTC->CR & CR_BITS;
+
+    // disable RTC interrupts
+    RTC->CR &= ~CR_BITS;
+
+    // clear RTC wake-up flags
+    RTC->ISR &= ~ISR_BITS;
+
+    #if defined(STM32F7)
+    // disable wake-up flags
+    PWR->CSR2 &= ~(PWR_CSR2_EWUP6 | PWR_CSR2_EWUP5 | PWR_CSR2_EWUP4 | PWR_CSR2_EWUP3 | PWR_CSR2_EWUP2 | PWR_CSR2_EWUP1);
+    // clear global wake-up flag
+    PWR->CR2 |= PWR_CR2_CWUPF6 | PWR_CR2_CWUPF5 | PWR_CR2_CWUPF4 | PWR_CR2_CWUPF3 | PWR_CR2_CWUPF2 | PWR_CR2_CWUPF1;
+    #elif defined(STM32H7)
+    // TODO
+    #else
+    // clear global wake-up flag
+    PWR->CR |= PWR_CR_CWUF;
+    #endif
+
+    // enable previously-enabled RTC interrupts
+    RTC->CR |= save_irq_bits;
+
+    // enter standby mode
+    HAL_PWR_EnterSTANDBYMode();
+    // we never return; MCU is reset on exit from standby
+}
+#endif