stm32/powerctrl: Fix STOP mode voltage scaling on H7 REV V devices.

1 files changed, 30 insertions, 0 deletions

diff --git a/ports/stm32/powerctrl.c b/ports/stm32/powerctrl.c
index 01ad18341..7d3681697 100644
--- a/ports/stm32/powerctrl.c
+++ b/ports/stm32/powerctrl.c
@@ -38,6 +38,12 @@
 #define RCC_SR_SFTRSTF  RCC_RSR_SFT2RSTF
 #endif
 #define RCC_SR_RMVF     RCC_RSR_RMVF
+// This macro returns the actual voltage scaling level factoring in the power overdrive bit.
+// If the current voltage scale is VOLTAGE_SCALE1 and PWER_ODEN bit is set return VOLTAGE_SCALE0
+// otherwise the current voltage scaling (level VOS1 to VOS3) set in PWER_CSR is returned instead.
+#define POWERCTRL_GET_VOLTAGE_SCALING()     \
+    (((PWR->CSR1 & PWR_CSR1_ACTVOS) && (SYSCFG->PWRCR & SYSCFG_PWRCR_ODEN)) ? \
+    PWR_REGULATOR_VOLTAGE_SCALE0 : (PWR->CSR1 & PWR_CSR1_ACTVOS))
 #else
 #define RCC_SR          CSR
 #define RCC_SR_SFTRSTF  RCC_CSR_SFTRSTF
@@ -510,6 +516,19 @@ void powerctrl_enter_stop_mode(void) {
     HAL_PWREx_EnableFlashPowerDown();
     #endif
 
+    #if defined(STM32H7)
+    // Save the current voltage scaling level to restore after exiting low power mode.
+    uint32_t vscaling = POWERCTRL_GET_VOLTAGE_SCALING();
+
+    // If the current voltage scaling level is 0, switch to level 1 before entering low power mode.
+    if (vscaling == PWR_REGULATOR_VOLTAGE_SCALE0) {
+        __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);
+        // Wait for PWR_FLAG_VOSRDY
+        while (!__HAL_PWR_GET_FLAG(PWR_FLAG_VOSRDY)) {
+        }
+    }
+    #endif
+
     #if defined(STM32F7)
     HAL_PWR_EnterSTOPMode((PWR_CR1_LPDS | PWR_CR1_LPUDS | PWR_CR1_FPDS | PWR_CR1_UDEN), PWR_STOPENTRY_WFI);
     #else
@@ -532,6 +551,17 @@ void powerctrl_enter_stop_mode(void) {
 
     #else
 
+    #if defined(STM32H7)
+    // When exiting from Stop or Standby modes, the Run mode voltage scaling is reset to
+    // the default VOS3 value. Restore the voltage scaling to the previous voltage scale.
+    if (vscaling != POWERCTRL_GET_VOLTAGE_SCALING()) {
+        __HAL_PWR_VOLTAGESCALING_CONFIG(vscaling);
+        // Wait for PWR_FLAG_VOSRDY
+        while (!__HAL_PWR_GET_FLAG(PWR_FLAG_VOSRDY)) {
+        }
+    }
+    #endif
+
     #if !defined(STM32L4)
     // enable clock
     __HAL_RCC_HSE_CONFIG(MICROPY_HW_RCC_HSE_STATE);