From 2f053c90bd27c63126ae9560e6498fccae3474b1 Mon Sep 17 00:00:00 2001
From: Alexandre Abadie <alexandre.abadie@inria.fr>
Date: Thu, 17 Sep 2020 17:01:46 +0200
Subject: [PATCH] cpu/stm32gx: improve clock initialization sequence

---
 cpu/stm32/stmclk/stmclk_gx.c | 83 ++++++++++++++++++++++--------------
 1 file changed, 51 insertions(+), 32 deletions(-)

diff --git a/cpu/stm32/stmclk/stmclk_gx.c b/cpu/stm32/stmclk/stmclk_gx.c
index 627cfbefda..f02d80de9a 100644
--- a/cpu/stm32/stmclk/stmclk_gx.c
+++ b/cpu/stm32/stmclk/stmclk_gx.c
@@ -131,6 +131,40 @@
 #endif
 #endif /* CPU_FAM_STM32G4 */
 
+/* Check whether PLL must be enabled:
+  - When PLLCLK is used as SYSCLK
+*/
+#if IS_ACTIVE(CONFIG_USE_CLOCK_PLL)
+#define CLOCK_ENABLE_PLL            1
+#else
+#define CLOCK_ENABLE_PLL            0
+#endif
+
+/* Check whether HSE is required:
+  - When HSE is used as SYSCLK
+  - When PLL is used as SYSCLK and the board provides HSE (since HSE will be
+    used as PLL input clock)
+*/
+#if IS_ACTIVE(CONFIG_USE_CLOCK_HSE) || \
+    (IS_ACTIVE(CONFIG_BOARD_HAS_HSE) && IS_ACTIVE(CONFIG_USE_CLOCK_PLL))
+#define CLOCK_ENABLE_HSE            1
+#else
+#define CLOCK_ENABLE_HSE            0
+#endif
+
+/* Check whether HSI is required:
+  - When HSI is used as SYSCLK
+  - When PLL is used as SYSCLK and the board doesn't provide HSE (since HSI will be
+    used as PLL input clock)
+*/
+#if IS_ACTIVE(CONFIG_USE_CLOCK_HSI) || \
+    (!IS_ACTIVE(CONFIG_BOARD_HAS_HSE) && IS_ACTIVE(CONFIG_USE_CLOCK_PLL))
+#define CLOCK_ENABLE_HSI            1
+#else
+#define CLOCK_ENABLE_HSI            0
+#endif
+
+
 /** Determine the required flash wait states from the core clock frequency */
 #if defined(CPU_FAM_STM32G0)
 #if CLOCK_CORECLOCK >= 48000000
@@ -194,34 +228,33 @@ void stmclk_init_sysclk(void)
     }
 #endif
 
-    /* Only enable the HSE clock if it's available and used by the clock
-       configuration:
-        - as direct system clock source
-        - as PLL input clock
-    */
-    if (IS_ACTIVE(CONFIG_BOARD_HAS_HSE) &&
-        (IS_ACTIVE(CONFIG_USE_CLOCK_PLL) || IS_ACTIVE(CONFIG_USE_CLOCK_HSE))) {
+    /* Enable HSE if required */
+    if (IS_ACTIVE(CLOCK_ENABLE_HSE)) {
         RCC->CR |= RCC_CR_HSEON;
         while (!(RCC->CR & RCC_CR_HSERDY)) {}
     }
 
+    /* Enable PLL if required */
+    if (IS_ACTIVE(CLOCK_ENABLE_PLL)) {
+        RCC->PLLCFGR = (PLL_SRC | PLL_M | PLL_N | PLL_R | RCC_PLLCFGR_PLLREN);
+        RCC->CR |= RCC_CR_PLLON;
+        while (!(RCC->CR & RCC_CR_PLLRDY)) {}
+    }
+
+    /* Configure SYSCLK */
     if (IS_ACTIVE(CONFIG_USE_CLOCK_HSE)) {
 #if defined(CPU_FAM_STM32G0)
         RCC->CFGR = (RCC_CFGR_SW_HSE | CLOCK_AHB_DIV | CLOCK_APB1_DIV);
 #elif defined(CPU_FAM_STM32G4)
         RCC->CFGR = (RCC_CFGR_SW_HSE | CLOCK_AHB_DIV | CLOCK_APB1_DIV | CLOCK_APB2_DIV);
 #endif
+        RCC->CFGR |= RCC_CFGR_SW_HSE;
         while ((RCC->CFGR & RCC_CFGR_SWS) != RCC_CFGR_SWS_HSE) {}
     }
     else if (IS_ACTIVE(CONFIG_USE_CLOCK_PLL)) {
-        /* now we can safely configure and start the PLL */
-        RCC->PLLCFGR = (PLL_SRC | PLL_M | PLL_N | PLL_R | RCC_PLLCFGR_PLLREN);
-        RCC->CR |= RCC_CR_PLLON;
-        while (!(RCC->CR & RCC_CR_PLLRDY)) {}
-
 #if defined(CPU_FAM_STM32G4)
         if (CLOCK_AHB > MHZ(80)) {
-            /* Divide HCLK by before enabling the PLL */
+            /* Divide HCLK by 2 before enabling the PLL */
             RCC->CFGR |= RCC_CFGR_HPRE_DIV2;
         }
 #endif
@@ -241,29 +274,15 @@ void stmclk_init_sysclk(void)
 #endif
     }
 
-    if (!IS_ACTIVE(CONFIG_USE_CLOCK_HSI) ||
-        (IS_ACTIVE(CONFIG_USE_CLOCK_PLL) && IS_ACTIVE(CONFIG_BOARD_HAS_HSE))) {
+    if (!IS_ACTIVE(CLOCK_ENABLE_HSI)) {
         /* Disable HSI only if not used */
         stmclk_disable_hsi();
     }
 
-#if defined(CPU_FAM_STM32G4)
-    if (IS_USED(MODULE_PERIPH_HWRNG)) {
-        /* HWRNG is clocked by HSI48 so enable this clock when the peripheral is used */
-        RCC->CRRCR |= RCC_CRRCR_HSI48ON;
-        while (!(RCC->CRRCR & RCC_CRRCR_HSI48RDY)) {}
-    }
-
-    if (IS_USED(MODULE_PERIPH_RTT)) {
-        /* Ensure LPTIM1 clock source (LSI or LSE) is correctly reset when initializing
-           the clock, this is particularly useful after waking up from deep sleep */
-        if (IS_ACTIVE(CONFIG_BOARD_HAS_LSE)) {
-            RCC->CCIPR |= RCC_CCIPR_LPTIM1SEL_0 | RCC_CCIPR_LPTIM1SEL_1;
-        }
-        else {
-            RCC->CCIPR |= RCC_CCIPR_LPTIM1SEL_0;
-        }
-    }
+#if IS_USED(MODULE_PERIPH_HWRNG)
+    /* HWRNG is clocked by HSI48 so enable this clock when the peripheral is used */
+    RCC->CRRCR |= RCC_CRRCR_HSI48ON;
+    while (!(RCC->CRRCR & RCC_CRRCR_HSI48RDY)) {}
 #endif
 
     irq_restore(is);