Finish initial H7 RCC support

6 files changed, 424 insertions, 37 deletions

diff --git a/embassy-stm32/src/lib.rs b/embassy-stm32/src/lib.rs
index 8d7b67b74..2fc07cd5e 100644
--- a/embassy-stm32/src/lib.rs
+++ b/embassy-stm32/src/lib.rs
@@ -13,7 +13,8 @@ pub mod fmt;
 pub mod dma;
 pub mod exti;
 pub mod gpio;
-mod rcc;
+pub mod pwr;
+pub mod rcc;
 #[cfg(feature = "_rng")]
 pub mod rng;
 #[cfg(feature = "_sdmmc")]
diff --git a/embassy-stm32/src/pwr/h7.rs b/embassy-stm32/src/pwr/h7.rs
new file mode 100644
index 000000000..939e93eb2
--- /dev/null
+++ b/embassy-stm32/src/pwr/h7.rs
@@ -0,0 +1,71 @@
+use crate::pac::peripherals;
+use crate::pac::{PWR, RCC, SYSCFG};
+
+/// Voltage Scale
+///
+/// Represents the voltage range feeding the CPU core. The maximum core
+/// clock frequency depends on this value.
+#[derive(Copy, Clone, PartialEq)]
+pub enum VoltageScale {
+    /// VOS 0 range VCORE 1.26V - 1.40V
+    Scale0,
+    /// VOS 1 range VCORE 1.15V - 1.26V
+    Scale1,
+    /// VOS 2 range VCORE 1.05V - 1.15V
+    Scale2,
+    /// VOS 3 range VCORE 0.95V - 1.05V
+    Scale3,
+}
+
+/// Power Configuration
+///
+/// Generated when the PWR peripheral is frozen. The existence of this
+/// value indicates that the voltage scaling configuration can no
+/// longer be changed.
+pub struct Power {
+    pub(crate) vos: VoltageScale,
+}
+
+impl Power {
+    pub fn new(_peri: peripherals::PWR, enable_overdrive: bool) -> Self {
+        use crate::pac::rcc::vals::Apb4enrSyscfgen;
+
+        // NOTE(unsafe) we have the PWR singleton
+        unsafe {
+            // NB. The lower bytes of CR3 can only be written once after
+            // POR, and must be written with a valid combination. Refer to
+            // RM0433 Rev 7 6.8.4. This is partially enforced by dropping
+            // `self` at the end of this method, but of course we cannot
+            // know what happened between the previous POR and here.
+            PWR.cr3().modify(|w| {
+                w.set_scuen(true);
+                w.set_ldoen(true);
+                w.set_bypass(false);
+            });
+            // Validate the supply configuration. If you are stuck here, it is
+            // because the voltages on your board do not match those specified
+            // in the D3CR.VOS and CR3.SDLEVEL fields. By default after reset
+            // VOS = Scale 3, so check that the voltage on the VCAP pins =
+            // 1.0V.
+            while !PWR.csr1().read().actvosrdy() {}
+
+            // Go to Scale 1
+            PWR.d3cr().modify(|w| w.set_vos(0b11));
+            while !PWR.d3cr().read().vosrdy() {}
+
+            let vos = if !enable_overdrive {
+                VoltageScale::Scale1
+            } else {
+                critical_section::with(|_| {
+                    RCC.apb4enr()
+                        .modify(|w| w.set_syscfgen(Apb4enrSyscfgen::ENABLED));
+
+                    SYSCFG.pwrcr().modify(|w| w.set_oden(1));
+                });
+                while !PWR.d3cr().read().vosrdy() {}
+                VoltageScale::Scale0
+            };
+            Self { vos }
+        }
+    }
+}
diff --git a/embassy-stm32/src/pwr/mod.rs b/embassy-stm32/src/pwr/mod.rs
new file mode 100644
index 000000000..0bf62ef7c
--- /dev/null
+++ b/embassy-stm32/src/pwr/mod.rs
@@ -0,0 +1,4 @@
+#[cfg(feature = "_stm32h7")]
+mod h7;
+#[cfg(feature = "_stm32h7")]
+pub use h7::*;
diff --git a/embassy-stm32/src/rcc/h7/mod.rs b/embassy-stm32/src/rcc/h7/mod.rs
index b27eff747..8112ae999 100644
--- a/embassy-stm32/src/rcc/h7/mod.rs
+++ b/embassy-stm32/src/rcc/h7/mod.rs
@@ -1,11 +1,12 @@
 use core::marker::PhantomData;
 
 use embassy::util::Unborrow;
-use embassy_extras::unborrow;
 
-use crate::fmt::assert;
+use crate::fmt::{assert, panic};
 use crate::pac::peripherals;
-use crate::pac::RCC;
+use crate::pac::rcc::vals::Timpre;
+use crate::pac::{RCC, SYSCFG};
+use crate::pwr::{Power, VoltageScale};
 use crate::time::Hertz;
 
 mod pll;
@@ -17,6 +18,39 @@ const CSI: Hertz = Hertz(4_000_000);
 const HSI48: Hertz = Hertz(48_000_000);
 const LSI: Hertz = Hertz(32_000);
 
+/// Core clock frequencies
+#[derive(Clone, Copy)]
+pub struct CoreClocks {
+    pub hclk: Hertz,
+    pub pclk1: Hertz,
+    pub pclk2: Hertz,
+    pub pclk3: Hertz,
+    pub pclk4: Hertz,
+    pub ppre1: u8,
+    pub ppre2: u8,
+    pub ppre3: u8,
+    pub ppre4: u8,
+    pub csi_ck: Option<Hertz>,
+    pub hsi_ck: Option<Hertz>,
+    pub hsi48_ck: Option<Hertz>,
+    pub lsi_ck: Option<Hertz>,
+    pub per_ck: Option<Hertz>,
+    pub hse_ck: Option<Hertz>,
+    pub pll1_p_ck: Option<Hertz>,
+    pub pll1_q_ck: Option<Hertz>,
+    pub pll1_r_ck: Option<Hertz>,
+    pub pll2_p_ck: Option<Hertz>,
+    pub pll2_q_ck: Option<Hertz>,
+    pub pll2_r_ck: Option<Hertz>,
+    pub pll3_p_ck: Option<Hertz>,
+    pub pll3_q_ck: Option<Hertz>,
+    pub pll3_r_ck: Option<Hertz>,
+    pub timx_ker_ck: Option<Hertz>,
+    pub timy_ker_ck: Option<Hertz>,
+    pub sys_ck: Hertz,
+    pub c_ck: Hertz,
+}
+
 /// Configuration of the core clocks
 #[non_exhaustive]
 #[derive(Default)]
@@ -34,29 +68,6 @@ pub struct Config {
     pub pll1: PllConfig,
     pub pll2: PllConfig,
     pub pll3: PllConfig,
-    pub vos: VoltageScale,
-}
-
-/// Voltage Scale
-///
-/// Represents the voltage range feeding the CPU core. The maximum core
-/// clock frequency depends on this value.
-#[derive(Copy, Clone, PartialEq)]
-pub enum VoltageScale {
-    /// VOS 0 range VCORE 1.26V - 1.40V
-    Scale0,
-    /// VOS 1 range VCORE 1.15V - 1.26V
-    Scale1,
-    /// VOS 2 range VCORE 1.05V - 1.15V
-    Scale2,
-    /// VOS 3 range VCORE 0.95V - 1.05V
-    Scale3,
-}
-
-impl Default for VoltageScale {
-    fn default() -> Self {
-        Self::Scale1
-    }
 }
 
 pub struct Rcc<'d> {
@@ -91,12 +102,18 @@ impl<'d> Rcc<'d> {
     /// function may also panic if a clock specification can be
     /// achieved, but the mechanism for doing so is not yet
     /// implemented here.
-    pub fn freeze(mut self) {
-        use crate::pac::rcc::vals::{Ckpersel, Hpre, Hsidiv, Hsion, Lsion, Timpre};
+    pub fn freeze(mut self, pwr: &Power) -> CoreClocks {
+        use crate::pac::rcc::vals::{
+            Apb4enrSyscfgen, Ckpersel, D1ppre, D2ppre1, D3ppre, Hpre, Hsebyp, Hsidiv, Hsion, Lsion,
+            Pllsrc, Sw,
+        };
 
         let srcclk = self.config.hse.unwrap_or(HSI); // Available clocks
         let (sys_ck, sys_use_pll1_p) = self.sys_ck_setup(srcclk);
 
+        // Configure traceclk from PLL if needed
+        self.traceclk_setup(sys_use_pll1_p);
+
         // NOTE(unsafe) We have exclusive access to the RCC
         let (pll1_p_ck, pll1_q_ck, pll1_r_ck) =
             unsafe { pll_setup(srcclk.0, &self.config.pll1, 0) };
@@ -137,13 +154,10 @@ impl<'d> Rcc<'d> {
         let d1cpre_div = 1;
         let sys_d1cpre_ck = sys_ck.0 / d1cpre_div;
 
-        // Timer prescaler selection
-        let timpre = Timpre::DEFAULTX2;
-
         // Refer to part datasheet "General operating conditions"
         // table for (rev V). We do not assert checks for earlier
         // revisions which may have lower limits.
-        let (sys_d1cpre_ck_max, rcc_hclk_max, pclk_max) = match self.config.vos {
+        let (sys_d1cpre_ck_max, rcc_hclk_max, pclk_max) = match pwr.vos {
             VoltageScale::Scale0 => (480_000_000, 240_000_000, 120_000_000),
             VoltageScale::Scale1 => (400_000_000, 200_000_000, 100_000_000),
             VoltageScale::Scale2 => (300_000_000, 150_000_000, 75_000_000),
@@ -160,7 +174,7 @@ impl<'d> Rcc<'d> {
 
         // Estimate divisor
         let (hpre_bits, hpre_div) = match (sys_d1cpre_ck + rcc_hclk - 1) / rcc_hclk {
-            0 => unreachable!(),
+            0 => panic!(),
             1 => (Hpre::DIV1, 1),
             2 => (Hpre::DIV2, 2),
             3..=5 => (Hpre::DIV4, 4),
@@ -175,8 +189,246 @@ impl<'d> Rcc<'d> {
         let rcc_hclk = sys_d1cpre_ck / hpre_div;
         assert!(rcc_hclk <= rcc_hclk_max);
         let rcc_aclk = rcc_hclk; // AXI clock is always equal to AHB clock on H7
+                                 // Timer prescaler selection
+        let timpre = Timpre::DEFAULTX2;
+
+        let requested_pclk1 = self
+            .config
+            .pclk1
+            .map(|v| v.0)
+            .unwrap_or_else(|| pclk_max.min(rcc_hclk / 2));
+        let (rcc_pclk1, ppre1_bits, ppre1, rcc_timerx_ker_ck) =
+            Self::ppre_calculate(requested_pclk1, rcc_hclk, pclk_max, Some(timpre));
+
+        let requested_pclk2 = self
+            .config
+            .pclk2
+            .map(|v| v.0)
+            .unwrap_or_else(|| pclk_max.min(rcc_hclk / 2));
+        let (rcc_pclk2, ppre2_bits, ppre2, rcc_timery_ker_ck) =
+            Self::ppre_calculate(requested_pclk2, rcc_hclk, pclk_max, Some(timpre));
+
+        let requested_pclk3 = self
+            .config
+            .pclk3
+            .map(|v| v.0)
+            .unwrap_or_else(|| pclk_max.min(rcc_hclk / 2));
+        let (rcc_pclk3, ppre3_bits, ppre3, _) =
+            Self::ppre_calculate(requested_pclk3, rcc_hclk, pclk_max, None);
+
+        let requested_pclk4 = self
+            .config
+            .pclk4
+            .map(|v| v.0)
+            .unwrap_or_else(|| pclk_max.min(rcc_hclk / 2));
+        let (rcc_pclk4, ppre4_bits, ppre4, _) =
+            Self::ppre_calculate(requested_pclk4, rcc_hclk, pclk_max, None);
+
+        Self::flash_setup(rcc_aclk, pwr.vos);
+
+        // Start switching clocks -------------------
+        // NOTE(unsafe) We have the RCC singleton
+        unsafe {
+            // Ensure CSI is on and stable
+            RCC.cr().modify(|w| w.set_csion(Hsion::ON));
+            while !RCC.cr().read().csirdy() {}
+
+            // Ensure HSI48 is on and stable
+            RCC.cr().modify(|w| w.set_hsi48on(Hsion::ON));
+            while RCC.cr().read().hsi48on() == Hsion::OFF {}
+
+            // XXX: support MCO ?
+
+            let hse_ck = match self.config.hse {
+                Some(hse) => {
+                    // Ensure HSE is on and stable
+                    RCC.cr().modify(|w| {
+                        w.set_hseon(Hsion::ON);
+                        w.set_hsebyp(if self.config.bypass_hse {
+                            Hsebyp::BYPASSED
+                        } else {
+                            Hsebyp::NOTBYPASSED
+                        });
+                    });
+                    while !RCC.cr().read().hserdy() {}
+                    Some(hse)
+                }
+                None => None,
+            };
+
+            let pllsrc = if self.config.hse.is_some() {
+                Pllsrc::HSE
+            } else {
+                Pllsrc::HSI
+            };
+            RCC.pllckselr().modify(|w| w.set_pllsrc(pllsrc));
+
+            if pll1_p_ck.is_some() {
+                RCC.cr().modify(|w| w.set_pll1on(Hsion::ON));
+                while !RCC.cr().read().pll1rdy() {}
+            }
+
+            if pll2_p_ck.is_some() {
+                RCC.cr().modify(|w| w.set_pll2on(Hsion::ON));
+                while !RCC.cr().read().pll2rdy() {}
+            }
+
+            if pll3_p_ck.is_some() {
+                RCC.cr().modify(|w| w.set_pll3on(Hsion::ON));
+                while !RCC.cr().read().pll3rdy() {}
+            }
 
-        todo!()
+            // Core Prescaler / AHB Prescaler / APB3 Prescaler
+            RCC.d1cfgr().modify(|w| {
+                w.set_d1cpre(Hpre(d1cpre_bits));
+                w.set_d1ppre(D1ppre(ppre3_bits));
+                w.set_hpre(hpre_bits)
+            });
+            // Ensure core prescaler value is valid before future lower
+            // core voltage
+            while RCC.d1cfgr().read().d1cpre().0 != d1cpre_bits {}
+
+            // APB1 / APB2 Prescaler
+            RCC.d2cfgr().modify(|w| {
+                w.set_d2ppre1(D2ppre1(ppre1_bits));
+                w.set_d2ppre2(D2ppre1(ppre2_bits));
+            });
+
+            // APB4 Prescaler
+            RCC.d3cfgr().modify(|w| w.set_d3ppre(D3ppre(ppre4_bits)));
+
+            // Peripheral Clock (per_ck)
+            RCC.d1ccipr().modify(|w| w.set_ckpersel(ckpersel));
+
+            // Set timer clocks prescaler setting
+            RCC.cfgr().modify(|w| w.set_timpre(timpre));
+
+            // Select system clock source
+            let sw = match (sys_use_pll1_p, self.config.hse.is_some()) {
+                (true, _) => Sw::PLL1,
+                (false, true) => Sw::HSE,
+                _ => Sw::HSI,
+            };
+            RCC.cfgr().modify(|w| w.set_sw(sw));
+            while RCC.cfgr().read().sws() != sw.0 {}
+
+            // IO compensation cell - Requires CSI clock and SYSCFG
+            assert!(RCC.cr().read().csirdy());
+            RCC.apb4enr()
+                .modify(|w| w.set_syscfgen(Apb4enrSyscfgen::ENABLED));
+
+            // Enable the compensation cell, using back-bias voltage code
+            // provide by the cell.
+            critical_section::with(|_| {
+                SYSCFG.cccsr().modify(|w| {
+                    w.set_en(true);
+                    w.set_cs(false);
+                    w.set_hslv(false);
+                })
+            });
+            while !SYSCFG.cccsr().read().ready() {}
+
+            CoreClocks {
+                hclk: Hertz(rcc_hclk),
+                pclk1: Hertz(rcc_pclk1),
+                pclk2: Hertz(rcc_pclk2),
+                pclk3: Hertz(rcc_pclk3),
+                pclk4: Hertz(rcc_pclk4),
+                ppre1,
+                ppre2,
+                ppre3,
+                ppre4,
+                csi_ck: Some(CSI),
+                hsi_ck: Some(HSI),
+                hsi48_ck: Some(HSI48),
+                lsi_ck: Some(LSI),
+                per_ck: Some(per_ck),
+                hse_ck,
+                pll1_p_ck: pll1_p_ck.map(Hertz),
+                pll1_q_ck: pll1_q_ck.map(Hertz),
+                pll1_r_ck: pll1_r_ck.map(Hertz),
+                pll2_p_ck: pll2_p_ck.map(Hertz),
+                pll2_q_ck: pll2_q_ck.map(Hertz),
+                pll2_r_ck: pll2_r_ck.map(Hertz),
+                pll3_p_ck: pll3_p_ck.map(Hertz),
+                pll3_q_ck: pll3_q_ck.map(Hertz),
+                pll3_r_ck: pll3_r_ck.map(Hertz),
+                timx_ker_ck: rcc_timerx_ker_ck.map(Hertz),
+                timy_ker_ck: rcc_timery_ker_ck.map(Hertz),
+                sys_ck,
+                c_ck: Hertz(sys_d1cpre_ck),
+            }
+        }
+    }
+
+    /// Enables debugging during WFI/WFE
+    ///
+    /// Set `enable_dma1` to true if you do not have at least one bus master (other than the CPU)
+    /// enable during WFI/WFE
+    pub fn enable_debug_wfe(&mut self, enable_dma1: bool) {
+        use crate::pac::rcc::vals::Ahb1enrDma1en;
+
+        // NOTE(unsafe) We have exclusive access to the RCC
+        unsafe {
+            if enable_dma1 {
+                RCC.ahb1enr()
+                    .modify(|w| w.set_dma1en(Ahb1enrDma1en::ENABLED));
+            }
+        }
+    }
+
+    /// Setup traceclk
+    /// Returns a pll1_r_ck
+    fn traceclk_setup(&mut self, sys_use_pll1_p: bool) {
+        let pll1_r_ck = match (sys_use_pll1_p, self.config.pll1.r_ck) {
+            // pll1_p_ck selected as system clock but pll1_r_ck not
+            // set. The traceclk mux is synchronous with the system
+            // clock mux, but has pll1_r_ck as an input. In order to
+            // keep traceclk running, we force a pll1_r_ck.
+            (true, None) => Some(Hertz(self.config.pll1.p_ck.unwrap().0 / 2)),
+
+            // Either pll1 not selected as system clock, free choice
+            // of pll1_r_ck. Or pll1 is selected, assume user has set
+            // a suitable pll1_r_ck frequency.
+            _ => self.config.pll1.r_ck,
+        };
+        self.config.pll1.r_ck = pll1_r_ck;
+    }
+
+    /// Divider calculator for pclk 1 - 4
+    ///
+    /// Returns real pclk, bits, ppre and the timer kernel clock
+    fn ppre_calculate(
+        requested_pclk: u32,
+        hclk: u32,
+        max_pclk: u32,
+        tim_pre: Option<Timpre>,
+    ) -> (u32, u8, u8, Option<u32>) {
+        let (bits, ppre) = match (hclk + requested_pclk - 1) / requested_pclk {
+            0 => panic!(),
+            1 => (0b000, 1),
+            2 => (0b100, 2),
+            3..=5 => (0b101, 4),
+            6..=11 => (0b110, 8),
+            _ => (0b111, 16),
+        };
+        let real_pclk = hclk / u32::from(ppre);
+        assert!(real_pclk < max_pclk);
+
+        let tim_ker_clk = if let Some(tim_pre) = tim_pre {
+            let clk = match (bits, tim_pre) {
+                (0b101, Timpre::DEFAULTX2) => hclk / 2,
+                (0b110, Timpre::DEFAULTX4) => hclk / 2,
+                (0b110, Timpre::DEFAULTX2) => hclk / 4,
+                (0b111, Timpre::DEFAULTX4) => hclk / 4,
+                (0b111, Timpre::DEFAULTX2) => hclk / 8,
+                _ => hclk,
+            };
+            Some(clk)
+        } else {
+            None
+        };
+        (real_pclk, bits, ppre, tim_ker_clk)
     }
 
     /// Setup sys_ck
@@ -209,4 +461,64 @@ impl<'d> Rcc<'d> {
             (sys_ck, false)
         }
     }
+
+    fn flash_setup(rcc_aclk: u32, vos: VoltageScale) {
+        use crate::pac::FLASH;
+
+        // ACLK in MHz, round down and subtract 1 from integers. eg.
+        // 61_999_999 -> 61MHz
+        // 62_000_000 -> 61MHz
+        // 62_000_001 -> 62MHz
+        let rcc_aclk_mhz = (rcc_aclk - 1) / 1_000_000;
+
+        // See RM0433 Rev 7 Table 17. FLASH recommended number of wait
+        // states and programming delay
+        let (wait_states, progr_delay) = match vos {
+            // VOS 0 range VCORE 1.26V - 1.40V
+            VoltageScale::Scale0 => match rcc_aclk_mhz {
+                0..=69 => (0, 0),
+                70..=139 => (1, 1),
+                140..=184 => (2, 1),
+                185..=209 => (2, 2),
+                210..=224 => (3, 2),
+                225..=239 => (4, 2),
+                _ => (7, 3),
+            },
+            // VOS 1 range VCORE 1.15V - 1.26V
+            VoltageScale::Scale1 => match rcc_aclk_mhz {
+                0..=69 => (0, 0),
+                70..=139 => (1, 1),
+                140..=184 => (2, 1),
+                185..=209 => (2, 2),
+                210..=224 => (3, 2),
+                _ => (7, 3),
+            },
+            // VOS 2 range VCORE 1.05V - 1.15V
+            VoltageScale::Scale2 => match rcc_aclk_mhz {
+                0..=54 => (0, 0),
+                55..=109 => (1, 1),
+                110..=164 => (2, 1),
+                165..=224 => (3, 2),
+                _ => (7, 3),
+            },
+            // VOS 3 range VCORE 0.95V - 1.05V
+            VoltageScale::Scale3 => match rcc_aclk_mhz {
+                0..=44 => (0, 0),
+                45..=89 => (1, 1),
+                90..=134 => (2, 1),
+                135..=179 => (3, 2),
+                180..=224 => (4, 2),
+                _ => (7, 3),
+            },
+        };
+
+        // NOTE(unsafe) Atomic write
+        unsafe {
+            FLASH.acr().write(|w| {
+                w.set_wrhighfreq(progr_delay);
+                w.set_latency(wait_states)
+            });
+            while FLASH.acr().read().latency() != wait_states {}
+        }
+    }
 }
diff --git a/embassy-stm32/src/rcc/h7/pll.rs b/embassy-stm32/src/rcc/h7/pll.rs
index ead33e81e..af958d093 100644
--- a/embassy-stm32/src/rcc/h7/pll.rs
+++ b/embassy-stm32/src/rcc/h7/pll.rs
@@ -1,4 +1,4 @@
-use super::{Config, Hertz, HSI, RCC};
+use super::{Hertz, RCC};
 use crate::fmt::assert;
 
 const VCO_MIN: u32 = 150_000_000;
diff --git a/embassy-stm32/src/sdmmc/v2.rs b/embassy-stm32/src/sdmmc/v2.rs
index a4da4be8e..d1d485c29 100644
--- a/embassy-stm32/src/sdmmc/v2.rs
+++ b/embassy-stm32/src/sdmmc/v2.rs
@@ -1,7 +1,6 @@
 #![macro_use]
 
 use core::default::Default;
-use core::future::Future;
 use core::marker::PhantomData;
 use core::task::Poll;