Merge pull request #3008 from aurelj/stm32-rcc-hsi

stm32: ensure the core runs on HSI clock while setting up rcc

5 files changed, 95 insertions, 56 deletions

diff --git a/embassy-stm32/src/rcc/c0.rs b/embassy-stm32/src/rcc/c0.rs
index 349f978c5..5adf37941 100644
--- a/embassy-stm32/src/rcc/c0.rs
+++ b/embassy-stm32/src/rcc/c0.rs
@@ -76,25 +76,29 @@ impl Default for Config {
 }
 
 pub(crate) unsafe fn init(config: Config) {
+    // Turn on the HSI
+    match config.hsi {
+        None => RCC.cr().modify(|w| w.set_hsion(true)),
+        Some(hsi) => RCC.cr().modify(|w| {
+            w.set_hsidiv(hsi.sys_div);
+            w.set_hsikerdiv(hsi.ker_div);
+            w.set_hsion(true);
+        }),
+    }
+    while !RCC.cr().read().hsirdy() {}
+
+    // Use the HSI clock as system clock during the actual clock setup
+    RCC.cfgr().modify(|w| w.set_sw(Sysclk::HSISYS));
+    while RCC.cfgr().read().sws() != Sysclk::HSISYS {}
+
     // Configure HSI
     let (hsi, hsisys, hsiker) = match config.hsi {
-        None => {
-            RCC.cr().modify(|w| w.set_hsion(false));
-            (None, None, None)
-        }
-        Some(hsi) => {
-            RCC.cr().modify(|w| {
-                w.set_hsidiv(hsi.sys_div);
-                w.set_hsikerdiv(hsi.ker_div);
-                w.set_hsion(true);
-            });
-            while !RCC.cr().read().hsirdy() {}
-            (
-                Some(HSI_FREQ),
-                Some(HSI_FREQ / hsi.sys_div),
-                Some(HSI_FREQ / hsi.ker_div),
-            )
-        }
+        None => (None, None, None),
+        Some(hsi) => (
+            Some(HSI_FREQ),
+            Some(HSI_FREQ / hsi.sys_div),
+            Some(HSI_FREQ / hsi.ker_div),
+        ),
     };
 
     // Configure HSE
@@ -150,6 +154,12 @@ pub(crate) unsafe fn init(config: Config) {
         w.set_hpre(config.ahb_pre);
         w.set_ppre(config.apb1_pre);
     });
+    while RCC.cfgr().read().sws() != config.sys {}
+
+    // Disable HSI if not used
+    if config.hsi.is_none() {
+        RCC.cr().modify(|w| w.set_hsion(false));
+    }
 
     let rtc = config.ls.init();
 
diff --git a/embassy-stm32/src/rcc/f013.rs b/embassy-stm32/src/rcc/f013.rs
index f33351e74..0946287ea 100644
--- a/embassy-stm32/src/rcc/f013.rs
+++ b/embassy-stm32/src/rcc/f013.rs
@@ -135,17 +135,18 @@ impl Default for Config {
 
 /// Initialize and Set the clock frequencies
 pub(crate) unsafe fn init(config: Config) {
+    // Turn on the HSI
+    RCC.cr().modify(|w| w.set_hsion(true));
+    while !RCC.cr().read().hsirdy() {}
+
+    // Use the HSI clock as system clock during the actual clock setup
+    RCC.cfgr().modify(|w| w.set_sw(Sysclk::HSI));
+    while RCC.cfgr().read().sws() != Sysclk::HSI {}
+
     // Configure HSI
     let hsi = match config.hsi {
-        false => {
-            RCC.cr().modify(|w| w.set_hsion(false));
-            None
-        }
-        true => {
-            RCC.cr().modify(|w| w.set_hsion(true));
-            while !RCC.cr().read().hsirdy() {}
-            Some(HSI_FREQ)
-        }
+        false => None,
+        true => Some(HSI_FREQ),
     };
 
     // Configure HSE
@@ -297,6 +298,11 @@ pub(crate) unsafe fn init(config: Config) {
     RCC.cfgr().modify(|w| w.set_sw(config.sys));
     while RCC.cfgr().read().sws() != config.sys {}
 
+    // Disable HSI if not used
+    if !config.hsi {
+        RCC.cr().modify(|w| w.set_hsion(false));
+    }
+
     let rtc = config.ls.init();
 
     // TODO: all this ADC stuff should probably go into the ADC module, not here.
diff --git a/embassy-stm32/src/rcc/g0.rs b/embassy-stm32/src/rcc/g0.rs
index ea4422ccc..c2fa0ca39 100644
--- a/embassy-stm32/src/rcc/g0.rs
+++ b/embassy-stm32/src/rcc/g0.rs
@@ -116,17 +116,18 @@ pub struct PllFreq {
 }
 
 pub(crate) unsafe fn init(config: Config) {
+    // Turn on the HSI
+    RCC.cr().modify(|w| w.set_hsion(true));
+    while !RCC.cr().read().hsirdy() {}
+
+    // Use the HSI clock as system clock during the actual clock setup
+    RCC.cfgr().modify(|w| w.set_sw(Sysclk::HSI));
+    while RCC.cfgr().read().sws() != Sysclk::HSI {}
+
     // Configure HSI
     let hsi = match config.hsi {
-        false => {
-            RCC.cr().modify(|w| w.set_hsion(false));
-            None
-        }
-        true => {
-            RCC.cr().modify(|w| w.set_hsion(true));
-            while !RCC.cr().read().hsirdy() {}
-            Some(HSI_FREQ)
-        }
+        false => None,
+        true => Some(HSI_FREQ),
     };
 
     // Configure HSE
@@ -259,6 +260,12 @@ pub(crate) unsafe fn init(config: Config) {
         w.set_hpre(config.ahb_pre);
         w.set_ppre(config.apb1_pre);
     });
+    while RCC.cfgr().read().sws() != config.sys {}
+
+    // Disable HSI if not used
+    if !config.hsi {
+        RCC.cr().modify(|w| w.set_hsion(false));
+    }
 
     if config.low_power_run {
         assert!(sys <= Hertz(2_000_000));
diff --git a/embassy-stm32/src/rcc/g4.rs b/embassy-stm32/src/rcc/g4.rs
index cd2d2a8a2..c261c0fed 100644
--- a/embassy-stm32/src/rcc/g4.rs
+++ b/embassy-stm32/src/rcc/g4.rs
@@ -117,17 +117,18 @@ pub struct PllFreq {
 }
 
 pub(crate) unsafe fn init(config: Config) {
+    // Turn on the HSI
+    RCC.cr().modify(|w| w.set_hsion(true));
+    while !RCC.cr().read().hsirdy() {}
+
+    // Use the HSI clock as system clock during the actual clock setup
+    RCC.cfgr().modify(|w| w.set_sw(Sysclk::HSI));
+    while RCC.cfgr().read().sws() != Sysclk::HSI {}
+
     // Configure HSI
     let hsi = match config.hsi {
-        false => {
-            RCC.cr().modify(|w| w.set_hsion(false));
-            None
-        }
-        true => {
-            RCC.cr().modify(|w| w.set_hsion(true));
-            while !RCC.cr().read().hsirdy() {}
-            Some(HSI_FREQ)
-        }
+        false => None,
+        true => Some(HSI_FREQ),
     };
 
     // Configure HSE
@@ -285,6 +286,12 @@ pub(crate) unsafe fn init(config: Config) {
         w.set_ppre1(config.apb1_pre);
         w.set_ppre2(config.apb2_pre);
     });
+    while RCC.cfgr().read().sws() != config.sys {}
+
+    // Disable HSI if not used
+    if !config.hsi {
+        RCC.cr().modify(|w| w.set_hsion(false));
+    }
 
     if config.low_power_run {
         assert!(sys <= Hertz(2_000_000));
diff --git a/embassy-stm32/src/rcc/h.rs b/embassy-stm32/src/rcc/h.rs
index 4d7004872..e3c7dd158 100644
--- a/embassy-stm32/src/rcc/h.rs
+++ b/embassy-stm32/src/rcc/h.rs
@@ -402,20 +402,24 @@ pub(crate) unsafe fn init(config: Config) {
         }
     }
 
+    // Turn on the HSI
+    match config.hsi {
+        None => RCC.cr().modify(|w| w.set_hsion(true)),
+        Some(hsidiv) => RCC.cr().modify(|w| {
+            w.set_hsidiv(hsidiv);
+            w.set_hsion(true);
+        }),
+    }
+    while !RCC.cr().read().hsirdy() {}
+
+    // Use the HSI clock as system clock during the actual clock setup
+    RCC.cfgr().modify(|w| w.set_sw(Sysclk::HSI));
+    while RCC.cfgr().read().sws() != Sysclk::HSI {}
+
     // Configure HSI
     let hsi = match config.hsi {
-        None => {
-            RCC.cr().modify(|w| w.set_hsion(false));
-            None
-        }
-        Some(hsidiv) => {
-            RCC.cr().modify(|w| {
-                w.set_hsidiv(hsidiv);
-                w.set_hsion(true);
-            });
-            while !RCC.cr().read().hsirdy() {}
-            Some(HSI_FREQ / hsidiv)
-        }
+        None => None,
+        Some(hsidiv) => Some(HSI_FREQ / hsidiv),
     };
 
     // Configure HSE
@@ -608,6 +612,11 @@ pub(crate) unsafe fn init(config: Config) {
     RCC.cfgr().modify(|w| w.set_sw(config.sys));
     while RCC.cfgr().read().sws() != config.sys {}
 
+    // Disable HSI if not used
+    if config.hsi.is_none() {
+        RCC.cr().modify(|w| w.set_hsion(false));
+    }
+
     // IO compensation cell - Requires CSI clock and SYSCFG
     #[cfg(any(stm32h7))] // TODO h5, h7rs
     if csi.is_some() {