1 files changed, 44 insertions, 15 deletions

diff --git a/src/tq.rs b/src/tq.rs
index f341d8cc..38640259 100644
--- a/src/tq.rs
+++ b/src/tq.rs
@@ -1,11 +1,11 @@
 use crate::{
+    linked_list::{ArrayLength, LinkedList, Min, Node},
     time::{Clock, Instant},
     Monotonic,
 };
 use core::cmp::Ordering;
-use heapless::{binary_heap::Min, ArrayLength, BinaryHeap};
 
-#[inline]
+#[inline(always)]
 fn unwrapper<T, E>(val: Result<T, E>) -> T {
     if let Ok(v) = val {
         v
@@ -14,18 +14,22 @@ fn unwrapper<T, E>(val: Result<T, E>) -> T {
     }
 }
 
-pub struct TimerQueue<Mono, Task, N>(pub BinaryHeap<NotReady<Mono, Task>, N, Min>)
+pub struct TimerQueue<Mono, Task, N>(pub LinkedList<NotReady<Mono, Task>, Min, N>)
 where
     Mono: Monotonic,
-    N: ArrayLength<NotReady<Mono, Task>>,
+    N: ArrayLength<Node<NotReady<Mono, Task>>>,
     Task: Copy;
 
 impl<Mono, Task, N> TimerQueue<Mono, Task, N>
 where
     Mono: Monotonic,
-    N: ArrayLength<NotReady<Mono, Task>>,
+    N: ArrayLength<Node<NotReady<Mono, Task>>>,
     Task: Copy,
 {
+    pub fn new() -> Self {
+        TimerQueue(LinkedList::new())
+    }
+
     /// # Safety
     ///
     /// Writing to memory with a transmute in order to enable
@@ -43,26 +47,20 @@ 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
         let if_heap_max_greater_than_nr = self
             .0
             .peek()
-            .map(|head| {
-                is_empty = false;
-                nr.instant < head.instant
-            })
+            .map(|head| nr.instant < head.instant)
             .unwrap_or(true);
+
         if if_heap_max_greater_than_nr {
-            if Mono::DISABLE_INTERRUPT_ON_EMPTY_QUEUE && is_empty {
-                // mem::transmute::<_, SYST>(()).enable_interrupt();A
+            if Mono::DISABLE_INTERRUPT_ON_EMPTY_QUEUE && self.0.is_empty() {
                 mono.enable_timer();
                 enable_interrupt();
             }
 
-            // Set SysTick pending
-            // SCB::set_pendst();
             pend_handler();
         }
 
@@ -75,8 +73,39 @@ where
         self.0.is_empty()
     }
 
+    /// Cancel the marker value
+    pub fn cancel_marker(&mut self, marker: u32) -> Option<(Task, u8)> {
+        if let Some(val) = self.0.find_mut(|nr| nr.marker == marker) {
+            let nr = val.pop();
+
+            Some((nr.task, nr.index))
+        } else {
+            None
+        }
+    }
+
+    /// Update the instant at an marker value to a new instant
+    pub fn update_marker<F: FnOnce()>(
+        &mut self,
+        marker: u32,
+        new_marker: u32,
+        instant: Instant<Mono>,
+        pend_handler: F,
+    ) -> Result<(), ()> {
+        if let Some(mut val) = self.0.find_mut(|nr| nr.marker == marker) {
+            val.instant = instant;
+            val.marker = new_marker;
+
+            // On update pend the handler to reconfigure the next compare match
+            pend_handler();
+
+            Ok(())
+        } else {
+            Err(())
+        }
+    }
+
     /// Dequeue a task from the TimerQueue
-    #[inline]
     pub fn dequeue<F>(&mut self, disable_interrupt: F, mono: &mut Mono) -> Option<(Task, u8)>
     where
         F: FnOnce(),