From ebf2f058a4d2a1fcf118144b9893dc3038939bad Mon Sep 17 00:00:00 2001
From: Emil Fresk <emil.fresk@gmail.com>
Date: Thu, 18 Feb 2021 19:30:59 +0100
Subject: Now with new monotonic trait and crate

---
 src/export.rs |  4 ++--
 src/lib.rs    |  6 ++----
 src/tq.rs     | 68 +++++++++++++++++++++++++++++++++++------------------------
 3 files changed, 44 insertions(+), 34 deletions(-)

(limited to 'src')

diff --git a/src/export.rs b/src/export.rs
index ab5984e..91a4a5e 100644
--- a/src/export.rs
+++ b/src/export.rs
@@ -16,7 +16,7 @@ pub use cortex_m::{
 use heapless::spsc::SingleCore;
 pub use heapless::{consts, i::Queue as iQueue, spsc::Queue};
 pub use heapless::{i::BinaryHeap as iBinaryHeap, BinaryHeap};
-pub use rtic_core::monotonic::Monotonic;
+pub use rtic_monotonic as monotonic;
 
 pub type SCFQ<N> = Queue<u8, N, u8, SingleCore>;
 pub type SCRQ<T, N> = Queue<(T, u8), N, u8, SingleCore>;
@@ -116,7 +116,7 @@ where
 #[inline(always)]
 pub fn assert_monotonic<T>()
 where
-    T: Monotonic,
+    T: monotonic::Monotonic,
 {
 }
 
diff --git a/src/lib.rs b/src/lib.rs
index 1d4df65..16f2e9f 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -37,10 +37,8 @@
 
 use cortex_m::{interrupt::InterruptNumber, peripheral::NVIC};
 pub use cortex_m_rtic_macros::app;
-pub use rtic_core::{
-    monotonic::{self, embedded_time as time, Monotonic},
-    prelude as mutex_prelude, Exclusive, Mutex,
-};
+pub use rtic_core::{prelude as mutex_prelude, Exclusive, Mutex};
+pub use rtic_monotonic::{self, embedded_time as time, Monotonic};
 
 #[doc(hidden)]
 pub mod export;
diff --git a/src/tq.rs b/src/tq.rs
index 4c89a66..6697f10 100644
--- a/src/tq.rs
+++ b/src/tq.rs
@@ -1,4 +1,7 @@
-use crate::{time::Instant, Monotonic};
+use crate::{
+    time::{Clock, Instant},
+    Monotonic,
+};
 use core::cmp::Ordering;
 use heapless::{binary_heap::Min, ArrayLength, BinaryHeap};
 
@@ -42,7 +45,7 @@ where
             })
             .unwrap_or(true);
         if if_heap_max_greater_than_nr {
-            if is_empty {
+            if Mono::DISABLE_INTERRUPT_ON_EMPTY_QUEUE && is_empty {
                 // mem::transmute::<_, SYST>(()).enable_interrupt();
                 enable_interrupt();
             }
@@ -61,44 +64,53 @@ where
         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<F>(&mut self, disable_interrupt: F) -> Option<(Task, u8)>
+    pub fn dequeue<F>(&mut self, disable_interrupt: F, mono: &mut Mono) -> Option<(Task, u8)>
     where
         F: FnOnce(),
     {
-        unsafe {
-            Mono::clear_compare();
+        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() };
 
-            if let Some(instant) = self.0.peek().map(|p| p.instant) {
-                if instant < Mono::now() {
-                    // task became ready
-                    let nr = self.0.pop_unchecked();
+                Some((nr.task, nr.index))
+            } else {
+                // TODO: Fix this hack...
+                // Extract the compare time.
+                mono.set_compare(*instant.duration_since_epoch().integer());
+
+                // 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 {
-                    // TODO: Fix this hack...
-                    // Extract the compare time.
-                    Mono::set_compare(*instant.duration_since_epoch().integer());
-
-                    // 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 < Mono::now() {
-                        let nr = self.0.pop_unchecked();
-
-                        Some((nr.task, nr.index))
-                    } else {
-                        None
-                    }
+                    None
                 }
-            } else {
-                // The queue is empty, disable the interrupt.
+            }
+        } else {
+            // The queue is empty, disable the interrupt.
+            if Mono::DISABLE_INTERRUPT_ON_EMPTY_QUEUE {
                 disable_interrupt();
-
-                None
             }
+
+            None
         }
     }
 }
-- 
cgit v1.2.3