rp clock configuration

2 files changed, 436 insertions, 95 deletions

diff --git a/embassy-rp/src/clocks.rs b/embassy-rp/src/clocks.rs
index 8a34b293d..4ae967aae 100644
--- a/embassy-rp/src/clocks.rs
+++ b/embassy-rp/src/clocks.rs
@@ -2,10 +2,138 @@ use pac::clocks::vals::*;
 
 use crate::{pac, reset};
 
-const XOSC_MHZ: u32 = 12;
+static mut EXTERNAL_HZ: u32 = 0;
+
+pub struct ClockConfig {
+    rosc_config: Option<RoscConfig>,
+    xosc_config: Option<XoscConfig>,
+    ref_clk_config: (RefClkSrc, u8),
+    sys_clk_config: (SysClkSrc, u32),
+    peri_clk_src: Option<ClkPeriCtrlAuxsrc>,
+    usb_clk_config: Option<(ClkUsbCtrlAuxsrc, u8)>,
+    adc_clk_config: Option<(ClkAdcCtrlAuxsrc, u8)>,
+    rtc_clk_config: Option<(ClkRtcCtrlAuxsrc, u32)>,
+}
+
+impl ClockConfig {
+    pub fn crystal(crystal_hz: u32) -> Self {
+        Self {
+            rosc_config: Some(RoscConfig {
+                range: pac::rosc::vals::FreqRange::MEDIUM,
+                drive_strength_0: 0,
+                drive_strength_1: 0,
+                drive_strength_2: 0,
+                drive_strength_3: 0,
+                drive_strength_4: 0,
+                drive_strength_5: 0,
+                drive_strength_6: 0,
+                drive_strength_7: 0,
+                div: 16,
+            }),
+            xosc_config: 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_config: (RefClkSrc::Xosc, 1),
+            sys_clk_config: (SysClkSrc::Aux(ClkSysCtrlAuxsrc::CLKSRC_PLL_SYS), 1),
+            peri_clk_src: Some(ClkPeriCtrlAuxsrc::CLK_SYS),
+            usb_clk_config: Some((ClkUsbCtrlAuxsrc::CLKSRC_PLL_SYS, 1)),
+            adc_clk_config: Some((ClkAdcCtrlAuxsrc::CLKSRC_PLL_USB, 1)),
+            rtc_clk_config: Some((ClkRtcCtrlAuxsrc::CLKSRC_PLL_USB, 1024)),
+        }
+    }
+
+    pub fn rosc() -> Self {
+        Self {
+            rosc_config: Some(RoscConfig {
+                range: pac::rosc::vals::FreqRange::HIGH,
+                drive_strength_0: 0,
+                drive_strength_1: 0,
+                drive_strength_2: 0,
+                drive_strength_3: 0,
+                drive_strength_4: 0,
+                drive_strength_5: 0,
+                drive_strength_6: 0,
+                drive_strength_7: 0,
+                div: 1,
+            }),
+            xosc_config: None,
+            ref_clk_config: (RefClkSrc::Rosc, 4),
+            sys_clk_config: (SysClkSrc::Aux(ClkSysCtrlAuxsrc::ROSC_CLKSRC), 1),
+            peri_clk_src: Some(ClkPeriCtrlAuxsrc::ROSC_CLKSRC_PH),
+            usb_clk_config: None,
+            adc_clk_config: Some((ClkAdcCtrlAuxsrc::ROSC_CLKSRC_PH, 1)),
+            rtc_clk_config: Some((ClkRtcCtrlAuxsrc::ROSC_CLKSRC_PH, 1024)),
+        }
+    }
+}
+pub enum ExternalClock {
+    Crystal,
+    Clock,
+}
+
+pub struct XoscConfig {
+    hz: u32,
+    clock_type: ExternalClock,
+    sys_pll: Option<PllConfig>,
+    usb_pll: Option<PllConfig>,
+}
+
+pub struct RoscConfig {
+    range: pac::rosc::vals::FreqRange,
+    drive_strength_0: u8,
+    drive_strength_1: u8,
+    drive_strength_2: u8,
+    drive_strength_3: u8,
+    drive_strength_4: u8,
+    drive_strength_5: u8,
+    drive_strength_6: u8,
+    drive_strength_7: u8,
+    div: u16,
+}
+
+pub struct PllConfig {
+    pub refdiv: u32,
+    pub vco_freq: u32,
+    pub post_div1: u8,
+    pub post_div2: u8,
+}
+
+pub struct RefClkConfig {
+    pub src: RefClkSrc,
+    pub div: u8,
+}
+
+pub enum RefClkSrc {
+    Xosc,
+    Rosc,
+    Aux(ClkRefCtrlAuxsrc),
+}
+
+pub struct SysClkConfig {
+    pub src: SysClkSrc,
+    pub div: u32,
+}
+
+pub enum SysClkSrc {
+    Ref,
+    Aux(ClkSysCtrlAuxsrc),
+}
 
 /// 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 +143,325 @@ 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_config {
+        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_config {
+        EXTERNAL_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);
-    });
-    while c.clk_sys_selected().read() != 1 << ClkSysCtrlSrc::CLKSRC_CLK_SYS_AUX.0 {}
+        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);
+        }
+    }
 
-    // CLK USB = PLL USB (48MHz) / 1 = 48MHz
-    c.clk_usb_div().write(|w| w.set_int(1));
-    c.clk_usb_ctrl().write(|w| {
+    let (src, div) = config.ref_clk_config;
+    match 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 {}
+            c.clk_ref_div().write(|w| w.set_int(div));
+        }
+        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 {}
+            c.clk_ref_div().write(|w| w.set_int(div));
+        }
+        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(div));
+        }
+    }
+
+    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);
+    let (src, div) = config.sys_clk_config;
+    match 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 {}
+            c.clk_sys_div().write(|w| w.set_int(div));
+        }
+        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_div().write(|w| w.set_int(div));
+            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 {}
+        }
+    }
+
+    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((src, div)) = config.usb_clk_config {
+        // CLK USB = PLL USB (48MHz) / 1 = 48MHz
+        c.clk_usb_div().write(|w| w.set_int(div));
+        c.clk_usb_ctrl().write(|w| {
+            w.set_enable(true);
+            w.set_auxsrc(src);
+        });
+    } else {
+        peris.set_usbctrl(false);
+    }
+
+    if let Some((src, div)) = config.adc_clk_config {
+        // CLK ADC = PLL USB (48MHZ) / 1 = 48MHz
+        c.clk_adc_div().write(|w| w.set_int(div));
+        c.clk_adc_ctrl().write(|w| {
+            w.set_enable(true);
+            w.set_auxsrc(src);
+        });
+    } else {
+        peris.set_adc(false);
+    }
+
+    if let Some((src, div)) = config.rtc_clk_config {
+        // 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(div));
+        c.clk_rtc_ctrl().write(|w| {
+            w.set_enable(true);
+            w.set_auxsrc(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(crate) 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 => clk_sys_pll_freq(),
+                ClkSysCtrlAuxsrc::CLKSRC_PLL_USB => clk_usb_pll_freq(),
+                ClkSysCtrlAuxsrc::ROSC_CLKSRC => estimate_rosc_freq(),
+                ClkSysCtrlAuxsrc::XOSC_CLKSRC => unsafe { EXTERNAL_HZ },
+                // TODO not sure how to handle clkin sources
+                _ => todo!(),
+            }
+        }
+        _ => 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(crate) fn clk_sys_pll_freq() -> u32 {
+    let p = pac::PLL_SYS;
+
+    let input_freq = unsafe { EXTERNAL_HZ };
+    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(crate) fn clk_usb_pll_freq() -> u32 {
+    let p = pac::PLL_USB;
+
+    let input_freq = unsafe { EXTERNAL_HZ };
+    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(crate) fn clk_peri_freq() -> u32 {
-    125_000_000
+    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 => clk_sys_pll_freq(),
+        ClkPeriCtrlAuxsrc::ROSC_CLKSRC_PH => estimate_rosc_freq(),
+        ClkPeriCtrlAuxsrc::XOSC_CLKSRC => unsafe { EXTERNAL_HZ },
+        // TODO not sure how to handle clkin sources
+        _ => todo!(),
+    }
+}
+
+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 => unsafe { EXTERNAL_HZ },
+        ClkRefCtrlSrc::CLKSRC_CLK_REF_AUX => todo!(),
+        _ => unreachable!(),
+    };
+
+    let mut div = unsafe { c.clk_ref_div().read().int() } as u32;
+    if div == 0 {
+        div = 4;
+    }
+
+    base / div
 }
 
 pub(crate) fn clk_rtc_freq() -> u32 {
-    46875
+    let c = pac::CLOCKS;
+    let src = unsafe { c.clk_rtc_ctrl().read().auxsrc() };
+
+    let base = match src {
+        ClkRtcCtrlAuxsrc::XOSC_CLKSRC => unsafe { EXTERNAL_HZ },
+        ClkRtcCtrlAuxsrc::ROSC_CLKSRC_PH => estimate_rosc_freq(),
+        ClkRtcCtrlAuxsrc::CLKSRC_PLL_USB => clk_usb_pll_freq(),
+        ClkRtcCtrlAuxsrc::CLKSRC_PLL_SYS => clk_sys_pll_freq(),
+        // TODO not sure how to handle clkin sources
+        _ => todo!(),
+    };
+
+    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() {
-    const XOSC_MHZ: u32 = 12;
+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 +470,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 +503,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,8 +518,8 @@ 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
diff --git a/embassy-rp/src/lib.rs b/embassy-rp/src/lib.rs
index cba7559df..118ce5237 100644
--- a/embassy-rp/src/lib.rs
+++ b/embassy-rp/src/lib.rs
@@ -136,23 +136,35 @@ embassy_hal_common::peripherals! {
 static BOOT2: [u8; 256] = *include_bytes!("boot2.bin");
 
 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();