Clock init (#49)

Add preliminary clock init function

rp2040-hal/src/clocks/macros.rs

@@ -7,6 +7,10 @@ macro_rules! int_division {
                     w
                 });
             }
+
+            fn get_int_div(&self) -> usize {
+                unsafe { self.shared_dev.get() }.$div.read().int().bits() as usize
+            }
         }
     };
 }
@@ -20,6 +24,32 @@ macro_rules! frac_division {
                     w
                 });
             }
+
+            fn get_frac_div(&self) -> usize {
+                unsafe { self.shared_dev.get() }
+                    .$div
+                    .read()
+                    .frac()
+                    .bits()
+                    .into()
+            }
+        }
+    };
+}
+
+macro_rules! division {
+    ($name:ident, $div:ident) => {
+        impl ClockDivision for $name {
+            fn set_div(&mut self, div: u32) {
+                unsafe { self.shared_dev.get() }.$div.modify(|_, w| unsafe {
+                    w.bits(div);
+                    w
+                });
+            }
+
+            fn get_div(&self) -> u32 {
+                unsafe { self.shared_dev.get() }.$div.read().bits()
+            }
         }
     };
 }

rp2040-hal/src/clocks/mod.rs

@@ -1,11 +1,40 @@
 //! Clocks (CLOCKS)
-// See [Chapter 2 Section 15](https://datasheets.raspberrypi.org/rp2040/rp2040_datasheet.pdf) for more details
+//!
+//!
+//!
+//! Usage:
+//! ```rust
+//! let mut p = rp2040_pac::Peripherals::take().unwrap();
+//! let mut watchdog = Watchdog::new(p.WATCHDOG);
+//! let mut clocks = ClocksManager::new(p.CLOCKS, &mut watchdog);
+//! // Enable the xosc
+//! let xosc = setup_xosc_blocking(p.XOSC, XOSC_MHZ.Hz()).ok().unwrap();
+//!
+//!
+//! // Configure PLLs
+//! //                   REF     FBDIV VCO            POSTDIV
+//! // PLL SYS: 12 / 1 = 12MHz * 125 = 1500MHZ / 6 / 2 = 125MHz
+//! // PLL USB: 12 / 1 = 12MHz * 40  = 480 MHz / 5 / 2 =  48MHz
+//! let pll_sys =
+//!     setup_pll_blocking(p.PLL_SYS, 12.MHz().into(), PLL_SYS_125MHZ, &mut clocks, &mut p.RESETS).ok().unwrap();
+//! let pll_usb =
+//!     setup_pll_blocking(p.PLL_USB, 12.MHz().into(), PLL_USB_48MHZ, &mut clocks, &mut p.RESETS).ok().unwrap();
+//! clocks.init(&xosc, &pll_sys, &pll_usb);
+//! ```
+//!
+//! See [Chapter 2 Section 15](https://datasheets.raspberrypi.org/rp2040/rp2040_datasheet.pdf) for more details
 
-use crate::pac::*;
+use crate::{
+    pll::{Locked, PhaseLockedLoop},
+    watchdog::Watchdog,
+    xosc::{CrystalOscillator, Stable},
+};
+use core::convert::TryInto;
+use embedded_time::rate::*;
+use pac::{clocks, CLOCKS, PLL_SYS, PLL_USB};
 
 #[macro_use]
 mod macros;
-
 #[derive(Copy, Clone)]
 /// Provides refs to the CLOCKS block.
 pub struct ShareableClocks {
@@ -22,6 +51,17 @@ impl ShareableClocks {
     }
 }
 
+const XOSC_MHZ: u32 = 12_000_000_u32;
+
+fn make_div<S: TryInto<Hertz<u64>>, F: TryInto<Hertz<u64>>>(
+    src_freq: S,
+    freq: F,
+) -> Result<u32, ()> {
+    let src_freq = *src_freq.try_into().map_err(|_| ())?.integer();
+    let freq = *freq.try_into().map_err(|_| ())?.integer();
+    let div: u64 = (src_freq << 8).wrapping_div(freq);
+    Ok(div as u32)
+}
 /// Abstraction layer providing Clock Management.
 pub struct ClocksManager {
     clocks: CLOCKS,
@@ -29,7 +69,13 @@ pub struct ClocksManager {
 }
 impl ClocksManager {
     /// Exchanges CLOCKS block against Self.
-    pub fn new(mut clocks_block: CLOCKS) -> Self {
+    pub fn new(mut clocks_block: CLOCKS, watchdog: &mut Watchdog) -> Self {
+        // Start tick in watchdog
+        watchdog.enable_tick_generation(XOSC_MHZ as u8);
+
+        // Disable resus that may be enabled from previous software
+        clocks_block.clk_sys_resus_ctrl.write_with_zero(|w| w);
+
         let shared_clocks = ShareableClocks::new(&mut clocks_block);
         ClocksManager {
             clocks: clocks_block,
@@ -37,6 +83,81 @@ impl ClocksManager {
         }
     }
 
+    /// Initialize the clocks
+    pub fn init(
+        &self,
+        _: &CrystalOscillator<Stable>,
+        _: &PhaseLockedLoop<Locked, PLL_SYS>,
+        _: &PhaseLockedLoop<Locked, PLL_USB>,
+    ) {
+        // Configure clocks
+        // CLK_REF = XOSC (12MHz) / 1 = 12MHz
+        let mut ref_clock = self.ref_clock();
+        let div = make_div(12u32.MHz(), 12u32.MHz()).unwrap();
+        // If increasing divisor, set divisor before source.
+        if div > ref_clock.get_div() {
+            ref_clock.set_div(div);
+        }
+        ref_clock.set_xosc_src();
+        ref_clock.await_select(2);
+        ref_clock.set_div(div);
+
+        // CLK SYS = PLL SYS (125MHz) / 1 = 125MHz
+        let mut sys_clock = self.sys_clock();
+        let div = make_div(125u32.MHz(), 125u32.MHz()).unwrap();
+        // If increasing divisor, set divisor before source.
+        if div > sys_clock.get_div() {
+            sys_clock.set_div(div);
+        }
+        sys_clock.set_pll_sys_auxsrc();
+        sys_clock.set_self_aux_src();
+        sys_clock.await_select(1);
+        sys_clock.set_div(div);
+
+        // CLK USB = PLL USB (48MHz) / 1 = 48MHz
+        let mut usb_clock = self.usb_clock();
+        let div = make_div(48u32.MHz(), 48u32.MHz()).unwrap();
+        // If increasing divisor, set divisor before source.
+        if div > usb_clock.get_div() {
+            usb_clock.set_div(div);
+        }
+        usb_clock.disable();
+        usb_clock.set_pll_usb_auxsrc();
+        usb_clock.enable();
+        usb_clock.set_div(div);
+
+        // CLK ADC = PLL USB (48MHZ) / 1 = 48MHz
+        let mut adc_clock = self.adc_clock();
+        let div = make_div(48u32.MHz(), 48u32.MHz()).unwrap();
+        // If increasing divisor, set divisor before source.
+        if div > adc_clock.get_div() {
+            adc_clock.set_div(div);
+        }
+        adc_clock.disable();
+        adc_clock.set_pll_usb_auxsrc();
+        adc_clock.enable();
+        adc_clock.set_div(div);
+
+        // CLK RTC = PLL USB (48MHz) / 1024 = 46875Hz
+        let mut rtc_clock = self.rtc_clock();
+        let div = make_div(48u32.MHz(), 46875u32.Hz()).unwrap();
+        // If increasing divisor, set divisor before source.
+        if div > rtc_clock.get_div() {
+            rtc_clock.set_div(div);
+        }
+        rtc_clock.disable();
+        rtc_clock.set_pll_usb_auxsrc();
+        rtc_clock.enable();
+        rtc_clock.set_div(div);
+
+        // CLK PERI = clk_sys. Used as reference clock for Peripherals. No dividers so just select and enable
+        // Normally choose clk_sys or clk_usb
+        let mut peripheral_clock = self.peripheral_clock();
+        peripheral_clock.disable();
+        peripheral_clock.set_clksys_auxsrc();
+        peripheral_clock.enable();
+    }
+
     /// Releases the CLOCKS block
     pub fn free(self) -> CLOCKS {
         self.clocks
@@ -117,12 +238,24 @@ impl ClocksManager {
 pub trait IntegerDivision {
     /// Set integer divider value.
     fn set_int_div(&mut self, div: usize);
+    /// Get integer diveder value.
+    fn get_int_div(&self) -> usize;
 }
 
 /// For clocks with a fraction divider.
 pub trait FractionDivision {
     /// Set fraction divider value.
     fn set_frac_div(&mut self, div: usize);
+    /// Get fraction divider value.
+    fn get_frac_div(&self) -> usize;
+}
+
+/// For clocks with a divider
+pub trait ClockDivision {
+    /// Set integer divider value.
+    fn set_div(&mut self, div: u32);
+    /// Get integer diveder value.
+    fn get_div(&self) -> u32;
 }
 
 /// For clocks that can have XOSC as source.
@@ -242,6 +375,7 @@ clockadc_auxsource!(GpioOutput0Clock, clk_gpout0_ctrl);
 clockrtc_auxsource!(GpioOutput0Clock, clk_gpout0_ctrl);
 clockref_auxsource!(GpioOutput0Clock, clk_gpout0_ctrl);
 
+division!(GpioOutput0Clock, clk_gpout0_div);
 int_division!(GpioOutput0Clock, clk_gpout0_div, u32);
 frac_division!(GpioOutput0Clock, clk_gpout0_div, u8);
 
@@ -265,6 +399,7 @@ clockadc_auxsource!(GpioOutput1Clock, clk_gpout1_ctrl);
 clockrtc_auxsource!(GpioOutput1Clock, clk_gpout1_ctrl);
 clockref_auxsource!(GpioOutput1Clock, clk_gpout1_ctrl);
 
+division!(GpioOutput1Clock, clk_gpout1_div);
 int_division!(GpioOutput1Clock, clk_gpout1_div, u32);
 frac_division!(GpioOutput1Clock, clk_gpout1_div, u8);
 
@@ -288,6 +423,7 @@ clockadc_auxsource!(GpioOutput2Clock, clk_gpout2_ctrl);
 clockrtc_auxsource!(GpioOutput2Clock, clk_gpout2_ctrl);
 clockref_auxsource!(GpioOutput2Clock, clk_gpout2_ctrl);
 
+division!(GpioOutput2Clock, clk_gpout2_div);
 int_division!(GpioOutput2Clock, clk_gpout2_div, u32);
 frac_division!(GpioOutput2Clock, clk_gpout2_div, u8);
 
@@ -311,6 +447,7 @@ clockadc_auxsource!(GpioOutput3Clock, clk_gpout3_ctrl);
 clockrtc_auxsource!(GpioOutput3Clock, clk_gpout3_ctrl);
 clockref_auxsource!(GpioOutput3Clock, clk_gpout3_ctrl);
 
+division!(GpioOutput3Clock, clk_gpout3_div);
 int_division!(GpioOutput3Clock, clk_gpout3_div, u32);
 frac_division!(GpioOutput3Clock, clk_gpout3_div, u8);
 
@@ -318,6 +455,30 @@ frac_division!(GpioOutput3Clock, clk_gpout3_div, u8);
 pub struct ReferenceClock {
     shared_dev: ShareableClocks,
 }
+
+impl ReferenceClock {
+    /// WIP - Helper function to reset source (blocking)
+    pub fn reset_source_await(&mut self) {
+        let shared_dev = unsafe { self.shared_dev.get() };
+
+        shared_dev.clk_ref_ctrl.write(|w| {
+            unsafe { w.src().bits(0) };
+            w
+        });
+
+        self.await_select(0x0);
+    }
+
+    /// WIP - Helper function to select new source (blocking)
+    pub fn await_select(&self, clock: u8) {
+        let shared_dev = unsafe { self.shared_dev.get() };
+
+        while (shared_dev.clk_ref_selected.read().bits() & (1 << clock)) == 0 {
+            cortex_m::asm::nop();
+        }
+    }
+}
+
 rosc_source!(ReferenceClock, clk_ref_ctrl);
 selfaux_source!(ReferenceClock, clk_ref_ctrl, clksrc_clk_ref_aux);
 xosc_source!(ReferenceClock, clk_ref_ctrl);
@@ -327,15 +488,14 @@ pll_usb_auxsource!(ReferenceClock, clk_ref_ctrl);
 gpin0_auxsource!(ReferenceClock, clk_ref_ctrl);
 gpin1_auxsource!(ReferenceClock, clk_ref_ctrl);
 
-int_division!(ReferenceClock, clk_ref_div, u8);
+division!(ReferenceClock, clk_ref_div);
-
 /// System Clock
 pub struct SystemClock {
     shared_dev: ShareableClocks,
 }
 impl SystemClock {
     /// WIP - Helper function to reset source (blocking)
-    pub fn reset_source_await(&self) {
+    pub fn reset_source_await(&mut self) {
         let shared_dev = unsafe { self.shared_dev.get() };
 
         shared_dev.clk_sys_ctrl.write(|w| {
@@ -343,14 +503,14 @@ impl SystemClock {
             w
         });
 
-        self.await_select(0x1);
+        self.await_select(0x0);
     }
 
     /// WIP - Helper function to select new source (blocking)
     pub fn await_select(&self, clock: u8) {
         let shared_dev = unsafe { self.shared_dev.get() };
 
-        while shared_dev.clk_sys_selected.read().bits() != clock as u32 {
+        while (shared_dev.clk_sys_selected.read().bits() & (1 << clock)) == 0 {
             cortex_m::asm::nop();
         }
     }
@@ -368,6 +528,7 @@ xosc_auxsource!(SystemClock, clk_sys_ctrl);
 gpin0_auxsource!(SystemClock, clk_sys_ctrl);
 gpin1_auxsource!(SystemClock, clk_sys_ctrl);
 
+division!(SystemClock, clk_sys_div);
 int_division!(SystemClock, clk_sys_div, u32);
 frac_division!(SystemClock, clk_sys_div, u8);
 
@@ -400,7 +561,7 @@ xosc_auxsource!(UsbClock, clk_usb_ctrl);
 gpin0_auxsource!(UsbClock, clk_usb_ctrl);
 gpin1_auxsource!(UsbClock, clk_usb_ctrl);
 
-int_division!(UsbClock, clk_usb_div, u8);
+division!(UsbClock, clk_usb_div);
 
 /// Adc Clock
 pub struct AdcClock {
@@ -416,7 +577,7 @@ xosc_auxsource!(AdcClock, clk_adc_ctrl);
 gpin0_auxsource!(AdcClock, clk_adc_ctrl);
 gpin1_auxsource!(AdcClock, clk_adc_ctrl);
 
-int_division!(AdcClock, clk_adc_div, u8);
+division!(AdcClock, clk_adc_div);
 
 /// RTC Clock
 pub struct RtcClock {
@@ -432,5 +593,6 @@ xosc_auxsource!(RtcClock, clk_rtc_ctrl);
 gpin0_auxsource!(RtcClock, clk_rtc_ctrl);
 gpin1_auxsource!(RtcClock, clk_rtc_ctrl);
 
+division!(RtcClock, clk_rtc_div);
 int_division!(RtcClock, clk_rtc_div, u32);
 frac_division!(RtcClock, clk_rtc_div, u8);

rp2040-hal/src/pll.rs

@@ -13,8 +13,9 @@ use embedded_time::{
 };
 
 use nb::Error::WouldBlock;
+use pac::RESETS;
 
-use crate::resets::SubsystemReset;
+use crate::{clocks::ClocksManager, resets::SubsystemReset};
 
 /// State of the PLL
 pub trait State {}
@@ -264,3 +265,28 @@ impl<D: PhaseLockedLoopDevice> PhaseLockedLoop<Locking, D> {
         self.transition(Locked)
     }
 }
+
+/// Blocking helper method to setup the PLL without going through all the steps.
+pub fn setup_pll_blocking<D: PhaseLockedLoopDevice, R: Rate>(
+    dev: D,
+    xosc_frequency: Generic<u32>,
+    config: PLLConfig<R>,
+    clocks: &mut ClocksManager,
+    resets: &mut RESETS,
+) -> Result<PhaseLockedLoop<Locked, D>, Error>
+where
+    R: Into<Hertz<u64>>,
+{
+    // Before we touch PLLs, switch sys and ref cleanly away from their aux sources.
+    let mut sys_clock = clocks.sys_clock();
+    sys_clock.reset_source_await();
+
+    let mut ref_clock = clocks.ref_clock();
+    ref_clock.reset_source_await();
+
+    let initialized_pll = PhaseLockedLoop::new(dev, xosc_frequency, config)?.initialize(resets);
+
+    let locked_pll_token = nb::block!(initialized_pll.await_lock()).unwrap();
+
+    Ok(initialized_pll.get_locked(locked_pll_token))
+}