Add PLL config support for F2

2 files changed, 227 insertions, 21 deletions

diff --git a/embassy-stm32/src/rcc/f2.rs b/embassy-stm32/src/rcc/f2.rs
index e44613926..7074d7c3a 100644
--- a/embassy-stm32/src/rcc/f2.rs
+++ b/embassy-stm32/src/rcc/f2.rs
@@ -1,7 +1,8 @@
-use core::ops::Div;
+use core::convert::TryFrom;
+use core::ops::{Div, Mul};
 
 use crate::pac::flash::vals::Latency;
-use crate::pac::rcc::vals::{Hpre, Ppre, Sw};
+use crate::pac::rcc::vals::{Hpre, Pllp, Pllsrc, Ppre, Sw};
 use crate::pac::{FLASH, RCC};
 use crate::rcc::{set_freqs, Clocks};
 use crate::time::Hertz;
@@ -20,6 +21,7 @@ pub struct HSEConfig {
 pub enum ClockSrc {
     HSE,
     HSI,
+    PLL,
 }
 
 /// HSE clock source
@@ -31,6 +33,170 @@ pub enum HSESrc {
     Bypass,
 }
 
+#[derive(Clone, Copy)]
+pub struct PLLConfig {
+    pub pre_div: PLLPreDiv,
+    pub mul: PLLMul,
+    pub main_div: PLLMainDiv,
+    pub pll48_div: PLL48Div,
+}
+
+impl Default for PLLConfig {
+    fn default() -> Self {
+        PLLConfig {
+            pre_div: PLLPreDiv(16),
+            mul: PLLMul(192),
+            main_div: PLLMainDiv::Div2,
+            pll48_div: PLL48Div(4),
+        }
+    }
+}
+
+impl PLLConfig {
+    pub fn clocks(&self, src_freq: Hertz) -> PLLClocks {
+        let in_freq = src_freq / self.pre_div;
+        let vco_freq = src_freq * self.mul / self.pre_div;
+        let main_freq = vco_freq / self.main_div;
+        let pll48_freq = vco_freq / self.pll48_div;
+        PLLClocks {
+            in_freq,
+            vco_freq,
+            main_freq,
+            pll48_freq,
+        }
+    }
+}
+
+/// Clock source for both main PLL and PLLI2S
+#[derive(Clone, Copy, PartialEq)]
+pub enum PLLSrc {
+    HSE,
+    HSI,
+}
+
+impl Into<Pllsrc> for PLLSrc {
+    fn into(self) -> Pllsrc {
+        match self {
+            PLLSrc::HSE => Pllsrc::HSE,
+            PLLSrc::HSI => Pllsrc::HSI,
+        }
+    }
+}
+
+/// Division factor for both main PLL and PLLI2S
+#[derive(Clone, Copy, PartialEq)]
+#[repr(transparent)]
+pub struct PLLPreDiv(u8);
+
+impl TryFrom<u8> for PLLPreDiv {
+    type Error = &'static str;
+
+    fn try_from(value: u8) -> Result<Self, Self::Error> {
+        match value {
+            2..=63 => Ok(PLLPreDiv(value)),
+            _ => Err("PLLPreDiv must be within range 2..=63"),
+        }
+    }
+}
+
+impl Div<PLLPreDiv> for Hertz {
+    type Output = Hertz;
+
+    fn div(self, rhs: PLLPreDiv) -> Self::Output {
+        Hertz(self.0 / u32::from(rhs.0))
+    }
+}
+
+/// Multiplication factor for main PLL
+#[derive(Clone, Copy, PartialEq)]
+#[repr(transparent)]
+pub struct PLLMul(u16);
+
+impl Mul<PLLMul> for Hertz {
+    type Output = Hertz;
+
+    fn mul(self, rhs: PLLMul) -> Self::Output {
+        Hertz(self.0 * u32::from(rhs.0))
+    }
+}
+
+impl TryFrom<u16> for PLLMul {
+    type Error = &'static str;
+
+    fn try_from(value: u16) -> Result<Self, Self::Error> {
+        match value {
+            192..=432 => Ok(PLLMul(value)),
+            _ => Err("PLLMul must be within range 192..=432"),
+        }
+    }
+}
+
+/// PLL division factor for the main system clock
+#[derive(Clone, Copy, PartialEq)]
+pub enum PLLMainDiv {
+    Div2,
+    Div4,
+    Div6,
+    Div8,
+}
+
+impl Into<Pllp> for PLLMainDiv {
+    fn into(self) -> Pllp {
+        match self {
+            PLLMainDiv::Div2 => Pllp::DIV2,
+            PLLMainDiv::Div4 => Pllp::DIV4,
+            PLLMainDiv::Div6 => Pllp::DIV8,
+            PLLMainDiv::Div8 => Pllp::DIV8,
+        }
+    }
+}
+
+impl Div<PLLMainDiv> for Hertz {
+    type Output = Hertz;
+
+    fn div(self, rhs: PLLMainDiv) -> Self::Output {
+        let divisor = match rhs {
+            PLLMainDiv::Div2 => 2,
+            PLLMainDiv::Div4 => 4,
+            PLLMainDiv::Div6 => 6,
+            PLLMainDiv::Div8 => 8,
+        };
+        Hertz(self.0 / divisor)
+    }
+}
+
+/// PLL division factor for USB OTG FS / SDIO / RNG
+#[derive(Clone, Copy, PartialEq)]
+#[repr(transparent)]
+pub struct PLL48Div(u8);
+
+impl Div<PLL48Div> for Hertz {
+    type Output = Hertz;
+
+    fn div(self, rhs: PLL48Div) -> Self::Output {
+        Hertz(self.0 / u32::from(rhs.0))
+    }
+}
+
+impl TryFrom<u8> for PLL48Div {
+    type Error = &'static str;
+
+    fn try_from(value: u8) -> Result<Self, Self::Error> {
+        match value {
+            2..=15 => Ok(PLL48Div(value)),
+            _ => Err("PLL48Div must be within range 2..=15"),
+        }
+    }
+}
+
+#[derive(Clone, Copy, PartialEq)]
+pub struct PLLClocks {
+    pub in_freq: Hertz,
+    pub vco_freq: Hertz,
+    pub main_freq: Hertz,
+    pub pll48_freq: Hertz,
+}
+
 /// AHB prescaler
 #[derive(Clone, Copy, PartialEq)]
 pub enum AHBPrescaler {
@@ -213,6 +379,9 @@ impl VoltageRange {
 /// Clocks configuration
 pub struct Config {
     pub hse: Option<HSEConfig>,
+    pub hsi: bool,
+    pub pll_mux: PLLSrc,
+    pub pll: PLLConfig,
     pub mux: ClockSrc,
     pub voltage: VoltageRange,
     pub ahb_pre: AHBPrescaler,
@@ -225,6 +394,9 @@ impl Default for Config {
     fn default() -> Config {
         Config {
             hse: None,
+            hsi: true,
+            pll_mux: PLLSrc::HSI,
+            pll: PLLConfig::default(),
             voltage: VoltageRange::Min1V8,
             mux: ClockSrc::HSI,
             ahb_pre: AHBPrescaler::NotDivided,
@@ -234,31 +406,53 @@ impl Default for Config {
     }
 }
 
-#[inline]
-unsafe fn enable_hse(source: HSESrc) {
-    RCC.cr().write(|w| {
-        w.set_hsebyp(match source {
-            HSESrc::Bypass => true,
-            HSESrc::Crystal => false,
-        });
-        w.set_hseon(true)
-    });
-    while !RCC.cr().read().hserdy() {}
-}
-
-#[inline]
-unsafe fn enable_hsi() {
+pub(crate) unsafe fn init(config: Config) {
+    // Make sure HSI is enabled
     RCC.cr().write(|w| w.set_hsion(true));
     while !RCC.cr().read().hsirdy() {}
-}
 
-pub(crate) unsafe fn init(config: Config) {
     if let Some(hse_config) = config.hse {
-        enable_hse(hse_config.source);
+        RCC.cr().modify(|w| {
+            w.set_hsebyp(match hse_config.source {
+                HSESrc::Bypass => true,
+                HSESrc::Crystal => false,
+            });
+            w.set_hseon(true)
+        });
+        while !RCC.cr().read().hserdy() {}
     }
+
+    let pll_src_freq = match config.pll_mux {
+        PLLSrc::HSE => {
+            config
+                .hse
+                .expect("HSE must be configured to be used as PLL input")
+                .frequency
+        }
+        PLLSrc::HSI => HSI,
+    };
+
+    // Reference: STM32F215xx/217xx datasheet Table 33. Main PLL characteristics
+    let pll_clocks = config.pll.clocks(pll_src_freq);
+    assert!(Hertz(950_000) <= pll_clocks.in_freq && pll_clocks.in_freq <= Hertz(2_100_000));
+    assert!(Hertz(192_000_000) <= pll_clocks.vco_freq && pll_clocks.vco_freq <= Hertz(432_000_000));
+    assert!(
+        Hertz(24_000_000) <= pll_clocks.main_freq && pll_clocks.main_freq <= Hertz(120_000_000)
+    );
+    // USB actually requires == 48 MHz, but other PLL48 peripherals are fine with <= 48MHz
+    assert!(pll_clocks.pll48_freq <= Hertz(48_000_000));
+
+    RCC.pllcfgr().write(|w| {
+        w.set_pllsrc(config.pll_mux.into());
+        w.set_pllm(config.pll.pre_div.0);
+        w.set_plln(config.pll.mul.0);
+        w.set_pllp(config.pll.main_div.into());
+        w.set_pllq(config.pll.pll48_div.0);
+    });
+
     let (sys_clk, sw) = match config.mux {
         ClockSrc::HSI => {
-            enable_hsi();
+            assert!(config.hsi, "HSI must be enabled to be used as system clock");
             (HSI, Sw::HSI)
         }
         ClockSrc::HSE => {
@@ -267,6 +461,11 @@ pub(crate) unsafe fn init(config: Config) {
                 .expect("HSE must be configured to be used as system clock");
             (hse_config.frequency, Sw::HSE)
         }
+        ClockSrc::PLL => {
+            RCC.cr().modify(|w| w.set_pllon(true));
+            while !RCC.cr().read().pllrdy() {}
+            (pll_clocks.main_freq, Sw::PLL)
+        }
     };
     // RM0033 Figure 9. Clock tree suggests max SYSCLK/HCLK is 168 MHz, but datasheet specifies PLL
     // max output to be 120 MHz, so there's no way to get higher frequencies
@@ -285,6 +484,12 @@ pub(crate) unsafe fn init(config: Config) {
         w.set_ppre1(config.apb1_pre.into());
         w.set_ppre2(config.apb2_pre.into());
     });
+    while RCC.cfgr().read().sws() != sw.0 {}
+
+    // Turn off HSI to save power if we don't need it
+    if !config.hsi {
+        RCC.cr().modify(|w| w.set_hsion(false));
+    }
 
     let (apb1_freq, apb1_tim_freq) = match config.apb1_pre {
         APBPrescaler::NotDivided => (ahb_freq, ahb_freq),
@@ -315,5 +520,6 @@ pub(crate) unsafe fn init(config: Config) {
         apb1_tim: apb1_tim_freq,
         apb2: apb2_freq,
         apb2_tim: apb2_tim_freq,
+        pll48: Some(pll_clocks.pll48_freq),
     });
 }
diff --git a/embassy-stm32/src/rcc/mod.rs b/embassy-stm32/src/rcc/mod.rs
index 9a95836a6..d3710b8c3 100644
--- a/embassy-stm32/src/rcc/mod.rs
+++ b/embassy-stm32/src/rcc/mod.rs
@@ -53,7 +53,7 @@ pub struct Clocks {
     #[cfg(any(rcc_h7, rcc_h7ab))]
     pub ahb4: Hertz,
 
-    #[cfg(any(rcc_f4, rcc_f410, rcc_f7))]
+    #[cfg(any(rcc_f2, rcc_f4, rcc_f410, rcc_f7))]
     pub pll48: Option<Hertz>,
 
     #[cfg(rcc_f1)]