Merge #1436

1436: rp: Clock configuration r=CBJamo a=CBJamo

Draft of a more complete clock config for the 2040.

I also extended and made public the clk_<name>_freq functions. I know at least the ws2812 pio example would like to get the sys clock at runtime rather than just using a constant. I suspect most pio-based peripherals will want access to the clocks.

Open questions:

1. Best way to handle the 3 external clock frequencies. I think the XIN (aka crystal) freq should just be set by the init function then never changed, though if it's an external clock that could change? I'm not sure anyone would ever want to do that but maybe it should be handled just in case? The other two should probably be set by the application.
2. Better estimation of ROSC frequency. Right now it's really just a lookup table of the speed from the single sample I did this testing on, and only uses the frequency range and div, drive strength is ignored.
3. Probably some kind of warning should be generated if the random bit from the rosc won't be useful, not sure how to do that.
4. Should clocks only be allowed to be configured at init, or should they be modifiable at runtime? For example, switching the RTC to a clock in pin when a pps source is available.

Bonus feature to support clock output. I only implemented the bare minimum, and only for gpout0. I'm sure there's a clean way with macros to impl all 4 without just copy/paste, but I haven't learned macros yet.

Co-authored-by: Caleb Jamison <[email protected]>

4 files changed, 734 insertions, 98 deletions

diff --git a/embassy-rp/Cargo.toml b/embassy-rp/Cargo.toml
index 836dca28e..e395a994f 100644
--- a/embassy-rp/Cargo.toml
+++ b/embassy-rp/Cargo.toml
@@ -70,7 +70,7 @@ embedded-storage = { version = "0.3" }
 rand_core = "0.6.4"
 fixed = "1.23.1"
 
-rp-pac = { version = "3", features = ["rt"] }
+rp-pac = { version = "4", features = ["rt"] }
 
 embedded-hal-02 = { package = "embedded-hal", version = "0.2.6", features = ["unproven"] }
 embedded-hal-1 = { package = "embedded-hal", version = "=1.0.0-alpha.10", optional = true}
diff --git a/embassy-rp/src/clocks.rs b/embassy-rp/src/clocks.rs
index 8a34b293d..1354ccd27 100644
--- a/embassy-rp/src/clocks.rs
+++ b/embassy-rp/src/clocks.rs
@@ -1,11 +1,183 @@
+use embassy_hal_common::{into_ref, PeripheralRef};
 use pac::clocks::vals::*;
 
-use crate::{pac, reset};
+use crate::{pac, reset, Peripheral};
+
+// TODO fix terrible use of global here
+static mut XIN_HZ: u32 = 0;
+
+pub use rp_pac::clocks::vals::{
+    ClkAdcCtrlAuxsrc as AdcAuxsrc, ClkGpoutCtrlAuxsrc as GpoutSrc, ClkPeriCtrlAuxsrc as PeriClkAuxsrc,
+    ClkRefCtrlAuxsrc as RefAuxsrc, ClkRtcCtrlAuxsrc as RtcAuxsrc, ClkSysCtrlAuxsrc as SysAuxsrc,
+    ClkUsbCtrlAuxsrc as UsbAuxsrc,
+};
+
+#[non_exhaustive]
+pub struct ClockConfig {
+    pub rosc: Option<RoscConfig>,
+    pub xosc: Option<XoscConfig>,
+    pub ref_clk: RefClkConfig,
+    pub sys_clk: SysClkConfig,
+    pub peri_clk_src: Option<ClkPeriCtrlAuxsrc>,
+    pub usb_clk: Option<UsbClkConfig>,
+    pub adc_clk: Option<AdcClkConfig>,
+    pub rtc_clk: Option<RtcClkConfig>,
+}
+
+impl ClockConfig {
+    pub fn crystal(crystal_hz: u32) -> Self {
+        Self {
+            rosc: Some(RoscConfig {
+                range: pac::rosc::vals::FreqRange::MEDIUM,
+                drive_strength: [0; 8],
+                div: 16,
+            }),
+            xosc: Some(XoscConfig {
+                hz: crystal_hz,
+                clock_type: ExternalClock::Crystal,
+                sys_pll: Some(PllConfig {
+                    refdiv: 1,
+                    vco_freq: 1500_000_000,
+                    post_div1: 6,
+                    post_div2: 2,
+                }),
+                usb_pll: Some(PllConfig {
+                    refdiv: 1,
+                    vco_freq: 480_000_000,
+                    post_div1: 5,
+                    post_div2: 2,
+                }),
+            }),
+            ref_clk: RefClkConfig {
+                src: RefClkSrc::Xosc,
+                div: 1,
+            },
+            sys_clk: SysClkConfig {
+                src: SysClkSrc::Aux(ClkSysCtrlAuxsrc::CLKSRC_PLL_SYS),
+                div_int: 1,
+                div_frac: 0,
+            },
+            peri_clk_src: Some(ClkPeriCtrlAuxsrc::CLK_SYS),
+            usb_clk: Some(UsbClkConfig {
+                src: ClkUsbCtrlAuxsrc::CLKSRC_PLL_USB,
+                div: 1,
+                phase: 0,
+            }),
+            adc_clk: Some(AdcClkConfig {
+                src: ClkAdcCtrlAuxsrc::CLKSRC_PLL_USB,
+                div: 1,
+                phase: 0,
+            }),
+            rtc_clk: Some(RtcClkConfig {
+                src: ClkRtcCtrlAuxsrc::CLKSRC_PLL_USB,
+                div_int: 1024,
+                div_frac: 0,
+                phase: 0,
+            }),
+        }
+    }
+
+    pub fn rosc() -> Self {
+        Self {
+            rosc: Some(RoscConfig {
+                range: pac::rosc::vals::FreqRange::HIGH,
+                drive_strength: [0; 8],
+                div: 1,
+            }),
+            xosc: None,
+            ref_clk: RefClkConfig {
+                src: RefClkSrc::Rosc,
+                div: 1,
+            },
+            sys_clk: SysClkConfig {
+                src: SysClkSrc::Aux(ClkSysCtrlAuxsrc::ROSC_CLKSRC),
+                div_int: 1,
+                div_frac: 0,
+            },
+            peri_clk_src: Some(ClkPeriCtrlAuxsrc::ROSC_CLKSRC_PH),
+            usb_clk: None,
+            adc_clk: Some(AdcClkConfig {
+                src: ClkAdcCtrlAuxsrc::ROSC_CLKSRC_PH,
+                div: 1,
+                phase: 0,
+            }),
+            rtc_clk: Some(RtcClkConfig {
+                src: ClkRtcCtrlAuxsrc::ROSC_CLKSRC_PH,
+                div_int: 1024,
+                div_frac: 0,
+                phase: 0,
+            }),
+        }
+    }
+}
+
+pub struct RoscConfig {
+    pub range: pac::rosc::vals::FreqRange,
+    pub drive_strength: [u8; 8],
+    pub div: u16,
+}
 
-const XOSC_MHZ: u32 = 12;
+pub struct XoscConfig {
+    pub hz: u32,
+    pub clock_type: ExternalClock,
+    pub sys_pll: Option<PllConfig>,
+    pub usb_pll: Option<PllConfig>,
+}
+
+pub struct PllConfig {
+    pub refdiv: u32,
+    pub vco_freq: u32,
+    pub post_div1: u8,
+    pub post_div2: u8,
+}
+
+pub enum ExternalClock {
+    Crystal,
+    Clock,
+}
+pub struct RefClkConfig {
+    pub src: RefClkSrc,
+    pub div: u8,
+}
+
+pub enum RefClkSrc {
+    Xosc,
+    Rosc,
+    Aux(ClkRefCtrlAuxsrc),
+}
+
+pub enum SysClkSrc {
+    Ref,
+    Aux(ClkSysCtrlAuxsrc),
+}
+
+pub struct SysClkConfig {
+    pub src: SysClkSrc,
+    pub div_int: u32,
+    pub div_frac: u8,
+}
+
+pub struct UsbClkConfig {
+    pub src: ClkUsbCtrlAuxsrc,
+    pub div: u8,
+    pub phase: u8,
+}
+
+pub struct AdcClkConfig {
+    pub src: ClkAdcCtrlAuxsrc,
+    pub div: u8,
+    pub phase: u8,
+}
+
+pub struct RtcClkConfig {
+    pub src: ClkRtcCtrlAuxsrc,
+    pub div_int: u32,
+    pub div_frac: u8,
+    pub phase: u8,
+}
 
 /// safety: must be called exactly once at bootup
-pub(crate) unsafe fn init() {
+pub(crate) unsafe fn init(config: ClockConfig) {
     // Reset everything except:
     // - QSPI (we're using it to run this code!)
     // - PLLs (it may be suicide if that's what's clocking us)
@@ -15,124 +187,387 @@ pub(crate) unsafe fn init() {
     peris.set_pads_qspi(false);
     peris.set_pll_sys(false);
     peris.set_pll_usb(false);
+    // TODO investigate if usb should be unreset here
     peris.set_usbctrl(false);
     peris.set_syscfg(false);
     reset::reset(peris);
 
-    // Remove reset from peripherals which are clocked only by clk_sys and
-    // clk_ref. Other peripherals stay in reset until we've configured clocks.
-    let mut peris = reset::ALL_PERIPHERALS;
-    peris.set_adc(false);
-    peris.set_rtc(false);
-    peris.set_spi0(false);
-    peris.set_spi1(false);
-    peris.set_uart0(false);
-    peris.set_uart1(false);
-    peris.set_usbctrl(false);
-    reset::unreset_wait(peris);
-
-    // Start tick in watchdog
-    // xosc 12 mhz
-    pac::WATCHDOG.tick().write(|w| {
-        w.set_cycles(XOSC_MHZ as u16);
-        w.set_enable(true);
-    });
-
     // Disable resus that may be enabled from previous software
     let c = pac::CLOCKS;
     c.clk_sys_resus_ctrl()
         .write_value(pac::clocks::regs::ClkSysResusCtrl(0));
 
-    // start XOSC
-    start_xosc();
-
     // Before we touch PLLs, switch sys and ref cleanly away from their aux sources.
     c.clk_sys_ctrl().modify(|w| w.set_src(ClkSysCtrlSrc::CLK_REF));
     while c.clk_sys_selected().read() != 1 {}
     c.clk_ref_ctrl().modify(|w| w.set_src(ClkRefCtrlSrc::ROSC_CLKSRC_PH));
     while c.clk_ref_selected().read() != 1 {}
 
-    // 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
-    configure_pll(pac::PLL_SYS, 1, 1500_000_000, 6, 2);
-    configure_pll(pac::PLL_USB, 1, 480_000_000, 5, 2);
+    if let Some(config) = config.rosc {
+        configure_rosc(config);
+    }
 
-    // CLK_REF = XOSC (12MHz) / 1 = 12MHz2Mhz
-    c.clk_ref_ctrl().write(|w| {
-        w.set_src(ClkRefCtrlSrc::XOSC_CLKSRC);
-    });
-    while c.clk_ref_selected().read() != 1 << ClkRefCtrlSrc::XOSC_CLKSRC.0 {}
-    c.clk_ref_div().write(|w| w.set_int(1));
+    if let Some(config) = config.xosc {
+        XIN_HZ = config.hz;
 
-    // CLK SYS = PLL SYS (125MHz) / 1 = 125MHz
-    c.clk_sys_ctrl().write(|w| {
-        w.set_src(ClkSysCtrlSrc::CLK_REF);
-    });
-    while c.clk_sys_selected().read() != 1 << ClkSysCtrlSrc::CLK_REF.0 {}
-    c.clk_sys_div().write(|w| w.set_int(1));
-    c.clk_sys_ctrl().write(|w| {
-        w.set_auxsrc(ClkSysCtrlAuxsrc::CLKSRC_PLL_SYS);
-        w.set_src(ClkSysCtrlSrc::CLKSRC_CLK_SYS_AUX);
+        pac::WATCHDOG.tick().write(|w| {
+            w.set_cycles((config.hz / 1_000_000) as u16);
+            w.set_enable(true);
+        });
+
+        // start XOSC
+        match config.clock_type {
+            ExternalClock::Crystal => start_xosc(config.hz),
+            // TODO The datasheet says the xosc needs to be put into a bypass mode to use an
+            // external clock, but is mum about how to do that.
+            ExternalClock::Clock => todo!(),
+        }
+
+        if let Some(sys_pll_config) = config.sys_pll {
+            configure_pll(pac::PLL_SYS, config.hz, sys_pll_config);
+        }
+        if let Some(usb_pll_config) = config.usb_pll {
+            configure_pll(pac::PLL_USB, config.hz, usb_pll_config);
+        }
+    }
+
+    match config.ref_clk.src {
+        RefClkSrc::Xosc => {
+            c.clk_ref_ctrl().write(|w| {
+                w.set_src(ClkRefCtrlSrc::XOSC_CLKSRC);
+            });
+            while c.clk_ref_selected().read() != 1 << ClkRefCtrlSrc::XOSC_CLKSRC.0 {}
+        }
+        RefClkSrc::Rosc => {
+            c.clk_ref_ctrl().write(|w| {
+                w.set_src(ClkRefCtrlSrc::ROSC_CLKSRC_PH);
+            });
+            while c.clk_ref_selected().read() != 1 << ClkRefCtrlSrc::ROSC_CLKSRC_PH.0 {}
+        }
+        RefClkSrc::Aux(src) => {
+            c.clk_ref_ctrl().write(|w| {
+                w.set_auxsrc(src);
+                w.set_src(ClkRefCtrlSrc::CLKSRC_CLK_REF_AUX);
+            });
+            while c.clk_ref_selected().read() != 1 << ClkRefCtrlSrc::CLKSRC_CLK_REF_AUX.0 {}
+        }
+    }
+    c.clk_ref_div().write(|w| {
+        w.set_int(config.ref_clk.div);
     });
-    while c.clk_sys_selected().read() != 1 << ClkSysCtrlSrc::CLKSRC_CLK_SYS_AUX.0 {}
 
-    // CLK USB = PLL USB (48MHz) / 1 = 48MHz
-    c.clk_usb_div().write(|w| w.set_int(1));
-    c.clk_usb_ctrl().write(|w| {
+    pac::WATCHDOG.tick().write(|w| {
+        w.set_cycles((clk_ref_freq() / 1_000_000) as u16);
         w.set_enable(true);
-        w.set_auxsrc(ClkUsbCtrlAuxsrc::CLKSRC_PLL_USB);
     });
 
-    // CLK ADC = PLL USB (48MHZ) / 1 = 48MHz
-    c.clk_adc_div().write(|w| w.set_int(1));
-    c.clk_adc_ctrl().write(|w| {
-        w.set_enable(true);
-        w.set_auxsrc(ClkAdcCtrlAuxsrc::CLKSRC_PLL_USB);
+    match config.sys_clk.src {
+        SysClkSrc::Ref => {
+            c.clk_sys_ctrl().write(|w| {
+                w.set_src(ClkSysCtrlSrc::CLK_REF);
+            });
+            while c.clk_sys_selected().read() != 1 << ClkSysCtrlSrc::CLK_REF.0 {}
+        }
+        SysClkSrc::Aux(src) => {
+            c.clk_sys_ctrl().write(|w| {
+                w.set_src(ClkSysCtrlSrc::CLK_REF);
+            });
+            while c.clk_sys_selected().read() != 1 << ClkSysCtrlSrc::CLK_REF.0 {}
+
+            c.clk_sys_ctrl().write(|w| {
+                w.set_auxsrc(src);
+                w.set_src(ClkSysCtrlSrc::CLKSRC_CLK_SYS_AUX);
+            });
+            while c.clk_sys_selected().read() != 1 << ClkSysCtrlSrc::CLKSRC_CLK_SYS_AUX.0 {}
+        }
+    }
+    c.clk_sys_div().write(|w| {
+        w.set_int(config.sys_clk.div_int);
+        w.set_frac(config.sys_clk.div_frac);
+    });
+
+    let mut peris = reset::ALL_PERIPHERALS;
+
+    if let Some(src) = config.peri_clk_src {
+        c.clk_peri_ctrl().write(|w| {
+            w.set_enable(true);
+            w.set_auxsrc(src);
+        });
+    } else {
+        peris.set_spi0(false);
+        peris.set_spi1(false);
+        peris.set_uart0(false);
+        peris.set_uart1(false);
+    }
+
+    if let Some(conf) = config.usb_clk {
+        // CLK USB = PLL USB (48MHz) / 1 = 48MHz
+        c.clk_usb_div().write(|w| w.set_int(conf.div));
+        c.clk_usb_ctrl().write(|w| {
+            w.set_phase(conf.phase);
+            w.set_enable(true);
+            w.set_auxsrc(conf.src);
+        });
+    } else {
+        peris.set_usbctrl(false);
+    }
+
+    if let Some(conf) = config.adc_clk {
+        // CLK ADC = PLL USB (48MHZ) / 1 = 48MHz
+        c.clk_adc_div().write(|w| w.set_int(conf.div));
+        c.clk_adc_ctrl().write(|w| {
+            w.set_phase(conf.phase);
+            w.set_enable(true);
+            w.set_auxsrc(conf.src);
+        });
+    } else {
+        peris.set_adc(false);
+    }
+
+    if let Some(conf) = config.rtc_clk {
+        // CLK RTC = PLL USB (48MHz) / 1024 = 46875Hz
+        c.clk_rtc_ctrl().modify(|w| {
+            w.set_enable(false);
+        });
+        c.clk_rtc_div().write(|w| {
+            w.set_int(conf.div_int);
+            w.set_frac(conf.div_frac);
+        });
+        c.clk_rtc_ctrl().write(|w| {
+            w.set_phase(conf.phase);
+            w.set_enable(true);
+            w.set_auxsrc(conf.src);
+        });
+    } else {
+        peris.set_rtc(false);
+    }
+
+    // Peripheral clocks should now all be running
+    reset::unreset_wait(peris);
+}
+
+unsafe fn configure_rosc(config: RoscConfig) {
+    let p = pac::ROSC;
+
+    p.freqa().write(|w| {
+        w.set_passwd(pac::rosc::vals::Passwd::PASS);
+        w.set_ds0(config.drive_strength[0]);
+        w.set_ds1(config.drive_strength[1]);
+        w.set_ds2(config.drive_strength[2]);
+        w.set_ds3(config.drive_strength[3]);
     });
 
-    // CLK RTC = PLL USB (48MHz) / 1024 = 46875Hz
-    c.clk_rtc_ctrl().modify(|w| {
-        w.set_enable(false);
+    p.freqb().write(|w| {
+        w.set_passwd(pac::rosc::vals::Passwd::PASS);
+        w.set_ds4(config.drive_strength[4]);
+        w.set_ds5(config.drive_strength[5]);
+        w.set_ds6(config.drive_strength[6]);
+        w.set_ds7(config.drive_strength[7]);
     });
-    c.clk_rtc_div().write(|w| w.set_int(1024));
-    c.clk_rtc_ctrl().write(|w| {
-        w.set_enable(true);
-        w.set_auxsrc(ClkRtcCtrlAuxsrc::CLKSRC_PLL_USB);
+
+    p.div().write(|w| {
+        w.set_div(pac::rosc::vals::Div(config.div + pac::rosc::vals::Div::PASS.0));
     });
 
-    // CLK PERI = clk_sys. Used as reference clock for Peripherals. No dividers so just select and enable
-    // Normally choose clk_sys or clk_usb
-    c.clk_peri_ctrl().write(|w| {
-        w.set_enable(true);
-        w.set_auxsrc(ClkPeriCtrlAuxsrc::CLK_SYS);
+    p.ctrl().write(|w| {
+        w.set_enable(pac::rosc::vals::Enable::ENABLE);
+        w.set_freq_range(config.range);
     });
+}
 
-    // Peripheral clocks should now all be running
-    let peris = reset::ALL_PERIPHERALS;
-    reset::unreset_wait(peris);
+pub fn estimate_rosc_freq() -> u32 {
+    let p = pac::ROSC;
+
+    let base = match unsafe { p.ctrl().read().freq_range() } {
+        pac::rosc::vals::FreqRange::LOW => 84_000_000,
+        pac::rosc::vals::FreqRange::MEDIUM => 104_000_000,
+        pac::rosc::vals::FreqRange::HIGH => 140_000_000,
+        pac::rosc::vals::FreqRange::TOOHIGH => 208_000_000,
+        _ => unreachable!(),
+    };
+    let mut div = unsafe { p.div().read().0 - pac::rosc::vals::Div::PASS.0 as u32 };
+    if div == 0 {
+        div = 32
+    }
+
+    base / div
 }
 
-pub(crate) fn _clk_sys_freq() -> u32 {
-    125_000_000
+pub fn xosc_freq() -> u32 {
+    unsafe { XIN_HZ }
 }
 
-pub(crate) fn clk_peri_freq() -> u32 {
-    125_000_000
+pub fn gpin0_freq() -> u32 {
+    todo!()
+}
+pub fn gpin1_freq() -> u32 {
+    todo!()
 }
 
-pub(crate) fn clk_rtc_freq() -> u32 {
-    46875
+pub fn pll_sys_freq() -> u32 {
+    let p = pac::PLL_SYS;
+
+    let input_freq = xosc_freq();
+    let cs = unsafe { p.cs().read() };
+
+    let refdiv = cs.refdiv() as u32;
+    let fbdiv = unsafe { p.fbdiv_int().read().fbdiv_int() } as u32;
+    let (postdiv1, postdiv2) = unsafe {
+        let prim = p.prim().read();
+        (prim.postdiv1() as u32, prim.postdiv2() as u32)
+    };
+
+    (((input_freq / refdiv) * fbdiv) / postdiv1) / postdiv2
 }
 
-unsafe fn start_xosc() {
-    const XOSC_MHZ: u32 = 12;
+pub fn pll_usb_freq() -> u32 {
+    let p = pac::PLL_USB;
+
+    let input_freq = xosc_freq();
+    let cs = unsafe { p.cs().read() };
+
+    let refdiv = cs.refdiv() as u32;
+    let fbdiv = unsafe { p.fbdiv_int().read().fbdiv_int() } as u32;
+    let (postdiv1, postdiv2) = unsafe {
+        let prim = p.prim().read();
+        (prim.postdiv1() as u32, prim.postdiv2() as u32)
+    };
+
+    (((input_freq / refdiv) * fbdiv) / postdiv1) / postdiv2
+}
+
+pub fn clk_sys_freq() -> u32 {
+    let c = pac::CLOCKS;
+    let ctrl = unsafe { c.clk_sys_ctrl().read() };
+
+    let base = match ctrl.src() {
+        ClkSysCtrlSrc::CLK_REF => clk_ref_freq(),
+        ClkSysCtrlSrc::CLKSRC_CLK_SYS_AUX => match ctrl.auxsrc() {
+            ClkSysCtrlAuxsrc::CLKSRC_PLL_SYS => pll_sys_freq(),
+            ClkSysCtrlAuxsrc::CLKSRC_PLL_USB => pll_usb_freq(),
+            ClkSysCtrlAuxsrc::ROSC_CLKSRC => estimate_rosc_freq(),
+            ClkSysCtrlAuxsrc::XOSC_CLKSRC => xosc_freq(),
+            ClkSysCtrlAuxsrc::CLKSRC_GPIN0 => gpin0_freq(),
+            ClkSysCtrlAuxsrc::CLKSRC_GPIN1 => gpin1_freq(),
+            _ => unreachable!(),
+        },
+        _ => unreachable!(),
+    };
+
+    let div = unsafe { c.clk_sys_div().read() };
+    let int = if div.int() == 0 { 65536 } else { div.int() };
+    // TODO handle fractional clock div
+    let _frac = div.frac();
+
+    base / int
+}
+
+pub fn clk_ref_freq() -> u32 {
+    let c = pac::CLOCKS;
+    let ctrl = unsafe { c.clk_ref_ctrl().read() };
+
+    let base = match ctrl.src() {
+        ClkRefCtrlSrc::ROSC_CLKSRC_PH => estimate_rosc_freq(),
+        ClkRefCtrlSrc::XOSC_CLKSRC => xosc_freq(),
+        ClkRefCtrlSrc::CLKSRC_CLK_REF_AUX => match ctrl.auxsrc() {
+            ClkRefCtrlAuxsrc::CLKSRC_PLL_USB => pll_usb_freq(),
+            ClkRefCtrlAuxsrc::CLKSRC_GPIN0 => gpin0_freq(),
+            ClkRefCtrlAuxsrc::CLKSRC_GPIN1 => gpin1_freq(),
+            _ => unreachable!(),
+        },
+        _ => unreachable!(),
+    };
+
+    let div = unsafe { c.clk_ref_div().read() };
+    let int = if div.int() == 0 { 4 } else { div.int() as u32 };
+
+    base / int
+}
+
+pub fn clk_peri_freq() -> u32 {
+    let c = pac::CLOCKS;
+    let src = unsafe { c.clk_peri_ctrl().read().auxsrc() };
+
+    match src {
+        ClkPeriCtrlAuxsrc::CLK_SYS => clk_sys_freq(),
+        ClkPeriCtrlAuxsrc::CLKSRC_PLL_SYS => pll_sys_freq(),
+        ClkPeriCtrlAuxsrc::CLKSRC_PLL_USB => pll_usb_freq(),
+        ClkPeriCtrlAuxsrc::ROSC_CLKSRC_PH => estimate_rosc_freq(),
+        ClkPeriCtrlAuxsrc::XOSC_CLKSRC => xosc_freq(),
+        ClkPeriCtrlAuxsrc::CLKSRC_GPIN0 => gpin0_freq(),
+        ClkPeriCtrlAuxsrc::CLKSRC_GPIN1 => gpin1_freq(),
+        _ => unreachable!(),
+    }
+}
+
+pub fn clk_usb_freq() -> u32 {
+    let c = pac::CLOCKS;
+    let ctrl = unsafe { c.clk_usb_ctrl().read() };
+
+    let base = match ctrl.auxsrc() {
+        ClkUsbCtrlAuxsrc::CLKSRC_PLL_SYS => pll_sys_freq(),
+        ClkUsbCtrlAuxsrc::CLKSRC_PLL_USB => pll_usb_freq(),
+        ClkUsbCtrlAuxsrc::ROSC_CLKSRC_PH => estimate_rosc_freq(),
+        ClkUsbCtrlAuxsrc::XOSC_CLKSRC => xosc_freq(),
+        ClkUsbCtrlAuxsrc::CLKSRC_GPIN0 => gpin0_freq(),
+        ClkUsbCtrlAuxsrc::CLKSRC_GPIN1 => gpin1_freq(),
+        _ => unreachable!(),
+    };
+
+    let div = unsafe { c.clk_ref_div().read() };
+    let int = if div.int() == 0 { 4 } else { div.int() as u32 };
+
+    base / int
+}
+
+pub fn clk_adc_freq() -> u32 {
+    let c = pac::CLOCKS;
+    let ctrl = unsafe { c.clk_adc_ctrl().read() };
+
+    let base = match ctrl.auxsrc() {
+        ClkAdcCtrlAuxsrc::CLKSRC_PLL_SYS => pll_sys_freq(),
+        ClkAdcCtrlAuxsrc::CLKSRC_PLL_USB => pll_usb_freq(),
+        ClkAdcCtrlAuxsrc::ROSC_CLKSRC_PH => estimate_rosc_freq(),
+        ClkAdcCtrlAuxsrc::XOSC_CLKSRC => xosc_freq(),
+        ClkAdcCtrlAuxsrc::CLKSRC_GPIN0 => gpin0_freq(),
+        ClkAdcCtrlAuxsrc::CLKSRC_GPIN1 => gpin1_freq(),
+        _ => unreachable!(),
+    };
+
+    let div = unsafe { c.clk_adc_div().read() };
+    let int = if div.int() == 0 { 4 } else { div.int() as u32 };
+
+    base / int
+}
+
+pub fn clk_rtc_freq() -> u32 {
+    let c = pac::CLOCKS;
+    let src = unsafe { c.clk_rtc_ctrl().read().auxsrc() };
+
+    let base = match src {
+        ClkRtcCtrlAuxsrc::CLKSRC_PLL_USB => pll_usb_freq(),
+        ClkRtcCtrlAuxsrc::CLKSRC_PLL_SYS => pll_sys_freq(),
+        ClkRtcCtrlAuxsrc::ROSC_CLKSRC_PH => estimate_rosc_freq(),
+        ClkRtcCtrlAuxsrc::XOSC_CLKSRC => xosc_freq(),
+        ClkRtcCtrlAuxsrc::CLKSRC_GPIN0 => gpin0_freq(),
+        ClkRtcCtrlAuxsrc::CLKSRC_GPIN1 => gpin1_freq(),
+        _ => unreachable!(),
+    };
+
+    let div = unsafe { c.clk_rtc_div().read() };
+    let int = if div.int() == 0 { 65536 } else { div.int() };
+    // TODO handle fractional clock div
+    let _frac = div.frac();
+
+    base / int
+}
+
+unsafe fn start_xosc(crystal_hz: u32) {
     pac::XOSC
         .ctrl()
         .write(|w| w.set_freq_range(pac::xosc::vals::CtrlFreqRange::_1_15MHZ));
 
-    let startup_delay = (((XOSC_MHZ * 1_000_000) / 1000) + 128) / 256;
+    let startup_delay = ((crystal_hz / 1000) + 128) / 256;
     pac::XOSC.startup().write(|w| w.set_delay(startup_delay as u16));
     pac::XOSC.ctrl().write(|w| {
         w.set_freq_range(pac::xosc::vals::CtrlFreqRange::_1_15MHZ);
@@ -141,24 +576,24 @@ unsafe fn start_xosc() {
     while !pac::XOSC.status().read().stable() {}
 }
 
-unsafe fn configure_pll(p: pac::pll::Pll, refdiv: u32, vco_freq: u32, post_div1: u8, post_div2: u8) {
-    let ref_freq = XOSC_MHZ * 1_000_000 / refdiv;
+unsafe fn configure_pll(p: pac::pll::Pll, input_freq: u32, config: PllConfig) {
+    let ref_freq = input_freq / config.refdiv;
 
-    let fbdiv = vco_freq / ref_freq;
+    let fbdiv = config.vco_freq / ref_freq;
     assert!(fbdiv >= 16 && fbdiv <= 320);
-    assert!(post_div1 >= 1 && post_div1 <= 7);
-    assert!(post_div2 >= 1 && post_div2 <= 7);
-    assert!(post_div2 <= post_div1);
-    assert!(ref_freq <= (vco_freq / 16));
+    assert!(config.post_div1 >= 1 && config.post_div1 <= 7);
+    assert!(config.post_div2 >= 1 && config.post_div2 <= 7);
+    assert!(config.post_div2 <= config.post_div1);
+    assert!(ref_freq <= (config.vco_freq / 16));
 
     // do not disrupt PLL that is already correctly configured and operating
     let cs = p.cs().read();
     let prim = p.prim().read();
     if cs.lock()
-        && cs.refdiv() == refdiv as u8
+        && cs.refdiv() == config.refdiv as u8
         && p.fbdiv_int().read().fbdiv_int() == fbdiv as u16
-        && prim.postdiv1() == post_div1
-        && prim.postdiv2() == post_div2
+        && prim.postdiv1() == config.post_div1
+        && prim.postdiv2() == config.post_div2
     {
         return;
     }
@@ -174,7 +609,7 @@ unsafe fn configure_pll(p: pac::pll::Pll, refdiv: u32, vco_freq: u32, post_div1:
     reset::unreset_wait(peris);
 
     // Load VCO-related dividers before starting VCO
-    p.cs().write(|w| w.set_refdiv(refdiv as _));
+    p.cs().write(|w| w.set_refdiv(config.refdiv as _));
     p.fbdiv_int().write(|w| w.set_fbdiv_int(fbdiv as _));
 
     // Turn on PLL
@@ -189,14 +624,169 @@ unsafe fn configure_pll(p: pac::pll::Pll, refdiv: u32, vco_freq: u32, post_div1:
 
     // Wait for PLL to lock
     p.prim().write(|w| {
-        w.set_postdiv1(post_div1);
-        w.set_postdiv2(post_div2);
+        w.set_postdiv1(config.post_div1);
+        w.set_postdiv2(config.post_div2);
     });
 
     // Turn on post divider
     p.pwr().modify(|w| w.set_postdivpd(false));
 }
 
+pub trait GpinPin: crate::gpio::Pin {
+    fn number(&self) -> usize;
+}
+
+macro_rules! impl_gpinpin {
+    ($name:ident, $pin_num:expr, $gpin_num:expr) => {
+        impl GpinPin for crate::peripherals::$name {
+            fn number(&self) -> usize {
+                $gpin_num
+            }
+        }
+    };
+}
+
+impl_gpinpin!(PIN_20, 20, 0);
+impl_gpinpin!(PIN_22, 22, 1);
+
+pub struct Gpin<'d, T: GpinPin> {
+    gpin: PeripheralRef<'d, T>,
+}
+
+impl<'d, T: GpinPin> Gpin<'d, T> {
+    pub fn new(gpin: impl Peripheral<P = T> + 'd) -> Self {
+        into_ref!(gpin);
+
+        unsafe {
+            gpin.io().ctrl().write(|w| w.set_funcsel(0x08));
+        }
+
+        Self { gpin }
+    }
+}
+
+impl<'d, T: GpinPin> Drop for Gpin<'d, T> {
+    fn drop(&mut self) {
+        unsafe {
+            self.gpin
+                .io()
+                .ctrl()
+                .write(|w| w.set_funcsel(pac::io::vals::Gpio0ctrlFuncsel::NULL.0));
+        }
+    }
+}
+
+pub trait GpoutPin: crate::gpio::Pin {
+    fn number(&self) -> usize;
+}
+
+macro_rules! impl_gpoutpin {
+    ($name:ident, $gpout_num:expr) => {
+        impl GpoutPin for crate::peripherals::$name {
+            fn number(&self) -> usize {
+                $gpout_num
+            }
+        }
+    };
+}
+
+impl_gpoutpin!(PIN_21, 0);
+impl_gpoutpin!(PIN_23, 1);
+impl_gpoutpin!(PIN_24, 2);
+impl_gpoutpin!(PIN_25, 3);
+
+pub struct Gpout<'d, T: GpoutPin> {
+    gpout: PeripheralRef<'d, T>,
+}
+
+impl<'d, T: GpoutPin> Gpout<'d, T> {
+    pub fn new(gpout: impl Peripheral<P = T> + 'd) -> Self {
+        into_ref!(gpout);
+
+        unsafe {
+            gpout.io().ctrl().write(|w| w.set_funcsel(0x08));
+        }
+
+        Self { gpout }
+    }
+
+    pub fn set_div(&self, int: u32, frac: u8) {
+        unsafe {
+            let c = pac::CLOCKS;
+            c.clk_gpout_div(self.gpout.number()).write(|w| {
+                w.set_int(int);
+                w.set_frac(frac);
+            });
+        }
+    }
+
+    pub fn set_src(&self, src: ClkGpoutCtrlAuxsrc) {
+        unsafe {
+            let c = pac::CLOCKS;
+            c.clk_gpout_ctrl(self.gpout.number()).modify(|w| {
+                w.set_auxsrc(src);
+            });
+        }
+    }
+
+    pub fn enable(&self) {
+        unsafe {
+            let c = pac::CLOCKS;
+            c.clk_gpout_ctrl(self.gpout.number()).modify(|w| {
+                w.set_enable(true);
+            });
+        }
+    }
+
+    pub fn disable(&self) {
+        unsafe {
+            let c = pac::CLOCKS;
+            c.clk_gpout_ctrl(self.gpout.number()).modify(|w| {
+                w.set_enable(false);
+            });
+        }
+    }
+
+    pub fn get_freq(&self) -> u32 {
+        let c = pac::CLOCKS;
+        let src = unsafe { c.clk_gpout_ctrl(self.gpout.number()).read().auxsrc() };
+
+        let base = match src {
+            ClkGpoutCtrlAuxsrc::CLKSRC_PLL_SYS => pll_sys_freq(),
+            ClkGpoutCtrlAuxsrc::CLKSRC_GPIN0 => gpin0_freq(),
+            ClkGpoutCtrlAuxsrc::CLKSRC_GPIN1 => gpin1_freq(),
+            ClkGpoutCtrlAuxsrc::CLKSRC_PLL_USB => pll_usb_freq(),
+            ClkGpoutCtrlAuxsrc::ROSC_CLKSRC => estimate_rosc_freq(),
+            ClkGpoutCtrlAuxsrc::XOSC_CLKSRC => xosc_freq(),
+            ClkGpoutCtrlAuxsrc::CLK_SYS => clk_sys_freq(),
+            ClkGpoutCtrlAuxsrc::CLK_USB => clk_usb_freq(),
+            ClkGpoutCtrlAuxsrc::CLK_ADC => clk_adc_freq(),
+            ClkGpoutCtrlAuxsrc::CLK_RTC => clk_rtc_freq(),
+            ClkGpoutCtrlAuxsrc::CLK_REF => clk_ref_freq(),
+            _ => unreachable!(),
+        };
+
+        let div = unsafe { c.clk_gpout_div(self.gpout.number()).read() };
+        let int = if div.int() == 0 { 65536 } else { div.int() };
+        // TODO handle fractional clock div
+        let _frac = div.frac();
+
+        base / int
+    }
+}
+
+impl<'d, T: GpoutPin> Drop for Gpout<'d, T> {
+    fn drop(&mut self) {
+        self.disable();
+        unsafe {
+            self.gpout
+                .io()
+                .ctrl()
+                .write(|w| w.set_funcsel(pac::io::vals::Gpio0ctrlFuncsel::NULL.0));
+        }
+    }
+}
+
 /// Random number generator based on the ROSC RANDOMBIT register.
 ///
 /// This will not produce random values if the ROSC is stopped or run at some
diff --git a/embassy-rp/src/lib.rs b/embassy-rp/src/lib.rs
index 99f62738d..980ebe7f4 100644
--- a/embassy-rp/src/lib.rs
+++ b/embassy-rp/src/lib.rs
@@ -159,23 +159,35 @@ select_bootloader! {
 }
 
 pub mod config {
+    use crate::clocks::ClockConfig;
+
     #[non_exhaustive]
-    pub struct Config {}
+    pub struct Config {
+        pub clocks: ClockConfig,
+    }
 
     impl Default for Config {
         fn default() -> Self {
-            Self {}
+            Self {
+                clocks: ClockConfig::crystal(12_000_000),
+            }
+        }
+    }
+
+    impl Config {
+        pub fn new(clocks: ClockConfig) -> Self {
+            Self { clocks }
         }
     }
 }
 
-pub fn init(_config: config::Config) -> Peripherals {
+pub fn init(config: config::Config) -> Peripherals {
     // Do this first, so that it panics if user is calling `init` a second time
     // before doing anything important.
     let peripherals = Peripherals::take();
 
     unsafe {
-        clocks::init();
+        clocks::init(config.clocks);
         #[cfg(feature = "time-driver")]
         timer::init();
         dma::init();
diff --git a/examples/rp/src/bin/gpout.rs b/examples/rp/src/bin/gpout.rs
new file mode 100644
index 000000000..236a653ac
--- /dev/null
+++ b/examples/rp/src/bin/gpout.rs
@@ -0,0 +1,34 @@
+#![no_std]
+#![no_main]
+#![feature(type_alias_impl_trait)]
+
+use defmt::*;
+use embassy_executor::Spawner;
+use embassy_rp::clocks;
+use embassy_time::{Duration, Timer};
+use {defmt_rtt as _, panic_probe as _};
+
+#[embassy_executor::main]
+async fn main(_spawner: Spawner) {
+    let p = embassy_rp::init(Default::default());
+
+    let gpout3 = clocks::Gpout::new(p.PIN_25);
+    gpout3.set_div(1000, 0);
+    gpout3.enable();
+
+    loop {
+        gpout3.set_src(clocks::GpoutSrc::CLK_SYS);
+        info!(
+            "Pin 25 is now outputing CLK_SYS/1000, should be toggling at {}",
+            gpout3.get_freq()
+        );
+        Timer::after(Duration::from_secs(2)).await;
+
+        gpout3.set_src(clocks::GpoutSrc::CLK_REF);
+        info!(
+            "Pin 25 is now outputing CLK_REF/1000, should be toggling at {}",
+            gpout3.get_freq()
+        );
+        Timer::after(Duration::from_secs(2)).await;
+    }
+}