5 files changed, 543 insertions, 6 deletions

diff --git a/embassy-stm32/src/pwr/u5.rs b/embassy-stm32/src/pwr/u5.rs
index 8b1378917..a90659d9c 100644
--- a/embassy-stm32/src/pwr/u5.rs
+++ b/embassy-stm32/src/pwr/u5.rs
@@ -1 +1,32 @@
+use crate::peripherals;
 
+/// 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 {
+    // Highest frequency
+    Range1,
+    Range2,
+    Range3,
+    // Lowest power
+    Range4,
+}
+
+/// 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) -> Self {
+        Self {
+            vos: VoltageScale::Range4,
+        }
+    }
+}
diff --git a/embassy-stm32/src/rcc/u5/mod.rs b/embassy-stm32/src/rcc/u5/mod.rs
index dbf5e8dfb..6e68b2205 100644
--- a/embassy-stm32/src/rcc/u5/mod.rs
+++ b/embassy-stm32/src/rcc/u5/mod.rs
@@ -1,8 +1,295 @@
-pub struct Config {}
+use crate::pac;
+use crate::peripherals::{self, RCC};
+use crate::pwr::{Power, VoltageScale};
+use crate::rcc::{set_freqs, Clocks};
+use crate::time::{Hertz, U32Ext};
+use stm32_metapac::rcc::vals::{Hpre, Msirange, Msirgsel, Pllm, Pllsrc, Ppre, Sw};
+
+/// HSI16 speed
+pub const HSI16_FREQ: u32 = 16_000_000;
+
+#[derive(Copy, Clone)]
+pub enum ClockSrc {
+    MSI(MSIRange),
+    HSE(Hertz),
+    HSI16,
+    PLL1R(PllSrc, PllM, PllN, PllClkDiv),
+}
+
+#[derive(Clone, Copy, Debug)]
+pub enum PllSrc {
+    MSI(MSIRange),
+    HSE(Hertz),
+    HSI16,
+}
+
+impl Into<Pllsrc> for PllSrc {
+    fn into(self) -> Pllsrc {
+        match self {
+            PllSrc::MSI(..) => Pllsrc::MSIS,
+            PllSrc::HSE(..) => Pllsrc::HSE,
+            PllSrc::HSI16 => Pllsrc::HSI16,
+        }
+    }
+}
+
+seq_macro::seq!(N in 2..=128 {
+    #[derive(Copy, Clone, Debug)]
+    pub enum PllClkDiv {
+        NotDivided,
+        #(
+            Div#N = (N-1),
+        )*
+    }
+
+    impl PllClkDiv {
+        fn to_div(&self) -> u8 {
+            match self {
+                PllClkDiv::NotDivided => 1,
+                #(
+                    PllClkDiv::Div#N => (N + 1),
+                )*
+            }
+        }
+    }
+});
+
+impl Into<u8> for PllClkDiv {
+    fn into(self) -> u8 {
+        (self as u8) + 1
+    }
+}
+
+seq_macro::seq!(N in 4..=512 {
+    #[derive(Copy, Clone, Debug)]
+    pub enum PllN {
+        NotMultiplied,
+        #(
+            Mul#N = (N-1),
+        )*
+    }
+
+    impl PllN {
+        fn to_mul(&self) -> u16 {
+            match self {
+                PllN::NotMultiplied => 1,
+                #(
+                    PllN::Mul#N => (N + 1),
+                )*
+            }
+        }
+    }
+});
+
+impl Into<u16> for PllN {
+    fn into(self) -> u16 {
+        (self as u16) + 1
+    }
+}
+
+// Pre-division
+#[derive(Copy, Clone, Debug)]
+pub enum PllM {
+    NotDivided = 0b0000,
+    Div2 = 0b0001,
+    Div3 = 0b0010,
+    Div4 = 0b0011,
+    Div5 = 0b0100,
+    Div6 = 0b0101,
+    Div7 = 0b0110,
+    Div8 = 0b0111,
+    Div9 = 0b1000,
+    Div10 = 0b1001,
+    Div11 = 0b1010,
+    Div12 = 0b1011,
+    Div13 = 0b1100,
+    Div14 = 0b1101,
+    Div15 = 0b1110,
+    Div16 = 0b1111,
+}
+
+impl Into<Pllm> for PllM {
+    fn into(self) -> Pllm {
+        Pllm(self as u8)
+    }
+}
+
+/// AHB prescaler
+#[derive(Clone, Copy, PartialEq)]
+pub enum AHBPrescaler {
+    NotDivided,
+    Div2,
+    Div4,
+    Div8,
+    Div16,
+    Div64,
+    Div128,
+    Div256,
+    Div512,
+}
+
+impl Into<Hpre> for AHBPrescaler {
+    fn into(self) -> Hpre {
+        match self {
+            AHBPrescaler::NotDivided => Hpre::NONE,
+            AHBPrescaler::Div2 => Hpre::DIV2,
+            AHBPrescaler::Div4 => Hpre::DIV4,
+            AHBPrescaler::Div8 => Hpre::DIV8,
+            AHBPrescaler::Div16 => Hpre::DIV16,
+            AHBPrescaler::Div64 => Hpre::DIV64,
+            AHBPrescaler::Div128 => Hpre::DIV128,
+            AHBPrescaler::Div256 => Hpre::DIV256,
+            AHBPrescaler::Div512 => Hpre::DIV512,
+        }
+    }
+}
+
+impl Into<u8> for AHBPrescaler {
+    fn into(self) -> u8 {
+        match self {
+            AHBPrescaler::NotDivided => 1,
+            AHBPrescaler::Div2 => 0x08,
+            AHBPrescaler::Div4 => 0x09,
+            AHBPrescaler::Div8 => 0x0a,
+            AHBPrescaler::Div16 => 0x0b,
+            AHBPrescaler::Div64 => 0x0c,
+            AHBPrescaler::Div128 => 0x0d,
+            AHBPrescaler::Div256 => 0x0e,
+            AHBPrescaler::Div512 => 0x0f,
+        }
+    }
+}
+
+impl Default for AHBPrescaler {
+    fn default() -> Self {
+        AHBPrescaler::NotDivided
+    }
+}
+
+/// APB prescaler
+#[derive(Clone, Copy)]
+pub enum APBPrescaler {
+    NotDivided,
+    Div2,
+    Div4,
+    Div8,
+    Div16,
+}
+
+impl Into<Ppre> for APBPrescaler {
+    fn into(self) -> Ppre {
+        match self {
+            APBPrescaler::NotDivided => Ppre::NONE,
+            APBPrescaler::Div2 => Ppre::DIV2,
+            APBPrescaler::Div4 => Ppre::DIV4,
+            APBPrescaler::Div8 => Ppre::DIV8,
+            APBPrescaler::Div16 => Ppre::DIV16,
+        }
+    }
+}
+
+impl Default for APBPrescaler {
+    fn default() -> Self {
+        APBPrescaler::NotDivided
+    }
+}
+
+impl Into<u8> for APBPrescaler {
+    fn into(self) -> u8 {
+        match self {
+            APBPrescaler::NotDivided => 1,
+            APBPrescaler::Div2 => 0x04,
+            APBPrescaler::Div4 => 0x05,
+            APBPrescaler::Div8 => 0x06,
+            APBPrescaler::Div16 => 0x07,
+        }
+    }
+}
+
+impl Into<Sw> for ClockSrc {
+    fn into(self) -> Sw {
+        match self {
+            ClockSrc::MSI(..) => Sw::MSIS,
+            ClockSrc::HSE(..) => Sw::HSE,
+            ClockSrc::HSI16 => Sw::HSI16,
+            ClockSrc::PLL1R(..) => Sw::PLL1R,
+        }
+    }
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum MSIRange {
+    Range48mhz = 48_000_000,
+    Range24mhz = 24_000_000,
+    Range16mhz = 16_000_000,
+    Range12mhz = 12_000_000,
+    Range4mhz = 4_000_000,
+    Range2mhz = 2_000_000,
+    Range1_33mhz = 1_330_000,
+    Range1mhz = 1_000_000,
+    Range3_072mhz = 3_072_000,
+    Range1_536mhz = 1_536_000,
+    Range1_024mhz = 1_024_000,
+    Range768khz = 768_000,
+    Range400khz = 400_000,
+    Range200khz = 200_000,
+    Range133khz = 133_000,
+    Range100khz = 100_000,
+}
+
+impl Into<u32> for MSIRange {
+    fn into(self) -> u32 {
+        self as u32
+    }
+}
+
+impl Into<Msirange> for MSIRange {
+    fn into(self) -> Msirange {
+        match self {
+            MSIRange::Range48mhz => Msirange::RANGE_48MHZ,
+            MSIRange::Range24mhz => Msirange::RANGE_24MHZ,
+            MSIRange::Range16mhz => Msirange::RANGE_16MHZ,
+            MSIRange::Range12mhz => Msirange::RANGE_12MHZ,
+            MSIRange::Range4mhz => Msirange::RANGE_4MHZ,
+            MSIRange::Range2mhz => Msirange::RANGE_2MHZ,
+            MSIRange::Range1_33mhz => Msirange::RANGE_1_33MHZ,
+            MSIRange::Range1mhz => Msirange::RANGE_1MHZ,
+            MSIRange::Range3_072mhz => Msirange::RANGE_3_072MHZ,
+            MSIRange::Range1_536mhz => Msirange::RANGE_1_536MHZ,
+            MSIRange::Range1_024mhz => Msirange::RANGE_1_024MHZ,
+            MSIRange::Range768khz => Msirange::RANGE_768KHZ,
+            MSIRange::Range400khz => Msirange::RANGE_400KHZ,
+            MSIRange::Range200khz => Msirange::RANGE_200KHZ,
+            MSIRange::Range133khz => Msirange::RANGE_133KHZ,
+            MSIRange::Range100khz => Msirange::RANGE_100KHZ,
+        }
+    }
+}
+
+impl Default for MSIRange {
+    fn default() -> Self {
+        MSIRange::Range4mhz
+    }
+}
+
+#[derive(Copy, Clone)]
+pub struct Config {
+    mux: ClockSrc,
+    ahb_pre: AHBPrescaler,
+    apb1_pre: APBPrescaler,
+    apb2_pre: APBPrescaler,
+    apb3_pre: APBPrescaler,
+}
 
 impl Config {
     pub fn new() -> Self {
-        Config {}
+        Config {
+            mux: ClockSrc::MSI(MSIRange::default()),
+            ahb_pre: Default::default(),
+            apb1_pre: Default::default(),
+            apb2_pre: Default::default(),
+            apb3_pre: Default::default(),
+        }
     }
 }
 
@@ -12,4 +299,218 @@ impl Default for Config {
     }
 }
 
-pub unsafe fn init(_config: Config) {}
+/// Extension trait that freezes the `RCC` peripheral with provided clocks configuration
+pub trait RccExt {
+    fn freeze(self, config: Config, power: &Power) -> Clocks;
+}
+
+impl RccExt for RCC {
+    #[inline]
+    fn freeze(self, cfgr: Config, power: &Power) -> Clocks {
+        let rcc = pac::RCC;
+
+        let sys_clk = match cfgr.mux {
+            ClockSrc::MSI(range) => {
+                unsafe {
+                    rcc.icscr1().modify(|w| {
+                        let bits: Msirange = range.into();
+                        w.set_msisrange(bits);
+                        w.set_msirgsel(Msirgsel::RCC_ICSCR1);
+                    });
+                    rcc.cr().write(|w| {
+                        w.set_msipllen(false);
+                        w.set_msison(true);
+                        w.set_msison(true);
+                    });
+                    while !rcc.cr().read().msisrdy() {}
+                }
+
+                range.into()
+            }
+            ClockSrc::HSE(freq) => {
+                unsafe {
+                    rcc.cr().write(|w| w.set_hseon(true));
+                    while !rcc.cr().read().hserdy() {}
+                }
+
+                freq.0
+            }
+            ClockSrc::HSI16 => {
+                unsafe {
+                    rcc.cr().write(|w| w.set_hsion(true));
+                    while !rcc.cr().read().hsirdy() {}
+                }
+
+                HSI16_FREQ
+            }
+            ClockSrc::PLL1R(src, m, n, div) => {
+                let freq = match src {
+                    PllSrc::MSI(_) => MSIRange::default().into(),
+                    PllSrc::HSE(hertz) => hertz.0,
+                    PllSrc::HSI16 => HSI16_FREQ,
+                };
+
+                // disable
+                unsafe {
+                    rcc.cr().modify(|w| w.set_pllon(0, false));
+                    while rcc.cr().read().pllrdy(0) {}
+                }
+
+                let vco = freq * n as u8 as u32;
+                let pll_ck = vco / (div as u8 as u32 + 1);
+
+                unsafe {
+                    rcc.pll1cfgr().write(|w| {
+                        w.set_pllm(m.into());
+                        w.set_pllsrc(src.into());
+                    });
+
+                    rcc.pll1divr().modify(|w| {
+                        w.set_pllr(div.to_div());
+                        w.set_plln(n.to_mul());
+                    });
+
+                    // Enable PLL
+                    rcc.cr().modify(|w| w.set_pllon(0, true));
+                    while !rcc.cr().read().pllrdy(0) {}
+                    rcc.pll1cfgr().modify(|w| w.set_pllren(true));
+                }
+
+                unsafe {
+                    rcc.cr().write(|w| w.set_pllon(0, true));
+                    while !rcc.cr().read().pllrdy(0) {}
+                }
+
+                pll_ck
+            }
+        };
+
+        // states and programming delay
+        let wait_states = match power.vos {
+            // VOS 0 range VCORE 1.26V - 1.40V
+            VoltageScale::Range1 => {
+                if sys_clk < 32_000_000 {
+                    0
+                } else if sys_clk < 64_000_000 {
+                    1
+                } else if sys_clk < 96_000_000 {
+                    2
+                } else if sys_clk < 128_000_000 {
+                    3
+                } else {
+                    4
+                }
+            }
+            // VOS 1 range VCORE 1.15V - 1.26V
+            VoltageScale::Range2 => {
+                if sys_clk < 30_000_000 {
+                    0
+                } else if sys_clk < 60_000_000 {
+                    1
+                } else if sys_clk < 90_000_000 {
+                    2
+                } else {
+                    3
+                }
+            }
+            // VOS 2 range VCORE 1.05V - 1.15V
+            VoltageScale::Range3 => {
+                if sys_clk < 24_000_000 {
+                    0
+                } else if sys_clk < 48_000_000 {
+                    1
+                } else {
+                    2
+                }
+            }
+            // VOS 3 range VCORE 0.95V - 1.05V
+            VoltageScale::Range4 => {
+                if sys_clk < 12_000_000 {
+                    0
+                } else {
+                    1
+                }
+            }
+        };
+
+        unsafe {
+            pac::FLASH.acr().modify(|w| {
+                w.set_latency(wait_states);
+            })
+        }
+
+        unsafe {
+            rcc.cfgr1().modify(|w| {
+                w.set_sw(cfgr.mux.into());
+            });
+
+            rcc.cfgr2().modify(|w| {
+                w.set_hpre(cfgr.ahb_pre.into());
+                w.set_ppre1(cfgr.apb1_pre.into());
+                w.set_ppre2(cfgr.apb2_pre.into());
+            });
+
+            rcc.cfgr3().modify(|w| {
+                w.set_ppre3(cfgr.apb3_pre.into());
+            });
+        }
+
+        let ahb_freq: u32 = match cfgr.ahb_pre {
+            AHBPrescaler::NotDivided => sys_clk,
+            pre => {
+                let pre: u8 = pre.into();
+                let pre = 1 << (pre as u32 - 7);
+                sys_clk / pre
+            }
+        };
+
+        let (apb1_freq, apb1_tim_freq) = match cfgr.apb1_pre {
+            APBPrescaler::NotDivided => (ahb_freq, ahb_freq),
+            pre => {
+                let pre: u8 = pre.into();
+                let pre: u8 = 1 << (pre - 3);
+                let freq = ahb_freq / pre as u32;
+                (freq, freq * 2)
+            }
+        };
+
+        let (apb2_freq, apb2_tim_freq) = match cfgr.apb2_pre {
+            APBPrescaler::NotDivided => (ahb_freq, ahb_freq),
+            pre => {
+                let pre: u8 = pre.into();
+                let pre: u8 = 1 << (pre - 3);
+                let freq = ahb_freq / (1 << (pre as u8 - 3));
+                (freq, freq * 2)
+            }
+        };
+
+        let (apb3_freq, _apb3_tim_freq) = match cfgr.apb3_pre {
+            APBPrescaler::NotDivided => (ahb_freq, ahb_freq),
+            pre => {
+                let pre: u8 = pre.into();
+                let pre: u8 = 1 << (pre - 3);
+                let freq = ahb_freq / (1 << (pre as u8 - 3));
+                (freq, freq * 2)
+            }
+        };
+
+        Clocks {
+            sys: sys_clk.hz(),
+            ahb1: ahb_freq.hz(),
+            ahb2: ahb_freq.hz(),
+            ahb3: ahb_freq.hz(),
+            apb1: apb1_freq.hz(),
+            apb2: apb2_freq.hz(),
+            apb3: apb3_freq.hz(),
+            apb1_tim: apb1_tim_freq.hz(),
+            apb2_tim: apb2_tim_freq.hz(),
+        }
+    }
+}
+
+pub unsafe fn init(config: Config) {
+    let r = <peripherals::RCC as embassy::util::Steal>::steal();
+    let power = Power::new(<peripherals::PWR as embassy::util::Steal>::steal());
+    let clocks = r.freeze(config, &power);
+    set_freqs(clocks);
+}
diff --git a/examples/stm32u5/.cargo/config.toml b/examples/stm32u5/.cargo/config.toml
index ba3ef1dc5..9f5e14801 100644
--- a/examples/stm32u5/.cargo/config.toml
+++ b/examples/stm32u5/.cargo/config.toml
@@ -1,6 +1,7 @@
 [target.'cfg(all(target_arch = "arm", target_os = "none"))']
 # replace STM32F429ZITx with your chip as listed in `probe-run --list-chips`
-runner = "probe-run --chip STM32U585AIIx"
+#runner = "probe-run --chip STM32U585AIIx"
+runner = "/Users/bob/outboard-repos/probe-run/target/debug/probe-run-rp --chip STM32U585AIIx"
 
 [build]
 target = "thumbv7em-none-eabi"
diff --git a/examples/stm32u5/Cargo.toml b/examples/stm32u5/Cargo.toml
index b3c3c9700..e84e1992f 100644
--- a/examples/stm32u5/Cargo.toml
+++ b/examples/stm32u5/Cargo.toml
@@ -22,8 +22,8 @@ embassy-traits = { version = "0.1.0", path = "../../embassy-traits", features =
 embassy-stm32 = { version = "0.1.0", path = "../../embassy-stm32", features = ["defmt", "defmt-trace", "unstable-pac", "stm32u585ai", "memory-x" ]  }
 embassy-hal-common = {version = "0.1.0", path = "../../embassy-hal-common" }
 
-defmt = "0.2.3"
-defmt-rtt = "0.2.0"
+defmt = "0.2"
+defmt-rtt = "0.2"
 
 cortex-m = "0.7.3"
 cortex-m-rt = "0.7.0"
@@ -35,3 +35,7 @@ heapless = { version = "0.7.5", default-features = false }
 
 micromath = "2.0.0"
 
+#[patch.crates-io]
+#defmt = { git="https://github.com/knurling-rs/defmt.git" }
+#defmt-rtt = { git="https://github.com/knurling-rs/defmt.git" }
+
diff --git a/stm32-data b/stm32-data
-Subproject e106785ebae82f43c4abd33b9a3f3ea3b31d608
+Subproject 103d86c0558b5ce90ab4765f87b5e756b55312f