From 463a275bc4e99a1d37d523a120fe45976ea021fe Mon Sep 17 00:00:00 2001
From: iabdalkader <i.abdalkader@gmail.com>
Date: Wed, 2 Dec 2020 23:33:56 +0200
Subject: [PATCH] stm32/powerctrl: On H7, re-enable disabled OSCs/PLLs on exit
 from STOP.

This commit saves OSCs/PLLs state before STOP mode and restores them on
exit.  Some boards use HSI48 for USB for example, others have PLL2/3
enabled, etc.
---
 ports/stm32/powerctrl.c | 39 ++++++++++++++++++++++++++++++++++++---
 1 file changed, 36 insertions(+), 3 deletions(-)

diff --git a/ports/stm32/powerctrl.c b/ports/stm32/powerctrl.c
index 7d36816979..c36351e311 100644
--- a/ports/stm32/powerctrl.c
+++ b/ports/stm32/powerctrl.c
@@ -517,6 +517,9 @@ void powerctrl_enter_stop_mode(void) {
     #endif
 
     #if defined(STM32H7)
+    // Save RCC CR to re-enable OSCs and PLLs after wake up from low power mode.
+    uint32_t rcc_cr = RCC->CR;
+
     // Save the current voltage scaling level to restore after exiting low power mode.
     uint32_t vscaling = POWERCTRL_GET_VOLTAGE_SCALING();
 
@@ -607,9 +610,39 @@ void powerctrl_enter_stop_mode(void) {
     #endif
 
     #if defined(STM32H7)
-    // Enable PLL3 for USB
-    RCC->CR |= RCC_CR_PLL3ON;
-    while (!(RCC->CR & RCC_CR_PLL3RDY)) {
+    // Enable HSI
+    if (rcc_cr & RCC_CR_HSION) {
+        RCC->CR |= RCC_CR_HSION;
+        while (!(RCC->CR & RCC_CR_HSIRDY)) {
+        }
+    }
+
+    // Enable CSI
+    if (rcc_cr & RCC_CR_CSION) {
+        RCC->CR |= RCC_CR_CSION;
+        while (!(RCC->CR & RCC_CR_CSIRDY)) {
+        }
+    }
+
+    // Enable HSI48
+    if (rcc_cr & RCC_CR_HSI48ON) {
+        RCC->CR |= RCC_CR_HSI48ON;
+        while (!(RCC->CR & RCC_CR_HSI48RDY)) {
+        }
+    }
+
+    // Enable PLL2
+    if (rcc_cr & RCC_CR_PLL2ON) {
+        RCC->CR |= RCC_CR_PLL2ON;
+        while (!(RCC->CR & RCC_CR_PLL2RDY)) {
+        }
+    }
+
+    // Enable PLL3
+    if (rcc_cr & RCC_CR_PLL3ON) {
+        RCC->CR |= RCC_CR_PLL3ON;
+        while (!(RCC->CR & RCC_CR_PLL3RDY)) {
+        }
     }
     #endif