Add rtic-timer (timerqueue + monotonic) and rtic-monotonics (systick-monotonic)

4 files changed, 527 insertions, 517 deletions

diff --git a/rtic-timer/src/lib.rs b/rtic-timer/src/lib.rs
index e7051d27..d7faa07f 100644
--- a/rtic-timer/src/lib.rs
+++ b/rtic-timer/src/lib.rs
@@ -1,138 +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;
 
-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 linked_list;
 
-mod sll;
-use sll::{IntrusiveSortedLinkedList, Min as IsslMin, Node as IntrusiveNode};
+use linked_list::{Link, LinkedList};
 
-pub struct Timer {
-    cnt: AtomicU32,
-    // queue: IntrusiveSortedLinkedList<'static, WakerNotReady<Mono>, IsslMin>,
+/// 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,
+        }
+    }
 }
 
-#[allow(non_snake_case)]
-#[no_mangle]
-fn SysTick() {
-    // ..
-    let cnt = unsafe {
-        static mut CNT: u32 = 0;
-        &mut CNT
-    };
+impl<Mono: Monotonic> PartialEq for WaitingWaker<Mono> {
+    fn eq(&self, other: &Self) -> bool {
+        self.release_at == other.release_at
+    }
+}
 
-    *cnt = cnt.wrapping_add(1);
+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)
+    }
 }
 
-/// 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,
+/// 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,
 }
 
-impl<const TIMER_HZ: u32> Systick<TIMER_HZ> {
-    /// Provide a new `Monotonic` based on SysTick.
+/// 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`
     ///
-    /// The `sysclk` parameter is the speed at which SysTick runs at. This value should come from
-    /// the clock generation function of the used HAL.
+    /// # Safety
     ///
-    /// 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;
+    /// 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();
 
-        assert!(reload <= 0x00ff_ffff);
-        assert!(reload > 0);
+        loop {
+            let mut release_at = None;
+            let head = self.queue.pop_if(|head| {
+                release_at = Some(head.release_at);
 
-        systick.disable_counter();
-        systick.set_clock_source(SystClkSource::Core);
-        systick.set_reload(reload);
+                Mono::now() >= head.release_at
+            });
 
-        Systick { systick, cnt: 0 }
-    }
-}
+            match (head, release_at) {
+                (Some(link), _) => {
+                    link.waker.wake();
+                }
+                (None, Some(instant)) => {
+                    Mono::enable_timer();
+                    Mono::set_compare(instant);
 
-impl<const TIMER_HZ: u32> Monotonic for Systick<TIMER_HZ> {
-    const DISABLE_INTERRUPT_ON_EMPTY_QUEUE: bool = false;
+                    if Mono::now() >= instant {
+                        // The time for the next instant passed while handling it,
+                        // continue dequeueing
+                        continue;
+                    }
 
-    type Instant = fugit::TimerInstantU64<TIMER_HZ>;
-    type Duration = fugit::TimerDurationU64<TIMER_HZ>;
+                    break;
+                }
+                (None, None) => {
+                    // Queue is empty
+                    Mono::disable_timer();
 
-    fn now(&mut self) -> Self::Instant {
-        if self.systick.has_wrapped() {
-            self.cnt = self.cnt.wrapping_add(1);
+                    break;
+                }
+            }
         }
-
-        Self::Instant::from_ticks(self.cnt)
     }
 
-    unsafe fn reset(&mut self) {
-        self.systick.clear_current();
-        self.systick.enable_counter();
-    }
+    /// 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);
 
-    #[inline(always)]
-    fn set_compare(&mut self, _val: Self::Instant) {
-        // No need to do something here, we get interrupts anyway.
+        pin_mut!(future);
+        pin_mut!(delay);
+
+        match select(future, delay).await {
+            Either::Left((r, _)) => Ok(r),
+            Either::Right(_) => Err(TimeoutError),
+        }
     }
 
-    #[inline(always)]
-    fn clear_compare_flag(&mut self) {
-        // NOOP with SysTick interrupt
+    /// 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
     }
 
-    #[inline(always)]
-    fn zero() -> Self::Instant {
-        Self::Instant::from_ticks(0)
+    /// 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;
     }
 
-    #[inline(always)]
-    fn on_interrupt(&mut self) {
-        if self.systick.has_wrapped() {
-            self.cnt = self.cnt.wrapping_add(1);
+    /// 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 WakerNotReady<Mono>
-where
-    Mono: Monotonic,
-{
-    pub waker: Waker,
-    pub instant: Mono::Instant,
-    pub marker: u32,
+struct OnDrop<F: FnOnce()> {
+    f: core::mem::MaybeUninit<F>,
 }
 
-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<F: FnOnce()> OnDrop<F> {
+    pub fn new(f: F) -> Self {
+        Self {
+            f: core::mem::MaybeUninit::new(f),
+        }
     }
-}
 
-impl<Mono> PartialEq for WakerNotReady<Mono>
-where
-    Mono: Monotonic,
-{
-    fn eq(&self, other: &Self) -> bool {
-        self.instant == other.instant
+    #[allow(unused)]
+    pub fn defuse(self) {
+        core::mem::forget(self)
     }
 }
 
-impl<Mono> PartialOrd for WakerNotReady<Mono>
-where
-    Mono: Monotonic,
-{
-    fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
-        Some(self.cmp(other))
+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-timer/src/linked_list.rs b/rtic-timer/src/linked_list.rs
new file mode 100644
index 00000000..42ff8cb6
--- /dev/null
+++ b/rtic-timer/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-timer/src/monotonic.rs b/rtic-timer/src/monotonic.rs
new file mode 100644
index 00000000..9b3742fa
--- /dev/null
+++ b/rtic-timer/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() {}
+}
diff --git a/rtic-timer/src/sll.rs b/rtic-timer/src/sll.rs
deleted file mode 100644
index 43b53c17..00000000
--- a/rtic-timer/src/sll.rs
+++ /dev/null
@@ -1,421 +0,0 @@
-//! 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);
-    }
-}