stm32/powerctrl: Factor code that configures PLLSAI on F7 MCUs.

3 files changed, 27 insertions, 51 deletions

diff --git a/ports/stm32/powerctrl.c b/ports/stm32/powerctrl.c
index 2dac225c7..66fd691a8 100644
--- a/ports/stm32/powerctrl.c
+++ b/ports/stm32/powerctrl.c
@@ -32,11 +32,31 @@
 #if !defined(STM32F0)
 
 // Assumes that PLL is used as the SYSCLK source
-int powerctrl_rcc_clock_config_pll(RCC_ClkInitTypeDef *rcc_init, uint32_t sysclk_mhz) {
+int powerctrl_rcc_clock_config_pll(RCC_ClkInitTypeDef *rcc_init, uint32_t sysclk_mhz, bool need_pllsai) {
     uint32_t flash_latency;
 
     #if defined(STM32F7)
 
+    if (need_pllsai) {
+        // Configure PLLSAI at 48MHz for those peripherals that need this freq
+        const uint32_t pllsain = 192;
+        const uint32_t pllsaip = 4;
+        const uint32_t pllsaiq = 2;
+        RCC->PLLSAICFGR = pllsaiq << RCC_PLLSAICFGR_PLLSAIQ_Pos
+            | (pllsaip / 2 - 1) << RCC_PLLSAICFGR_PLLSAIP_Pos
+            | pllsain << RCC_PLLSAICFGR_PLLSAIN_Pos;
+        RCC->CR |= RCC_CR_PLLSAION;
+        uint32_t ticks = mp_hal_ticks_ms();
+        while (!(RCC->CR & RCC_CR_PLLSAIRDY)) {
+            if (mp_hal_ticks_ms() - ticks > 200) {
+                return -MP_ETIMEDOUT;
+            }
+        }
+        RCC->DCKCFGR2 |= RCC_DCKCFGR2_CK48MSEL;
+    } else {
+        RCC->DCKCFGR2 &= ~RCC_DCKCFGR2_CK48MSEL;
+    }
+
     // If possible, scale down the internal voltage regulator to save power
     uint32_t volt_scale;
     if (sysclk_mhz <= 151) {
@@ -111,9 +131,7 @@ int powerctrl_set_sysclk(uint32_t sysclk, uint32_t ahb, uint32_t apb1, uint32_t
     // Default PLL parameters that give 48MHz on PLL48CK
     uint32_t m = HSE_VALUE / 1000000, n = 336, p = 2, q = 7;
     uint32_t sysclk_source;
-    #if defined(STM32F7)
     bool need_pllsai = false;
-    #endif
 
     // Search for a valid PLL configuration that keeps USB at 48MHz
     uint32_t sysclk_mhz = sysclk / 1000000;
@@ -212,32 +230,10 @@ set_clk:
         return -MP_EIO;
     }
 
-    #if defined(STM32F7)
-    if (need_pllsai) {
-        // Configure PLLSAI at 48MHz for those peripherals that need this freq
-        const uint32_t pllsain = 192;
-        const uint32_t pllsaip = 4;
-        const uint32_t pllsaiq = 2;
-        RCC->PLLSAICFGR = pllsaiq << RCC_PLLSAICFGR_PLLSAIQ_Pos
-            | (pllsaip / 2 - 1) << RCC_PLLSAICFGR_PLLSAIP_Pos
-            | pllsain << RCC_PLLSAICFGR_PLLSAIN_Pos;
-        RCC->CR |= RCC_CR_PLLSAION;
-        uint32_t ticks = mp_hal_ticks_ms();
-        while (!(RCC->CR & RCC_CR_PLLSAIRDY)) {
-            if (mp_hal_ticks_ms() - ticks > 200) {
-                return -MP_ETIMEDOUT;
-            }
-        }
-        RCC->DCKCFGR2 |= RCC_DCKCFGR2_CK48MSEL;
-    } else {
-        RCC->DCKCFGR2 &= ~RCC_DCKCFGR2_CK48MSEL;
-    }
-    #endif
-
     // Set PLL as system clock source if wanted
     if (sysclk_source == RCC_SYSCLKSOURCE_PLLCLK) {
         RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_SYSCLK;
-        int ret = powerctrl_rcc_clock_config_pll(&RCC_ClkInitStruct, sysclk_mhz);
+        int ret = powerctrl_rcc_clock_config_pll(&RCC_ClkInitStruct, sysclk_mhz, need_pllsai);
         if (ret != 0) {
             return ret;
         }
diff --git a/ports/stm32/powerctrl.h b/ports/stm32/powerctrl.h
index c3f340973..b9de324fb 100644
--- a/ports/stm32/powerctrl.h
+++ b/ports/stm32/powerctrl.h
@@ -28,7 +28,7 @@
 
 #include <stdint.h>
 
-int powerctrl_rcc_clock_config_pll(RCC_ClkInitTypeDef *rcc_init, uint32_t sysclk_mhz);
+int powerctrl_rcc_clock_config_pll(RCC_ClkInitTypeDef *rcc_init, uint32_t sysclk_mhz, bool need_pllsai);
 int powerctrl_set_sysclk(uint32_t sysclk, uint32_t ahb, uint32_t apb1, uint32_t apb2);
 
 #endif // MICROPY_INCLUDED_STM32_POWERCTRL_H
diff --git a/ports/stm32/system_stm32.c b/ports/stm32/system_stm32.c
index 93c554b44..b57b8e10f 100644
--- a/ports/stm32/system_stm32.c
+++ b/ports/stm32/system_stm32.c
@@ -513,28 +513,6 @@ void SystemClock_Config(void)
       __fatal_error("HAL_RCC_OscConfig");
     }
 
-    #if defined(STM32F7)
-    uint32_t vco_out = RCC_OscInitStruct.PLL.PLLN * (HSE_VALUE / 1000000) / RCC_OscInitStruct.PLL.PLLM;
-    bool need_pllsai = vco_out % 48 != 0;
-    if (need_pllsai) {
-        // Configure PLLSAI at 48MHz for those peripherals that need this freq
-        const uint32_t pllsain = 192;
-        const uint32_t pllsaip = 4;
-        const uint32_t pllsaiq = 2;
-        RCC->PLLSAICFGR = pllsaiq << RCC_PLLSAICFGR_PLLSAIQ_Pos
-            | (pllsaip / 2 - 1) << RCC_PLLSAICFGR_PLLSAIP_Pos
-            | pllsain << RCC_PLLSAICFGR_PLLSAIN_Pos;
-        RCC->CR |= RCC_CR_PLLSAION;
-        uint32_t ticks = mp_hal_ticks_ms();
-        while (!(RCC->CR & RCC_CR_PLLSAIRDY)) {
-            if (mp_hal_ticks_ms() - ticks > 200) {
-                __fatal_error("PLLSAIRDY timeout");
-            }
-        }
-        RCC->DCKCFGR2 |= RCC_DCKCFGR2_CK48MSEL;
-    }
-    #endif
-
 #if defined(STM32H7)
     /* PLL3 for USB Clock */
     PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_USB;
@@ -560,8 +538,10 @@ void SystemClock_Config(void)
   }
 #endif
 
-    uint32_t sysclk_mhz = RCC_OscInitStruct.PLL.PLLN * (HSE_VALUE / 1000000) / RCC_OscInitStruct.PLL.PLLM / RCC_OscInitStruct.PLL.PLLP;
-    if (powerctrl_rcc_clock_config_pll(&RCC_ClkInitStruct, sysclk_mhz) != 0) {
+    uint32_t vco_out = RCC_OscInitStruct.PLL.PLLN * (HSE_VALUE / 1000000) / RCC_OscInitStruct.PLL.PLLM;
+    uint32_t sysclk_mhz = vco_out / RCC_OscInitStruct.PLL.PLLP;
+    bool need_pllsai = vco_out % 48 != 0;
+    if (powerctrl_rcc_clock_config_pll(&RCC_ClkInitStruct, sysclk_mhz, need_pllsai) != 0) {
         __fatal_error("HAL_RCC_ClockConfig");
     }