diff --git a/embassy-stm32/src/lib.rs b/embassy-stm32/src/lib.rs index 8c87ea7d..ec8648ee 100644 --- a/embassy-stm32/src/lib.rs +++ b/embassy-stm32/src/lib.rs @@ -197,6 +197,11 @@ pub fn init(config: Config) -> Peripherals { // must be after rcc init #[cfg(feature = "_time-driver")] time_driver::init(); + + #[cfg(feature = "low-power")] + while !crate::rcc::low_power_ready() { + crate::rcc::clock_refcount_sub(); + } } p diff --git a/embassy-stm32/src/low_power.rs b/embassy-stm32/src/low_power.rs index 7814fa38..65b93f8a 100644 --- a/embassy-stm32/src/low_power.rs +++ b/embassy-stm32/src/low_power.rs @@ -3,46 +3,52 @@ use core::marker::PhantomData; use cortex_m::peripheral::SCB; use embassy_executor::*; -use embassy_time::Duration; use crate::interrupt; use crate::interrupt::typelevel::Interrupt; use crate::pac::EXTI; use crate::rcc::low_power_ready; +use crate::time_driver::{get_driver, RtcDriver}; const THREAD_PENDER: usize = usize::MAX; -const THRESHOLD: Duration = Duration::from_millis(500); -use crate::rtc::{Rtc, RtcInstant}; +use crate::rtc::Rtc; -static mut RTC: Option<&'static Rtc> = None; +static mut EXECUTOR: Option = None; foreach_interrupt! { (RTC, rtc, $block:ident, WKUP, $irq:ident) => { #[interrupt] unsafe fn $irq() { - Executor::on_wakeup_irq(); + unsafe { EXECUTOR.as_mut().unwrap() }.on_wakeup_irq(); } }; } +// pub fn timer_driver_pause_time() { +// pause_time(); +// } + pub fn stop_with_rtc(rtc: &'static Rtc) { - crate::interrupt::typelevel::RTC_WKUP::unpend(); - unsafe { crate::interrupt::typelevel::RTC_WKUP::enable() }; - - EXTI.rtsr(0).modify(|w| w.set_line(22, true)); - EXTI.imr(0).modify(|w| w.set_line(22, true)); - - unsafe { RTC = Some(rtc) }; + unsafe { EXECUTOR.as_mut().unwrap() }.stop_with_rtc(rtc) } -pub fn start_wakeup_alarm(requested_duration: embassy_time::Duration) -> RtcInstant { - unsafe { RTC }.unwrap().start_wakeup_alarm(requested_duration) -} - -pub fn stop_wakeup_alarm() -> RtcInstant { - unsafe { RTC }.unwrap().stop_wakeup_alarm() -} +// pub fn start_wakeup_alarm(requested_duration: embassy_time::Duration) { +// let rtc_instant = unsafe { EXECUTOR.as_mut().unwrap() } +// .rtc +// .unwrap() +// .start_wakeup_alarm(requested_duration); +// +// unsafe { EXECUTOR.as_mut().unwrap() }.last_stop = Some(rtc_instant); +// } +// +// pub fn set_sleepdeep() { +// unsafe { EXECUTOR.as_mut().unwrap() }.scb.set_sleepdeep(); +// } +// +// pub fn stop_wakeup_alarm() -> RtcInstant { +// unsafe { EXECUTOR.as_mut().unwrap() }.rtc.unwrap().stop_wakeup_alarm() +// } /// Thread mode executor, using WFE/SEV. /// @@ -57,54 +63,62 @@ pub fn stop_wakeup_alarm() -> RtcInstant { pub struct Executor { inner: raw::Executor, not_send: PhantomData<*mut ()>, + scb: SCB, + time_driver: &'static RtcDriver, } impl Executor { /// Create a new Executor. - pub fn new() -> Self { - Self { - inner: raw::Executor::new(THREAD_PENDER as *mut ()), - not_send: PhantomData, + pub fn take() -> &'static mut Self { + unsafe { + assert!(EXECUTOR.is_none()); + + EXECUTOR = Some(Self { + inner: raw::Executor::new(THREAD_PENDER as *mut ()), + not_send: PhantomData, + scb: cortex_m::Peripherals::steal().SCB, + time_driver: get_driver(), + }); + + EXECUTOR.as_mut().unwrap() } } - unsafe fn on_wakeup_irq() { - info!("on wakeup irq"); + unsafe fn on_wakeup_irq(&mut self) { + trace!("low power: on wakeup irq"); - cortex_m::asm::bkpt(); + self.time_driver.resume_time(); + trace!("low power: resume time"); } - fn time_until_next_alarm(&self) -> Duration { - Duration::from_secs(3) + pub(self) fn stop_with_rtc(&mut self, rtc: &'static Rtc) { + trace!("low power: stop with rtc configured"); + + self.time_driver.set_rtc(rtc); + + crate::interrupt::typelevel::RTC_WKUP::unpend(); + unsafe { crate::interrupt::typelevel::RTC_WKUP::enable() }; + + EXTI.rtsr(0).modify(|w| w.set_line(22, true)); + EXTI.imr(0).modify(|w| w.set_line(22, true)); } - fn get_scb() -> SCB { - unsafe { cortex_m::Peripherals::steal() }.SCB - } - - fn configure_pwr(&self) { - trace!("configure_pwr"); + fn configure_pwr(&mut self) { + trace!("low power: configure_pwr"); + self.scb.clear_sleepdeep(); if !low_power_ready() { + trace!("low power: configure_pwr: low power not ready"); return; } - let time_until_next_alarm = self.time_until_next_alarm(); - if time_until_next_alarm < THRESHOLD { + if self.time_driver.pause_time().is_err() { + trace!("low power: configure_pwr: time driver failed to pause"); return; } - trace!("low power stop required"); - - critical_section::with(|_| { - trace!("executor: set wakeup alarm..."); - - start_wakeup_alarm(time_until_next_alarm); - - trace!("low power wait for rtc ready..."); - - Self::get_scb().set_sleepdeep(); - }); + trace!("low power: enter stop..."); + self.scb.set_sleepdeep(); } /// Run the executor. @@ -126,11 +140,11 @@ impl Executor { /// /// This function never returns. pub fn run(&'static mut self, init: impl FnOnce(Spawner)) -> ! { - init(self.inner.spawner()); + init(unsafe { EXECUTOR.as_mut().unwrap() }.inner.spawner()); loop { unsafe { - self.inner.poll(); + EXECUTOR.as_mut().unwrap().inner.poll(); self.configure_pwr(); asm!("wfe"); }; diff --git a/embassy-stm32/src/rcc/mod.rs b/embassy-stm32/src/rcc/mod.rs index 3c75923e..45a4d880 100644 --- a/embassy-stm32/src/rcc/mod.rs +++ b/embassy-stm32/src/rcc/mod.rs @@ -86,6 +86,8 @@ static CLOCK_REFCOUNT: AtomicU32 = AtomicU32::new(0); #[cfg(feature = "low-power")] pub fn low_power_ready() -> bool { + trace!("clock refcount: {}", CLOCK_REFCOUNT.load(Ordering::SeqCst)); + CLOCK_REFCOUNT.load(Ordering::SeqCst) == 0 } diff --git a/embassy-stm32/src/rtc/mod.rs b/embassy-stm32/src/rtc/mod.rs index a6102077..1f1abb78 100644 --- a/embassy-stm32/src/rtc/mod.rs +++ b/embassy-stm32/src/rtc/mod.rs @@ -1,6 +1,14 @@ //! RTC peripheral abstraction mod datetime; +#[cfg(feature = "low-power")] +use core::cell::Cell; + +#[cfg(feature = "low-power")] +use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex; +#[cfg(feature = "low-power")] +use embassy_sync::blocking_mutex::Mutex; + pub use self::datetime::{DateTime, DayOfWeek, Error as DateTimeError}; /// refer to AN4759 to compare features of RTC2 and RTC3 @@ -30,9 +38,73 @@ pub enum RtcError { NotRunning, } +#[cfg(feature = "low-power")] +/// Represents an instant in time that can be substracted to compute a duration +struct RtcInstant { + second: u8, + subsecond: u16, +} + +#[cfg(feature = "low-power")] +impl RtcInstant { + pub fn now() -> Self { + let tr = RTC::regs().tr().read(); + let tr2 = RTC::regs().tr().read(); + let ssr = RTC::regs().ssr().read().ss(); + let ssr2 = RTC::regs().ssr().read().ss(); + + let st = bcd2_to_byte((tr.st(), tr.su())); + let st2 = bcd2_to_byte((tr2.st(), tr2.su())); + + assert!(st == st2); + assert!(ssr == ssr2); + + let _ = RTC::regs().dr().read(); + + let subsecond = ssr; + let second = st; + + // trace!("rtc: instant now: st, ssr: {}, {}", st, ssr); + + Self { second, subsecond } + } +} + +#[cfg(feature = "low-power")] +impl core::ops::Sub for RtcInstant { + type Output = embassy_time::Duration; + + fn sub(self, rhs: Self) -> Self::Output { + use embassy_time::{Duration, TICK_HZ}; + + let second = if self.second < rhs.second { + self.second + 60 + } else { + self.second + }; + + // TODO: read prescaler + + let self_ticks = second as u32 * 256 + (255 - self.subsecond as u32); + let other_ticks = rhs.second as u32 * 256 + (255 - rhs.subsecond as u32); + let rtc_ticks = self_ticks - other_ticks; + + // trace!( + // "rtc: instant sub: self, other, rtc ticks: {}, {}, {}", + // self_ticks, + // other_ticks, + // rtc_ticks + // ); + + Duration::from_ticks(((rtc_ticks * TICK_HZ as u32) / 256u32) as u64) + } +} + /// RTC Abstraction pub struct Rtc { rtc_config: RtcConfig, + #[cfg(feature = "low-power")] + stop_time: Mutex>>, } #[derive(Copy, Clone, Debug, PartialEq)] @@ -108,8 +180,15 @@ impl Rtc { pub fn new(_rtc: impl Peripheral

, rtc_config: RtcConfig) -> Self { RTC::enable_peripheral_clk(); + #[cfg(not(feature = "low-power"))] let mut rtc_struct = Self { rtc_config }; + #[cfg(feature = "low-power")] + let mut rtc_struct = Self { + rtc_config, + stop_time: Mutex::const_new(CriticalSectionRawMutex::new(), Cell::new(None)), + }; + Self::enable(); rtc_struct.configure(rtc_config); diff --git a/embassy-stm32/src/rtc/v2.rs b/embassy-stm32/src/rtc/v2.rs index bcb127ec..bf926f98 100644 --- a/embassy-stm32/src/rtc/v2.rs +++ b/embassy-stm32/src/rtc/v2.rs @@ -1,67 +1,12 @@ use stm32_metapac::rtc::vals::{Init, Osel, Pol}; +#[cfg(feature = "low-power")] +use super::RtcInstant; use super::{sealed, RtcClockSource, RtcConfig}; use crate::pac::rtc::Rtc; use crate::peripherals::RTC; use crate::rtc::sealed::Instance; -#[cfg(all(feature = "time", any(stm32wb, stm32f4)))] -pub struct RtcInstant { - ssr: u16, - st: u8, -} - -#[cfg(all(feature = "time", any(stm32wb, stm32f4)))] -impl RtcInstant { - pub fn now() -> Self { - // TODO: read value twice - use crate::rtc::bcd2_to_byte; - - let tr = RTC::regs().tr().read(); - let tr2 = RTC::regs().tr().read(); - let ssr = RTC::regs().ssr().read().ss(); - let ssr2 = RTC::regs().ssr().read().ss(); - - let st = bcd2_to_byte((tr.st(), tr.su())); - let st2 = bcd2_to_byte((tr2.st(), tr2.su())); - - assert!(st == st2); - assert!(ssr == ssr2); - - let _ = RTC::regs().dr().read(); - - trace!("ssr: {}", ssr); - trace!("st: {}", st); - - Self { ssr, st } - } -} - -#[cfg(all(feature = "time", any(stm32wb, stm32f4)))] -impl core::ops::Sub for RtcInstant { - type Output = embassy_time::Duration; - - fn sub(self, rhs: Self) -> Self::Output { - use embassy_time::{Duration, TICK_HZ}; - - let st = if self.st < rhs.st { self.st + 60 } else { self.st }; - - // TODO: read prescaler - - let self_ticks = st as u32 * 256 + (255 - self.ssr as u32); - let other_ticks = rhs.st as u32 * 256 + (255 - rhs.ssr as u32); - let rtc_ticks = self_ticks - other_ticks; - - trace!("self, other, rtc ticks: {}, {}, {}", self_ticks, other_ticks, rtc_ticks); - - Duration::from_ticks( - ((((st as u32 * 256 + (255u32 - self.ssr as u32)) - (rhs.st as u32 * 256 + (255u32 - rhs.ssr as u32))) - * TICK_HZ as u32) as u32 - / 256u32) as u64, - ) - } -} - #[allow(dead_code)] #[derive(Clone, Copy, Debug)] pub(crate) enum WakeupPrescaler { @@ -144,15 +89,10 @@ impl super::Rtc { } } - #[allow(dead_code)] - #[cfg(all(feature = "time", any(stm32wb, stm32f4)))] - /// start the wakeup alarm and return the actual duration of the alarm - /// the actual duration will be the closest value possible that is less - /// than the requested duration. - /// - /// note: this api is exposed for testing purposes until low power is implemented. - /// it is not intended to be public - pub(crate) fn start_wakeup_alarm(&self, requested_duration: embassy_time::Duration) -> RtcInstant { + #[cfg(feature = "low-power")] + /// start the wakeup alarm and wtih a duration that is as close to but less than + /// the requested duration, and record the instant the wakeup alarm was started + pub(crate) fn start_wakeup_alarm(&self, requested_duration: embassy_time::Duration) { use embassy_time::{Duration, TICK_HZ}; use crate::rcc::get_freqs; @@ -174,37 +114,45 @@ impl super::Rtc { rtc_ticks as u64 * TICK_HZ * (>::into(prescaler) as u64) / rtc_hz, ); - trace!("set wakeup timer for {} ms", duration.as_millis()); - self.write(false, |regs| { - regs.cr().modify(|w| w.set_wutie(true)); - regs.cr().modify(|w| w.set_wute(false)); regs.isr().modify(|w| w.set_wutf(false)); while !regs.isr().read().wutwf() {} regs.cr().modify(|w| w.set_wucksel(prescaler.into())); regs.cr().modify(|w| w.set_wute(true)); + regs.cr().modify(|w| w.set_wutie(true)); }); - RtcInstant::now() + trace!("rtc: start wakeup alarm for {} ms", duration.as_millis()); + + critical_section::with(|cs| assert!(self.stop_time.borrow(cs).replace(Some(RtcInstant::now())).is_none())) } - #[allow(dead_code)] - #[cfg(all(feature = "time", any(stm32wb, stm32f4)))] - /// stop the wakeup alarm and return the time remaining - /// - /// note: this api is exposed for testing purposes until low power is implemented. - /// it is not intended to be public - pub(crate) fn stop_wakeup_alarm(&self) -> RtcInstant { - trace!("disable wakeup timer..."); + #[cfg(feature = "low-power")] + /// stop the wakeup alarm and return the time elapsed since `start_wakeup_alarm` + /// was called, otherwise none + pub(crate) fn stop_wakeup_alarm(&self) -> Option { + use crate::interrupt::typelevel::Interrupt; + + trace!("rtc: stop wakeup alarm..."); self.write(false, |regs| { + regs.cr().modify(|w| w.set_wutie(false)); regs.cr().modify(|w| w.set_wute(false)); regs.isr().modify(|w| w.set_wutf(false)); + + crate::pac::EXTI.pr(0).modify(|w| w.set_line(22, true)); + crate::interrupt::typelevel::RTC_WKUP::unpend(); }); - RtcInstant::now() + critical_section::with(|cs| { + if let Some(stop_time) = self.stop_time.borrow(cs).take() { + Some(RtcInstant::now() - stop_time) + } else { + None + } + }) } #[allow(dead_code)] diff --git a/embassy-stm32/src/time_driver.rs b/embassy-stm32/src/time_driver.rs index 2622442f..99d423d0 100644 --- a/embassy-stm32/src/time_driver.rs +++ b/embassy-stm32/src/time_driver.rs @@ -14,6 +14,8 @@ use stm32_metapac::timer::regs; use crate::interrupt::typelevel::Interrupt; use crate::pac::timer::vals; use crate::rcc::sealed::RccPeripheral; +#[cfg(feature = "low-power")] +use crate::rtc::Rtc; use crate::timer::sealed::{Basic16bitInstance as BasicInstance, GeneralPurpose16bitInstance as Instance}; use crate::{interrupt, peripherals}; @@ -130,12 +132,14 @@ impl AlarmState { } } -struct RtcDriver { +pub(crate) struct RtcDriver { /// Number of 2^15 periods elapsed since boot. period: AtomicU32, alarm_count: AtomicU8, /// Timestamp at which to fire alarm. u64::MAX if no alarm is scheduled. alarms: Mutex, + #[cfg(feature = "low-power")] + rtc: Mutex>>, } const ALARM_STATE_NEW: AlarmState = AlarmState::new(); @@ -144,6 +148,8 @@ embassy_time::time_driver_impl!(static DRIVER: RtcDriver = RtcDriver { period: AtomicU32::new(0), alarm_count: AtomicU8::new(0), alarms: Mutex::const_new(CriticalSectionRawMutex::new(), [ALARM_STATE_NEW; ALARM_COUNT]), + #[cfg(feature = "low-power")] + rtc: Mutex::const_new(CriticalSectionRawMutex::new(), Cell::new(None)), }); impl RtcDriver { @@ -259,6 +265,117 @@ impl RtcDriver { let f: fn(*mut ()) = unsafe { mem::transmute(alarm.callback.get()) }; f(alarm.ctx.get()); } + + #[cfg(feature = "low-power")] + /// Set the rtc but panic if it's already been set + pub(crate) fn set_rtc(&self, rtc: &'static Rtc) { + critical_section::with(|cs| assert!(self.rtc.borrow(cs).replace(Some(rtc)).is_none())); + } + + #[cfg(feature = "low-power")] + /// Compute the approximate amount of time until the next alarm + fn time_until_next_alarm(&self) -> embassy_time::Duration { + critical_section::with(|cs| { + let now = self.now() + 32; + + embassy_time::Duration::from_ticks( + self.alarms + .borrow(cs) + .iter() + .map(|alarm: &AlarmState| alarm.timestamp.get().saturating_sub(now)) + .min() + .unwrap_or(u64::MAX), + ) + }) + } + + #[cfg(feature = "low-power")] + /// Add the given offset to the current time + fn add_time(&self, offset: embassy_time::Duration) { + let offset = offset.as_ticks(); + let cnt = T::regs_gp16().cnt().read().cnt() as u32; + let period = self.period.load(Ordering::SeqCst); + + // Correct the race, if it exists + let period = if period & 1 == 1 && cnt < u16::MAX as u32 / 2 { + period + 1 + } else { + period + }; + + // Normalize to the full overflow + let period = (period / 2) * 2; + + // Add the offset + let period = period + 2 * (offset / u16::MAX as u64) as u32; + let cnt = cnt + (offset % u16::MAX as u64) as u32; + + let (cnt, period) = if cnt > u16::MAX as u32 { + (cnt - u16::MAX as u32, period + 2) + } else { + (cnt, period) + }; + + let period = if cnt > u16::MAX as u32 / 2 { period + 1 } else { period }; + + self.period.store(period, Ordering::SeqCst); + T::regs_gp16().cnt().write(|w| w.set_cnt(cnt as u16)); + + // Now, recompute all alarms + critical_section::with(|cs| { + for i in 0..ALARM_COUNT { + let alarm_handle = unsafe { AlarmHandle::new(i as u8) }; + let alarm = self.get_alarm(cs, alarm_handle); + + self.set_alarm(alarm_handle, alarm.timestamp.get()); + } + }) + } + + #[cfg(feature = "low-power")] + /// Stop the wakeup alarm, if enabled, and add the appropriate offset + fn stop_wakeup_alarm(&self) { + critical_section::with(|cs| { + if let Some(offset) = self.rtc.borrow(cs).get().unwrap().stop_wakeup_alarm() { + self.add_time(offset); + } + }); + } + + #[cfg(feature = "low-power")] + /// Pause the timer if ready; return err if not + pub(crate) fn pause_time(&self) -> Result<(), ()> { + /* + If the wakeup timer is currently running, then we need to stop it and + add the elapsed time to the current time + */ + self.stop_wakeup_alarm(); + + let time_until_next_alarm = self.time_until_next_alarm(); + if time_until_next_alarm < embassy_time::Duration::from_millis(250) { + Err(()) + } else { + critical_section::with(|cs| { + self.rtc + .borrow(cs) + .get() + .unwrap() + .start_wakeup_alarm(time_until_next_alarm); + }); + + T::regs_gp16().cr1().modify(|w| w.set_cen(false)); + + Ok(()) + } + } + + #[cfg(feature = "low-power")] + /// Resume the timer with the given offset + pub(crate) fn resume_time(&self) { + self.stop_wakeup_alarm(); + + T::regs_gp16().cr1().modify(|w| w.set_cen(true)); + } } impl Driver for RtcDriver { @@ -329,6 +446,11 @@ impl Driver for RtcDriver { } } +#[cfg(feature = "low-power")] +pub(crate) fn get_driver() -> &'static RtcDriver { + &DRIVER +} + pub(crate) fn init() { DRIVER.init() } diff --git a/tests/stm32/Cargo.toml b/tests/stm32/Cargo.toml index 754356cb..1f8c7373 100644 --- a/tests/stm32/Cargo.toml +++ b/tests/stm32/Cargo.toml @@ -7,7 +7,7 @@ autobins = false [features] stm32f103c8 = ["embassy-stm32/stm32f103c8", "not-gpdma"] # Blue Pill -stm32f429zi = ["embassy-stm32/stm32f429zi", "chrono", "can", "not-gpdma", "dac-adc-pin"] # Nucleo "sdmmc" +stm32f429zi = ["embassy-stm32/stm32f429zi", "chrono", "stop", "can", "not-gpdma", "dac-adc-pin"] # Nucleo "sdmmc" stm32g071rb = ["embassy-stm32/stm32g071rb", "not-gpdma", "dac-adc-pin"] # Nucleo stm32c031c6 = ["embassy-stm32/stm32c031c6", "not-gpdma"] # Nucleo stm32g491re = ["embassy-stm32/stm32g491re", "not-gpdma"] # Nucleo @@ -17,6 +17,7 @@ stm32h563zi = ["embassy-stm32/stm32h563zi"] # Nucleo stm32u585ai = ["embassy-stm32/stm32u585ai"] # IoT board sdmmc = [] +stop = ["embassy-stm32/low-power"] chrono = ["embassy-stm32/chrono", "dep:chrono"] can = [] ble = ["dep:embassy-stm32-wpan", "embassy-stm32-wpan/ble"] @@ -47,6 +48,7 @@ micromath = "2.0.0" panic-probe = { version = "0.3.0", features = ["print-defmt"] } rand_core = { version = "0.6", default-features = false } rand_chacha = { version = "0.3", default-features = false } +static_cell = {version = "1.1", features = ["nightly"] } chrono = { version = "^0.4", default-features = false, optional = true} @@ -87,6 +89,11 @@ name = "spi_dma" path = "src/bin/spi_dma.rs" required-features = [] +[[bin]] +name = "stop" +path = "src/bin/stop.rs" +required-features = [ "stop", "chrono",] + [[bin]] name = "timer" path = "src/bin/timer.rs" diff --git a/tests/stm32/src/bin/stop.rs b/tests/stm32/src/bin/stop.rs new file mode 100644 index 00000000..4a49bde9 --- /dev/null +++ b/tests/stm32/src/bin/stop.rs @@ -0,0 +1,53 @@ +// required-features: stop,chrono + +#![no_std] +#![no_main] +#![feature(type_alias_impl_trait)] +#[path = "../common.rs"] +mod common; + +use chrono::NaiveDate; +use common::*; +use cortex_m_rt::entry; +use embassy_executor::Spawner; +use embassy_stm32::low_power::{stop_with_rtc, Executor}; +use embassy_stm32::rtc::{Rtc, RtcClockSource, RtcConfig}; +use embassy_time::{Duration, Timer}; +use static_cell::make_static; + +#[entry] +fn main() -> ! { + let executor = Executor::take(); + executor.run(|spawner| { + unwrap!(spawner.spawn(async_main(spawner))); + }); +} + +#[embassy_executor::task] +async fn async_main(_spawner: Spawner) { + let mut config = config(); + + config.rcc.rtc = Some(RtcClockSource::LSI); + + let p = embassy_stm32::init(config); + info!("Hello World!"); + + let now = NaiveDate::from_ymd_opt(2020, 5, 15) + .unwrap() + .and_hms_opt(10, 30, 15) + .unwrap(); + + let mut rtc = Rtc::new(p.RTC, RtcConfig::default()); + + rtc.set_datetime(now.into()).expect("datetime not set"); + + let rtc = make_static!(rtc); + + stop_with_rtc(rtc); + + info!("Waiting 5 seconds"); + Timer::after(Duration::from_secs(5)).await; + + info!("Test OK"); + cortex_m::asm::bkpt(); +}