stm32/rcc: misc cleanups.

5 files changed, 119 insertions, 187 deletions

diff --git a/embassy-stm32/src/rcc/f4f7.rs b/embassy-stm32/src/rcc/f4f7.rs
index 3f9a2be67..2e4f95722 100644
--- a/embassy-stm32/src/rcc/f4f7.rs
+++ b/embassy-stm32/src/rcc/f4f7.rs
@@ -166,8 +166,8 @@ pub(crate) unsafe fn init(config: Config) {
     };
 
     let hclk = sys / config.ahb_pre;
-    let (pclk1, pclk1_tim) = calc_pclk(hclk, config.apb1_pre);
-    let (pclk2, pclk2_tim) = calc_pclk(hclk, config.apb2_pre);
+    let (pclk1, pclk1_tim) = super::util::calc_pclk(hclk, config.apb1_pre);
+    let (pclk2, pclk2_tim) = super::util::calc_pclk(hclk, config.apb2_pre);
 
     assert!(max::SYSCLK.contains(&sys));
     assert!(max::HCLK.contains(&hclk));
@@ -326,15 +326,6 @@ fn flash_setup(clk: Hertz) {
     while FLASH.acr().read().latency() != latency {}
 }
 
-fn calc_pclk<D>(hclk: Hertz, ppre: D) -> (Hertz, Hertz)
-where
-    Hertz: core::ops::Div<D, Output = Hertz>,
-{
-    let pclk = hclk / ppre;
-    let pclk_tim = if hclk == pclk { pclk } else { pclk * 2u32 };
-    (pclk, pclk_tim)
-}
-
 #[cfg(stm32f7)]
 mod max {
     use core::ops::RangeInclusive;
diff --git a/embassy-stm32/src/rcc/h.rs b/embassy-stm32/src/rcc/h.rs
index 8883763ec..7e924f0ac 100644
--- a/embassy-stm32/src/rcc/h.rs
+++ b/embassy-stm32/src/rcc/h.rs
@@ -6,8 +6,11 @@ use crate::pac::pwr::vals::Vos;
 pub use crate::pac::rcc::vals::Adcdacsel as AdcClockSource;
 #[cfg(stm32h7)]
 pub use crate::pac::rcc::vals::Adcsel as AdcClockSource;
-use crate::pac::rcc::vals::{Ckpersel, Hsidiv, Pllrge, Pllsrc, Pllvcosel, Sw, Timpre};
-pub use crate::pac::rcc::vals::{Ckpersel as PerClockSource, Plldiv as PllDiv, Pllm as PllPreDiv, Plln as PllMul};
+pub use crate::pac::rcc::vals::{
+    Ckpersel as PerClockSource, Hsidiv as HSIPrescaler, Plldiv as PllDiv, Pllm as PllPreDiv, Plln as PllMul,
+    Pllsrc as PllSource, Sw as Sysclk,
+};
+use crate::pac::rcc::vals::{Ckpersel, Pllrge, Pllvcosel, Timpre};
 use crate::pac::{FLASH, PWR, RCC};
 use crate::rcc::{set_freqs, Clocks};
 use crate::time::Hertz;
@@ -58,41 +61,9 @@ pub struct Hse {
     pub mode: HseMode,
 }
 
-#[derive(Clone, Copy, Eq, PartialEq)]
-pub enum Hsi {
-    /// 64Mhz
-    Mhz64,
-    /// 32Mhz (divided by 2)
-    Mhz32,
-    /// 16Mhz (divided by 4)
-    Mhz16,
-    /// 8Mhz (divided by 8)
-    Mhz8,
-}
-
-#[derive(Clone, Copy, Eq, PartialEq)]
-pub enum Sysclk {
-    /// HSI selected as sysclk
-    HSI,
-    /// HSE selected as sysclk
-    HSE,
-    /// CSI selected as sysclk
-    CSI,
-    /// PLL1_P selected as sysclk
-    Pll1P,
-}
-
-#[derive(Clone, Copy, Eq, PartialEq)]
-pub enum PllSource {
-    Hsi,
-    Csi,
-    Hse,
-}
-
 #[derive(Clone, Copy)]
 pub struct Pll {
     /// Source clock selection.
-    #[cfg(stm32h5)]
     pub source: PllSource,
 
     /// PLL pre-divider (DIVM).
@@ -152,15 +123,12 @@ impl From<TimerPrescaler> for Timpre {
 /// Configuration of the core clocks
 #[non_exhaustive]
 pub struct Config {
-    pub hsi: Option<Hsi>,
+    pub hsi: Option<HSIPrescaler>,
     pub hse: Option<Hse>,
     pub csi: bool,
     pub hsi48: bool,
     pub sys: Sysclk,
 
-    #[cfg(stm32h7)]
-    pub pll_src: PllSource,
-
     pub pll1: Option<Pll>,
     pub pll2: Option<Pll>,
     #[cfg(any(rcc_h5, stm32h7))]
@@ -184,13 +152,11 @@ pub struct Config {
 impl Default for Config {
     fn default() -> Self {
         Self {
-            hsi: Some(Hsi::Mhz64),
+            hsi: Some(HSIPrescaler::DIV1),
             hse: None,
             csi: false,
             hsi48: false,
             sys: Sysclk::HSI,
-            #[cfg(stm32h7)]
-            pll_src: PllSource::Hsi,
             pll1: None,
             pll2: None,
             #[cfg(any(rcc_h5, stm32h7))]
@@ -303,19 +269,13 @@ pub(crate) unsafe fn init(config: Config) {
             RCC.cr().modify(|w| w.set_hsion(false));
             None
         }
-        Some(hsi) => {
-            let (freq, hsidiv) = match hsi {
-                Hsi::Mhz64 => (HSI_FREQ / 1u32, Hsidiv::DIV1),
-                Hsi::Mhz32 => (HSI_FREQ / 2u32, Hsidiv::DIV2),
-                Hsi::Mhz16 => (HSI_FREQ / 4u32, Hsidiv::DIV4),
-                Hsi::Mhz8 => (HSI_FREQ / 8u32, Hsidiv::DIV8),
-            };
+        Some(hsidiv) => {
             RCC.cr().modify(|w| {
                 w.set_hsidiv(hsidiv);
                 w.set_hsion(true);
             });
             while !RCC.cr().read().hsirdy() {}
-            Some(freq)
+            Some(HSI_FREQ / hsidiv)
         }
     };
 
@@ -360,25 +320,29 @@ pub(crate) unsafe fn init(config: Config) {
         }
     };
 
+    // H7 has shared PLLSRC, check it's equal in all PLLs.
+    #[cfg(stm32h7)]
+    {
+        let plls = [&config.pll1, &config.pll2, &config.pll3];
+        if !super::util::all_equal(plls.into_iter().flatten().map(|p| p.source)) {
+            panic!("Source must be equal across all enabled PLLs.")
+        };
+    }
+
     // Configure PLLs.
-    let pll_input = PllInput {
-        csi,
-        hse,
-        hsi,
-        #[cfg(stm32h7)]
-        source: config.pll_src,
-    };
+    let pll_input = PllInput { csi, hse, hsi };
     let pll1 = init_pll(0, config.pll1, &pll_input);
     let pll2 = init_pll(1, config.pll2, &pll_input);
     #[cfg(any(rcc_h5, stm32h7))]
     let pll3 = init_pll(2, config.pll3, &pll_input);
 
     // Configure sysclk
-    let (sys, sw) = match config.sys {
-        Sysclk::HSI => (unwrap!(hsi), Sw::HSI),
-        Sysclk::HSE => (unwrap!(hse), Sw::HSE),
-        Sysclk::CSI => (unwrap!(csi), Sw::CSI),
-        Sysclk::Pll1P => (unwrap!(pll1.p), Sw::PLL1_P),
+    let sys = match config.sys {
+        Sysclk::HSI => unwrap!(hsi),
+        Sysclk::HSE => unwrap!(hse),
+        Sysclk::CSI => unwrap!(csi),
+        Sysclk::PLL1_P => unwrap!(pll1.p),
+        _ => unreachable!(),
     };
 
     // Check limits.
@@ -502,8 +466,8 @@ pub(crate) unsafe fn init(config: Config) {
 
     RCC.cfgr().modify(|w| w.set_timpre(config.timer_prescaler.into()));
 
-    RCC.cfgr().modify(|w| w.set_sw(sw));
-    while RCC.cfgr().read().sws() != sw {}
+    RCC.cfgr().modify(|w| w.set_sw(config.sys));
+    while RCC.cfgr().read().sws() != config.sys {}
 
     // IO compensation cell - Requires CSI clock and SYSCFG
     #[cfg(stm32h7)] // TODO h5
@@ -588,8 +552,6 @@ struct PllInput {
     hsi: Option<Hertz>,
     hse: Option<Hertz>,
     csi: Option<Hertz>,
-    #[cfg(stm32h7)]
-    source: PllSource,
 }
 
 struct PllOutput {
@@ -619,15 +581,11 @@ fn init_pll(num: usize, config: Option<Pll>, input: &PllInput) -> PllOutput {
         };
     };
 
-    #[cfg(stm32h5)]
-    let source = config.source;
-    #[cfg(stm32h7)]
-    let source = input.source;
-
-    let (in_clk, src) = match source {
-        PllSource::Hsi => (unwrap!(input.hsi), Pllsrc::HSI),
-        PllSource::Hse => (unwrap!(input.hse), Pllsrc::HSE),
-        PllSource::Csi => (unwrap!(input.csi), Pllsrc::CSI),
+    let in_clk = match config.source {
+        PllSource::DISABLE => panic!("must not set PllSource::Disable"),
+        PllSource::HSI => unwrap!(input.hsi),
+        PllSource::HSE => unwrap!(input.hse),
+        PllSource::CSI => unwrap!(input.csi),
     };
 
     let ref_clk = in_clk / config.prediv as u32;
@@ -667,7 +625,7 @@ fn init_pll(num: usize, config: Option<Pll>, input: &PllInput) -> PllOutput {
 
     #[cfg(stm32h5)]
     RCC.pllcfgr(num).write(|w| {
-        w.set_pllsrc(src);
+        w.set_pllsrc(config.source);
         w.set_divm(config.prediv);
         w.set_pllvcosel(vco_range);
         w.set_pllrge(ref_range);
@@ -681,7 +639,7 @@ fn init_pll(num: usize, config: Option<Pll>, input: &PllInput) -> PllOutput {
     {
         RCC.pllckselr().modify(|w| {
             w.set_divm(num, config.prediv);
-            w.set_pllsrc(src);
+            w.set_pllsrc(config.source);
         });
         RCC.pllcfgr().modify(|w| {
             w.set_pllvcosel(num, vco_range);
diff --git a/embassy-stm32/src/rcc/l0l1.rs b/embassy-stm32/src/rcc/l0l1.rs
index 52e9ccb3c..9fb2062d6 100644
--- a/embassy-stm32/src/rcc/l0l1.rs
+++ b/embassy-stm32/src/rcc/l0l1.rs
@@ -156,23 +156,9 @@ pub(crate) unsafe fn init(config: Config) {
         w.set_ppre2(config.apb2_pre);
     });
 
-    let ahb_freq = sys_clk / config.ahb_pre;
-
-    let (apb1_freq, apb1_tim_freq) = match config.apb1_pre {
-        APBPrescaler::DIV1 => (ahb_freq, ahb_freq),
-        pre => {
-            let freq = ahb_freq / pre;
-            (freq, freq * 2u32)
-        }
-    };
-
-    let (apb2_freq, apb2_tim_freq) = match config.apb2_pre {
-        APBPrescaler::DIV1 => (ahb_freq, ahb_freq),
-        pre => {
-            let freq = ahb_freq / pre;
-            (freq, freq * 2u32)
-        }
-    };
+    let hclk1 = sys_clk / config.ahb_pre;
+    let (pclk1, pclk1_tim) = super::util::calc_pclk(hclk1, config.apb1_pre);
+    let (pclk2, pclk2_tim) = super::util::calc_pclk(hclk1, config.apb2_pre);
 
     #[cfg(crs)]
     if config.enable_hsi48 {
@@ -209,11 +195,11 @@ pub(crate) unsafe fn init(config: Config) {
 
     set_freqs(Clocks {
         sys: sys_clk,
-        hclk1: ahb_freq,
-        pclk1: apb1_freq,
-        pclk2: apb2_freq,
-        pclk1_tim: apb1_tim_freq,
-        pclk2_tim: apb2_tim_freq,
+        hclk1,
+        pclk1,
+        pclk2,
+        pclk1_tim,
+        pclk2_tim,
         rtc,
     });
 }
diff --git a/embassy-stm32/src/rcc/l4l5.rs b/embassy-stm32/src/rcc/l4l5.rs
index 50128d389..2f89f6821 100644
--- a/embassy-stm32/src/rcc/l4l5.rs
+++ b/embassy-stm32/src/rcc/l4l5.rs
@@ -235,7 +235,7 @@ pub(crate) unsafe fn init(config: Config) {
 
     // L4 has shared PLLSRC, PLLM, check it's equal in all PLLs.
     #[cfg(all(stm32l4, not(rcc_l4plus)))]
-    match get_equal(_plls.into_iter().flatten().map(|p| (p.source, p.prediv))) {
+    match super::util::get_equal(_plls.into_iter().flatten().map(|p| (p.source, p.prediv))) {
         Err(()) => panic!("Source must be equal across all enabled PLLs."),
         Ok(None) => {}
         Ok(Some((source, prediv))) => RCC.pllcfgr().write(|w| {
@@ -246,7 +246,7 @@ pub(crate) unsafe fn init(config: Config) {
 
     // L4+, WL has shared PLLSRC, check it's equal in all PLLs.
     #[cfg(any(rcc_l4plus, stm32wl))]
-    match get_equal(_plls.into_iter().flatten().map(|p| p.source)) {
+    match super::util::get_equal(_plls.into_iter().flatten().map(|p| p.source)) {
         Err(()) => panic!("Source must be equal across all enabled PLLs."),
         Ok(None) => {}
         Ok(Some(source)) => RCC.pllcfgr().write(|w| {
@@ -265,7 +265,7 @@ pub(crate) unsafe fn init(config: Config) {
         ClockSrc::HSE => hse.unwrap(),
         ClockSrc::HSI => hsi.unwrap(),
         ClockSrc::MSI => msi.unwrap(),
-        ClockSrc::PLL1_R => pll._r.unwrap(),
+        ClockSrc::PLL1_R => pll.r.unwrap(),
     };
 
     #[cfg(stm32l4)]
@@ -276,8 +276,8 @@ pub(crate) unsafe fn init(config: Config) {
     let _clk48 = match config.clk48_src {
         Clk48Src::HSI48 => hsi48,
         Clk48Src::MSI => msi,
-        Clk48Src::PLLSAI1_Q => pllsai1._q,
-        Clk48Src::PLL1_Q => pll._q,
+        Clk48Src::PLLSAI1_Q => pllsai1.q,
+        Clk48Src::PLL1_Q => pll.q,
     };
 
     #[cfg(rcc_l4plus)]
@@ -285,32 +285,21 @@ pub(crate) unsafe fn init(config: Config) {
     #[cfg(all(stm32l4, not(rcc_l4plus)))]
     assert!(sys_clk.0 <= 80_000_000);
 
-    let ahb_freq = sys_clk / config.ahb_pre;
-
-    let (apb1_freq, apb1_tim_freq) = match config.apb1_pre {
-        APBPrescaler::DIV1 => (ahb_freq, ahb_freq),
-        pre => {
-            let freq = ahb_freq / pre;
-            (freq, freq * 2u32)
-        }
-    };
-
-    let (apb2_freq, apb2_tim_freq) = match config.apb2_pre {
-        APBPrescaler::DIV1 => (ahb_freq, ahb_freq),
-        pre => {
-            let freq = ahb_freq / pre;
-            (freq, freq * 2u32)
-        }
-    };
-
+    let hclk1 = sys_clk / config.ahb_pre;
+    let (pclk1, pclk1_tim) = super::util::calc_pclk(hclk1, config.apb1_pre);
+    let (pclk2, pclk2_tim) = super::util::calc_pclk(hclk1, config.apb2_pre);
+    #[cfg(not(any(stm32wl5x, stm32wb)))]
+    let hclk2 = hclk1;
     #[cfg(any(stm32wl5x, stm32wb))]
-    let _ahb2_freq = sys_clk / config.core2_ahb_pre;
+    let hclk2 = sys_clk / config.core2_ahb_pre;
+    #[cfg(not(any(stm32wl, stm32wb)))]
+    let hclk3 = hclk1;
     #[cfg(any(stm32wl, stm32wb))]
-    let ahb3_freq = sys_clk / config.shared_ahb_pre;
+    let hclk3 = sys_clk / config.shared_ahb_pre;
 
     // Set flash wait states
     #[cfg(stm32l4)]
-    let latency = match sys_clk.0 {
+    let latency = match hclk1.0 {
         0..=16_000_000 => 0,
         0..=32_000_000 => 1,
         0..=48_000_000 => 2,
@@ -318,7 +307,7 @@ pub(crate) unsafe fn init(config: Config) {
         _ => 4,
     };
     #[cfg(stm32l5)]
-    let latency = match sys_clk.0 {
+    let latency = match hclk1.0 {
         // VCORE Range 0 (performance), others TODO
         0..=20_000_000 => 0,
         0..=40_000_000 => 1,
@@ -328,14 +317,14 @@ pub(crate) unsafe fn init(config: Config) {
         _ => 5,
     };
     #[cfg(stm32wl)]
-    let latency = match ahb3_freq.0 {
+    let latency = match hclk3.0 {
         // VOS RANGE1, others TODO.
         ..=18_000_000 => 0,
         ..=36_000_000 => 1,
         _ => 2,
     };
     #[cfg(stm32wb)]
-    let latency = match ahb3_freq.0 {
+    let latency = match hclk3.0 {
         // VOS RANGE1, others TODO.
         ..=18_000_000 => 0,
         ..=36_000_000 => 1,
@@ -369,18 +358,15 @@ pub(crate) unsafe fn init(config: Config) {
 
     set_freqs(Clocks {
         sys: sys_clk,
-        hclk1: ahb_freq,
-        hclk2: ahb_freq,
-        #[cfg(not(stm32wl))]
-        hclk3: ahb_freq,
-        pclk1: apb1_freq,
-        pclk2: apb2_freq,
-        pclk1_tim: apb1_tim_freq,
-        pclk2_tim: apb2_tim_freq,
-        #[cfg(stm32wl)]
-        hclk3: ahb3_freq,
+        hclk1,
+        hclk2,
+        hclk3,
+        pclk1,
+        pclk2,
+        pclk1_tim,
+        pclk2_tim,
         #[cfg(stm32wl)]
-        pclk3: ahb3_freq,
+        pclk3: hclk3,
         #[cfg(rcc_l4)]
         hsi: None,
         #[cfg(rcc_l4)]
@@ -419,26 +405,18 @@ fn msirange_to_hertz(range: MSIRange) -> Hertz {
     }
 }
 
-#[allow(unused)]
-fn get_equal<T: Eq>(mut iter: impl Iterator<Item = T>) -> Result<Option<T>, ()> {
-    let Some(x) = iter.next() else { return Ok(None) };
-    if !iter.all(|y| y == x) {
-        return Err(());
-    }
-    return Ok(Some(x));
-}
-
 struct PllInput {
     hsi: Option<Hertz>,
     hse: Option<Hertz>,
     msi: Option<Hertz>,
 }
 
+#[allow(unused)]
 #[derive(Default)]
 struct PllOutput {
-    _p: Option<Hertz>,
-    _q: Option<Hertz>,
-    _r: Option<Hertz>,
+    p: Option<Hertz>,
+    q: Option<Hertz>,
+    r: Option<Hertz>,
 }
 
 #[derive(PartialEq, Eq, Clone, Copy)]
@@ -450,29 +428,33 @@ enum PllInstance {
     Pllsai2,
 }
 
-fn init_pll(instance: PllInstance, config: Option<Pll>, input: &PllInput) -> PllOutput {
-    // Disable PLL
+fn pll_enable(instance: PllInstance, enabled: bool) {
     match instance {
         PllInstance::Pll => {
-            RCC.cr().modify(|w| w.set_pllon(false));
-            while RCC.cr().read().pllrdy() {}
+            RCC.cr().modify(|w| w.set_pllon(enabled));
+            while RCC.cr().read().pllrdy() != enabled {}
         }
         #[cfg(any(stm32l4, stm32l5, stm32wb))]
         PllInstance::Pllsai1 => {
-            RCC.cr().modify(|w| w.set_pllsai1on(false));
-            while RCC.cr().read().pllsai1rdy() {}
+            RCC.cr().modify(|w| w.set_pllsai1on(enabled));
+            while RCC.cr().read().pllsai1rdy() != enabled {}
         }
         #[cfg(any(stm32l47x, stm32l48x, stm32l49x, stm32l4ax, rcc_l4plus, stm32l5))]
         PllInstance::Pllsai2 => {
-            RCC.cr().modify(|w| w.set_pllsai2on(false));
-            while RCC.cr().read().pllsai2rdy() {}
+            RCC.cr().modify(|w| w.set_pllsai2on(enabled));
+            while RCC.cr().read().pllsai2rdy() != enabled {}
         }
     }
+}
+
+fn init_pll(instance: PllInstance, config: Option<Pll>, input: &PllInput) -> PllOutput {
+    // Disable PLL
+    pll_enable(instance, false);
 
     let Some(pll) = config else { return PllOutput::default() };
 
     let pll_src = match pll.source {
-        PLLSource::DISABLE => panic!("must not select PLL source as NONE"),
+        PLLSource::DISABLE => panic!("must not select PLL source as DISABLE"),
         PLLSource::HSE => input.hse,
         PLLSource::HSI => input.hsi,
         PLLSource::MSI => input.msi,
@@ -535,22 +517,7 @@ fn init_pll(instance: PllInstance, config: Option<Pll>, input: &PllInput) -> Pll
     }
 
     // Enable PLL
-    match instance {
-        PllInstance::Pll => {
-            RCC.cr().modify(|w| w.set_pllon(true));
-            while !RCC.cr().read().pllrdy() {}
-        }
-        #[cfg(any(stm32l4, stm32l5, stm32wb))]
-        PllInstance::Pllsai1 => {
-            RCC.cr().modify(|w| w.set_pllsai1on(true));
-            while !RCC.cr().read().pllsai1rdy() {}
-        }
-        #[cfg(any(stm32l47x, stm32l48x, stm32l49x, stm32l4ax, rcc_l4plus, stm32l5))]
-        PllInstance::Pllsai2 => {
-            RCC.cr().modify(|w| w.set_pllsai2on(true));
-            while !RCC.cr().read().pllsai2rdy() {}
-        }
-    }
+    pll_enable(instance, true);
 
-    PllOutput { _p: p, _q: q, _r: r }
+    PllOutput { p, q, r }
 }
diff --git a/embassy-stm32/src/rcc/mod.rs b/embassy-stm32/src/rcc/mod.rs
index 4f3d5b98b..8cf2d6ab0 100644
--- a/embassy-stm32/src/rcc/mod.rs
+++ b/embassy-stm32/src/rcc/mod.rs
@@ -246,3 +246,33 @@ pub(crate) mod sealed {
 }
 
 pub trait RccPeripheral: sealed::RccPeripheral + 'static {}
+
+#[allow(unused)]
+mod util {
+    use crate::time::Hertz;
+
+    pub fn calc_pclk<D>(hclk: Hertz, ppre: D) -> (Hertz, Hertz)
+    where
+        Hertz: core::ops::Div<D, Output = Hertz>,
+    {
+        let pclk = hclk / ppre;
+        let pclk_tim = if hclk == pclk { pclk } else { pclk * 2u32 };
+        (pclk, pclk_tim)
+    }
+
+    pub fn all_equal<T: Eq>(mut iter: impl Iterator<Item = T>) -> bool {
+        let Some(x) = iter.next() else { return true };
+        if !iter.all(|y| y == x) {
+            return false;
+        }
+        true
+    }
+
+    pub fn get_equal<T: Eq>(mut iter: impl Iterator<Item = T>) -> Result<Option<T>, ()> {
+        let Some(x) = iter.next() else { return Ok(None) };
+        if !iter.all(|y| y == x) {
+            return Err(());
+        }
+        Ok(Some(x))
+    }
+}