From 5b7f71882b0eaa964f76f6ec0a6a036a9ed469b5 Mon Sep 17 00:00:00 2001
From: iabdalkader <i.abdalkader@gmail.com>
Date: Sat, 3 Dec 2022 19:58:24 +0100
Subject: [PATCH] stm32/powerctrl: Improve standby mode entry code for H7 MCUs.

Changes in this commit:
- Clear and mask D2 EXTIs.
- Set correct voltage scaling level for standby mode.
- Disable debug MCU (if debugging is disabled), for all MCU series.
---
 ports/stm32/powerctrl.c | 27 +++++++++++++++++++++++++--
 1 file changed, 25 insertions(+), 2 deletions(-)

diff --git a/ports/stm32/powerctrl.c b/ports/stm32/powerctrl.c
index c0a0595e5d..ca5a424732 100644
--- a/ports/stm32/powerctrl.c
+++ b/ports/stm32/powerctrl.c
@@ -875,6 +875,18 @@ void powerctrl_enter_standby_mode(void) {
     MICROPY_BOARD_ENTER_STANDBY
     #endif
 
+    #if defined(STM32H7)
+    // Note: According to ST reference manual, RM0399, Rev 3, Section 7.7.10,
+    // before entering Standby mode, voltage scale VOS0 must not be active.
+    uint32_t vscaling = POWERCTRL_GET_VOLTAGE_SCALING();
+    if (vscaling == PWR_REGULATOR_VOLTAGE_SCALE0) {
+        __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE3);
+        // Wait for PWR_FLAG_VOSRDY
+        while (!__HAL_PWR_GET_FLAG(PWR_FLAG_VOSRDY)) {
+        }
+    }
+    #endif
+
     // 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
@@ -926,8 +938,14 @@ void powerctrl_enter_standby_mode(void) {
     // Restore EWUP state
     PWR->CSR2 |= csr2_ewup;
     #elif defined(STM32H7)
-    EXTI_D1->PR1 = 0x3fffff;
-    PWR->WKUPCR |= PWR_WAKEUP_FLAG1 | PWR_WAKEUP_FLAG2 | PWR_WAKEUP_FLAG3 | PWR_WAKEUP_FLAG4 | PWR_WAKEUP_FLAG5 | PWR_WAKEUP_FLAG6;
+    // Clear and mask D1 EXTIs.
+    EXTI_D1->PR1 = 0x3fffffu;
+    EXTI_D1->IMR1 &= ~(0xFFFFu); // 16 lines
+    // Clear and mask D2 EXTIs.
+    EXTI_D2->PR1 = 0x3fffffu;
+    EXTI_D2->IMR1 &= ~(0xFFFFu); // 16 lines
+    // Clear all wake-up flags.
+    PWR->WKUPCR |= PWR_WAKEUP_FLAG_ALL;
     #elif defined(STM32G0) || defined(STM32G4) || defined(STM32L4) || defined(STM32WB)
     // clear all wake-up flags
     PWR->SCR |= PWR_SCR_CWUF5 | PWR_SCR_CWUF4 | PWR_SCR_CWUF3 | PWR_SCR_CWUF2 | PWR_SCR_CWUF1;
@@ -952,6 +970,11 @@ void powerctrl_enter_standby_mode(void) {
     PWR->CSR1 |= PWR_CSR1_EIWUP;
     #endif
 
+    #if defined(NDEBUG)
+    // Disable Debug MCU.
+    DBGMCU->CR = 0;
+    #endif
+
     // enter standby mode
     HAL_PWR_EnterSTANDBYMode();
     // we never return; MCU is reset on exit from standby