rp: add generic dormant-sleep functionality

this is "generic" in that it doesn't require the user to set up anything
specific to go to dormant sleep, unlike the C sdk which requires clock
sources to be configured explicitly and doesn't much care about PLLs. we
will instead take a snapshot of the current clock configuration, switch
to a known clock source (very slow rosc, in this case), go to sleep, and
on wakeup undo everything we've done (ensuring stability of PLLs and
such).

tested locally, but adding tests to HIL seems infeasible. we'd need at
least another pico or extensive modifications to teleprobe since
dormant-sleep breaks SWD (except to rescue-dp), neither of which is
feasible at this point. if we *did* want to add tests we should check
for both rtc wakeups (with an external rtc clock source) and gpio wakeups.

1 files changed, 130 insertions, 0 deletions

diff --git a/embassy-rp/src/clocks.rs b/embassy-rp/src/clocks.rs
index a33980230..7b25ecffb 100644
--- a/embassy-rp/src/clocks.rs
+++ b/embassy-rp/src/clocks.rs
@@ -1,3 +1,4 @@
+use core::arch::asm;
 use core::marker::PhantomData;
 use core::sync::atomic::{AtomicU16, AtomicU32, Ordering};
 
@@ -6,6 +7,7 @@ use pac::clocks::vals::*;
 
 use crate::gpio::sealed::Pin;
 use crate::gpio::AnyPin;
+use crate::pac::common::{Reg, RW};
 use crate::{pac, reset, Peripheral};
 
 // NOTE: all gpin handling is commented out for future reference.
@@ -873,3 +875,131 @@ impl rand_core::RngCore for RoscRng {
         dest.fill_with(Self::next_u8)
     }
 }
+/// Enter the `DORMANT` sleep state. This will stop *all* internal clocks
+/// and can only be exited through resets, dormant-wake GPIO interrupts,
+/// and RTC interrupts. If RTC is clocked from an internal clock source
+/// it will be stopped and not function as a wakeup source.
+#[cfg(target_arch = "arm")]
+pub fn dormant_sleep() {
+    struct Set<T: Copy, F: Fn()>(Reg<T, RW>, T, F);
+
+    impl<T: Copy, F: Fn()> Drop for Set<T, F> {
+        fn drop(&mut self) {
+            self.0.write_value(self.1);
+            self.2();
+        }
+    }
+
+    fn set_with_post_restore<T: Copy, After: Fn(), F: FnOnce(&mut T) -> After>(
+        reg: Reg<T, RW>,
+        f: F,
+    ) -> Set<T, impl Fn()> {
+        reg.modify(|w| {
+            let old = *w;
+            let after = f(w);
+            Set(reg, old, after)
+        })
+    }
+
+    fn set<T: Copy, F: FnOnce(&mut T)>(reg: Reg<T, RW>, f: F) -> Set<T, impl Fn()> {
+        set_with_post_restore(reg, |r| {
+            f(r);
+            || ()
+        })
+    }
+
+    // disable all clocks that are not vital in preparation for disabling clock sources.
+    // we'll keep gpout and rtc clocks untouched, gpout because we don't care about them
+    // and rtc because it's a possible wakeup source. if clk_rtc is not configured for
+    // gpin we'll never wake from rtc, but that's what the user asked for then.
+    let _stop_adc = set(pac::CLOCKS.clk_adc_ctrl(), |w| w.set_enable(false));
+    let _stop_usb = set(pac::CLOCKS.clk_usb_ctrl(), |w| w.set_enable(false));
+    let _stop_peri = set(pac::CLOCKS.clk_peri_ctrl(), |w| w.set_enable(false));
+    // set up rosc. we could ask the use to tell us which clock source to wake from like
+    // the C SDK does, but that seems rather unfriendly. we *may* disturb rtc by changing
+    // rosc configuration if it's currently the rtc clock source, so we'll configure rosc
+    // to the slowest frequency to minimize that impact.
+    let _configure_rosc = (
+        set(pac::ROSC.ctrl(), |w| {
+            w.set_enable(pac::rosc::vals::Enable::ENABLE);
+            w.set_freq_range(pac::rosc::vals::FreqRange::LOW);
+        }),
+        // div=32
+        set(pac::ROSC.div(), |w| w.set_div(pac::rosc::vals::Div(0xaa0))),
+    );
+    while !pac::ROSC.status().read().stable() {}
+    // switch over to rosc as the system clock source. this will change clock sources for
+    // watchdog and timer clocks, but timers won't be a concern and the watchdog won't
+    // speed up by enough to worry about (unless it's clocked from gpin, which we don't
+    // support anyway).
+    let _switch_clk_ref = set(pac::CLOCKS.clk_ref_ctrl(), |w| {
+        w.set_src(pac::clocks::vals::ClkRefCtrlSrc::ROSC_CLKSRC_PH);
+    });
+    let _switch_clk_sys = set(pac::CLOCKS.clk_sys_ctrl(), |w| {
+        w.set_src(pac::clocks::vals::ClkSysCtrlSrc::CLK_REF);
+    });
+    // oscillator dormancy does not power down plls, we have to do that ourselves. we'll
+    // restore them to their prior glory when woken though since the system may be clocked
+    // from either (and usb/adc will probably need the USB PLL anyway)
+    let _stop_pll_sys = set_with_post_restore(pac::PLL_SYS.pwr(), |w| {
+        let wake = !w.pd() && !w.vcopd();
+        w.set_pd(true);
+        w.set_vcopd(true);
+        move || while wake && !pac::PLL_SYS.cs().read().lock() {}
+    });
+    let _stop_pll_usb = set_with_post_restore(pac::PLL_USB.pwr(), |w| {
+        let wake = !w.pd() && !w.vcopd();
+        w.set_pd(true);
+        w.set_vcopd(true);
+        move || while wake && !pac::PLL_USB.cs().read().lock() {}
+    });
+    // dormancy only stops the oscillator we're telling to go dormant, the other remains
+    // running. nothing can use xosc at this point any more. not doing this costs an 200µA.
+    let _stop_xosc = set_with_post_restore(pac::XOSC.ctrl(), |w| {
+        let wake = w.enable() == pac::xosc::vals::Enable::ENABLE;
+        if wake {
+            w.set_enable(pac::xosc::vals::Enable::DISABLE);
+        }
+        move || while wake && !pac::XOSC.status().read().stable() {}
+    });
+    let _power_down_xip_cache = set(pac::XIP_CTRL.ctrl(), |w| w.set_power_down(true));
+
+    // only power down memory if we're running from XIP (or ROM? how?).
+    // powering down memory otherwise would require a lot of exacting checks that
+    // are better done by the user in a local copy of this function.
+    // powering down memories saves ~100µA, so it's well worth doing.
+    unsafe {
+        let is_in_flash = {
+            // we can't rely on the address of this function as rust sees it since linker
+            // magic or even boot2 may place it into ram.
+            let pc: usize;
+            asm!(
+                "mov {pc}, pc",
+                pc = out (reg) pc
+            );
+            pc < 0x20000000
+        };
+        if is_in_flash {
+            // we will be powering down memories, so we must be *absolutely*
+            // certain that we're running entirely from XIP and registers until
+            // memories are powered back up again. accessing memory that's powered
+            // down may corrupt memory contents (see section 2.11.4 of the manual).
+            // additionally a 20ns wait time is needed after powering up memories
+            // again. rosc is likely to run at only a few MHz at most, so the
+            // inter-instruction delay alone will be enough to satisfy this bound.
+            asm!(
+                "ldr {old_mem}, [{mempowerdown}]",
+                "str {power_down_mems}, [{mempowerdown}]",
+                "str {coma}, [{dormant}]",
+                "str {old_mem}, [{mempowerdown}]",
+                old_mem = out (reg) _,
+                mempowerdown = in (reg) pac::SYSCFG.mempowerdown().as_ptr(),
+                power_down_mems = in (reg) 0b11111111,
+                dormant = in (reg) pac::ROSC.dormant().as_ptr(),
+                coma = in (reg) 0x636f6d61,
+            );
+        } else {
+            pac::ROSC.dormant().write_value(0x636f6d61);
+        }
+    }
+}