1 files changed, 418 insertions, 77 deletions

diff --git a/embassy-mcxa/src/clocks/mod.rs b/embassy-mcxa/src/clocks/mod.rs
index b41a1ba46..04559fd04 100644
--- a/embassy-mcxa/src/clocks/mod.rs
+++ b/embassy-mcxa/src/clocks/mod.rs
@@ -88,6 +88,7 @@ pub fn init(settings: ClocksConfig) -> Result<(), ClockError> {
     operator.configure_sirc_clocks()?;
     operator.configure_fro16k_clocks()?;
     operator.configure_sosc()?;
+    operator.configure_spll()?;
 
     // For now, just use FIRC as the main/cpu clock, which should already be
     // the case on reset
@@ -199,6 +200,9 @@ pub struct Clocks {
 
     /// `pll1_clk` is the output of the main system PLL, `pll1`.
     pub pll1_clk: Option<Clock>,
+
+    /// `pll1_clk_div` is a configurable frequency clock, sourced from `pll1_clk`
+    pub pll1_clk_div: Option<Clock>,
 }
 
 /// `ClockError` is the main error returned when configuring or checking clock state
@@ -435,72 +439,49 @@ pub unsafe fn pulse_reset<G: Gate>() {
 /// selected clocks are active at a suitable level at time of construction. These methods
 /// return the frequency of the requested clock, in Hertz, or a [`ClockError`].
 impl Clocks {
-    /// Ensure the `fro_lf_div` clock is active and valid at the given power state.
-    pub fn ensure_fro_lf_div_active(&self, at_level: &PoweredClock) -> Result<u32, ClockError> {
-        let Some(clk) = self.fro_lf_div.as_ref() else {
+    fn ensure_clock_active(
+        &self,
+        clock: &Option<Clock>,
+        name: &'static str,
+        at_level: &PoweredClock,
+    ) -> Result<u32, ClockError> {
+        let Some(clk) = clock.as_ref() else {
             return Err(ClockError::BadConfig {
-                clock: "fro_lf_div",
+                clock: name,
                 reason: "required but not active",
             });
         };
         if !clk.power.meets_requirement_of(at_level) {
             return Err(ClockError::BadConfig {
-                clock: "fro_lf_div",
+                clock: name,
                 reason: "not low power active",
             });
         }
         Ok(clk.frequency)
     }
 
+    /// Ensure the `fro_lf_div` clock is active and valid at the given power state.
+    #[inline]
+    pub fn ensure_fro_lf_div_active(&self, at_level: &PoweredClock) -> Result<u32, ClockError> {
+        self.ensure_clock_active(&self.fro_lf_div, "fro_lf_div", at_level)
+    }
+
     /// Ensure the `fro_hf` clock is active and valid at the given power state.
+    #[inline]
     pub fn ensure_fro_hf_active(&self, at_level: &PoweredClock) -> Result<u32, ClockError> {
-        let Some(clk) = self.fro_hf.as_ref() else {
-            return Err(ClockError::BadConfig {
-                clock: "fro_hf",
-                reason: "required but not active",
-            });
-        };
-        if !clk.power.meets_requirement_of(at_level) {
-            return Err(ClockError::BadConfig {
-                clock: "fro_hf",
-                reason: "not low power active",
-            });
-        }
-        Ok(clk.frequency)
+        self.ensure_clock_active(&self.fro_hf, "fro_hf", at_level)
     }
 
     /// Ensure the `fro_hf_div` clock is active and valid at the given power state.
+    #[inline]
     pub fn ensure_fro_hf_div_active(&self, at_level: &PoweredClock) -> Result<u32, ClockError> {
-        let Some(clk) = self.fro_hf_div.as_ref() else {
-            return Err(ClockError::BadConfig {
-                clock: "fro_hf_div",
-                reason: "required but not active",
-            });
-        };
-        if !clk.power.meets_requirement_of(at_level) {
-            return Err(ClockError::BadConfig {
-                clock: "fro_hf_div",
-                reason: "not low power active",
-            });
-        }
-        Ok(clk.frequency)
+        self.ensure_clock_active(&self.fro_hf_div, "fro_hf_div", at_level)
     }
 
     /// Ensure the `clk_in` clock is active and valid at the given power state.
+    #[inline]
     pub fn ensure_clk_in_active(&self, at_level: &PoweredClock) -> Result<u32, ClockError> {
-        let Some(clk) = self.clk_in.as_ref() else {
-            return Err(ClockError::BadConfig {
-                clock: "clk_in",
-                reason: "required but not active",
-            });
-        };
-        if !clk.power.meets_requirement_of(at_level) {
-            return Err(ClockError::BadConfig {
-                clock: "clk_in",
-                reason: "not low power active",
-            });
-        }
-        Ok(clk.frequency)
+        self.ensure_clock_active(&self.clk_in, "clk_in", at_level)
     }
 
     /// Ensure the `clk_16k_vsys` clock is active and valid at the given power state.
@@ -530,30 +511,21 @@ impl Clocks {
     }
 
     /// Ensure the `clk_1m` clock is active and valid at the given power state.
+    #[inline]
     pub fn ensure_clk_1m_active(&self, at_level: &PoweredClock) -> Result<u32, ClockError> {
-        let Some(clk) = self.clk_1m.as_ref() else {
-            return Err(ClockError::BadConfig {
-                clock: "clk_1m",
-                reason: "required but not active",
-            });
-        };
-        if !clk.power.meets_requirement_of(at_level) {
-            return Err(ClockError::BadConfig {
-                clock: "clk_1m",
-                reason: "not low power active",
-            });
-        }
-        Ok(clk.frequency)
+        self.ensure_clock_active(&self.clk_1m, "clk_1m", at_level)
     }
 
     /// Ensure the `pll1_clk` clock is active and valid at the given power state.
-    pub fn ensure_pll1_clk_active(&self, _at_level: &PoweredClock) -> Result<u32, ClockError> {
-        Err(ClockError::NotImplemented { clock: "pll1_clk" })
+    #[inline]
+    pub fn ensure_pll1_clk_active(&self, at_level: &PoweredClock) -> Result<u32, ClockError> {
+        self.ensure_clock_active(&self.pll1_clk, "pll1_clk", at_level)
     }
 
     /// Ensure the `pll1_clk_div` clock is active and valid at the given power state.
-    pub fn ensure_pll1_clk_div_active(&self, _at_level: &PoweredClock) -> Result<u32, ClockError> {
-        Err(ClockError::NotImplemented { clock: "pll1_clk_div" })
+    #[inline]
+    pub fn ensure_pll1_clk_div_active(&self, at_level: &PoweredClock) -> Result<u32, ClockError> {
+        self.ensure_clock_active(&self.pll1_clk_div, "pll1_clk_div", at_level)
     }
 
     /// Ensure the `CPU_CLK` or `SYSTEM_CLK` is active
@@ -825,7 +797,7 @@ impl ClockOperator<'_> {
         Ok(())
     }
 
-    /// Configure the FRO16K/clk_16k clock family
+    /// Configure the ROSC/FRO16K/clk_16k clock family
     fn configure_fro16k_clocks(&mut self) -> Result<(), ClockError> {
         let Some(fro16k) = self.config.fro16k.as_ref() else {
             return Ok(());
@@ -866,21 +838,30 @@ impl ClockOperator<'_> {
         Ok(())
     }
 
+    fn ensure_ldo_active(&mut self) {
+        // TODO: Config for the LDO? For now, just enable
+        // using the default settings:
+        // LDOBYPASS: 0/not bypassed
+        // VOUT_SEL: 0b100: 1.1v
+        // LDOEN: 0/Disabled
+        let already_enabled = {
+            let ldocsr = self.scg0.ldocsr().read();
+            ldocsr.ldoen().is_enabled() && ldocsr.vout_ok().is_enabled()
+        };
+        if !already_enabled {
+            self.scg0.ldocsr().modify(|_r, w| w.ldoen().enabled());
+            while self.scg0.ldocsr().read().vout_ok().is_disabled() {}
+        }
+    }
+
     /// Configure the SOSC/clk_in oscillator
     fn configure_sosc(&mut self) -> Result<(), ClockError> {
         let Some(parts) = self.config.sosc.as_ref() else {
             return Ok(());
         };
 
-        let scg0 = unsafe { pac::Scg0::steal() };
-
-        // TODO: Config for the LDO? For now, if we have Sosc, just enable
-        // using the default settings:
-        // LDOBYPASS: 0/not bypassed
-        // VOUT_SEL: 0b100: 1.1v
-        // LDOEN: 0/Disabled
-        scg0.ldocsr().modify(|_r, w| w.ldoen().enabled());
-        while scg0.ldocsr().read().vout_ok().is_disabled() {}
+        // Enable (and wait for) LDO to be active
+        self.ensure_ldo_active();
 
         // TODO: something something pins? This seems to work when the pins are
         // not enabled, even if GPIO hasn't been initialized at all yet.
@@ -920,14 +901,14 @@ impl ClockOperator<'_> {
         };
 
         // Set source/erefs and range
-        scg0.sosccfg().modify(|_r, w| {
+        self.scg0.sosccfg().modify(|_r, w| {
             w.erefs().variant(eref);
             w.range().variant(range);
             w
         });
 
         // Disable lock
-        scg0.sosccsr().modify(|_r, w| w.lk().clear_bit());
+        self.scg0.sosccsr().modify(|_r, w| w.lk().clear_bit());
 
         // TODO: We could enable the SOSC clock monitor. There are some things to
         // figure out first:
@@ -938,7 +919,7 @@ impl ClockOperator<'_> {
         // * We need to decide if we need an interrupt or a reset if the monitor trips
 
         // Apply remaining config
-        scg0.sosccsr().modify(|_r, w| {
+        self.scg0.sosccsr().modify(|_r, w| {
             // For now, just disable the monitor. See above.
             w.sosccm().disabled();
 
@@ -957,8 +938,8 @@ impl ClockOperator<'_> {
         });
 
         // Wait for SOSC to be valid, check for errors
-        while !scg0.sosccsr().read().soscvld().bit_is_set() {}
-        if scg0.sosccsr().read().soscerr().is_enabled_and_error() {
+        while !self.scg0.sosccsr().read().soscvld().bit_is_set() {}
+        if self.scg0.sosccsr().read().soscerr().is_enabled_and_error() {
             return Err(ClockError::BadConfig {
                 clock: "clk_in",
                 reason: "soscerr is set",
@@ -966,7 +947,7 @@ impl ClockOperator<'_> {
         }
 
         // Re-lock the sosc
-        scg0.sosccsr().modify(|_r, w| w.lk().set_bit());
+        self.scg0.sosccsr().modify(|_r, w| w.lk().set_bit());
 
         self.clocks.clk_in = Some(Clock {
             frequency: freq,
@@ -975,6 +956,366 @@ impl ClockOperator<'_> {
 
         Ok(())
     }
+
+    fn configure_spll(&mut self) -> Result<(), ClockError> {
+        // # Vocab
+        //
+        // | Name   | Meaning                                                     |
+        // | :---   | :---                                                        |
+        // | Fin    | Frequency of clkin                                          |
+        // | clkout | Output clock of the PLL                                     |
+        // | Fout   | Frequency of clkout (depends on mode)                       |
+        // | clkref | PLL Reference clock, the input clock to the PFD             |
+        // | Fref   | Frequency of clkref, Fref = Fin / N                         |
+        // | Fcco   | Frequency of the output clock of the CCO, Fcco = M * Fref   |
+        // | N      | Predivider value                                            |
+        // | M      | Feedback divider value                                      |
+        // | P      | Postdivider value                                           |
+        // | Tpon   | PLL start-up time                                           |
+
+        // No PLL? Nothing to do!
+        let Some(cfg) = self.config.spll.as_ref() else {
+            return Ok(());
+        };
+
+        // Ensure the LDO is active
+        self.ensure_ldo_active();
+
+        // match on the source, ensure it is active already
+        let res = match cfg.source {
+            config::SpllSource::Sosc => self
+                .clocks
+                .clk_in
+                .as_ref()
+                .map(|c| (c, pac::scg0::spllctrl::Source::Sosc))
+                .ok_or("sosc not active"),
+            config::SpllSource::Firc => self
+                .clocks
+                .clk_45m
+                .as_ref()
+                .map(|c| (c, pac::scg0::spllctrl::Source::Firc))
+                .ok_or("firc not active"),
+            config::SpllSource::Sirc => self
+                .clocks
+                .fro_12m
+                .as_ref()
+                .map(|c| (c, pac::scg0::spllctrl::Source::Sirc))
+                .ok_or("sirc not active"),
+        };
+        // This checks if active
+        let (clk, variant) = res.map_err(|s| ClockError::BadConfig {
+            clock: "spll",
+            reason: s,
+        })?;
+        // This checks the correct power reqs
+        if !clk.power.meets_requirement_of(&cfg.power) {
+            return Err(ClockError::BadConfig {
+                clock: "spll",
+                reason: "needs low power source",
+            });
+        }
+
+        // Bandwidth calc
+        //
+        // > In normal applications, you must calculate the bandwidth manually by using the feedback divider M (ranging from 1 to 216-1),
+        // > Equation 1, and Equation 2. The PLL is automatically stable in such case. In normal applications, SPLLCTRL[BANDDIRECT] must
+        // > be 0; in this case, the bandwidth changes as a function of M.
+        if clk.frequency == 0 {
+            return Err(ClockError::BadConfig {
+                clock: "spll",
+                reason: "internal error",
+            });
+        }
+
+        // These are calculated differently depending on the mode.
+        let f_in = clk.frequency;
+        let bp_pre: bool;
+        let bp_post: bool;
+        let bp_post2: bool;
+        let m: u16;
+        let p: Option<u8>;
+        let n: Option<u8>;
+
+        // Calculate both Fout and Fcco so we can ensure they don't overflow
+        // and are in range
+        let fout: Option<u32>;
+        let fcco: Option<u32>;
+
+        let m_check = |m: u16| {
+            if !(1..=u16::MAX).contains(&m) {
+                Err(ClockError::BadConfig {
+                    clock: "spll",
+                    reason: "m_mult out of range",
+                })
+            } else {
+                Ok(m)
+            }
+        };
+        let p_check = |p: u8| {
+            if !(1..=31).contains(&p) {
+                Err(ClockError::BadConfig {
+                    clock: "spll",
+                    reason: "p_div out of range",
+                })
+            } else {
+                Ok(p)
+            }
+        };
+        let n_check = |n: u8| {
+            if !(1..=u8::MAX).contains(&n) {
+                Err(ClockError::BadConfig {
+                    clock: "spll",
+                    reason: "n_div out of range",
+                })
+            } else {
+                Ok(n)
+            }
+        };
+
+        match cfg.mode {
+            // Fout = M x Fin
+            config::SpllMode::Mode1a { m_mult } => {
+                bp_pre = true;
+                bp_post = true;
+                bp_post2 = false;
+                m = m_check(m_mult)?;
+                p = None;
+                n = None;
+                fcco = f_in.checked_mul(m_mult as u32);
+                fout = fcco;
+            }
+            // if !bypass_p2_div: Fout = (M / (2 x P)) x Fin
+            // if  bypass_p2_div: Fout = (M /    P   ) x Fin
+            config::SpllMode::Mode1b {
+                m_mult,
+                p_div,
+                bypass_p2_div,
+            } => {
+                bp_pre = true;
+                bp_post = false;
+                bp_post2 = bypass_p2_div;
+                m = m_check(m_mult)?;
+                p = Some(p_check(p_div)?);
+                n = None;
+                let mut div = p_div as u32;
+                if !bypass_p2_div {
+                    div *= 2;
+                }
+                fcco = f_in.checked_mul(m_mult as u32);
+                fout = (f_in / div).checked_mul(m_mult as u32);
+            }
+            // Fout = (M / N) x Fin
+            config::SpllMode::Mode1c { m_mult, n_div } => {
+                bp_pre = false;
+                bp_post = true;
+                bp_post2 = false;
+                m = m_check(m_mult)?;
+                p = None;
+                n = Some(n_check(n_div)?);
+                fcco = (f_in / (n_div as u32)).checked_mul(m_mult as u32);
+                fout = fcco;
+            }
+            // if !bypass_p2_div: Fout = (M / (N x 2 x P)) x Fin
+            // if  bypass_p2_div: Fout = (M / (  N x P  )) x Fin
+            config::SpllMode::Mode1d {
+                m_mult,
+                n_div,
+                p_div,
+                bypass_p2_div,
+            } => {
+                bp_pre = false;
+                bp_post = false;
+                bp_post2 = bypass_p2_div;
+                m = m_check(m_mult)?;
+                p = Some(p_check(p_div)?);
+                n = Some(n_check(n_div)?);
+                // This can't overflow: u8 x u8 (x 2) always fits in u32
+                let mut div = (p_div as u32) * (n_div as u32);
+                if !bypass_p2_div {
+                    div *= 2;
+                }
+                fcco = (f_in / (n_div as u32)).checked_mul(m_mult as u32);
+                fout = (f_in / div).checked_mul(m_mult as u32);
+            }
+        };
+
+        // Dump all the PLL calcs if needed for debugging
+        #[cfg(feature = "defmt")]
+        {
+            defmt::debug!("f_in: {:?}", f_in);
+            defmt::debug!("bp_pre: {:?}", bp_pre);
+            defmt::debug!("bp_post: {:?}", bp_post);
+            defmt::debug!("bp_post2: {:?}", bp_post2);
+            defmt::debug!("m: {:?}", m);
+            defmt::debug!("p: {:?}", p);
+            defmt::debug!("n: {:?}", n);
+            defmt::debug!("fout: {:?}", fout);
+            defmt::debug!("fcco: {:?}", fcco);
+        }
+
+        // Ensure the Fcco and Fout calcs didn't overflow
+        let fcco = fcco.ok_or(ClockError::BadConfig {
+            clock: "spll",
+            reason: "fcco invalid1",
+        })?;
+        let fout = fout.ok_or(ClockError::BadConfig {
+            clock: "spll",
+            reason: "fout invalid",
+        })?;
+
+        // Fcco: 275MHz to 550MHz
+        if !(275_000_000..=550_000_000).contains(&fcco) {
+            return Err(ClockError::BadConfig {
+                clock: "spll",
+                reason: "fcco invalid2",
+            });
+        }
+
+        // TODO: Different for different CPUs?
+        const CPU_MAX_FREQ: u32 = 180_000_000;
+
+        // Fout: 4.3MHz to 2x Max CPU Frequency
+        if !(4_300_000..=(2 * CPU_MAX_FREQ)).contains(&fout) {
+            return Err(ClockError::BadConfig {
+                clock: "spll",
+                reason: "fout invalid",
+            });
+        }
+
+        // A = floor(m / 4) + 1
+        let selp_a = (m / 4) + 1;
+        // SELP = A  if A <  31
+        //      = 31 if A >= 31
+        let selp = selp_a.min(31);
+
+        // A = 1                    if        M >= 8000
+        //   = floor(8000 / M)      if 8000 > M >= 122
+        //   = 2 x floor(M / 4) / 3 if 122  > M >= 1
+        let seli_a = if m >= 8000 {
+            1
+        } else if m >= 122 {
+            8000 / m
+        } else {
+            (2 * (m / 4)) / 3
+        };
+        // SELI = A  if A <  63
+        //      = 63 if A >= 63
+        let seli = seli_a.min(63);
+        // SELR must be 0.
+        let selr = 0;
+
+        self.scg0.spllctrl().modify(|_r, w| {
+            w.source().variant(variant);
+            unsafe {
+                w.selp().bits(selp as u8);
+                w.seli().bits(seli as u8);
+                w.selr().bits(selr);
+            }
+            w
+        });
+
+        if let Some(n) = n {
+            self.scg0.spllndiv().modify(|_r, w| unsafe { w.ndiv().bits(n) });
+        }
+        if let Some(p) = p {
+            self.scg0.spllpdiv().modify(|_r, w| unsafe { w.pdiv().bits(p) });
+        }
+        self.scg0.spllmdiv().modify(|_r, w| unsafe { w.mdiv().bits(m) });
+
+        self.scg0.spllctrl().modify(|_r, w| {
+            w.bypassprediv().bit(bp_pre);
+            w.bypasspostdiv().bit(bp_post);
+            w.bypasspostdiv2().bit(bp_post2);
+
+            // TODO: support FRM?
+            w.frm().disabled();
+
+            w
+        });
+
+        // Unlock
+        self.scg0.spllcsr().modify(|_r, w| w.lk().write_enabled());
+
+        // TODO: Support clock monitors?
+        // self.scg0.spllcsr().modify(|_r, w| w.spllcm().?);
+
+        self.scg0.trim_lock().write(|w| unsafe {
+            w.trim_lock_key().bits(0x5a5a);
+            w.trim_unlock().not_locked()
+        });
+
+        // SPLLLOCK_CNFG: The lock time programmed in this register must be
+        // equal to meet the PLL 500μs lock time plus the 300 refclk count startup.
+        //
+        // LOCK_TIME = 500μs/T ref + 300, F ref = F in /N (input frequency divided by pre-divider ratio).
+        //
+        // 500us is 1/2000th of a second, therefore Fref / 2000 is the number of cycles in 500us.
+        let f_ref = if let Some(n) = n { f_in / (n as u32) } else { f_in };
+        let lock_time = f_ref.div_ceil(2000) + 300;
+        self.scg0
+            .splllock_cnfg()
+            .write(|w| unsafe { w.lock_time().bits(lock_time) });
+
+        // TODO: Support Spread spectrum?
+
+        self.scg0.spllcsr().modify(|_r, w| {
+            w.spllclken().enabled();
+            w.spllpwren().enabled();
+            w.spllsten().bit(matches!(cfg.power, PoweredClock::AlwaysEnabled));
+            w
+        });
+
+        // Wait for SPLL to set up
+        loop {
+            let csr = self.scg0.spllcsr().read();
+            if csr.spll_lock().is_enabled_and_valid() {
+                if csr.spllerr().is_enabled_and_error() {
+                    return Err(ClockError::BadConfig {
+                        clock: "spll",
+                        reason: "spllerr is set",
+                    });
+                }
+                break;
+            }
+        }
+
+        // Re-lock SPLL CSR
+        self.scg0.spllcsr().modify(|_r, w| w.lk().write_disabled());
+
+        // Store clock state
+        self.clocks.pll1_clk = Some(Clock {
+            frequency: fout,
+            power: cfg.power,
+        });
+
+        // Do we enable the `pll1_clk_div` output?
+        if let Some(d) = cfg.pll1_clk_div.as_ref() {
+            // Halt and reset the div; then set our desired div.
+            self.syscon.pll1clkdiv().write(|w| {
+                w.halt().halt();
+                w.reset().asserted();
+                unsafe { w.div().bits(d.into_bits()) };
+                w
+            });
+            // Then unhalt it, and reset it
+            self.syscon.pll1clkdiv().write(|w| {
+                w.halt().run();
+                w.reset().released();
+                w
+            });
+
+            // Wait for clock to stabilize
+            while self.syscon.pll1clkdiv().read().unstab().is_ongoing() {}
+
+            // Store off the clock info
+            self.clocks.pll1_clk_div = Some(Clock {
+                frequency: fout / d.into_divisor(),
+                power: cfg.power,
+            });
+        }
+
+        Ok(())
+    }
 }
 
 //