Merge #436

436: New monotonic r=AfoHT a=korken89

Design document: https://hackmd.io/vWa9GvssR8qBfUYgMZm0CQ

Closes #433 
Closes #432
Closes #427
Closes #426 
Closes #403
Closes #332
Closes #312 
Closes #309 
Closes #299 
Closes #292
Closes #247
Closes #219

Co-authored-by: Emil Fresk <emil.fresk@gmail.com>

1 files changed, 89 insertions, 80 deletions

diff --git a/src/tq.rs b/src/tq.rs
index b2a84c8..063bbd8 100644
--- a/src/tq.rs
+++ b/src/tq.rs
@@ -1,28 +1,21 @@
-use core::{
-    cmp::{self, Ordering},
-    convert::TryInto,
-    mem,
-    ops::Sub,
+use crate::{
+    time::{Clock, Instant},
+    Monotonic,
 };
-
-use cortex_m::peripheral::{SCB, SYST};
+use core::cmp::Ordering;
 use heapless::{binary_heap::Min, ArrayLength, BinaryHeap};
 
-use crate::Monotonic;
-
-pub struct TimerQueue<M, T, N>(pub BinaryHeap<NotReady<M, T>, N, Min>)
+pub struct TimerQueue<Mono, Task, N>(pub BinaryHeap<NotReady<Mono, Task>, N, Min>)
 where
-    M: Monotonic,
-    <M::Instant as Sub>::Output: TryInto<u32>,
-    N: ArrayLength<NotReady<M, T>>,
-    T: Copy;
+    Mono: Monotonic,
+    N: ArrayLength<NotReady<Mono, Task>>,
+    Task: Copy;
 
-impl<M, T, N> TimerQueue<M, T, N>
+impl<Mono, Task, N> TimerQueue<Mono, Task, N>
 where
-    M: Monotonic,
-    <M::Instant as Sub>::Output: TryInto<u32>,
-    N: ArrayLength<NotReady<M, T>>,
-    T: Copy,
+    Mono: Monotonic,
+    N: ArrayLength<NotReady<Mono, Task>>,
+    Task: Copy,
 {
     /// # Safety
     ///
@@ -31,7 +24,16 @@ where
     ///
     /// Enqueue a task without checking if it is full
     #[inline]
-    pub unsafe fn enqueue_unchecked(&mut self, nr: NotReady<M, T>) {
+    pub unsafe fn enqueue_unchecked<F1, F2>(
+        &mut self,
+        nr: NotReady<Mono, Task>,
+        enable_interrupt: F1,
+        pend_handler: F2,
+        mono: &mut Mono,
+    ) where
+        F1: FnOnce(),
+        F2: FnOnce(),
+    {
         let mut is_empty = true;
         // Check if the top contains a non-empty element and if that element is
         // greater than nr
@@ -44,111 +46,118 @@ where
             })
             .unwrap_or(true);
         if if_heap_max_greater_than_nr {
-            if is_empty {
-                mem::transmute::<_, SYST>(()).enable_interrupt();
+            if Mono::DISABLE_INTERRUPT_ON_EMPTY_QUEUE && is_empty {
+                // mem::transmute::<_, SYST>(()).enable_interrupt();A
+                mono.enable_timer();
+                enable_interrupt();
             }
 
             // Set SysTick pending
-            SCB::set_pendst();
+            // SCB::set_pendst();
+            pend_handler();
         }
 
         self.0.push_unchecked(nr);
     }
 
+    /// Check if the timer queue is empty.
+    #[inline]
+    pub fn is_empty(&self) -> bool {
+        self.0.is_empty()
+    }
+
+    #[inline]
+    fn unwrapper<T, E>(val: Result<T, E>) -> T {
+        if let Ok(v) = val {
+            v
+        } else {
+            unreachable!("Your monotonic is not infallible")
+        }
+    }
+
     /// Dequeue a task from the TimerQueue
     #[inline]
-    pub fn dequeue(&mut self) -> Option<(T, u8)> {
-        unsafe {
-            if let Some(instant) = self.0.peek().map(|p| p.instant) {
-                let now = M::now();
+    pub fn dequeue<F>(&mut self, disable_interrupt: F, mono: &mut Mono) -> Option<(Task, u8)>
+    where
+        F: FnOnce(),
+    {
+        mono.clear_compare_flag();
+
+        if let Some(instant) = self.0.peek().map(|p| p.instant) {
+            if instant <= Self::unwrapper(Clock::try_now(mono)) {
+                // task became ready
+                let nr = unsafe { self.0.pop_unchecked() };
+
+                Some((nr.task, nr.index))
+            } else {
+                // Set compare
+                mono.set_compare(&instant);
 
-                if instant < now {
-                    // task became ready
-                    let nr = self.0.pop_unchecked();
+                // Double check that the instant we set is really in the future, else
+                // dequeue. If the monotonic is fast enough it can happen that from the
+                // read of now to the set of the compare, the time can overflow. This is to
+                // guard against this.
+                if instant <= Self::unwrapper(Clock::try_now(mono)) {
+                    let nr = unsafe { self.0.pop_unchecked() };
 
                     Some((nr.task, nr.index))
                 } else {
-                    // set a new timeout
-                    const MAX: u32 = 0x00ffffff;
-
-                    let ratio = M::ratio();
-                    let dur = match (instant - now).try_into().ok().and_then(|x| {
-                        x.checked_mul(ratio.numerator)
-                            .map(|x| x / ratio.denominator)
-                    }) {
-                        None => MAX,
-
-                        // ARM Architecture Reference Manual says:
-                        // "Setting SYST_RVR to zero has the effect of
-                        // disabling the SysTick counter independently
-                        // of the counter enable bit."
-                        Some(0) => 1,
-
-                        Some(x) => cmp::min(MAX, x),
-                    };
-                    mem::transmute::<_, SYST>(()).set_reload(dur);
-
-                    // Start counting down from the new reload
-                    mem::transmute::<_, SYST>(()).clear_current();
-
                     None
                 }
-            } else {
-                // The queue is empty
-                mem::transmute::<_, SYST>(()).disable_interrupt();
-
-                None
             }
+        } else {
+            // The queue is empty, disable the interrupt.
+            if Mono::DISABLE_INTERRUPT_ON_EMPTY_QUEUE {
+                disable_interrupt();
+                mono.disable_timer();
+            }
+
+            None
         }
     }
 }
 
-pub struct NotReady<M, T>
+pub struct NotReady<Mono, Task>
 where
-    T: Copy,
-    M: Monotonic,
-    <M::Instant as Sub>::Output: TryInto<u32>,
+    Task: Copy,
+    Mono: Monotonic,
 {
     pub index: u8,
-    pub instant: M::Instant,
-    pub task: T,
+    pub instant: Instant<Mono>,
+    pub task: Task,
 }
 
-impl<M, T> Eq for NotReady<M, T>
+impl<Mono, Task> Eq for NotReady<Mono, Task>
 where
-    T: Copy,
-    M: Monotonic,
-    <M::Instant as Sub>::Output: TryInto<u32>,
+    Task: Copy,
+    Mono: Monotonic,
 {
 }
 
-impl<M, T> Ord for NotReady<M, T>
+impl<Mono, Task> Ord for NotReady<Mono, Task>
 where
-    T: Copy,
-    M: Monotonic,
-    <M::Instant as Sub>::Output: TryInto<u32>,
+    Task: Copy,
+    Mono: Monotonic,
 {
     fn cmp(&self, other: &Self) -> Ordering {
         self.instant.cmp(&other.instant)
     }
 }
 
-impl<M, T> PartialEq for NotReady<M, T>
+impl<Mono, Task> PartialEq for NotReady<Mono, Task>
 where
-    T: Copy,
-    M: Monotonic,
-    <M::Instant as Sub>::Output: TryInto<u32>,
+    Task: Copy,
+    Mono: Monotonic,
 {
     fn eq(&self, other: &Self) -> bool {
         self.instant == other.instant
     }
 }
 
-impl<M, T> PartialOrd for NotReady<M, T>
+impl<Mono, Task> PartialOrd for NotReady<Mono, Task>
 where
-    T: Copy,
-    M: Monotonic,
-    <M::Instant as Sub>::Output: TryInto<u32>,
+    Task: Copy,
+    Mono: Monotonic,
 {
     fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
         Some(self.cmp(&other))