diff options
Diffstat (limited to 'rtic-time/src')
| -rw-r--r-- | rtic-time/src/lib.rs | 336 | ||||
| -rw-r--r-- | rtic-time/src/linked_list.rs | 173 | ||||
| -rw-r--r-- | rtic-time/src/monotonic.rs | 60 |
3 files changed, 569 insertions, 0 deletions
diff --git a/rtic-time/src/lib.rs b/rtic-time/src/lib.rs new file mode 100644 index 00000000..d7faa07f --- /dev/null +++ b/rtic-time/src/lib.rs @@ -0,0 +1,336 @@ +//! Crate + +#![no_std] +#![no_main] +#![deny(missing_docs)] +#![allow(incomplete_features)] +#![feature(async_fn_in_trait)] + +pub mod monotonic; + +use core::future::{poll_fn, Future}; +use core::sync::atomic::{AtomicBool, AtomicUsize, Ordering}; +use core::task::{Poll, Waker}; +use futures_util::{ + future::{select, Either}, + pin_mut, +}; +pub use monotonic::Monotonic; + +mod linked_list; + +use linked_list::{Link, LinkedList}; + +/// Holds a waker and at which time instant this waker shall be awoken. +struct WaitingWaker<Mono: Monotonic> { + waker: Waker, + release_at: Mono::Instant, +} + +impl<Mono: Monotonic> Clone for WaitingWaker<Mono> { + fn clone(&self) -> Self { + Self { + waker: self.waker.clone(), + release_at: self.release_at, + } + } +} + +impl<Mono: Monotonic> PartialEq for WaitingWaker<Mono> { + fn eq(&self, other: &Self) -> bool { + self.release_at == other.release_at + } +} + +impl<Mono: Monotonic> PartialOrd for WaitingWaker<Mono> { + fn partial_cmp(&self, other: &Self) -> Option<core::cmp::Ordering> { + self.release_at.partial_cmp(&other.release_at) + } +} + +/// A generic timer queue for async executors. +/// +/// # Blocking +/// +/// The internal priority queue uses global critical sections to manage access. This means that +/// `await`ing a delay will cause a lock of the entire system for O(n) time. In practice the lock +/// duration is ~10 clock cycles per element in the queue. +/// +/// # Safety +/// +/// This timer queue is based on an intrusive linked list, and by extension the links are strored +/// on the async stacks of callers. The links are deallocated on `drop` or when the wait is +/// complete. +/// +/// Do not call `mem::forget` on an awaited future, or there will be dragons! +pub struct TimerQueue<Mono: Monotonic> { + queue: LinkedList<WaitingWaker<Mono>>, + initialized: AtomicBool, +} + +/// This indicates that there was a timeout. +pub struct TimeoutError; + +impl<Mono: Monotonic> TimerQueue<Mono> { + /// Make a new queue. + pub const fn new() -> Self { + Self { + queue: LinkedList::new(), + initialized: AtomicBool::new(false), + } + } + + /// Forwards the `Monotonic::now()` method. + #[inline(always)] + pub fn now(&self) -> Mono::Instant { + Mono::now() + } + + /// Takes the initialized monotonic to initialize the TimerQueue. + pub fn initialize(&self, monotonic: Mono) { + self.initialized.store(true, Ordering::SeqCst); + + // Don't run drop on `Mono` + core::mem::forget(monotonic); + } + + /// Call this in the interrupt handler of the hardware timer supporting the `Monotonic` + /// + /// # Safety + /// + /// It's always safe to call, but it must only be called from the interrupt of the + /// monotonic timer for correct operation. + pub unsafe fn on_monotonic_interrupt(&self) { + Mono::clear_compare_flag(); + Mono::on_interrupt(); + + loop { + let mut release_at = None; + let head = self.queue.pop_if(|head| { + release_at = Some(head.release_at); + + Mono::now() >= head.release_at + }); + + match (head, release_at) { + (Some(link), _) => { + link.waker.wake(); + } + (None, Some(instant)) => { + Mono::enable_timer(); + Mono::set_compare(instant); + + if Mono::now() >= instant { + // The time for the next instant passed while handling it, + // continue dequeueing + continue; + } + + break; + } + (None, None) => { + // Queue is empty + Mono::disable_timer(); + + break; + } + } + } + } + + /// Timeout at a specific time. + pub async fn timeout_at<F: Future>( + &self, + instant: Mono::Instant, + future: F, + ) -> Result<F::Output, TimeoutError> { + let delay = self.delay_until(instant); + + pin_mut!(future); + pin_mut!(delay); + + match select(future, delay).await { + Either::Left((r, _)) => Ok(r), + Either::Right(_) => Err(TimeoutError), + } + } + + /// Timeout after a specific duration. + #[inline] + pub async fn timeout_after<F: Future>( + &self, + duration: Mono::Duration, + future: F, + ) -> Result<F::Output, TimeoutError> { + self.timeout_at(Mono::now() + duration, future).await + } + + /// Delay for some duration of time. + #[inline] + pub async fn delay(&self, duration: Mono::Duration) { + let now = Mono::now(); + + self.delay_until(now + duration).await; + } + + /// Delay to some specific time instant. + pub async fn delay_until(&self, instant: Mono::Instant) { + if !self.initialized.load(Ordering::Relaxed) { + panic!( + "The timer queue is not initialized with a monotonic, you need to run `initialize`" + ); + } + + let mut first_run = true; + let queue = &self.queue; + let mut link = Link::new(WaitingWaker { + waker: poll_fn(|cx| Poll::Ready(cx.waker().clone())).await, + release_at: instant, + }); + + let marker = &AtomicUsize::new(0); + + let dropper = OnDrop::new(|| { + queue.delete(marker.load(Ordering::Relaxed)); + }); + + poll_fn(|_| { + if Mono::now() >= instant { + return Poll::Ready(()); + } + + if first_run { + first_run = false; + let (was_empty, addr) = queue.insert(&mut link); + marker.store(addr, Ordering::Relaxed); + + if was_empty { + // Pend the monotonic handler if the queue was empty to setup the timer. + Mono::pend_interrupt(); + } + } + + Poll::Pending + }) + .await; + + // Make sure that our link is deleted from the list before we drop this stack + drop(dropper); + } +} + +struct OnDrop<F: FnOnce()> { + f: core::mem::MaybeUninit<F>, +} + +impl<F: FnOnce()> OnDrop<F> { + pub fn new(f: F) -> Self { + Self { + f: core::mem::MaybeUninit::new(f), + } + } + + #[allow(unused)] + pub fn defuse(self) { + core::mem::forget(self) + } +} + +impl<F: FnOnce()> Drop for OnDrop<F> { + fn drop(&mut self) { + unsafe { self.f.as_ptr().read()() } + } +} + +// -------- Test program --------- +// +// +// use systick_monotonic::{Systick, TimerQueue}; +// +// // same panicking *behavior* as `panic-probe` but doesn't print a panic message +// // this prevents the panic message being printed *twice* when `defmt::panic` is invoked +// #[defmt::panic_handler] +// fn panic() -> ! { +// cortex_m::asm::udf() +// } +// +// /// Terminates the application and makes `probe-run` exit with exit-code = 0 +// pub fn exit() -> ! { +// loop { +// cortex_m::asm::bkpt(); +// } +// } +// +// defmt::timestamp!("{=u64:us}", { +// let time_us: fugit::MicrosDurationU32 = MONO.now().duration_since_epoch().convert(); +// +// time_us.ticks() as u64 +// }); +// +// make_systick_timer_queue!(MONO, Systick<1_000>); +// +// #[rtic::app( +// device = nrf52832_hal::pac, +// dispatchers = [SWI0_EGU0, SWI1_EGU1, SWI2_EGU2, SWI3_EGU3, SWI4_EGU4, SWI5_EGU5], +// )] +// mod app { +// use super::{Systick, MONO}; +// use fugit::ExtU32; +// +// #[shared] +// struct Shared {} +// +// #[local] +// struct Local {} +// +// #[init] +// fn init(cx: init::Context) -> (Shared, Local) { +// defmt::println!("init"); +// +// let systick = Systick::start(cx.core.SYST, 64_000_000); +// +// defmt::println!("initializing monotonic"); +// +// MONO.initialize(systick); +// +// async_task::spawn().ok(); +// async_task2::spawn().ok(); +// async_task3::spawn().ok(); +// +// (Shared {}, Local {}) +// } +// +// #[idle] +// fn idle(_: idle::Context) -> ! { +// defmt::println!("idle"); +// +// loop { +// core::hint::spin_loop(); +// } +// } +// +// #[task] +// async fn async_task(_: async_task::Context) { +// loop { +// defmt::println!("async task waiting for 1 second"); +// MONO.delay(1.secs()).await; +// } +// } +// +// #[task] +// async fn async_task2(_: async_task2::Context) { +// loop { +// defmt::println!(" async task 2 waiting for 0.5 second"); +// MONO.delay(500.millis()).await; +// } +// } +// +// #[task] +// async fn async_task3(_: async_task3::Context) { +// loop { +// defmt::println!(" async task 3 waiting for 0.2 second"); +// MONO.delay(200.millis()).await; +// } +// } +// } +// diff --git a/rtic-time/src/linked_list.rs b/rtic-time/src/linked_list.rs new file mode 100644 index 00000000..42ff8cb6 --- /dev/null +++ b/rtic-time/src/linked_list.rs @@ -0,0 +1,173 @@ +//! ... + +use core::marker::PhantomPinned; +use core::sync::atomic::{AtomicPtr, Ordering}; +use critical_section as cs; + +/// A sorted linked list for the timer queue. +pub struct LinkedList<T> { + head: AtomicPtr<Link<T>>, +} + +impl<T> LinkedList<T> { + /// Create a new linked list. + pub const fn new() -> Self { + Self { + head: AtomicPtr::new(core::ptr::null_mut()), + } + } +} + +impl<T: PartialOrd + Clone> LinkedList<T> { + /// Pop the first element in the queue if the closure returns true. + pub fn pop_if<F: FnOnce(&T) -> bool>(&self, f: F) -> Option<T> { + cs::with(|_| { + // Make sure all previous writes are visible + core::sync::atomic::fence(Ordering::SeqCst); + + let head = self.head.load(Ordering::Relaxed); + + // SAFETY: `as_ref` is safe as `insert` requires a valid reference to a link + if let Some(head) = unsafe { head.as_ref() } { + if f(&head.val) { + // Move head to the next element + self.head + .store(head.next.load(Ordering::Relaxed), Ordering::Relaxed); + + // We read the value at head + let head_val = head.val.clone(); + + return Some(head_val); + } + } + None + }) + } + + /// Delete a link at an address. + pub fn delete(&self, addr: usize) { + cs::with(|_| { + // Make sure all previous writes are visible + core::sync::atomic::fence(Ordering::SeqCst); + + let head = self.head.load(Ordering::Relaxed); + + // SAFETY: `as_ref` is safe as `insert` requires a valid reference to a link + let head_ref = if let Some(head_ref) = unsafe { head.as_ref() } { + head_ref + } else { + // 1. List is empty, do nothing + return; + }; + + if head as *const _ as usize == addr { + // 2. Replace head with head.next + self.head + .store(head_ref.next.load(Ordering::Relaxed), Ordering::Relaxed); + + return; + } + + // 3. search list for correct node + let mut curr = head_ref; + let mut next = head_ref.next.load(Ordering::Relaxed); + + // SAFETY: `as_ref` is safe as `insert` requires a valid reference to a link + while let Some(next_link) = unsafe { next.as_ref() } { + // Next is not null + + if next as *const _ as usize == addr { + curr.next + .store(next_link.next.load(Ordering::Relaxed), Ordering::Relaxed); + + return; + } + + // Continue searching + curr = next_link; + next = next_link.next.load(Ordering::Relaxed); + } + }) + } + + /// Insert a new link into the linked list. + /// The return is (was_empty, address), where the address of the link is for use with `delete`. + pub fn insert(&self, val: &mut Link<T>) -> (bool, usize) { + cs::with(|_| { + let addr = val as *const _ as usize; + + // Make sure all previous writes are visible + core::sync::atomic::fence(Ordering::SeqCst); + + let head = self.head.load(Ordering::Relaxed); + + // 3 cases to handle + + // 1. List is empty, write to head + // SAFETY: `as_ref` is safe as `insert` requires a valid reference to a link + let head_ref = if let Some(head_ref) = unsafe { head.as_ref() } { + head_ref + } else { + self.head.store(val, Ordering::Relaxed); + return (true, addr); + }; + + // 2. val needs to go in first + if val.val < head_ref.val { + // Set current head as next of `val` + val.next.store(head, Ordering::Relaxed); + + // `val` is now first in the queue + self.head.store(val, Ordering::Relaxed); + + return (false, addr); + } + + // 3. search list for correct place + let mut curr = head_ref; + let mut next = head_ref.next.load(Ordering::Relaxed); + + // SAFETY: `as_ref` is safe as `insert` requires a valid reference to a link + while let Some(next_link) = unsafe { next.as_ref() } { + // Next is not null + + if val.val < next_link.val { + // Replace next with `val` + val.next.store(next, Ordering::Relaxed); + + // Insert `val` + curr.next.store(val, Ordering::Relaxed); + + return (false, addr); + } + + // Continue searching + curr = next_link; + next = next_link.next.load(Ordering::Relaxed); + } + + // No next, write link to last position in list + curr.next.store(val, Ordering::Relaxed); + + (false, addr) + }) + } +} + +/// A link in the linked list. +pub struct Link<T> { + val: T, + next: AtomicPtr<Link<T>>, + _up: PhantomPinned, +} + +impl<T> Link<T> { + /// Create a new link. + pub const fn new(val: T) -> Self { + Self { + val, + next: AtomicPtr::new(core::ptr::null_mut()), + _up: PhantomPinned, + } + } +} diff --git a/rtic-time/src/monotonic.rs b/rtic-time/src/monotonic.rs new file mode 100644 index 00000000..9b3742fa --- /dev/null +++ b/rtic-time/src/monotonic.rs @@ -0,0 +1,60 @@ +//! ... + +/// # A monotonic clock / counter definition. +/// +/// ## Correctness +/// +/// The trait enforces that proper time-math is implemented between `Instant` and `Duration`. This +/// is a requirement on the time library that the user chooses to use. +pub trait Monotonic { + /// The time at time zero. + const ZERO: Self::Instant; + + /// The type for instant, defining an instant in time. + /// + /// **Note:** In all APIs in RTIC that use instants from this monotonic, this type will be used. + type Instant: Ord + + Copy + + core::ops::Add<Self::Duration, Output = Self::Instant> + + core::ops::Sub<Self::Duration, Output = Self::Instant> + + core::ops::Sub<Self::Instant, Output = Self::Duration>; + + /// The type for duration, defining an duration of time. + /// + /// **Note:** In all APIs in RTIC that use duration from this monotonic, this type will be used. + type Duration; + + /// Get the current time. + fn now() -> Self::Instant; + + /// Set the compare value of the timer interrupt. + /// + /// **Note:** This method does not need to handle race conditions of the monotonic, the timer + /// queue in RTIC checks this. + fn set_compare(instant: Self::Instant); + + /// Clear the compare interrupt flag. + fn clear_compare_flag(); + + /// Pend the timer's interrupt. + fn pend_interrupt(); + + /// Optional. Runs on interrupt before any timer queue handling. + fn on_interrupt() {} + + /// Optional. This is used to save power, this is called when the timer queue is not empty. + /// + /// Enabling and disabling the monotonic needs to propagate to `now` so that an instant + /// based of `now()` is still valid. + /// + /// NOTE: This may be called more than once. + fn enable_timer() {} + + /// Optional. This is used to save power, this is called when the timer queue is empty. + /// + /// Enabling and disabling the monotonic needs to propagate to `now` so that an instant + /// based of `now()` is still valid. + /// + /// NOTE: This may be called more than once. + fn disable_timer() {} +} |