Improve clock frequency stuff for uninitialized clocks and add some examples (#64)

* Improve clock frequency stuff for uninitialized clocks

- Made clocks singletons so the frequency handling actually works as expected
- Added initial frequencies
- Improved the docs
- Added a Clock trait

* Add pico examples.

These have the benefit of knowing which external crystal is attached.
Even though it always should be a 12 MHz crystal.
Thus we can setup the clocks properly

I also changed the rp2040 examples to work out of the box for pico boards since that will probably be used most of the time

README.md

@@ -55,11 +55,14 @@ To get a local copy up and running follow these simple steps.
 ### Installation
 
 1. Clone the repo or use the crate
-   ```
+
+   ```sh
    git clone https://github.com/rp-rs/rp-hal
    ```
+
    or
-   ```
+
+   ```sh
    cargo install rpXXXX-hal
    ```
 
@@ -70,7 +73,25 @@ Use this space to show useful examples of how a project can be used. Additional
 
 For more examples, please refer to the [Documentation](https://github.com/rp-rs/rp-hal)
 
+### Run examples
 
+Install [`uf2conv`](https://github.com/sajattack/uf2conv-rs) and [`cargo-binutils`](https://github.com/rust-embedded/cargo-binutils) as well as the `llvm-tools-preview` component:
+
+```sh
+cargo install uf2conv cargo-binutils
+rustup component add llvm-tools-preview
+```
+
+For boards with uf2 flashloaders you can use the following lines to run the examples:
+
+```sh
+export RPI_MOUNT_FOLDER=/media/RPI-RP2/
+export EXAMPLE=pico_blinky; # See `cargo check --example` for valid values
+cargo check --example $EXAMPLE && \
+cargo objcopy --release --example $EXAMPLE -- -O binary target/$EXAMPLE.bin && \
+uf2conv target/$EXAMPLE.bin --base 0x10000000 --family 0xe48bff56 --output target/$EXAMPLE.uf2 && \
+cp target/$EXAMPLE.uf2 $RPI_MOUNT_FOLDER
+```
 
 <!-- ROADMAP -->
 ## Roadmap
@@ -80,7 +101,6 @@ NOTE This HAL is under active development. As such, it is likely to remain volat
 See the [open issues](https://github.com/rp-rs/rp-hal/issues) for a list of proposed features (and known issues).
 
 
-
 <!-- CONTRIBUTING -->
 ## Contributing
 

boards/pico/Cargo.toml

@@ -13,6 +13,12 @@ license = "MIT OR Apache-2.0"
 cortex-m = "0.7.2"
 rp2040-hal = { path = "../../rp2040-hal", version = "0.1.0" }
 cortex-m-rt = { version = "0.6.14", optional = true }
+embedded-time = "0.10.1"
+
+[dev-dependencies]
+panic-halt= "0.2.0"
+embedded-hal ="0.2.5"
+rp2040-boot2 = "0.1.2"
 
 [features]
 default = ["rt"]

boards/pico/examples/pico_blinky.rs

@@ -0,0 +1,60 @@
+//! Blinks the LED on a Pico board
+//!
+//! This will blink an LED attached to GP25, which is the pin the Pico uses for the on-board LED.
+#![no_std]
+#![no_main]
+
+use cortex_m_rt::entry;
+use embedded_hal::digital::v2::OutputPin;
+use embedded_time::rate::*;
+use panic_halt as _;
+use pico::{
+    hal::{
+        clocks::{init_clocks_and_plls, Clock},
+        pac,
+        sio::Sio,
+        watchdog::Watchdog,
+    },
+    Pins, XOSC_CRYSTAL_FREQ,
+};
+#[link_section = ".boot2"]
+#[used]
+pub static BOOT2: [u8; 256] = rp2040_boot2::BOOT_LOADER;
+
+#[entry]
+fn main() -> ! {
+    let mut pac = pac::Peripherals::take().unwrap();
+    let core = pac::CorePeripherals::take().unwrap();
+
+    let mut watchdog = Watchdog::new(pac.WATCHDOG);
+
+    let clocks = init_clocks_and_plls(
+        XOSC_CRYSTAL_FREQ,
+        pac.XOSC,
+        pac.CLOCKS,
+        pac.PLL_SYS,
+        pac.PLL_USB,
+        &mut pac.RESETS,
+        &mut watchdog,
+    )
+    .ok()
+    .unwrap();
+
+    let mut delay = cortex_m::delay::Delay::new(core.SYST, *clocks.system_clock.freq().integer());
+
+    let sio = Sio::new(pac.SIO);
+    let pins = Pins::new(
+        pac.IO_BANK0,
+        pac.PADS_BANK0,
+        sio.gpio_bank0,
+        &mut pac.RESETS,
+    );
+    let mut led_pin = pins.led.into_push_pull_output();
+
+    loop {
+        led_pin.set_high().unwrap();
+        delay.delay_ms(500);
+        led_pin.set_low().unwrap();
+        delay.delay_ms(500);
+    }
+}

boards/pico/examples/pico_gpio_in_out.rs

@@ -0,0 +1,41 @@
+//! Toggle LED based on GPIO input
+//!
+//! This will control an LED on GP25 based on a button hooked up to GP15. The button should be tied
+//! to ground, as the input pin is pulled high internally by this example. When the button is
+//! pressed, the LED will turn off.
+#![no_std]
+#![no_main]
+
+use cortex_m_rt::entry;
+use embedded_hal::digital::v2::{InputPin, OutputPin};
+use hal::pac;
+use hal::sio::Sio;
+use panic_halt as _;
+use pico::{hal, Pins};
+
+#[link_section = ".boot2"]
+#[used]
+pub static BOOT2: [u8; 256] = rp2040_boot2::BOOT_LOADER;
+
+#[entry]
+fn main() -> ! {
+    let mut pac = pac::Peripherals::take().unwrap();
+
+    let sio = Sio::new(pac.SIO);
+    let pins = Pins::new(
+        pac.IO_BANK0,
+        pac.PADS_BANK0,
+        sio.gpio_bank0,
+        &mut pac.RESETS,
+    );
+    let mut led_pin = pins.led.into_push_pull_output();
+    let button_pin = pins.bootsel.into_pull_down_input();
+
+    loop {
+        if button_pin.is_low().unwrap() {
+            led_pin.set_high().unwrap();
+        } else {
+            led_pin.set_low().unwrap();
+        }
+    }
+}

boards/pico/examples/pico_pwm_blink.rs

@@ -0,0 +1,77 @@
+//! Blinks the LED on a Pico board
+//!
+//! This will fade in/out the LED attached to GP25, which is the pin the Pico uses for the on-board LED.
+#![no_std]
+#![no_main]
+
+use cortex_m_rt::entry;
+use embedded_hal::PwmPin;
+use embedded_time::fixed_point::FixedPoint;
+use panic_halt as _;
+use pico::{
+    hal::{
+        clocks::{init_clocks_and_plls, Clock},
+        pac,
+        pwm::*,
+        watchdog::Watchdog,
+    },
+    XOSC_CRYSTAL_FREQ,
+};
+
+#[link_section = ".boot2"]
+#[used]
+pub static BOOT2: [u8; 256] = rp2040_boot2::BOOT_LOADER;
+
+const LOW: u16 = 0;
+const HIGH: u16 = 25000;
+
+#[entry]
+fn main() -> ! {
+    let mut pac = pac::Peripherals::take().unwrap();
+    let core = pac::CorePeripherals::take().unwrap();
+
+    let mut watchdog = Watchdog::new(pac.WATCHDOG);
+
+    let clocks = init_clocks_and_plls(
+        XOSC_CRYSTAL_FREQ,
+        pac.XOSC,
+        pac.CLOCKS,
+        pac.PLL_SYS,
+        pac.PLL_USB,
+        &mut pac.RESETS,
+        &mut watchdog,
+    )
+    .ok()
+    .unwrap();
+
+    let mut delay = cortex_m::delay::Delay::new(core.SYST, *clocks.system_clock.freq().integer());
+
+    let mut pwm_pin = Pwm4::new(25);
+
+    //Instead of having it take references to all of these pac objects, eventually this should just
+    //take ownership of a GPIO pin.
+    pwm_pin.default_config(
+        &mut pac.PWM,
+        &mut pac.PADS_BANK0,
+        &mut pac.IO_BANK0,
+        &mut pac.RESETS,
+    );
+
+    pwm_pin.set_ph_correct();
+
+    pwm_pin.enable();
+
+    loop {
+        for i in (LOW..=HIGH).skip(100) {
+            delay.delay_us(8);
+            pwm_pin.set_duty(i);
+        }
+
+        for i in (LOW..=HIGH).rev().skip(100) {
+            delay.delay_us(8);
+            pwm_pin.set_duty(i);
+        }
+
+        delay.delay_ms(500);
+    }
+}

boards/pico/src/lib.rs

@@ -1,6 +1,6 @@
 #![no_std]
 
-extern crate rp2040_hal as hal;
+pub extern crate rp2040_hal as hal;
 
 #[cfg(feature = "rt")]
 extern crate cortex_m_rt;
@@ -43,3 +43,5 @@ hal::bsp_pins!(
         name: voltage_monitor
     },
 );
+
+pub const XOSC_CRYSTAL_FREQ: u32 = 12_000_000;

boards/pico_explorer/src/lib.rs

@@ -92,3 +92,5 @@ hal::bsp_pins!(
     Gpio27 { name: adc1 },
     Gpio28 { name: adc2 },
 );
+
+pub const XOSC_CRYSTAL_FREQ: u32 = 12_000_000;

boards/pico_lipo_16mb/src/lib.rs

@@ -41,3 +41,5 @@ hal::bsp_pins!(
     Gpio28 { name: gpio28 },
     Gpio29 { name: batt_sense },
 );
+
+pub const XOSC_CRYSTAL_FREQ: u32 = 12_000_000;

rp2040-hal/examples/gpio_in_out.rs

@@ -29,10 +29,10 @@ fn main() -> ! {
         &mut pac.RESETS,
     );
     let mut led_pin = pins.gpio25.into_push_pull_output();
-    let button_pin = pins.gpio15.into_pull_up_input();
+    let button_pin = pins.gpio23.into_pull_down_input();
 
     loop {
-        if button_pin.is_high().unwrap() {
+        if button_pin.is_low().unwrap() {
             led_pin.set_high().unwrap();
         } else {
             led_pin.set_low().unwrap();

rp2040-hal/examples/pwm_blink.rs

@@ -14,7 +14,7 @@ pub static BOOT2: [u8; 256] = rp2040_boot2::BOOT_LOADER;
 fn main() -> ! {
     let mut pac = rp2040_pac::Peripherals::take().unwrap();
 
-    let mut pwm_pin = Pwm0::new(0);
+    let mut pwm_pin = Pwm4::new(25);
 
     //Instead of having it take references to all of these pac objects, eventually this should just
     //take ownership of a GPIO pin.

rp2040-hal/src/clocks/available_clocks.rs

@@ -1,253 +0,0 @@
-//! Available clocks
-
-use super::{ClocksManager, ShareableClocks};
-use crate::{
-    gpio::{
-        bank0::{Gpio20, Gpio22},
-        FunctionClock, Pin,
-    },
-    pll::{Locked, PhaseLockedLoop},
-    rosc::{Enabled, RingOscillator},
-    typelevel::Sealed,
-    xosc::{CrystalOscillator, Stable},
-};
-use core::{convert::TryInto, marker::PhantomData};
-use embedded_time::rate::*;
-use pac::{PLL_SYS, PLL_USB};
-
-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)
-}
-
-/// For clocks with a divider
-trait ClockDivision {
-    /// Set integer divider value.
-    fn set_div(&mut self, div: u32);
-    /// Get integer diveder value.
-    fn get_div(&self) -> u32;
-}
-
-/// Clock with glitchless source
-pub trait GlitchlessClock {
-    /// Self type to hand to ChangingClockToken
-    type Clock: GlitchlessClock;
-
-    /// Await switching clock sources without glitches. Needs a token that is returned when setting
-    fn await_select(&self, clock_token: &ChangingClockToken<Self::Clock>) -> nb::Result<(), ()>;
-}
-
-/// Token which can be used to await the glitchless switch
-pub struct ChangingClockToken<G: GlitchlessClock> {
-    clock_nr: u8,
-    clock: PhantomData<G>,
-}
-
-/// For clocks that can be disabled
-pub trait StoppableClock {
-    /// Enables the clock.
-    fn enable(&mut self);
-
-    /// Disables the clock.
-    fn disable(&mut self);
-
-    /// Kills the clock.
-    fn kill(&mut self);
-}
-
-/// Trait for things that can be used as clock source
-pub trait ClockSource: Sealed {
-    /// Get the operating frequency for this source
-    ///
-    /// Used to determine the divisor
-    fn get_freq(&self) -> Hertz;
-}
-
-type PllSys = PhaseLockedLoop<Locked, PLL_SYS>;
-impl Sealed for PllSys {}
-impl ClockSource for PllSys {
-    fn get_freq(&self) -> Hertz {
-        self.operating_frequency()
-    }
-}
-
-type PllUsb = PhaseLockedLoop<Locked, PLL_USB>;
-impl Sealed for PllUsb {}
-impl ClockSource for PllUsb {
-    fn get_freq(&self) -> Hertz {
-        self.operating_frequency()
-    }
-}
-
-impl ClockSource for UsbClock {
-    fn get_freq(&self) -> Hertz {
-        self.frequency
-    }
-}
-
-impl ClockSource for AdcClock {
-    fn get_freq(&self) -> Hertz {
-        self.frequency
-    }
-}
-
-impl ClockSource for RtcClock {
-    fn get_freq(&self) -> Hertz {
-        self.frequency
-    }
-}
-
-impl ClockSource for SystemClock {
-    fn get_freq(&self) -> Hertz {
-        self.frequency
-    }
-}
-
-impl ClockSource for ReferenceClock {
-    fn get_freq(&self) -> Hertz {
-        self.frequency
-    }
-}
-
-type Xosc = CrystalOscillator<Stable>;
-impl Sealed for Xosc {}
-impl ClockSource for Xosc {
-    fn get_freq(&self) -> Hertz {
-        self.operating_frequency()
-    }
-}
-
-type Rosc = RingOscillator<Enabled>;
-impl Sealed for Rosc {}
-// We are assuming the second output is never phase shifted (see 2.17.4)
-impl ClockSource for RingOscillator<Enabled> {
-    fn get_freq(&self) -> Hertz {
-        self.operating_frequency()
-    }
-}
-
-// GPIN0
-type GPin0 = Pin<Gpio20, FunctionClock>;
-impl ClockSource for GPin0 {
-    fn get_freq(&self) -> Hertz {
-        todo!()
-    }
-}
-
-// GPIN1
-type GPin1 = Pin<Gpio22, FunctionClock>;
-impl ClockSource for Pin<Gpio22, FunctionClock> {
-    fn get_freq(&self) -> Hertz {
-        todo!()
-    }
-}
-
-/// Trait to contrain which ClockSource is valid for which Clock
-pub trait ValidSrc<Clock>: Sealed {
-    /// Which register values are acceptable
-    type Variant;
-
-    /// Is this a ClockSource for src or aux?
-    fn is_aux(&self) -> bool;
-    /// Get register value for this ClockSource
-    fn variant(&self) -> Self::Variant;
-}
-
-clock!(
-    /// GPIO Output 0 Clock
-    struct GpioOutput0Clock {
-        reg: clk_gpout0,
-        auxsrc: {PllSys:CLKSRC_PLL_SYS, GPin0:CLKSRC_GPIN0, GPin1:CLKSRC_GPIN1, PllUsb:CLKSRC_PLL_USB, Rosc: ROSC_CLKSRC, Xosc: XOSC_CLKSRC, SystemClock: CLK_SYS, UsbClock: CLK_USB, AdcClock: CLK_ADC, RtcClock: CLK_RTC, ReferenceClock:CLK_REF}
-    }
-);
-clock!(
-    /// GPIO Output 1 Clock
-    struct GpioOutput1Clock {
-        reg: clk_gpout1,
-        auxsrc: {PllSys:CLKSRC_PLL_SYS, GPin0:CLKSRC_GPIN0, GPin1:CLKSRC_GPIN1, PllUsb:CLKSRC_PLL_USB, Rosc: ROSC_CLKSRC, Xosc: XOSC_CLKSRC, SystemClock: CLK_SYS, UsbClock: CLK_USB, AdcClock: CLK_ADC, RtcClock: CLK_RTC, ReferenceClock:CLK_REF}
-    }
-);
-clock!(
-    /// GPIO Output 2 Clock
-    struct GpioOutput2Clock {
-        reg: clk_gpout2,
-        auxsrc: {PllSys:CLKSRC_PLL_SYS, GPin0:CLKSRC_GPIN0, GPin1:CLKSRC_GPIN1, PllUsb:CLKSRC_PLL_USB, Rosc: ROSC_CLKSRC_PH, Xosc: XOSC_CLKSRC, SystemClock: CLK_SYS, UsbClock: CLK_USB, AdcClock: CLK_ADC, RtcClock: CLK_RTC, ReferenceClock:CLK_REF}
-    }
-);
-clock!(
-    /// GPIO Output 3 Clock
-    struct GpioOutput3Clock {
-        reg: clk_gpout3,
-        auxsrc: {PllSys:CLKSRC_PLL_SYS, GPin0:CLKSRC_GPIN0, GPin1:CLKSRC_GPIN1, PllUsb:CLKSRC_PLL_USB, Rosc: ROSC_CLKSRC_PH, Xosc: XOSC_CLKSRC, SystemClock: CLK_SYS, UsbClock: CLK_USB, AdcClock: CLK_ADC, RtcClock: CLK_RTC, ReferenceClock:CLK_REF}
-    }
-);
-clock!(
-    /// Reference Clock
-    struct ReferenceClock {
-        reg: clk_ref,
-        src: {Rosc: ROSC_CLKSRC_PH, Xosc:XOSC_CLKSRC},
-        auxsrc: {PllUsb:CLKSRC_PLL_USB, GPin0:CLKSRC_GPIN0, GPin1:CLKSRC_GPIN1}
-    }
-);
-clock!(
-    /// System Clock
-    struct SystemClock {
-        reg: clk_sys,
-        src: {ReferenceClock: CLK_REF},
-        auxsrc: {PllSys: CLKSRC_PLL_SYS, PllUsb:CLKSRC_PLL_USB, Rosc: ROSC_CLKSRC, Xosc: XOSC_CLKSRC,GPin0:CLKSRC_GPIN0, GPin1:CLKSRC_GPIN1}
-    }
-);
-clock!(
-    /// Peripheral Clock
-    struct PeripheralClock {
-        reg: clk_peri,
-        auxsrc: {SystemClock: CLK_SYS, PllSys: CLKSRC_PLL_SYS, PllUsb:CLKSRC_PLL_USB, Rosc: ROSC_CLKSRC_PH, Xosc: XOSC_CLKSRC,GPin0:CLKSRC_GPIN0, GPin1:CLKSRC_GPIN1 },
-        div: false
-    }
-);
-clock!(
-    /// USB Clock
-    struct UsbClock {
-        reg: clk_usb,
-        auxsrc: {PllUsb:CLKSRC_PLL_USB,PllSys: CLKSRC_PLL_SYS,  Rosc: ROSC_CLKSRC_PH, Xosc: XOSC_CLKSRC,GPin0:CLKSRC_GPIN0, GPin1:CLKSRC_GPIN1}
-    }
-);
-clock!(
-    /// Adc Clock
-    struct AdcClock {
-        reg: clk_adc,
-        auxsrc: {PllUsb:CLKSRC_PLL_USB,PllSys: CLKSRC_PLL_SYS,  Rosc: ROSC_CLKSRC_PH, Xosc: XOSC_CLKSRC,GPin0:CLKSRC_GPIN0, GPin1:CLKSRC_GPIN1}
-    }
-);
-clock!(
-    /// RTC Clock
-    struct RtcClock {
-        reg: clk_rtc,
-        auxsrc: {PllUsb:CLKSRC_PLL_USB,PllSys: CLKSRC_PLL_SYS,  Rosc: ROSC_CLKSRC_PH, Xosc: XOSC_CLKSRC,GPin0:CLKSRC_GPIN0, GPin1:CLKSRC_GPIN1}
-    }
-);
-
-impl SystemClock {
-    fn get_default_clock_source(&self) -> pac::clocks::clk_sys_ctrl::SRC_A {
-        pac::clocks::clk_sys_ctrl::SRC_A::CLK_REF
-    }
-
-    fn get_aux_source(&self) -> pac::clocks::clk_sys_ctrl::SRC_A {
-        pac::clocks::clk_sys_ctrl::SRC_A::CLKSRC_CLK_SYS_AUX
-    }
-}
-
-impl ReferenceClock {
-    fn get_default_clock_source(&self) -> pac::clocks::clk_ref_ctrl::SRC_A {
-        pac::clocks::clk_ref_ctrl::SRC_A::ROSC_CLKSRC_PH
-    }
-
-    fn get_aux_source(&self) -> pac::clocks::clk_ref_ctrl::SRC_A {
-        pac::clocks::clk_ref_ctrl::SRC_A::CLKSRC_CLK_REF_AUX
-    }
-}

rp2040-hal/src/clocks/clock_sources.rs

@@ -0,0 +1,93 @@
+//! Available clocks
+
+use super::*;
+use crate::{
+    gpio::{
+        bank0::{Gpio20, Gpio22},
+        FunctionClock, Pin,
+    },
+    pll::{Locked, PhaseLockedLoop},
+    rosc::{Enabled, RingOscillator},
+    typelevel::Sealed,
+    xosc::{CrystalOscillator, Stable},
+};
+use pac::{PLL_SYS, PLL_USB};
+
+pub(crate) type PllSys = PhaseLockedLoop<Locked, PLL_SYS>;
+impl Sealed for PllSys {}
+impl ClockSource for PllSys {
+    fn get_freq(&self) -> Hertz {
+        self.operating_frequency()
+    }
+}
+
+pub(crate) type PllUsb = PhaseLockedLoop<Locked, PLL_USB>;
+impl Sealed for PllUsb {}
+impl ClockSource for PllUsb {
+    fn get_freq(&self) -> Hertz {
+        self.operating_frequency()
+    }
+}
+
+impl ClockSource for UsbClock {
+    fn get_freq(&self) -> Hertz {
+        self.frequency
+    }
+}
+
+impl ClockSource for AdcClock {
+    fn get_freq(&self) -> Hertz {
+        self.frequency
+    }
+}
+
+impl ClockSource for RtcClock {
+    fn get_freq(&self) -> Hertz {
+        self.frequency
+    }
+}
+
+impl ClockSource for SystemClock {
+    fn get_freq(&self) -> Hertz {
+        self.frequency
+    }
+}
+
+impl ClockSource for ReferenceClock {
+    fn get_freq(&self) -> Hertz {
+        self.frequency
+    }
+}
+
+pub(crate) type Xosc = CrystalOscillator<Stable>;
+impl Sealed for Xosc {}
+impl ClockSource for Xosc {
+    fn get_freq(&self) -> Hertz {
+        self.operating_frequency()
+    }
+}
+
+pub(crate) type Rosc = RingOscillator<Enabled>;
+impl Sealed for Rosc {}
+// We are assuming the second output is never phase shifted (see 2.17.4)
+impl ClockSource for RingOscillator<Enabled> {
+    fn get_freq(&self) -> Hertz {
+        self.operating_frequency()
+    }
+}
+
+// GPIN0
+pub(crate) type GPin0 = Pin<Gpio20, FunctionClock>;
+impl ClockSource for GPin0 {
+    fn get_freq(&self) -> Hertz {
+        todo!()
+    }
+}
+
+// GPIN1
+pub(crate) type GPin1 = Pin<Gpio22, FunctionClock>;
+impl ClockSource for Pin<Gpio22, FunctionClock> {
+    fn get_freq(&self) -> Hertz {
+        todo!()
+    }
+}

rp2040-hal/src/clocks/macros.rs

@@ -1,3 +1,62 @@
+macro_rules! clocks {
+    (
+        $(
+            $(#[$attr:meta])*
+            struct $name:ident {
+                init_freq: $init_freq:expr,
+                reg: $reg:ident,
+                $(src: {$($src:ident: $src_variant:ident),*},)?
+                auxsrc: {$($auxsrc:ident: $aux_variant:ident),*}
+                $(, div: $div:tt)?
+            }
+        )*
+
+    ) => {
+
+        $crate::paste::paste!{
+            /// Abstraction layer providing Clock Management.
+            pub struct ClocksManager {
+                clocks: CLOCKS,
+                $(
+                    #[doc = "`" $name "` field"]
+                    pub [<$name:snake>]: $name,
+                )*
+            }
+
+            impl ClocksManager {
+                /// Exchanges CLOCKS block against Self.
+                pub fn new(mut clocks_block: CLOCKS) -> Self {
+                    // 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,
+                        $(
+                            [<$name:snake>]: $name {
+                                shared_dev: shared_clocks,
+                                frequency: $init_freq.Hz(),
+                            },
+                        )*
+                    }
+                }
+            }
+        }
+
+        $(
+            clock!(
+                $(#[$attr])*
+                struct $name {
+                    reg: $reg,
+                    $(src: {$($src: $src_variant),*},)?
+                    auxsrc: {$($auxsrc: $aux_variant),*}
+                    $(, div: $div )?
+                }
+            );
+        )*
+    };
+}
+
 macro_rules! clock {
     {
         $(#[$attr:meta])*
@@ -16,8 +75,6 @@ macro_rules! clock {
 
         $crate::paste::paste!{
             $(impl ValidSrc<$name> for $src {
-                type Variant = [<$reg:camel SrcType>];
-
                 fn is_aux(&self) -> bool{
                     false
                 }
@@ -29,7 +86,7 @@ macro_rules! clock {
             impl GlitchlessClock for $name {
                 type Clock = Self;
 
-                fn await_select(&self, clock_token: &ChangingClockToken<Self>) -> nb::Result<(),()> {
+                fn await_select(&self, clock_token: &ChangingClockToken<Self>) -> nb::Result<(), Infallible> {
                     let shared_dev = unsafe { self.shared_dev.get() };
 
                     let selected = shared_dev.[<$reg _selected>].read().bits();
@@ -41,11 +98,11 @@ macro_rules! clock {
                 }
             }
 
-            /// Holds register value for ClockSource for this clock
+            #[doc = "Holds register value for ClockSource for `"$name"`"]
             pub enum [<$reg:camel SrcType>] {
-                /// Its an clock source that is to be used as source
+                #[doc = "Contains a valid clock source register value that is to be used to set a clock as glitchless source for `"$name"`"]
                 Src(pac::clocks::[<$reg _ctrl>]::SRC_A),
-                /// Its an clock source that is to be used as aux source
+                #[doc = "Contains a valid clock source register value that is to be used to set a clock as aux source for `"$name"`"]
                 Aux(pac::clocks::[<$reg _ctrl>]::AUXSRC_A)
             }
 
@@ -73,18 +130,20 @@ macro_rules! clock {
             }
 
             impl $name {
-                /// WIP - Helper function to reset source (blocking)
+                /// Reset clock back to its reset source
-                pub fn reset_source_await(&mut self) -> nb::Result<(), ()> {
+                pub fn reset_source_await(&mut self) -> nb::Result<(), Infallible> {
                     let shared_dev = unsafe { self.shared_dev.get() };
 
                     shared_dev.[<$reg _ctrl>].modify(|_, w| {
                         w.src().variant(self.get_default_clock_source())
                     });
 
+                    self.frequency = 12_000_000.Hz(); //TODO Get actual clock source.. Most likely 12 MHz though
+
                     self.await_select(&ChangingClockToken{clock_nr:0, clock: PhantomData::<Self>})
                 }
 
-                fn set_src<S:ClockSource + ValidSrc<$name, Variant=[<$reg:camel SrcType>]>>(&mut self, src: &S)-> ChangingClockToken<$name> {
+                fn set_src<S: ValidSrc<$name>>(&mut self, src: &S)-> ChangingClockToken<$name> {
                     let shared_dev = unsafe { self.shared_dev.get() };
 
                     shared_dev.[<$reg _ctrl>].modify(|_,w| {
@@ -107,17 +166,31 @@ macro_rules! clock {
                         clock_nr: pac::clocks::clk_ref_ctrl::SRC_A::CLKSRC_CLK_REF_AUX as u8,
                     }
                 }
+            }
 
-                /// Configure this clock based on a clock source and desired frequency
+            impl Clock for $name {
-                pub fn configure_clock<S:ClockSource + ValidSrc<$name, Variant=[<$reg:camel SrcType>]>>(&mut self, src: &S, freq: Hertz) -> bool{
+                type Variant = [<$reg:camel SrcType>];
-                    let src_freq: Hertz  = src.get_freq();
+
+                #[doc = "Get operating frequency for `"$name"`"]
+                fn freq(&self) -> Hertz {
+                    self.frequency
+                }
+
+                #[doc = "Configure `"$name"`"]
+                fn configure_clock<S: ValidSrc<$name>>(&mut self, src: &S, freq: Hertz) -> Result<(), ClockError>{
+                    let src_freq: Hertz<u64> = src.get_freq().into();
 
                     if freq.gt(&src_freq){
-                        return false;
+                        return Err(ClockError::CantIncreaseFreq);
                     }
 
                     // Div register is 24.8) int.frac divider so multiply by 2^8 (left shift by 8)
-                    let div = make_div(src_freq, freq).unwrap();
+                    let shifted_src_freq = src_freq * (1 << 8);
+                    let div = if freq.eq(&src_freq) {
+                        1 << 8
+                    } else {
+                        *(shifted_src_freq / *freq.integer() as u64).integer() as u32
+                    };
 
                     // If increasing divisor, set divisor before source. Otherwise set source
                     // before divisor. This avoids a momentary overspeed when e.g. switching
@@ -148,9 +221,10 @@ macro_rules! clock {
                     self.set_div(div);
 
                     // Store the configured frequency
-                    self.frequency = src_freq / div;
+                    // div contains both the integer part and the fractional part so we need to shift the src_freq equally
+                    self.frequency = (shifted_src_freq / div as u64).try_into().map_err(|_| ClockError::FrequencyToHigh)?;
 
-                    true
+                    Ok(())
                 }
             }
         }
@@ -168,7 +242,7 @@ macro_rules! clock {
             ($name, $reg, auxsrc={$($auxsrc: $variant),*})
         }
 
-        // Just to match proper divisable clocks so we don't have to do something special in configure function
+        // Just to match proper divisible clocks so we don't have to do something special in configure function
         impl ClockDivision for $name {
             fn set_div(&mut self, _: u32) {}
             fn get_div(&self) -> u32 {1}
@@ -214,9 +288,9 @@ macro_rules! divisable_clock {
 macro_rules! stoppable_clock {
     ($name:ident, $reg:ident) => {
         $crate::paste::paste!{
-            /// Holds register value for ClockSource for this clock
+            #[doc = "Holds register value for ClockSource for `"$name"`"]
             pub enum [<$reg:camel SrcType>] {
-                /// Its an clock source that is to be used as aux source
+                #[doc = "Contains a valid clock source register value that is to be used to set a clock as aux source for `"$name"`"]
                 Aux(pac::clocks::[<$reg _ctrl>]::AUXSRC_A)
             }
 
@@ -229,18 +303,21 @@ macro_rules! stoppable_clock {
             }
 
             impl StoppableClock for $name {
+                /// Enable the clock
                 fn enable(&mut self) {
                     unsafe { self.shared_dev.get() }.[<$reg _ctrl>].modify(|_, w| {
                         w.enable().set_bit()
                     });
                 }
 
+                /// Disable the clock cleanly
                 fn disable(&mut self) {
                     unsafe { self.shared_dev.get() }.[<$reg _ctrl>].modify(|_, w| {
                         w.enable().clear_bit()
                     });
                 }
 
+                /// Disable the clock asynchronously
                 fn kill(&mut self) {
                     unsafe { self.shared_dev.get() }.[<$reg _ctrl>].modify(|_, w| {
                         w.kill().set_bit()
@@ -248,17 +325,29 @@ macro_rules! stoppable_clock {
                 }
             }
 
-            impl $name {
+            impl Clock for $name {
-                /// Configure this clock based on a clock source and desired frequency
+                type Variant = [<$reg:camel SrcType>];
-                pub fn configure_clock<S:ClockSource + ValidSrc<$name, Variant=[<$reg:camel SrcType>]>>(&mut self, src: &S, freq: Hertz) -> bool{
+
-                    let src_freq: Hertz  = src.get_freq();
+                #[doc = "Get operating frequency for `"$name"`"]
+                fn freq(&self) -> Hertz {
+                    self.frequency
+                }
+
+                #[doc = "Configure `"$name"`"]
+                fn configure_clock<S: ValidSrc<$name>>(&mut self, src: &S, freq: Hertz) -> Result<(), ClockError>{
+                    let src_freq: Hertz<u64> = src.get_freq().into();
 
                     if freq.gt(&src_freq){
-                        return false;
+                        return Err(ClockError::CantIncreaseFreq);
                     }
 
                     // Div register is 24.8) int.frac divider so multiply by 2^8 (left shift by 8)
-                    let div = make_div(src_freq, freq).unwrap();
+                    let shifted_src_freq = src_freq * (1 << 8);
+                    let div = if freq.eq(&src_freq) {
+                        1 << 8
+                    } else {
+                        *(shifted_src_freq / *freq.integer() as u64).integer() as u32
+                    };
 
                     // If increasing divisor, set divisor before source. Otherwise set source
                     // before divisor. This avoids a momentary overspeed when e.g. switching
@@ -278,9 +367,9 @@ macro_rules! stoppable_clock {
                         // Delay for 3 cycles of the target clock, for ENABLE propagation.
                         // Note XOSC_COUNT is not helpful here because XOSC is not
                         // necessarily running, nor is timer... so, 3 cycles per loop:
-                        let sys_freq = 125_000_000.Hz(); // TODO
+                        let sys_freq = 125_000_000; // TODO get actual sys_clk frequency
-                        let delay_cyc = sys_freq.div( *self.frequency.integer() ) + 1u32.Hz();
+                        let delay_cyc = sys_freq / *self.frequency.integer() + 1u32;
-                        cortex_m::asm::delay(*delay_cyc.integer());
+                        cortex_m::asm::delay(delay_cyc);
                     }
 
                     // Set aux mux first, and then glitchless mux if this self has one
@@ -295,8 +384,9 @@ macro_rules! stoppable_clock {
                     self.set_div(div);
 
                     // Store the configured frequency
-                    self.frequency = src_freq / div;
+                    self.frequency = (shifted_src_freq / div as u64).try_into().map_err(|_| ClockError::FrequencyToHigh)?;
-                    true
+
+                    Ok(())
                 }
             }
         }
@@ -311,7 +401,6 @@ macro_rules! base_clock {
         $crate::paste::paste!{
 
             $(impl ValidSrc<$name> for $auxsrc {
-                type Variant = [<$reg:camel SrcType>];
 
                 fn is_aux(&self) -> bool{
                     true
@@ -321,18 +410,6 @@ macro_rules! base_clock {
                 }
             })*
 
-            impl ClocksManager {
-                    #[ doc = "Getter for the" $name ]
-                    pub fn [<$name:snake>](&self) -> $name {
-
-                        //TODO: Init clock here
-                        $name {
-                            shared_dev: self.shared_clocks,
-                            frequency: 0.Hz(),
-                        }
-                    }
-
-            }
             $(#[$attr])*
             pub struct $name {
                 shared_dev: ShareableClocks,
@@ -340,12 +417,7 @@ macro_rules! base_clock {
             }
 
             impl $name {
-                /// Returns the frequency of the configured clock
+                fn set_aux<S: ValidSrc<$name>>(&mut self, src: &S) {
-                pub fn freq(&self) -> Hertz {
-                    self.frequency
-                }
-
-                fn set_aux<S:ClockSource + ValidSrc<$name, Variant=[<$reg:camel SrcType>]>>(&mut self, src: &S) {
                     let shared_dev = unsafe { self.shared_dev.get() };
 
                     shared_dev.[<$reg _ctrl>].modify(|_,w| {

rp2040-hal/src/clocks/mod.rs

@@ -2,63 +2,78 @@
 //!
 //!
 //!
-//! Usage:
+//! ## Usage simple
+//! ```rust
+//! let mut p = rp2040_pac::Peripherals::take().unwrap();
+//! let mut watchdog = Watchdog::new(p.WATCHDOG);
+//! const XOSC_CRYSTAL_FREQ: u32 = 12_000_000; // Typically found in BSP crates
+//! let mut clocks = init_clocks_and_plls(XOSC_CRYSTAL_FREQ, p.XOSC, p.CLOCKS, p.PLL_SYS, p.PLL_USB, &mut p.RESETS, &mut watchdog).ok().unwrap();
+//! ```
+//!
+//! ## Usage extended
 //! ```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();
+//! const XOSC_CRYSTAL_FREQ: u32 = 12_000_000; // Typically found in BSP crates
+//! let xosc = setup_xosc_blocking(p.XOSC, XOSC_CRYSTAL_FREQ.Hz()).map_err(InitError::XoscErr)?;
 //!
 //! // 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_sys = setup_pll_blocking(p.PLL_SYS, xosc.into(), PLL_SYS_125MHZ, &mut clocks, &mut p.RESETS).map_err(InitError::PllError)?;
-//! let pll_usb = setup_pll_blocking(p.PLL_USB, 12.MHz().into(), PLL_USB_48MHZ, &mut clocks, &mut p.RESETS).ok().unwrap();
+//! let pll_usb = setup_pll_blocking(p.PLL_USB, xosc.into(), PLL_USB_48MHZ, &mut clocks, &mut p.RESETS).map_err(InitError::PllError)?;
 //!
 //! // Configure clocks
 //! // CLK_REF = XOSC (12MHz) / 1 = 12MHz
-//! let mut ref_clock = clocks.reference_clock();
+//! self.reference_clock.configure_clock(xosc, xosc.get_freq()).map_err(InitError::ClockError)?;
-//! ref_clock.configure_clock(&xosc, xosc.get_freq());
 //!
 //! // CLK SYS = PLL SYS (125MHz) / 1 = 125MHz
-//! let mut sys_clock = clocks.system_clock();
+//! self.system_clock.configure_clock(pll_sys, pll_sys.get_freq()).map_err(InitError::ClockError)?;
-//! sys_clock.configure_clock(&pll_sys, pll_sys.get_freq());
 //!
 //! // CLK USB = PLL USB (48MHz) / 1 = 48MHz
-//! let mut usb_clock = clocks.usb_clock();
+//! self.usb_clock.configure_clock(pll_usb, pll_usb.get_freq()).map_err(InitError::ClockError)?;
-//! usb_clock.configure_clock(&pll_usb, pll_usb.get_freq());
 //!
 //! // CLK ADC = PLL USB (48MHZ) / 1 = 48MHz
-//! let mut adc_clock = clocks.adc_clock();
+//! self.adc_clock.configure_clock(pll_usb, pll_usb.get_freq()).map_err(InitError::ClockError)?;
-//! adc_clock.configure_clock(&pll_usb, pll_usb.get_freq());
 //!
 //! // CLK RTC = PLL USB (48MHz) / 1024 = 46875Hz
-//! let mut rtc_clock = clocks.rtc_clock();
+//! self.rtc_clock.configure_clock(pll_usb, 46875u32.Hz()).map_err(InitError::ClockError)?;
-//! rtc_clock.configure_clock(&pll_usb, 46875u32.Hz());
 //!
 //! // 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 = clocks.peripheral_clock();
+//! self.peripheral_clock.configure_clock(&self.system_clock, self.system_clock.freq()).map_err(InitError::ClockError)?;
-//! peripheral_clock.configure_clock(&sys_clock, sys_clock.freq());
 //!
 //! ```
 //!
 //! See [Chapter 2 Section 15](https://datasheets.raspberrypi.org/rp2040/rp2040_datasheet.pdf) for more details
 
 use crate::{
-    clocks::available_clocks::ClockSource,
+    pll::{
-    pll::{Locked, PhaseLockedLoop},
+        common_configs::{PLL_SYS_125MHZ, PLL_USB_48MHZ},
+        setup_pll_blocking, Error as PllError, Locked, PhaseLockedLoop,
+    },
+    typelevel::Sealed,
     watchdog::Watchdog,
-    xosc::{CrystalOscillator, Stable},
+    xosc::{setup_xosc_blocking, CrystalOscillator, Error as XoscError, Stable},
+};
+use core::{
+    convert::{Infallible, TryInto},
+    marker::PhantomData,
 };
 use embedded_time::rate::*;
-use pac::{CLOCKS, PLL_SYS, PLL_USB};
+use pac::{CLOCKS, PLL_SYS, PLL_USB, RESETS, XOSC};
 
 #[macro_use]
 mod macros;
-pub mod available_clocks;
+mod clock_sources;
+
+use clock_sources::PllSys;
+
+use self::clock_sources::{GPin0, GPin1, PllUsb, Rosc, Xosc};
+
 #[derive(Copy, Clone)]
 /// Provides refs to the CLOCKS block.
 struct ShareableClocks {
@@ -75,61 +90,202 @@ impl ShareableClocks {
     }
 }
 
-const XOSC_MHZ: u32 = 12_000_000_u32;
+/// Something when wrong setting up the clock
-
+pub enum ClockError {
-/// Abstraction layer providing Clock Management.
+    /// The frequency desired is higher than the source frequency
-pub struct ClocksManager {
+    CantIncreaseFreq,
-    clocks: CLOCKS,
+    /// The desired frequency is to high (would overflow an u32)
-    shared_clocks: ShareableClocks,
+    FrequencyToHigh,
 }
+
+/// For clocks
+pub trait Clock: Sealed + Sized {
+    /// Enum with valid source clocks register values for `Clock`
+    type Variant;
+
+    /// Get operating frequency
+    fn freq(&self) -> Hertz;
+
+    /// Configure this clock based on a clock source and desired frequency
+    fn configure_clock<S: ValidSrc<Self>>(
+        &mut self,
+        src: &S,
+        freq: Hertz,
+    ) -> Result<(), ClockError>;
+}
+
+/// For clocks with a divider
+trait ClockDivision {
+    /// Set integer divider value.
+    fn set_div(&mut self, div: u32);
+    /// Get integer diveder value.
+    fn get_div(&self) -> u32;
+}
+
+/// Clock with glitchless source
+trait GlitchlessClock {
+    /// Self type to hand to ChangingClockToken
+    type Clock: Clock;
+
+    /// Await switching clock sources without glitches. Needs a token that is returned when setting
+    fn await_select(
+        &self,
+        clock_token: &ChangingClockToken<Self::Clock>,
+    ) -> nb::Result<(), Infallible>;
+}
+
+/// Token which can be used to await the glitchless switch
+pub struct ChangingClockToken<G: Clock> {
+    clock_nr: u8,
+    clock: PhantomData<G>,
+}
+
+/// For clocks that can be disabled
+pub trait StoppableClock {
+    /// Enables the clock.
+    fn enable(&mut self);
+
+    /// Disables the clock.
+    fn disable(&mut self);
+
+    /// Kills the clock.
+    fn kill(&mut self);
+}
+
+/// Trait for things that can be used as clock source
+pub trait ClockSource: Sealed {
+    /// Get the operating frequency for this source
+    ///
+    /// Used to determine the divisor
+    fn get_freq(&self) -> Hertz;
+}
+
+/// Trait to contrain which ClockSource is valid for which Clock
+pub trait ValidSrc<C: Clock>: Sealed + ClockSource {
+    /// Is this a ClockSource for src or aux?
+    fn is_aux(&self) -> bool;
+    /// Get register value for this ClockSource
+    fn variant(&self) -> C::Variant;
+}
+
+clocks! {
+    /// GPIO Output 0 Clock
+    struct GpioOutput0Clock {
+        init_freq: 0,
+        reg: clk_gpout0,
+        auxsrc: {PllSys:CLKSRC_PLL_SYS, GPin0:CLKSRC_GPIN0, GPin1:CLKSRC_GPIN1, PllUsb:CLKSRC_PLL_USB, Rosc: ROSC_CLKSRC, Xosc: XOSC_CLKSRC, SystemClock: CLK_SYS, UsbClock: CLK_USB, AdcClock: CLK_ADC, RtcClock: CLK_RTC, ReferenceClock:CLK_REF}
+    }
+    /// GPIO Output 1 Clock
+    struct GpioOutput1Clock {
+        init_freq: 0,
+        reg: clk_gpout1,
+        auxsrc: {PllSys:CLKSRC_PLL_SYS, GPin0:CLKSRC_GPIN0, GPin1:CLKSRC_GPIN1, PllUsb:CLKSRC_PLL_USB, Rosc: ROSC_CLKSRC, Xosc: XOSC_CLKSRC, SystemClock: CLK_SYS, UsbClock: CLK_USB, AdcClock: CLK_ADC, RtcClock: CLK_RTC, ReferenceClock:CLK_REF}
+    }
+    /// GPIO Output 2 Clock
+    struct GpioOutput2Clock {
+        init_freq: 0,
+        reg: clk_gpout2,
+        auxsrc: {PllSys:CLKSRC_PLL_SYS, GPin0:CLKSRC_GPIN0, GPin1:CLKSRC_GPIN1, PllUsb:CLKSRC_PLL_USB, Rosc: ROSC_CLKSRC_PH, Xosc: XOSC_CLKSRC, SystemClock: CLK_SYS, UsbClock: CLK_USB, AdcClock: CLK_ADC, RtcClock: CLK_RTC, ReferenceClock:CLK_REF}
+    }
+    /// GPIO Output 3 Clock
+    struct GpioOutput3Clock {
+        init_freq: 0,
+        reg: clk_gpout3,
+        auxsrc: {PllSys:CLKSRC_PLL_SYS, GPin0:CLKSRC_GPIN0, GPin1:CLKSRC_GPIN1, PllUsb:CLKSRC_PLL_USB, Rosc: ROSC_CLKSRC_PH, Xosc: XOSC_CLKSRC, SystemClock: CLK_SYS, UsbClock: CLK_USB, AdcClock: CLK_ADC, RtcClock: CLK_RTC, ReferenceClock:CLK_REF}
+    }
+    /// Reference Clock
+    struct ReferenceClock {
+        init_freq: 12_000_000,  // Starts from ROSC which actually varies with input voltage etc, but 12 MHz seems to be a good value
+        reg: clk_ref,
+        src: {Rosc: ROSC_CLKSRC_PH, Xosc:XOSC_CLKSRC},
+        auxsrc: {PllUsb:CLKSRC_PLL_USB, GPin0:CLKSRC_GPIN0, GPin1:CLKSRC_GPIN1}
+    }
+    /// System Clock
+    struct SystemClock {
+        init_freq: 12_000_000,  // ref_clk is 12 MHz
+        reg: clk_sys,
+        src: {ReferenceClock: CLK_REF},
+        auxsrc: {PllSys: CLKSRC_PLL_SYS, PllUsb:CLKSRC_PLL_USB, Rosc: ROSC_CLKSRC, Xosc: XOSC_CLKSRC,GPin0:CLKSRC_GPIN0, GPin1:CLKSRC_GPIN1}
+    }
+    /// Peripheral Clock
+    struct PeripheralClock {
+        init_freq: 12_000_000,  // sys_clk is 12 MHz
+        reg: clk_peri,
+        auxsrc: {SystemClock: CLK_SYS, PllSys: CLKSRC_PLL_SYS, PllUsb:CLKSRC_PLL_USB, Rosc: ROSC_CLKSRC_PH, Xosc: XOSC_CLKSRC,GPin0:CLKSRC_GPIN0, GPin1:CLKSRC_GPIN1 },
+        div: false
+    }
+    /// USB Clock
+    struct UsbClock {
+        init_freq: 0,
+        reg: clk_usb,
+        auxsrc: {PllUsb:CLKSRC_PLL_USB,PllSys: CLKSRC_PLL_SYS,  Rosc: ROSC_CLKSRC_PH, Xosc: XOSC_CLKSRC,GPin0:CLKSRC_GPIN0, GPin1:CLKSRC_GPIN1}
+    }
+    /// Adc Clock
+    struct AdcClock {
+        init_freq: 0,
+        reg: clk_adc,
+        auxsrc: {PllUsb:CLKSRC_PLL_USB,PllSys: CLKSRC_PLL_SYS,  Rosc: ROSC_CLKSRC_PH, Xosc: XOSC_CLKSRC,GPin0:CLKSRC_GPIN0, GPin1:CLKSRC_GPIN1}
+    }
+    /// RTC Clock
+    struct RtcClock {
+        init_freq: 0,
+        reg: clk_rtc,
+        auxsrc: {PllUsb:CLKSRC_PLL_USB,PllSys: CLKSRC_PLL_SYS,  Rosc: ROSC_CLKSRC_PH, Xosc: XOSC_CLKSRC,GPin0:CLKSRC_GPIN0, GPin1:CLKSRC_GPIN1}
+    }
+}
+
+impl SystemClock {
+    fn get_default_clock_source(&self) -> pac::clocks::clk_sys_ctrl::SRC_A {
+        pac::clocks::clk_sys_ctrl::SRC_A::CLK_REF
+    }
+
+    fn get_aux_source(&self) -> pac::clocks::clk_sys_ctrl::SRC_A {
+        pac::clocks::clk_sys_ctrl::SRC_A::CLKSRC_CLK_SYS_AUX
+    }
+}
+
+impl ReferenceClock {
+    fn get_default_clock_source(&self) -> pac::clocks::clk_ref_ctrl::SRC_A {
+        pac::clocks::clk_ref_ctrl::SRC_A::ROSC_CLKSRC_PH
+    }
+
+    fn get_aux_source(&self) -> pac::clocks::clk_ref_ctrl::SRC_A {
+        pac::clocks::clk_ref_ctrl::SRC_A::CLKSRC_CLK_REF_AUX
+    }
+}
+
 impl ClocksManager {
-    /// Exchanges CLOCKS block against 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,
-            shared_clocks,
-        }
-    }
-
     /// Initialize the clocks to a sane default
     pub fn init_default(
-        &self,
+        &mut self,
         xosc: &CrystalOscillator<Stable>,
         pll_sys: &PhaseLockedLoop<Locked, PLL_SYS>,
         pll_usb: &PhaseLockedLoop<Locked, PLL_USB>,
-    ) {
+    ) -> Result<(), ClockError> {
         // Configure clocks
         // CLK_REF = XOSC (12MHz) / 1 = 12MHz
-        let mut ref_clock = self.reference_clock();
+        self.reference_clock
-        ref_clock.configure_clock(xosc, xosc.get_freq());
+            .configure_clock(xosc, xosc.get_freq())?;
 
         // CLK SYS = PLL SYS (125MHz) / 1 = 125MHz
-        let mut sys_clock = self.system_clock();
+        self.system_clock
-        sys_clock.configure_clock(pll_sys, pll_sys.get_freq());
+            .configure_clock(pll_sys, pll_sys.get_freq())?;
 
         // CLK USB = PLL USB (48MHz) / 1 = 48MHz
-        let mut usb_clock = self.usb_clock();
+        self.usb_clock
-        usb_clock.configure_clock(pll_usb, pll_usb.get_freq());
+            .configure_clock(pll_usb, pll_usb.get_freq())?;
 
         // CLK ADC = PLL USB (48MHZ) / 1 = 48MHz
-        let mut adc_clock = self.adc_clock();
+        self.adc_clock
-        adc_clock.configure_clock(pll_usb, pll_usb.get_freq());
+            .configure_clock(pll_usb, pll_usb.get_freq())?;
 
         // CLK RTC = PLL USB (48MHz) / 1024 = 46875Hz
-        let mut rtc_clock = self.rtc_clock();
+        self.rtc_clock.configure_clock(pll_usb, 46875u32.Hz())?;
-        rtc_clock.configure_clock(pll_usb, 46875u32.Hz());
 
         // 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();
+        self.peripheral_clock
-        peripheral_clock.configure_clock(&sys_clock, sys_clock.freq());
+            .configure_clock(&self.system_clock, self.system_clock.freq())
     }
 
     /// Releases the CLOCKS block
@@ -137,3 +293,53 @@ impl ClocksManager {
         self.clocks
     }
 }
+
+/// Possible init errors
+pub enum InitError {
+    /// Something went wrong setting up the Xosc
+    XoscErr(XoscError),
+    /// Something went wrong setting up the Pll
+    PllError(PllError),
+    /// Something went wrong setting up the Clocks
+    ClockError(ClockError),
+}
+
+/// Initialize the clocks and plls according to the reference implementation
+pub fn init_clocks_and_plls(
+    xosc_crystal_freq: u32,
+    xosc_dev: XOSC,
+    clocks_dev: CLOCKS,
+    pll_sys_dev: PLL_SYS,
+    pll_usb_dev: PLL_USB,
+    resets: &mut RESETS,
+    watchdog: &mut Watchdog,
+) -> Result<ClocksManager, InitError> {
+    let xosc = setup_xosc_blocking(xosc_dev, xosc_crystal_freq.Hz()).map_err(InitError::XoscErr)?;
+
+    // Start tick in watchdog
+    watchdog.enable_tick_generation(xosc_crystal_freq as u8);
+
+    let mut clocks = ClocksManager::new(clocks_dev);
+
+    let pll_sys = setup_pll_blocking(
+        pll_sys_dev,
+        xosc.operating_frequency().into(),
+        PLL_SYS_125MHZ,
+        &mut clocks,
+        resets,
+    )
+    .map_err(InitError::PllError)?;
+    let pll_usb = setup_pll_blocking(
+        pll_usb_dev,
+        xosc.operating_frequency().into(),
+        PLL_USB_48MHZ,
+        &mut clocks,
+        resets,
+    )
+    .map_err(InitError::PllError)?;
+
+    clocks
+        .init_default(&xosc, &pll_sys, &pll_usb)
+        .map_err(InitError::ClockError)?;
+    Ok(clocks)
+}

rp2040-hal/src/pll.rs

@@ -296,11 +296,9 @@ where
     R: Into<Hertz<u64>>,
 {
     // Before we touch PLLs, switch sys and ref cleanly away from their aux sources.
-    let mut sys_clock = clocks.system_clock();
+    nb::block!(clocks.system_clock.reset_source_await()).unwrap();
-    nb::block!(sys_clock.reset_source_await()).unwrap();
 
-    let mut ref_clock = clocks.reference_clock();
+    nb::block!(clocks.reference_clock.reset_source_await()).unwrap();
-    nb::block!(ref_clock.reset_source_await()).unwrap();
 
     let initialized_pll = PhaseLockedLoop::new(dev, xosc_frequency, config)?.initialize(resets);
 

rp2040-hal/src/pwm.rs

@@ -249,10 +249,10 @@ pwm! {
         Pwm0: (pwm0, "pwm0", [0, 1, 16, 18], 0),
         Pwm1: (pwm1, "pwm1", [2, 3, 18, 19], 1),
         Pwm2: (pwm2, "pwm2", [4, 5, 20, 21], 2),
-        Pwm3: (pwm3, "pwm3", [6, 7, 22], 3),
+        Pwm3: (pwm3, "pwm3", [6, 7, 22, 23], 3),
-        Pwm4: (pwm4, "pwm4", [8, 9], 4),
+        Pwm4: (pwm4, "pwm4", [8, 9, 24, 25], 4),
         Pwm5: (pwm5, "pwm5", [10, 11, 26, 27], 5),
-        Pwm6: (pwm6, "pwm6", [12, 13, 28], 6),
+        Pwm6: (pwm6, "pwm6", [12, 13, 28, 29], 6),
         Pwm7: (pwm7, "pwm7", [14, 15], 7),
     ]
 }