basic support for adjusting frequency of pll0 and aclk in sysctl

Author: longfangsong

.gitignore

@@ -1,3 +1,4 @@
 /target
 **/*.rs.bk
-Cargo.lock
+Cargo.lock
+.idea

src/clock.rs

@@ -1,5 +1,6 @@
 //! Clock configuration
 //use crate::pac::PRCI;
+use crate::sysctl::ACLK;
 use crate::time::Hertz;
 
 /// Frozen clock frequencies
@@ -8,31 +9,31 @@ use crate::time::Hertz;
 /// longer be changed.
 #[derive(Clone, Copy)]
 pub struct Clocks {
-    cpu: Hertz,
-    apb0: Hertz,
+    pub(crate) aclk: Hertz,
+    pub(crate) apb0: Hertz,
 }
 
 impl Clocks {
     #[doc(hidden)]
     pub fn new() -> Self {
-/*
-        [MAIXPY]Pll0:freq:806000000
-        [MAIXPY]Pll1:freq:398666666
-        [MAIXPY]Pll2:freq:45066666
-        [MAIXPY]cpu:freq:403000000
-        [MAIXPY]kpu:freq:398666666
-        in freq: 26000000
-        cpu_freq: 390000000
-*/
+        /*
+                [MAIXPY]Pll0:freq:806000000
+                [MAIXPY]Pll1:freq:398666666
+                [MAIXPY]Pll2:freq:45066666
+                [MAIXPY]cpu:freq:403000000
+                [MAIXPY]kpu:freq:398666666
+                in freq: 26000000
+                cpu_freq: 390000000
+        */
         Self {
-            cpu: Hertz(403_000_000),
+            aclk: Hertz(390_000_000),
             apb0: Hertz(195_000_000),
         }
     }
 
     /// Returns CPU frequency
     pub fn cpu(&self) -> Hertz {
-        Hertz(self.cpu.0)
+        Hertz(self.aclk.0)
     }
 
     /// Returns APB0 frequency

src/sysctl.rs

@@ -1,6 +1,11 @@
 //! (TODO) System Controller (SYSCTL)
 
+use crate::clock::Clocks;
 use crate::pac::{sysctl, SYSCTL};
+use crate::time::Hertz;
+use core::sync::atomic::Ordering;
+
+const CLOCK_FREQ_IN0: u32 = 26_000_000;
 
 pub(crate) fn sysctl<'a>() -> &'a sysctl::RegisterBlock {
     unsafe { &*(SYSCTL::ptr()) }
@@ -18,37 +23,111 @@ pub(crate) fn peri_reset<'a>() -> &'a sysctl::PERI_RESET {
     &sysctl().peri_reset
 }
 
+/// Accept freq_in as the input frequency,
+/// and try to find a set of parameters (nr, od, nf),
+/// which results in a frequency as near as possible to freq
+/// note for a PLL:
+///   freq_out = freq_in / nr * nf / od
+/// The reason why we don't port the complex config algorithm from the
+/// official C language SDK is that doing floating number arithmetics
+/// efficiently in no_std rust now is currently not very convenient
+fn calculate_pll_config(freq_in: u32, freq: u32) -> (u8, u8, u8) {
+    // finding a set of (nr, od, nf) which minify abs(freq * nr * od - freq_in * nf)
+    // nr, od is in [0b1,    0b10_000], nr * od is in [0b1,   0b100_000_000]
+    // nf     is in [0b1, 0b1_000_000]
+
+    // for archiving a higher accuracy, we want the nr * od as large as possible
+    // use binary search to find the largest nr_od which freq <= freq_in * 0b1_000_000 / nr_od
+    let mut left = 1;
+    let mut right = 0b10_000 * 0b10_000 + 1;
+    while left + 1 < right {
+        let mid = (left + right) / 2;
+        let max_freq = freq_in * 0b1_000_000 / mid;
+        if freq >= max_freq {
+            // in [left, mid)
+            right = mid;
+        } else {
+            // in [mid, right)
+            left = mid;
+        }
+    }
+    let nr_od = left;
+    // so we got nf
+    let nf = freq * nr_od / freq_in;
+    let nf = nf.min(0b1_000_000) as u8;
+
+    // decompose nr_od
+    for nr in 1..=0b10_000 {
+        if (nr_od / nr) * nr == nr_od {
+            return (nr as u8, (nr_od / nr) as u8, nf);
+        }
+    }
+    unreachable!()
+}
+
 pub trait SysctlExt {
     fn constrain(self) -> Parts;
 }
 
 impl SysctlExt for SYSCTL {
     fn constrain(self) -> Parts {
-        Parts { 
+        Parts {
+            aclk: ACLK { _ownership: () },
             apb0: APB0 { _ownership: () },
+            pll0: PLL0 { _ownership: () },
         }
     }
 }
 
 // ref: sysctl.c
 pub struct Parts {
-    // todo: PLL0, PLL1, PLL2
-    // todo: CPU, SRAM, APB-bus, ROM, DMA, AI
+    /// entry for controlling the frequency of aclk
+    pub aclk: ACLK,
+    /// entry for controlling the enable/disable/frequency of pll0
+    pub pll0: PLL0,
+    /// entry for controlling the enable/disable/frequency of apb0
     pub apb0: APB0,
-    // pub apb1: APB1,
-    // pub apb2: APB2,
+    // todo: SRAM, APB-bus, ROM, DMA, AI, PLL1, PLL2, APB1, APB2
+}
+
+impl Parts {
+    pub fn clocks(&self) -> Clocks {
+        Clocks {
+            aclk: self.aclk.get_frequency(),
+            apb0: self.apb0.get_frequency(),
+        }
+    }
 }
 
 pub struct APB0 {
-    _ownership: ()
+    _ownership: (),
 }
 
 impl APB0 {
     pub(crate) fn enable(&mut self) {
-        clk_en_cent().modify(
-            |_r, w|
-                w.apb0_clk_en().set_bit()
-        );
+        clk_en_cent().modify(|_r, w| w.apb0_clk_en().set_bit());
+    }
+
+    pub fn set_frequency(&mut self, expected_freq: impl Into<Hertz>) -> Hertz {
+        let aclk = ACLK::steal();
+        let aclk_frequency = aclk.get_frequency().0 as i64;
+        // apb0_frequency = aclk_frequency / (apb0_clk_sel + 1)
+        let apb0_clk_sel = (aclk_frequency / expected_freq.into().0 as i64 - 1)
+            .max(0)
+            .min(0b111) as u8;
+        unsafe {
+            sysctl()
+                .clk_sel0
+                .modify(|_, w| w.apb0_clk_sel().bits(apb0_clk_sel));
+        }
+        Hertz(aclk_frequency as u32 / (apb0_clk_sel as u32 + 1))
+    }
+
+    pub fn get_frequency(&self) -> Hertz {
+        let aclk = ACLK::steal();
+        let aclk_frequency = aclk.get_frequency().0 as i64;
+        let apb0_clk_sel = sysctl().clk_sel0.read().apb0_clk_sel().bits();
+        Hertz(aclk_frequency as u32 / (apb0_clk_sel as u32 + 1))
     }
 }
 
@@ -59,3 +138,167 @@ impl APB0 {
 // pub struct APB2 {
 //     _ownership: ()
 // }
+
+/// PLL0, which source is CLOCK_FREQ_IN0,
+/// and the output can be used on ACLK(CPU), SPIs, etc.
+pub struct PLL0 {
+    _ownership: (),
+}
+
+impl PLL0 {
+    pub(crate) fn steal() -> Self {
+        PLL0 { _ownership: () }
+    }
+
+    #[inline(always)]
+    fn is_locked(&self) -> bool {
+        sysctl().pll_lock.read().pll_lock0() == 0b11
+    }
+
+    fn lock(&mut self) {
+        while !self.is_locked() {
+            sysctl()
+                .pll_lock
+                .modify(|_, w| w.pll_slip_clear0().set_bit())
+        }
+    }
+
+    #[inline(always)]
+    fn reset(&mut self) {
+        sysctl().pll0.modify(|_, w| w.reset().clear_bit());
+        sysctl().pll0.modify(|_, w| w.reset().set_bit());
+        core::sync::atomic::compiler_fence(Ordering::SeqCst);
+        core::sync::atomic::compiler_fence(Ordering::SeqCst);
+        sysctl().pll0.modify(|_, w| w.reset().clear_bit());
+    }
+
+    /// enable PLL0
+    pub fn enable(&mut self) {
+        sysctl()
+            .pll0
+            .modify(|_, w| w.bypass().clear_bit().pwrd().set_bit());
+        self.reset();
+        self.lock();
+        sysctl().pll0.modify(|_, w| w.out_en().set_bit());
+    }
+
+    /// disable PLL0
+    /// use with caution: PLL0 can be used as source clock of ACLK (so also CPU),
+    /// if you want to disable PLL0, please make the cpu use external clock first
+    pub fn disable(&mut self) {
+        sysctl()
+            .pll0
+            .modify(|_, w| w.bypass().set_bit().pwrd().clear_bit().out_en().clear_bit());
+    }
+
+    /// Set frequency of PLL0
+    /// Will set the frequency of PLL0 as close to frequency as possible
+    /// Return the real frequency of the PLL0
+    pub fn set_frequency(&mut self, frequency: impl Into<Hertz>) -> Hertz {
+        let is_aclk_using = sysctl().clk_sel0.read().aclk_sel().bit();
+        if is_aclk_using {
+            sysctl().clk_sel0.modify(|_, w| w.aclk_sel().clear_bit());
+        }
+        self.disable();
+        let (nr, od, nf) = calculate_pll_config(CLOCK_FREQ_IN0, frequency.into().0);
+        unsafe {
+            sysctl().pll0.modify(|_, w| {
+                w.clkr()
+                    .bits(nr - 1)
+                    .clkf()
+                    .bits(nf - 1)
+                    .clkod()
+                    .bits(od - 1)
+                    .bwadj()
+                    .bits(nf - 1)
+            });
+        }
+        self.enable();
+        // recover aclk_sel
+        if is_aclk_using {
+            sysctl().clk_sel0.modify(|_, w| w.aclk_sel().set_bit());
+        }
+        Hertz(CLOCK_FREQ_IN0 / nr as u32 * nf as u32 / od as u32)
+    }
+
+    /// Return the frequency of PLL0
+    pub fn get_frequency(&self) -> Hertz {
+        let nr = sysctl().pll0.read().clkr().bits() + 1;
+        let nf = sysctl().pll0.read().clkf().bits() + 1;
+        let od = sysctl().pll0.read().clkod().bits() + 1;
+        Hertz(CLOCK_FREQ_IN0 / nr as u32 * nf as u32 / od as u32)
+    }
+}
+
+pub struct ACLK {
+    _ownership: (),
+}
+
+/// ACLK clock frequency control
+impl ACLK {
+    pub fn steal() -> Self {
+        ACLK { _ownership: () }
+    }
+
+    /// make ACLK use external clock, ie. CLOCK_FREQ_IN0
+    pub(crate) fn use_external(&mut self) {
+        sysctl().clk_sel0.modify(|_, w| w.aclk_sel().clear_bit());
+    }
+
+    /// Return whether the ACLK is using external clock
+    pub fn is_using_external(&self) -> bool {
+        !sysctl().clk_sel0.read().aclk_sel().bit()
+    }
+
+    /// make ACLK use pll0 clock, with aclk_divider_sel
+    pub fn use_pll0(&mut self, aclk_divider_sel: u8) {
+        unsafe {
+            sysctl().clk_sel0.modify(|_, w| {
+                w.aclk_divider_sel()
+                    .bits(aclk_divider_sel)
+                    .aclk_sel()
+                    .set_bit()
+            });
+        }
+    }
+
+    /// Set the frequency of ACLK
+    /// if frequency == CLOCK_FREQ_IN0, use external clock directly
+    /// else frequency settings here are based on existing settings on PLL0
+    /// We won't adjust PLL0 here because there are so many devices based on it.
+    pub fn set_frequency(&mut self, expected_freq: impl Into<Hertz>) -> Hertz {
+        let expected_freq = expected_freq.into().0;
+        if expected_freq == CLOCK_FREQ_IN0 {
+            self.use_external();
+            Hertz(CLOCK_FREQ_IN0)
+        } else {
+            // aclk = pll0 / (2 << aclk_divider_sel)
+            let pll0 = PLL0::steal().get_frequency().0;
+            let mut aclk_divider_sel = 0u8;
+            // aclk_divider_sel is 2 bits
+            if expected_freq < pll0 / (2 << 0b11) {
+                aclk_divider_sel = 0b11;
+            } else {
+                for i in 0b00u8..0b11 {
+                    if pll0 / (2 << i) <= expected_freq {
+                        aclk_divider_sel = i;
+                        break;
+                    }
+                }
+            }
+            self.use_pll0(aclk_divider_sel);
+            Hertz(pll0 / (2 << aclk_divider_sel))
+        }
+    }
+
+    /// Get the frequency of ACLK
+    pub fn get_frequency(&self) -> Hertz {
+        if self.is_using_external() {
+            Hertz(CLOCK_FREQ_IN0)
+        } else {
+            let pll0 = PLL0::steal().get_frequency().0;
+            let aclk_divider_sel = sysctl().clk_sel0.read().aclk_divider_sel().bits();
+            Hertz(pll0 / (2 << aclk_divider_sel))
+        }
+    }
+}