From 4601782466c518d313ba79d9437bf7a3f8dbbf76 Mon Sep 17 00:00:00 2001 From: Emil Fresk Date: Sat, 14 Jan 2023 21:11:55 +0100 Subject: [PATCH] monotonic experiments --- Cargo.toml | 2 +- rtic-timer/Cargo.toml | 11 ++ rtic-timer/src/lib.rs | 138 ++++++++++++++ rtic-timer/src/sll.rs | 421 ++++++++++++++++++++++++++++++++++++++++++ 4 files changed, 571 insertions(+), 1 deletion(-) create mode 100644 rtic-timer/Cargo.toml create mode 100644 rtic-timer/src/lib.rs create mode 100644 rtic-timer/src/sll.rs diff --git a/Cargo.toml b/Cargo.toml index 6eb691df6e..c22d02372f 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -51,7 +51,7 @@ codegen-units = 1 lto = true [workspace] -members = ["macros", "xtask"] +members = ["macros", "xtask", "rtic-timer"] # do not optimize proc-macro deps or build scripts [profile.dev.build-override] diff --git a/rtic-timer/Cargo.toml b/rtic-timer/Cargo.toml new file mode 100644 index 0000000000..8e2e2ad602 --- /dev/null +++ b/rtic-timer/Cargo.toml @@ -0,0 +1,11 @@ +[package] +name = "rtic-timer" +version = "0.1.0" +edition = "2021" + +# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html + +[dependencies] +cortex-m = "0.7.6" +rtic-monotonic = "1.0.0" +fugit = "0.3.6" \ No newline at end of file diff --git a/rtic-timer/src/lib.rs b/rtic-timer/src/lib.rs new file mode 100644 index 0000000000..e7051d2779 --- /dev/null +++ b/rtic-timer/src/lib.rs @@ -0,0 +1,138 @@ +#![no_std] + +use core::sync::atomic::{AtomicU32, Ordering}; +use core::{cmp::Ordering, task::Waker}; +use cortex_m::peripheral::{syst::SystClkSource, SYST}; +pub use fugit::{self, ExtU64}; +pub use rtic_monotonic::Monotonic; + +mod sll; +use sll::{IntrusiveSortedLinkedList, Min as IsslMin, Node as IntrusiveNode}; + +pub struct Timer { + cnt: AtomicU32, + // queue: IntrusiveSortedLinkedList<'static, WakerNotReady, IsslMin>, +} + +#[allow(non_snake_case)] +#[no_mangle] +fn SysTick() { + // .. + let cnt = unsafe { + static mut CNT: u32 = 0; + &mut CNT + }; + + *cnt = cnt.wrapping_add(1); +} + +/// Systick implementing `rtic_monotonic::Monotonic` which runs at a +/// settable rate using the `TIMER_HZ` parameter. +pub struct Systick { + systick: SYST, + cnt: u64, +} + +impl Systick { + /// Provide a new `Monotonic` based on SysTick. + /// + /// The `sysclk` parameter is the speed at which SysTick runs at. This value should come from + /// the clock generation function of the used HAL. + /// + /// Notice that the actual rate of the timer is a best approximation based on the given + /// `sysclk` and `TIMER_HZ`. + pub fn new(mut systick: SYST, sysclk: u32) -> Self { + // + TIMER_HZ / 2 provides round to nearest instead of round to 0. + // - 1 as the counter range is inclusive [0, reload] + let reload = (sysclk + TIMER_HZ / 2) / TIMER_HZ - 1; + + assert!(reload <= 0x00ff_ffff); + assert!(reload > 0); + + systick.disable_counter(); + systick.set_clock_source(SystClkSource::Core); + systick.set_reload(reload); + + Systick { systick, cnt: 0 } + } +} + +impl Monotonic for Systick { + const DISABLE_INTERRUPT_ON_EMPTY_QUEUE: bool = false; + + type Instant = fugit::TimerInstantU64; + type Duration = fugit::TimerDurationU64; + + fn now(&mut self) -> Self::Instant { + if self.systick.has_wrapped() { + self.cnt = self.cnt.wrapping_add(1); + } + + Self::Instant::from_ticks(self.cnt) + } + + unsafe fn reset(&mut self) { + self.systick.clear_current(); + self.systick.enable_counter(); + } + + #[inline(always)] + fn set_compare(&mut self, _val: Self::Instant) { + // No need to do something here, we get interrupts anyway. + } + + #[inline(always)] + fn clear_compare_flag(&mut self) { + // NOOP with SysTick interrupt + } + + #[inline(always)] + fn zero() -> Self::Instant { + Self::Instant::from_ticks(0) + } + + #[inline(always)] + fn on_interrupt(&mut self) { + if self.systick.has_wrapped() { + self.cnt = self.cnt.wrapping_add(1); + } + } +} + +struct WakerNotReady +where + Mono: Monotonic, +{ + pub waker: Waker, + pub instant: Mono::Instant, + pub marker: u32, +} + +impl Eq for WakerNotReady where Mono: Monotonic {} + +impl Ord for WakerNotReady +where + Mono: Monotonic, +{ + fn cmp(&self, other: &Self) -> Ordering { + self.instant.cmp(&other.instant) + } +} + +impl PartialEq for WakerNotReady +where + Mono: Monotonic, +{ + fn eq(&self, other: &Self) -> bool { + self.instant == other.instant + } +} + +impl PartialOrd for WakerNotReady +where + Mono: Monotonic, +{ + fn partial_cmp(&self, other: &Self) -> Option { + Some(self.cmp(other)) + } +} diff --git a/rtic-timer/src/sll.rs b/rtic-timer/src/sll.rs new file mode 100644 index 0000000000..43b53c1749 --- /dev/null +++ b/rtic-timer/src/sll.rs @@ -0,0 +1,421 @@ +//! An intrusive sorted priority linked list, designed for use in `Future`s in RTIC. +use core::cmp::Ordering; +use core::fmt; +use core::marker::PhantomData; +use core::ops::{Deref, DerefMut}; +use core::ptr::NonNull; + +/// Marker for Min sorted [`IntrusiveSortedLinkedList`]. +pub struct Min; + +/// Marker for Max sorted [`IntrusiveSortedLinkedList`]. +pub struct Max; + +/// The linked list kind: min-list or max-list +pub trait Kind: private::Sealed { + #[doc(hidden)] + fn ordering() -> Ordering; +} + +impl Kind for Min { + fn ordering() -> Ordering { + Ordering::Less + } +} + +impl Kind for Max { + fn ordering() -> Ordering { + Ordering::Greater + } +} + +/// Sealed traits +mod private { + pub trait Sealed {} +} + +impl private::Sealed for Max {} +impl private::Sealed for Min {} + +/// A node in the [`IntrusiveSortedLinkedList`]. +pub struct Node { + pub val: T, + next: Option>>, +} + +impl Node { + pub fn new(val: T) -> Self { + Self { val, next: None } + } +} + +/// The linked list. +pub struct IntrusiveSortedLinkedList<'a, T, K> { + head: Option>>, + _kind: PhantomData, + _lt: PhantomData<&'a ()>, +} + +impl<'a, T, K> fmt::Debug for IntrusiveSortedLinkedList<'a, T, K> +where + T: Ord + core::fmt::Debug, + K: Kind, +{ + fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { + let mut l = f.debug_list(); + let mut current = self.head; + + while let Some(head) = current { + let head = unsafe { head.as_ref() }; + current = head.next; + + l.entry(&head.val); + } + + l.finish() + } +} + +impl<'a, T, K> IntrusiveSortedLinkedList<'a, T, K> +where + T: Ord, + K: Kind, +{ + pub const fn new() -> Self { + Self { + head: None, + _kind: PhantomData, + _lt: PhantomData, + } + } + + // Push to the list. + pub fn push(&mut self, new: &'a mut Node) { + unsafe { + if let Some(head) = self.head { + if head.as_ref().val.cmp(&new.val) != K::ordering() { + // This is newer than head, replace head + new.next = self.head; + self.head = Some(NonNull::new_unchecked(new)); + } else { + // It's not head, search the list for the correct placement + let mut current = head; + + while let Some(next) = current.as_ref().next { + if next.as_ref().val.cmp(&new.val) != K::ordering() { + break; + } + + current = next; + } + + new.next = current.as_ref().next; + current.as_mut().next = Some(NonNull::new_unchecked(new)); + } + } else { + // List is empty, place at head + self.head = Some(NonNull::new_unchecked(new)) + } + } + } + + /// Get an iterator over the sorted list. + pub fn iter(&self) -> Iter<'_, T, K> { + Iter { + _list: self, + index: self.head, + } + } + + /// Find an element in the list that can be changed and resorted. + pub fn find_mut(&mut self, mut f: F) -> Option> + where + F: FnMut(&T) -> bool, + { + let head = self.head?; + + // Special-case, first element + if f(&unsafe { head.as_ref() }.val) { + return Some(FindMut { + is_head: true, + prev_index: None, + index: self.head, + list: self, + maybe_changed: false, + }); + } + + let mut current = head; + + while let Some(next) = unsafe { current.as_ref() }.next { + if f(&unsafe { next.as_ref() }.val) { + return Some(FindMut { + is_head: false, + prev_index: Some(current), + index: Some(next), + list: self, + maybe_changed: false, + }); + } + + current = next; + } + + None + } + + /// Peek at the first element. + pub fn peek(&self) -> Option<&T> { + self.head.map(|head| unsafe { &head.as_ref().val }) + } + + /// Pops the first element in the list. + /// + /// Complexity is worst-case `O(1)`. + pub fn pop(&mut self) -> Option<&'a Node> { + if let Some(head) = self.head { + let v = unsafe { head.as_ref() }; + self.head = v.next; + Some(v) + } else { + None + } + } + + /// Checks if the linked list is empty. + #[inline] + pub fn is_empty(&self) -> bool { + self.head.is_none() + } +} + +/// Iterator for the linked list. +pub struct Iter<'a, T, K> +where + T: Ord, + K: Kind, +{ + _list: &'a IntrusiveSortedLinkedList<'a, T, K>, + index: Option>>, +} + +impl<'a, T, K> Iterator for Iter<'a, T, K> +where + T: Ord, + K: Kind, +{ + type Item = &'a T; + + fn next(&mut self) -> Option { + let index = self.index?; + + let node = unsafe { index.as_ref() }; + self.index = node.next; + + Some(&node.val) + } +} + +/// Comes from [`IntrusiveSortedLinkedList::find_mut`]. +pub struct FindMut<'a, 'b, T, K> +where + T: Ord + 'b, + K: Kind, +{ + list: &'a mut IntrusiveSortedLinkedList<'b, T, K>, + is_head: bool, + prev_index: Option>>, + index: Option>>, + maybe_changed: bool, +} + +impl<'a, 'b, T, K> FindMut<'a, 'b, T, K> +where + T: Ord, + K: Kind, +{ + unsafe fn pop_internal(&mut self) -> &'b mut Node { + if self.is_head { + // If it is the head element, we can do a normal pop + let mut head = self.list.head.unwrap_unchecked(); + let v = head.as_mut(); + self.list.head = v.next; + v + } else { + // Somewhere in the list + let mut prev = self.prev_index.unwrap_unchecked(); + let mut curr = self.index.unwrap_unchecked(); + + // Re-point the previous index + prev.as_mut().next = curr.as_ref().next; + + curr.as_mut() + } + } + + /// This will pop the element from the list. + /// + /// Complexity is worst-case `O(1)`. + #[inline] + pub fn pop(mut self) -> &'b mut Node { + unsafe { self.pop_internal() } + } + + /// This will resort the element into the correct position in the list if needed. The resorting + /// will only happen if the element has been accessed mutably. + /// + /// Same as calling `drop`. + /// + /// Complexity is worst-case `O(N)`. + #[inline] + pub fn finish(self) { + drop(self) + } +} + +impl<'b, T, K> Drop for FindMut<'_, 'b, T, K> +where + T: Ord + 'b, + K: Kind, +{ + fn drop(&mut self) { + // Only resort the list if the element has changed + if self.maybe_changed { + unsafe { + let val = self.pop_internal(); + self.list.push(val); + } + } + } +} + +impl Deref for FindMut<'_, '_, T, K> +where + T: Ord, + K: Kind, +{ + type Target = T; + + fn deref(&self) -> &Self::Target { + unsafe { &self.index.unwrap_unchecked().as_ref().val } + } +} + +impl DerefMut for FindMut<'_, '_, T, K> +where + T: Ord, + K: Kind, +{ + fn deref_mut(&mut self) -> &mut Self::Target { + self.maybe_changed = true; + unsafe { &mut self.index.unwrap_unchecked().as_mut().val } + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn const_new() { + static mut _V1: IntrusiveSortedLinkedList = IntrusiveSortedLinkedList::new(); + } + + #[test] + fn test_peek() { + let mut ll: IntrusiveSortedLinkedList = IntrusiveSortedLinkedList::new(); + + let mut a = Node { val: 1, next: None }; + ll.push(&mut a); + assert_eq!(ll.peek().unwrap(), &1); + + let mut a = Node { val: 2, next: None }; + ll.push(&mut a); + assert_eq!(ll.peek().unwrap(), &2); + + let mut a = Node { val: 3, next: None }; + ll.push(&mut a); + assert_eq!(ll.peek().unwrap(), &3); + + let mut ll: IntrusiveSortedLinkedList = IntrusiveSortedLinkedList::new(); + + let mut a = Node { val: 2, next: None }; + ll.push(&mut a); + assert_eq!(ll.peek().unwrap(), &2); + + let mut a = Node { val: 1, next: None }; + ll.push(&mut a); + assert_eq!(ll.peek().unwrap(), &1); + + let mut a = Node { val: 3, next: None }; + ll.push(&mut a); + assert_eq!(ll.peek().unwrap(), &1); + } + + #[test] + fn test_empty() { + let ll: IntrusiveSortedLinkedList = IntrusiveSortedLinkedList::new(); + + assert!(ll.is_empty()) + } + + #[test] + fn test_updating() { + let mut ll: IntrusiveSortedLinkedList = IntrusiveSortedLinkedList::new(); + + let mut a = Node { val: 1, next: None }; + ll.push(&mut a); + + let mut a = Node { val: 2, next: None }; + ll.push(&mut a); + + let mut a = Node { val: 3, next: None }; + ll.push(&mut a); + + let mut find = ll.find_mut(|v| *v == 2).unwrap(); + + *find += 1000; + find.finish(); + + assert_eq!(ll.peek().unwrap(), &1002); + + let mut find = ll.find_mut(|v| *v == 3).unwrap(); + + *find += 1000; + find.finish(); + + assert_eq!(ll.peek().unwrap(), &1003); + + // Remove largest element + ll.find_mut(|v| *v == 1003).unwrap().pop(); + + assert_eq!(ll.peek().unwrap(), &1002); + } + + #[test] + fn test_updating_1() { + let mut ll: IntrusiveSortedLinkedList = IntrusiveSortedLinkedList::new(); + + let mut a = Node { val: 1, next: None }; + ll.push(&mut a); + + let v = ll.pop().unwrap(); + + assert_eq!(v.val, 1); + } + + #[test] + fn test_updating_2() { + let mut ll: IntrusiveSortedLinkedList = IntrusiveSortedLinkedList::new(); + + let mut a = Node { val: 1, next: None }; + ll.push(&mut a); + + let mut find = ll.find_mut(|v| *v == 1).unwrap(); + + *find += 1000; + find.finish(); + + assert_eq!(ll.peek().unwrap(), &1001); + } +}