Add missing functions, Cleanup, Gpout example

2 files changed, 227 insertions, 53 deletions

diff --git a/embassy-rp/src/clocks.rs b/embassy-rp/src/clocks.rs
index 4ae967aae..5d3444442 100644
--- a/embassy-rp/src/clocks.rs
+++ b/embassy-rp/src/clocks.rs
@@ -2,7 +2,8 @@ use pac::clocks::vals::*;
 
 use crate::{pac, reset};
 
-static mut EXTERNAL_HZ: u32 = 0;
+// TODO fix terrible use of global here
+static mut XIN_HZ: u32 = 0;
 
 pub struct ClockConfig {
     rosc_config: Option<RoscConfig>,
@@ -49,7 +50,7 @@ impl ClockConfig {
             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)),
+            usb_clk_config: Some((ClkUsbCtrlAuxsrc::CLKSRC_PLL_USB, 1)),
             adc_clk_config: Some((ClkAdcCtrlAuxsrc::CLKSRC_PLL_USB, 1)),
             rtc_clk_config: Some((ClkRtcCtrlAuxsrc::CLKSRC_PLL_USB, 1024)),
         }
@@ -164,7 +165,7 @@ pub(crate) unsafe fn init(config: ClockConfig) {
     }
 
     if let Some(config) = config.xosc_config {
-        EXTERNAL_HZ = config.hz;
+        XIN_HZ = config.hz;
 
         pac::WATCHDOG.tick().write(|w| {
             w.set_cycles((config.hz / 1_000_000) as u16);
@@ -343,37 +344,21 @@ pub fn estimate_rosc_freq() -> u32 {
     base / div
 }
 
-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();
+pub fn xosc_freq() -> u32 {
+    unsafe { XIN_HZ }
+}
 
-    base / int
+pub fn gpin0_freq() -> u32 {
+    todo!()
+}
+pub fn gpin1_freq() -> u32 {
+    todo!()
 }
 
-pub(crate) fn clk_sys_pll_freq() -> u32 {
+pub fn pll_sys_freq() -> u32 {
     let p = pac::PLL_SYS;
 
-    let input_freq = unsafe { EXTERNAL_HZ };
+    let input_freq = xosc_freq();
     let cs = unsafe { p.cs().read() };
 
     let refdiv = cs.refdiv() as u32;
@@ -386,10 +371,10 @@ pub(crate) fn clk_sys_pll_freq() -> u32 {
     (((input_freq / refdiv) * fbdiv) / postdiv1) / postdiv2
 }
 
-pub(crate) fn clk_usb_pll_freq() -> u32 {
+pub fn pll_usb_freq() -> u32 {
     let p = pac::PLL_USB;
 
-    let input_freq = unsafe { EXTERNAL_HZ };
+    let input_freq = xosc_freq();
     let cs = unsafe { p.cs().read() };
 
     let refdiv = cs.refdiv() as u32;
@@ -402,18 +387,30 @@ pub(crate) fn clk_usb_pll_freq() -> u32 {
     (((input_freq / refdiv) * fbdiv) / postdiv1) / postdiv2
 }
 
-pub(crate) fn clk_peri_freq() -> u32 {
+pub fn clk_sys_freq() -> u32 {
     let c = pac::CLOCKS;
-    let src = unsafe { c.clk_peri_ctrl().read().auxsrc() };
+    let ctrl = unsafe { c.clk_sys_ctrl().read() };
 
-    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!(),
-    }
+    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 {
@@ -422,30 +419,90 @@ pub fn clk_ref_freq() -> u32 {
 
     let base = match ctrl.src() {
         ClkRefCtrlSrc::ROSC_CLKSRC_PH => estimate_rosc_freq(),
-        ClkRefCtrlSrc::XOSC_CLKSRC => unsafe { EXTERNAL_HZ },
-        ClkRefCtrlSrc::CLKSRC_CLK_REF_AUX => todo!(),
+        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 mut div = unsafe { c.clk_ref_div().read().int() } as u32;
-    if div == 0 {
-        div = 4;
+    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!(),
     }
+}
 
-    base / div
+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(crate) fn clk_rtc_freq() -> u32 {
+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::XOSC_CLKSRC => unsafe { EXTERNAL_HZ },
+        ClkRtcCtrlAuxsrc::CLKSRC_PLL_USB => pll_usb_freq(),
+        ClkRtcCtrlAuxsrc::CLKSRC_PLL_SYS => pll_sys_freq(),
         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!(),
+        ClkRtcCtrlAuxsrc::XOSC_CLKSRC => xosc_freq(),
+        ClkRtcCtrlAuxsrc::CLKSRC_GPIN0 => gpin0_freq(),
+        ClkRtcCtrlAuxsrc::CLKSRC_GPIN1 => gpin1_freq(),
+        _ => unreachable!(),
     };
 
     let div = unsafe { c.clk_rtc_div().read() };
@@ -456,6 +513,60 @@ pub(crate) fn clk_rtc_freq() -> u32 {
     base / int
 }
 
+pub fn clk_gpout0_freq() -> u32 {
+    let c = pac::CLOCKS;
+    let src = unsafe { c.clk_gpout0_ctrl().read().auxsrc() };
+
+    let base = match src {
+        ClkGpout0ctrlAuxsrc::CLKSRC_PLL_SYS => pll_sys_freq(),
+        ClkGpout0ctrlAuxsrc::CLKSRC_GPIN0 => gpin0_freq(),
+        ClkGpout0ctrlAuxsrc::CLKSRC_GPIN1 => gpin1_freq(),
+        ClkGpout0ctrlAuxsrc::CLKSRC_PLL_USB => pll_usb_freq(),
+        ClkGpout0ctrlAuxsrc::ROSC_CLKSRC => estimate_rosc_freq(),
+        ClkGpout0ctrlAuxsrc::XOSC_CLKSRC => xosc_freq(),
+        ClkGpout0ctrlAuxsrc::CLK_SYS => clk_sys_freq(),
+        ClkGpout0ctrlAuxsrc::CLK_USB => clk_usb_freq(),
+        ClkGpout0ctrlAuxsrc::CLK_ADC => clk_adc_freq(),
+        ClkGpout0ctrlAuxsrc::CLK_RTC => clk_rtc_freq(),
+        ClkGpout0ctrlAuxsrc::CLK_REF => clk_ref_freq(),
+        _ => unreachable!(),
+    };
+
+    let div = unsafe { c.clk_gpout0_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_gpout1_freq() -> u32 {
+    let c = pac::CLOCKS;
+    let src = unsafe { c.clk_gpout1_ctrl().read().auxsrc() };
+
+    let base = match src {
+        ClkGpout1ctrlAuxsrc::CLKSRC_PLL_SYS => pll_sys_freq(),
+        ClkGpout1ctrlAuxsrc::CLKSRC_GPIN0 => gpin0_freq(),
+        ClkGpout1ctrlAuxsrc::CLKSRC_GPIN1 => gpin1_freq(),
+        ClkGpout1ctrlAuxsrc::CLKSRC_PLL_USB => pll_usb_freq(),
+        ClkGpout1ctrlAuxsrc::ROSC_CLKSRC => estimate_rosc_freq(),
+        ClkGpout1ctrlAuxsrc::XOSC_CLKSRC => xosc_freq(),
+        ClkGpout1ctrlAuxsrc::CLK_SYS => clk_sys_freq(),
+        ClkGpout1ctrlAuxsrc::CLK_USB => clk_usb_freq(),
+        ClkGpout1ctrlAuxsrc::CLK_ADC => clk_adc_freq(),
+        ClkGpout1ctrlAuxsrc::CLK_RTC => clk_rtc_freq(),
+        ClkGpout1ctrlAuxsrc::CLK_REF => clk_ref_freq(),
+        _ => unreachable!(),
+    };
+
+    let div = unsafe { c.clk_gpout1_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()
@@ -526,6 +637,48 @@ unsafe fn configure_pll(p: pac::pll::Pll, input_freq: u32, config: PllConfig) {
     p.pwr().modify(|w| w.set_postdivpd(false));
 }
 
+pub struct Gpout0 {
+    _pin: crate::peripherals::PIN_21,
+}
+
+impl Gpout0 {
+    pub fn new(pin: crate::peripherals::PIN_21) -> Self {
+        unsafe {
+            let p = pac::IO_BANK0.gpio(21).ctrl();
+            p.write(|w| w.set_funcsel(pac::io::vals::Gpio21ctrlFuncsel::CLOCKS_GPOUT_0.0))
+        }
+        Self { _pin: pin }
+    }
+
+    pub fn set_div(&self, int: u32, frac: u8) {
+        unsafe {
+            let c = pac::CLOCKS;
+            c.clk_gpout0_div().write(|w| {
+                w.set_int(int);
+                w.set_frac(frac);
+            });
+        }
+    }
+
+    pub fn set_src(&self, src: ClkGpout0ctrlAuxsrc) {
+        unsafe {
+            let c = pac::CLOCKS;
+            c.clk_gpout0_ctrl().modify(|w| {
+                w.set_auxsrc(src);
+            });
+        }
+    }
+
+    pub fn enable(&self) {
+        unsafe {
+            let c = pac::CLOCKS;
+            c.clk_gpout0_ctrl().modify(|w| {
+                w.set_enable(true);
+            });
+        }
+    }
+}
+
 /// 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/examples/rp/src/bin/gpout.rs b/examples/rp/src/bin/gpout.rs
new file mode 100644
index 000000000..0503212e8
--- /dev/null
+++ b/examples/rp/src/bin/gpout.rs
@@ -0,0 +1,21 @@
+#![no_std]
+#![no_main]
+#![feature(type_alias_impl_trait)]
+
+use defmt::*;
+use embassy_executor::Spawner;
+use embassy_rp::{clocks, pac};
+use {defmt_rtt as _, panic_probe as _};
+
+#[embassy_executor::main]
+async fn main(_spawner: Spawner) {
+    let p = embassy_rp::init(Default::default());
+    //let mut led = Output::new(p.PIN_25, Level::Low);
+
+    let gpout0 = clocks::Gpout0::new(p.PIN_21);
+    gpout0.set_src(pac::clocks::vals::ClkGpout0ctrlAuxsrc::CLK_SYS);
+    gpout0.set_div(1000, 0);
+    gpout0.enable();
+
+    info!("Pin 21 should be toggling at {} hz", clocks::clk_gpout0_freq());
+}