diff --git a/rtic-time/Cargo.toml b/rtic-time/Cargo.toml index 462ad5d2b5..dda8e600cc 100644 --- a/rtic-time/Cargo.toml +++ b/rtic-time/Cargo.toml @@ -20,3 +20,10 @@ license = "MIT OR Apache-2.0" critical-section = "1" futures-util = { version = "0.3.25", default-features = false } rtic-common = { version = "1.0.0-alpha.0", path = "../rtic-common" } + +[dev-dependencies] +parking_lot = "0.12" +tokio = { version = "1.27", features = ["rt", "macros", "sync", "rt-multi-thread", "time"] } +critical-section = { version = "1", features = ["std"] } +pretty_env_logger = "0.4" +log = "0.4" diff --git a/rtic-time/tests/timer_queue.rs b/rtic-time/tests/timer_queue.rs new file mode 100644 index 0000000000..1ac96b80fd --- /dev/null +++ b/rtic-time/tests/timer_queue.rs @@ -0,0 +1,217 @@ +use std::{fmt::Debug, time::Duration}; + +use parking_lot::Mutex; +use rtic_time::{Monotonic, TimerQueue}; +use tokio::sync::watch; + +static START: Mutex> = Mutex::new(None); +pub struct StdTokioMono; + +// An instant that "starts" at Duration::ZERO, so we can +// have a zero value. +#[derive(PartialEq, Eq, PartialOrd, Ord, Clone, Copy, Debug)] +pub struct Instant(std::time::Duration); + +impl Instant { + pub fn init() { + assert!(START.lock().is_none()); + let _ = START.lock().insert(std::time::Instant::now()); + } + + pub fn now() -> Self { + let start = channel_read("Instant start not initialized", &START); + Self(start.elapsed()) + } +} + +impl core::ops::Add for Instant { + type Output = Instant; + + fn add(self, rhs: Duration) -> Self::Output { + Self(self.0 + rhs) + } +} + +impl core::ops::Sub for Instant { + type Output = Instant; + + fn sub(self, rhs: Duration) -> Self::Output { + Self(self.0 - rhs) + } +} + +impl core::ops::Sub for Instant { + type Output = Duration; + + fn sub(self, rhs: Instant) -> Self::Output { + self.0 - rhs.0 + } +} + +fn channel_read(msg: &str, channel: &Mutex>) -> T { + channel.lock().as_ref().expect(msg).clone() +} + +fn event_write(msg: &str, channel: &Mutex>>, value: T) { + channel.lock().as_ref().expect(msg).send(value).unwrap() +} + +static COMPARE_RX: Mutex>> = Mutex::new(None); +static COMPARE_TX: Mutex>> = Mutex::new(None); +static INTERRUPT_RX: Mutex>> = Mutex::new(None); +static INTERRUPT_TX: Mutex>> = Mutex::new(None); + +impl StdTokioMono { + /// Initialize the monotonic. + /// + /// Returns a [`watch::Sender`] that will cause the interrupt + /// & compare-change tasks to exit if a value is sent to it or it + /// is dropped. + #[must_use = "Dropping the returned Sender stops interrupts & compare-change events"] + pub fn init() -> watch::Sender<()> { + Instant::init(); + let (compare_tx, compare_rx) = watch::channel(Instant(Duration::ZERO)); + let (irq_tx, irq_rx) = watch::channel(()); + + assert!(COMPARE_RX.lock().is_none()); + assert!(COMPARE_TX.lock().is_none()); + let _ = COMPARE_RX.lock().insert(compare_rx); + let _ = COMPARE_TX.lock().insert(compare_tx); + + assert!(INTERRUPT_RX.lock().is_none()); + assert!(INTERRUPT_TX.lock().is_none()); + let _ = INTERRUPT_RX.lock().insert(irq_rx); + let _ = INTERRUPT_TX.lock().insert(irq_tx); + + Self::queue().initialize(Self); + + let (killer_tx, mut killer_rx) = watch::channel(()); + + let mut killer_clone = killer_rx.clone(); + // Set up a task that watches for changes to the COMPARE value, + // and re-starts a timeout based on that change + tokio::spawn(async move { + let mut compare_rx = channel_read("Compare RX not initialized", &COMPARE_RX); + + loop { + let compare = compare_rx.borrow().clone(); + + let end = channel_read("Start not initialized", &START) + compare.0; + + tokio::select! { + _ = killer_clone.changed() => break, + _ = compare_rx.changed() => {}, + _ = tokio::time::sleep_until(end.into()) => { + event_write("Interrupt TX not initialized", &INTERRUPT_TX, ()); + // Sleep for a bit to avoid re-firing the interrupt a bunch of + // times. + tokio::time::sleep(Duration::from_millis(1)).await; + }, + } + } + }); + + // Set up a task that emulates an interrupt handler, calling `on_monotonic_interrupt` + // whenever an "interrupt" is generated. + tokio::spawn(async move { + let mut interrupt_rx = channel_read("Interrupt RX not initialized.", &INTERRUPT_RX); + + loop { + tokio::select! { + _ = killer_rx.changed() => break, + _ = interrupt_rx.changed() => { + // TODO: verify that we get interrupts triggered by an + // explicit pend or due to COMPARE at the correct time. + } + } + + unsafe { + StdTokioMono::queue().on_monotonic_interrupt(); + } + } + }); + + killer_tx + } + + /// Used to access the underlying timer queue + pub fn queue() -> &'static TimerQueue { + &TIMER_QUEUE + } +} + +impl Monotonic for StdTokioMono { + const ZERO: Self::Instant = Instant(Duration::ZERO); + + type Instant = Instant; + + type Duration = Duration; + + fn now() -> Self::Instant { + Instant::now() + } + + fn set_compare(instant: Self::Instant) { + // TODO: verify that we receive the correct amount & values + // for `set_compare`. + + log::info!("Setting compare to {} ms", instant.0.as_millis()); + + event_write("Compare TX not initialized", &COMPARE_TX, instant); + } + + fn clear_compare_flag() {} + + fn pend_interrupt() { + event_write("Interrupt TX not initialized", &INTERRUPT_TX, ()); + } +} + +static TIMER_QUEUE: TimerQueue = TimerQueue::new(); + +#[tokio::test] +async fn main() { + pretty_env_logger::init(); + + let _interrupt_killer = StdTokioMono::init(); + + let start = std::time::Instant::now(); + + let build_delay_test = |threshold: u128, pre_delay: Option, delay: u64| { + let delay = Duration::from_millis(delay); + let pre_delay = pre_delay.map(Duration::from_millis); + + let total = if let Some(pre_delay) = pre_delay { + pre_delay + delay + } else { + delay + }; + let total_millis = total.as_millis(); + async move { + if let Some(pre_delay) = pre_delay { + tokio::time::sleep_until((start + pre_delay).into()).await; + } + + StdTokioMono::queue().delay(delay).await; + + let elapsed = start.elapsed().as_millis(); + log::info!("{total_millis} ms delay reached (after {elapsed} ms)"); + + if elapsed > total_millis.saturating_add(threshold) + || elapsed < total_millis.saturating_sub(threshold) + { + panic!("{total_millis} ms delay was not on time ({elapsed} ms passed instead)"); + } + } + }; + + // TODO: depending on the precision of the delays that can be used, this threshold + // may have to be altered a bit. + const TIME_THRESHOLD_MS: u128 = 5; + + let sec1 = build_delay_test(TIME_THRESHOLD_MS, Some(100), 100); + let sec2 = build_delay_test(TIME_THRESHOLD_MS, None, 300); + let sec3 = build_delay_test(TIME_THRESHOLD_MS, None, 400); + + tokio::join!(sec2, sec1, sec3); +}