monotonic experiments

4 files changed, 571 insertions, 1 deletions

diff --git a/Cargo.toml b/Cargo.toml
index 6eb691df..c22d0237 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 00000000..8e2e2ad6
--- /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 00000000..e7051d27
--- /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<Mono>, 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<const TIMER_HZ: u32> {
+    systick: SYST,
+    cnt: u64,
+}
+
+impl<const TIMER_HZ: u32> Systick<TIMER_HZ> {
+    /// 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<const TIMER_HZ: u32> Monotonic for Systick<TIMER_HZ> {
+    const DISABLE_INTERRUPT_ON_EMPTY_QUEUE: bool = false;
+
+    type Instant = fugit::TimerInstantU64<TIMER_HZ>;
+    type Duration = fugit::TimerDurationU64<TIMER_HZ>;
+
+    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<Mono>
+where
+    Mono: Monotonic,
+{
+    pub waker: Waker,
+    pub instant: Mono::Instant,
+    pub marker: u32,
+}
+
+impl<Mono> Eq for WakerNotReady<Mono> where Mono: Monotonic {}
+
+impl<Mono> Ord for WakerNotReady<Mono>
+where
+    Mono: Monotonic,
+{
+    fn cmp(&self, other: &Self) -> Ordering {
+        self.instant.cmp(&other.instant)
+    }
+}
+
+impl<Mono> PartialEq for WakerNotReady<Mono>
+where
+    Mono: Monotonic,
+{
+    fn eq(&self, other: &Self) -> bool {
+        self.instant == other.instant
+    }
+}
+
+impl<Mono> PartialOrd for WakerNotReady<Mono>
+where
+    Mono: Monotonic,
+{
+    fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
+        Some(self.cmp(other))
+    }
+}
diff --git a/rtic-timer/src/sll.rs b/rtic-timer/src/sll.rs
new file mode 100644
index 00000000..43b53c17
--- /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<T> {
+    pub val: T,
+    next: Option<NonNull<Node<T>>>,
+}
+
+impl<T> Node<T> {
+    pub fn new(val: T) -> Self {
+        Self { val, next: None }
+    }
+}
+
+/// The linked list.
+pub struct IntrusiveSortedLinkedList<'a, T, K> {
+    head: Option<NonNull<Node<T>>>,
+    _kind: PhantomData<K>,
+    _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<T>) {
+        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<F>(&mut self, mut f: F) -> Option<FindMut<'_, 'a, T, K>>
+    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<T>> {
+        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<NonNull<Node<T>>>,
+}
+
+impl<'a, T, K> Iterator for Iter<'a, T, K>
+where
+    T: Ord,
+    K: Kind,
+{
+    type Item = &'a T;
+
+    fn next(&mut self) -> Option<Self::Item> {
+        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<NonNull<Node<T>>>,
+    index: Option<NonNull<Node<T>>>,
+    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<T> {
+        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<T> {
+        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<T, K> 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<T, K> 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<u32, Max> = IntrusiveSortedLinkedList::new();
+    }
+
+    #[test]
+    fn test_peek() {
+        let mut ll: IntrusiveSortedLinkedList<u32, Max> = 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<u32, Min> = 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<u32, Max> = IntrusiveSortedLinkedList::new();
+
+        assert!(ll.is_empty())
+    }
+
+    #[test]
+    fn test_updating() {
+        let mut ll: IntrusiveSortedLinkedList<u32, Max> = 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<u32, Max> = 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<u32, Max> = 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);
+    }
+}