diff --git a/Cargo.toml b/Cargo.toml
index f0ca605178..7ebaac417a 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -58,6 +58,7 @@ rtic-monotonic = "0.1.0-alpha.0"
 rtic-core = "0.3.1"
 heapless = "0.6.1"
 bare-metal = "1.0.0"
+generic-array = "0.14"
 
 [dependencies.dwt-systick-monotonic]
 version = "0.1.0-alpha.0"
diff --git a/examples/double_schedule.rs b/examples/double_schedule.rs
index 403f35830f..9da57ae5f1 100644
--- a/examples/double_schedule.rs
+++ b/examples/double_schedule.rs
@@ -9,14 +9,11 @@ use panic_semihosting as _;
 
 #[rtic::app(device = lm3s6965, dispatchers = [SSI0])]
 mod app {
-    use dwt_systick_monotonic::{
-        consts::{U0, U8},
-        DwtSystick,
-    };
+    use dwt_systick_monotonic::DwtSystick;
     use rtic::time::duration::Seconds;
 
     #[monotonic(binds = SysTick, default = true)]
-    type MyMono = DwtSystick<U8, U0, U0>; // 8 MHz
+    type MyMono = DwtSystick<8_000_000>; // 8 MHz
 
     #[init]
     fn init(cx: init::Context) -> (init::LateResources, init::Monotonics) {
diff --git a/examples/periodic.rs b/examples/periodic.rs
index 82c21280e1..01061c9fbc 100644
--- a/examples/periodic.rs
+++ b/examples/periodic.rs
@@ -10,14 +10,11 @@ use panic_semihosting as _;
 // NOTE: does NOT work on QEMU!
 #[rtic::app(device = lm3s6965, dispatchers = [SSI0])]
 mod app {
-    use dwt_systick_monotonic::{
-        consts::{U0, U8},
-        DwtSystick,
-    };
+    use dwt_systick_monotonic::DwtSystick;
     use rtic::time::duration::Seconds;
 
     #[monotonic(binds = SysTick, default = true)]
-    type MyMono = DwtSystick<U8, U0, U0>; // 8 MHz
+    type MyMono = DwtSystick<8_000_000>; // 8 MHz
 
     #[init]
     fn init(cx: init::Context) -> (init::LateResources, init::Monotonics) {
diff --git a/examples/schedule.rs b/examples/schedule.rs
index d6d4499877..b89e51918d 100644
--- a/examples/schedule.rs
+++ b/examples/schedule.rs
@@ -11,14 +11,11 @@ use panic_semihosting as _;
 #[rtic::app(device = lm3s6965, dispatchers = [SSI0])]
 mod app {
     use cortex_m_semihosting::hprintln;
-    use dwt_systick_monotonic::{
-        consts::{U0, U8},
-        DwtSystick,
-    };
+    use dwt_systick_monotonic::DwtSystick;
     use rtic::time::duration::Seconds;
 
     #[monotonic(binds = SysTick, default = true)]
-    type MyMono = DwtSystick<U8, U0, U0>; // 8 MHz
+    type MyMono = DwtSystick<8_000_000>; // 8 MHz
 
     #[init()]
     fn init(cx: init::Context) -> (init::LateResources, init::Monotonics) {
diff --git a/examples/t-schedule.rs b/examples/t-schedule.rs
index bd0ab668b3..ef04c45180 100644
--- a/examples/t-schedule.rs
+++ b/examples/t-schedule.rs
@@ -9,14 +9,11 @@ use panic_semihosting as _;
 
 #[rtic::app(device = lm3s6965, dispatchers = [SSI0])]
 mod app {
-    use dwt_systick_monotonic::{
-        consts::{U0, U8},
-        DwtSystick,
-    };
+    use dwt_systick_monotonic::DwtSystick;
     use rtic::time::duration::Seconds;
 
     #[monotonic(binds = SysTick, default = true)]
-    type MyMono = DwtSystick<U8, U0, U0>; // 8 MHz
+    type MyMono = DwtSystick<8_000_000>; // 8 MHz
 
     #[init]
     fn init(cx: init::Context) -> (init::LateResources, init::Monotonics) {
@@ -26,19 +23,93 @@ mod app {
 
         let mono = DwtSystick::new(&mut dcb, dwt, systick, 8_000_000);
 
-        let _: Result<(), ()> = foo::spawn_after(Seconds(1_u32));
-        let _: Result<(), u32> = bar::spawn_after(Seconds(2_u32), 0);
-        let _: Result<(), (u32, u32)> = baz::spawn_after(Seconds(3_u32), 0, 1);
+        // Task without message passing
+
+        // Not default
+        let _: Result<foo::MyMono::SpawnHandle, ()> = foo::MyMono::spawn_at(MyMono::now());
+        let handle: Result<foo::MyMono::SpawnHandle, ()> = foo::MyMono::spawn_after(Seconds(1_u32));
+        let _: Result<foo::MyMono::SpawnHandle, ()> =
+            handle.unwrap().reschedule_after(Seconds(1_u32));
+
+        let handle: Result<foo::MyMono::SpawnHandle, ()> = foo::MyMono::spawn_after(Seconds(1_u32));
+        let _: Result<foo::MyMono::SpawnHandle, ()> = handle.unwrap().reschedule_at(MyMono::now());
+
+        let handle: Result<foo::MyMono::SpawnHandle, ()> = foo::MyMono::spawn_after(Seconds(1_u32));
+        let _: Result<(), ()> = handle.unwrap().cancel();
+
+        // Using default
+        let _: Result<foo::SpawnHandle, ()> = foo::spawn_at(MyMono::now());
+        let handle: Result<foo::SpawnHandle, ()> = foo::spawn_after(Seconds(1_u32));
+        let _: Result<foo::SpawnHandle, ()> = handle.unwrap().reschedule_after(Seconds(1_u32));
+
+        let handle: Result<foo::SpawnHandle, ()> = foo::spawn_after(Seconds(1_u32));
+        let _: Result<foo::SpawnHandle, ()> = handle.unwrap().reschedule_at(MyMono::now());
+
+        let handle: Result<foo::SpawnHandle, ()> = foo::spawn_after(Seconds(1_u32));
+        let _: Result<(), ()> = handle.unwrap().cancel();
+
+        // Task with single message passing
+
+        // Not default
+        let _: Result<bar::MyMono::SpawnHandle, u32> = bar::MyMono::spawn_at(MyMono::now(), 0);
+        let handle: Result<bar::MyMono::SpawnHandle, u32> =
+            bar::MyMono::spawn_after(Seconds(1_u32), 0);
+        let _: Result<bar::MyMono::SpawnHandle, ()> =
+            handle.unwrap().reschedule_after(Seconds(1_u32));
+
+        let handle: Result<bar::MyMono::SpawnHandle, u32> =
+            bar::MyMono::spawn_after(Seconds(1_u32), 0);
+        let _: Result<bar::MyMono::SpawnHandle, ()> = handle.unwrap().reschedule_at(MyMono::now());
+
+        let handle: Result<bar::MyMono::SpawnHandle, u32> =
+            bar::MyMono::spawn_after(Seconds(1_u32), 0);
+        let _: Result<u32, ()> = handle.unwrap().cancel();
+
+        // Using default
+        let _: Result<bar::SpawnHandle, u32> = bar::spawn_at(MyMono::now(), 0);
+        let handle: Result<bar::SpawnHandle, u32> = bar::spawn_after(Seconds(1_u32), 0);
+        let _: Result<bar::SpawnHandle, ()> = handle.unwrap().reschedule_after(Seconds(1_u32));
+
+        let handle: Result<bar::SpawnHandle, u32> = bar::spawn_after(Seconds(1_u32), 0);
+        let _: Result<bar::SpawnHandle, ()> = handle.unwrap().reschedule_at(MyMono::now());
+
+        let handle: Result<bar::SpawnHandle, u32> = bar::spawn_after(Seconds(1_u32), 0);
+        let _: Result<u32, ()> = handle.unwrap().cancel();
+
+        // Task with multiple message passing
+
+        // Not default
+        let _: Result<baz::MyMono::SpawnHandle, (u32, u32)> =
+            baz::MyMono::spawn_at(MyMono::now(), 0, 1);
+        let handle: Result<baz::MyMono::SpawnHandle, (u32, u32)> =
+            baz::MyMono::spawn_after(Seconds(1_u32), 0, 1);
+        let _: Result<baz::MyMono::SpawnHandle, ()> =
+            handle.unwrap().reschedule_after(Seconds(1_u32));
+
+        let handle: Result<baz::MyMono::SpawnHandle, (u32, u32)> =
+            baz::MyMono::spawn_after(Seconds(1_u32), 0, 1);
+        let _: Result<baz::MyMono::SpawnHandle, ()> = handle.unwrap().reschedule_at(MyMono::now());
+
+        let handle: Result<baz::MyMono::SpawnHandle, (u32, u32)> =
+            baz::MyMono::spawn_after(Seconds(1_u32), 0, 1);
+        let _: Result<(u32, u32), ()> = handle.unwrap().cancel();
+
+        // Using default
+        let _: Result<baz::SpawnHandle, (u32, u32)> = baz::spawn_at(MyMono::now(), 0, 1);
+        let handle: Result<baz::SpawnHandle, (u32, u32)> = baz::spawn_after(Seconds(1_u32), 0, 1);
+        let _: Result<baz::SpawnHandle, ()> = handle.unwrap().reschedule_after(Seconds(1_u32));
+
+        let handle: Result<baz::SpawnHandle, (u32, u32)> = baz::spawn_after(Seconds(1_u32), 0, 1);
+        let _: Result<baz::SpawnHandle, ()> = handle.unwrap().reschedule_at(MyMono::now());
+
+        let handle: Result<baz::SpawnHandle, (u32, u32)> = baz::spawn_after(Seconds(1_u32), 0, 1);
+        let _: Result<(u32, u32), ()> = handle.unwrap().cancel();
 
         (init::LateResources {}, init::Monotonics(mono))
     }
 
     #[idle]
     fn idle(_: idle::Context) -> ! {
-        let _: Result<(), ()> = foo::spawn_at(MyMono::now() + Seconds(3_u32));
-        let _: Result<(), u32> = bar::spawn_at(MyMono::now() + Seconds(4_u32), 0);
-        let _: Result<(), (u32, u32)> = baz::spawn_at(MyMono::now() + Seconds(5_u32), 0, 1);
-
         loop {
             cortex_m::asm::nop();
         }
diff --git a/examples/types.rs b/examples/types.rs
index ff7deb844d..cdcf80c2e2 100644
--- a/examples/types.rs
+++ b/examples/types.rs
@@ -10,13 +10,10 @@ use panic_semihosting as _;
 #[rtic::app(device = lm3s6965, peripherals = true, dispatchers = [SSI0])]
 mod app {
     use cortex_m_semihosting::debug;
-    use dwt_systick_monotonic::{
-        consts::{U0, U8},
-        DwtSystick,
-    };
+    use dwt_systick_monotonic::DwtSystick;
 
     #[monotonic(binds = SysTick, default = true)]
-    type MyMono = DwtSystick<U8, U0, U0>; // 8 MHz
+    type MyMono = DwtSystick<8_000_000>; // 8 MHz
 
     #[resources]
     struct Resources {
diff --git a/macros/src/codegen/module.rs b/macros/src/codegen/module.rs
index 75435b549e..e15aab1c83 100644
--- a/macros/src/codegen/module.rs
+++ b/macros/src/codegen/module.rs
@@ -21,6 +21,33 @@ pub fn codegen(
     let app_name = &app.name;
     let app_path = quote! {crate::#app_name};
 
+    let all_task_names: Vec<_> = app
+        .software_tasks
+        .iter()
+        .map(|(name, st)| {
+            if !st.is_extern {
+                let cfgs = &st.cfgs;
+                quote! {
+                    #(#cfgs)*
+                    #[allow(unused_imports)]
+                    use #app_path::#name as #name;
+                }
+            } else {
+                quote!()
+            }
+        })
+        .chain(app.hardware_tasks.iter().map(|(name, ht)| {
+            if !ht.is_extern {
+                quote! {
+                    #[allow(unused_imports)]
+                    use #app_path::#name as #name;
+                }
+            } else {
+                quote!()
+            }
+        }))
+        .collect();
+
     let mut lt = None;
     match ctxt {
         Context::Init => {
@@ -202,6 +229,9 @@ pub fn codegen(
 
         // Spawn caller
         items.push(quote!(
+
+        #(#all_task_names)*
+
         #(#cfgs)*
         /// Spawns the task directly
         pub fn spawn(#(#args,)*) -> Result<(), #ty> {
@@ -247,6 +277,7 @@ pub fn codegen(
             if monotonic.args.default {
                 items.push(quote!(pub use #m::spawn_after;));
                 items.push(quote!(pub use #m::spawn_at;));
+                items.push(quote!(pub use #m::SpawnHandle;));
             }
 
             let (enable_interrupt, pend) = if &*m_isr.to_string() == "SysTick" {
@@ -264,15 +295,68 @@ pub fn codegen(
             };
 
             let user_imports = &app.user_imports;
+            let tq_marker = util::mark_internal_ident(&util::timer_queue_marker_ident());
 
             items.push(quote!(
             /// Holds methods related to this monotonic
             pub mod #m {
+                // #(
+                //     #[allow(unused_imports)]
+                //     use #app_path::#all_task_names as #all_task_names;
+                // )*
+                use super::*;
+                #[allow(unused_imports)]
+                use #app_path::#tq_marker;
+                #[allow(unused_imports)]
+                use #app_path::#t;
                 #(
                     #[allow(unused_imports)]
                     #user_imports
                 )*
 
+                pub struct SpawnHandle {
+                    #[doc(hidden)]
+                    marker: u32,
+                }
+
+                impl SpawnHandle {
+                    pub fn cancel(self) -> Result<#ty, ()> {
+                        rtic::export::interrupt::free(|_| unsafe {
+                            let tq = &mut *#app_path::#tq.as_mut_ptr();
+                            if let Some((_task, index)) = tq.cancel_marker(self.marker) {
+                                // Get the message
+                                let msg = #app_path::#inputs.get_unchecked(usize::from(index)).as_ptr().read();
+                                // Return the index to the free queue
+                                #app_path::#fq.split().0.enqueue_unchecked(index);
+
+                                Ok(msg)
+                            } else {
+                                Err(())
+                            }
+                        })
+                    }
+
+                    #[inline]
+                    pub fn reschedule_after<D>(self, duration: D) -> Result<Self, ()>
+                        where D: rtic::time::duration::Duration + rtic::time::fixed_point::FixedPoint,
+                                 D::T: Into<<#app_path::#mono_type as rtic::time::Clock>::T>,
+                    {
+                        self.reschedule_at(#app_path::#m::now() + duration)
+                    }
+
+                    pub fn reschedule_at(self, instant: rtic::time::Instant<#app_path::#mono_type>) -> Result<Self, ()>
+                    {
+                        rtic::export::interrupt::free(|_| unsafe {
+                            let marker = #tq_marker;
+                            #tq_marker = #tq_marker.wrapping_add(1);
+
+                            let tq = &mut *#app_path::#tq.as_mut_ptr();
+
+                            tq.update_marker(self.marker, marker, instant, || #pend).map(|_| SpawnHandle { marker })
+                        })
+                    }
+                }
+
                 #(#cfgs)*
                 /// Spawns the task after a set duration relative to the current time
                 ///
@@ -281,7 +365,7 @@ pub fn codegen(
                 pub fn spawn_after<D>(
                     duration: D
                     #(,#args)*
-                ) -> Result<(), #ty>
+                ) -> Result<SpawnHandle, #ty>
                     where D: rtic::time::duration::Duration + rtic::time::fixed_point::FixedPoint,
                         D::T: Into<<#app_path::#mono_type as rtic::time::Clock>::T>,
                 {
@@ -300,7 +384,7 @@ pub fn codegen(
                 pub fn spawn_at(
                     instant: rtic::time::Instant<#app_path::#mono_type>
                     #(,#args)*
-                ) -> Result<(), #ty> {
+                ) -> Result<SpawnHandle, #ty> {
                     unsafe {
                         let input = #tupled;
                         if let Some(index) = rtic::export::interrupt::free(|_| #app_path::#fq.dequeue()) {
@@ -314,15 +398,21 @@ pub fn codegen(
                                 .as_mut_ptr()
                                 .write(instant);
 
-                            let nr = rtic::export::NotReady {
-                                instant,
-                                index,
-                                task: #app_path::#t::#name,
-                            };
+                            rtic::export::interrupt::free(|_| {
+                                let marker = #tq_marker;
+                                let nr = rtic::export::NotReady {
+                                    instant,
+                                    index,
+                                    task: #app_path::#t::#name,
+                                    marker,
+                                };
+
+                                #tq_marker = #tq_marker.wrapping_add(1);
+
+                                let tq = unsafe { &mut *#app_path::#tq.as_mut_ptr() };
 
-                            rtic::export::interrupt::free(|_|
                                 if let Some(mono) = #app_path::#m_ident.as_mut() {
-                                    #app_path::#tq.enqueue_unchecked(
+                                    tq.enqueue_unchecked(
                                         nr,
                                         || #enable_interrupt,
                                         || #pend,
@@ -331,9 +421,10 @@ pub fn codegen(
                                     // We can only use the timer queue if `init` has returned, and it
                                     // writes the `Some(monotonic)` we are accessing here.
                                     core::hint::unreachable_unchecked()
-                                });
+                                }
 
-                            Ok(())
+                                Ok(SpawnHandle { marker })
+                            })
                         } else {
                             Err(input)
                         }
diff --git a/macros/src/codegen/pre_init.rs b/macros/src/codegen/pre_init.rs
index d5105445dd..287f41a43e 100644
--- a/macros/src/codegen/pre_init.rs
+++ b/macros/src/codegen/pre_init.rs
@@ -77,12 +77,16 @@ pub fn codegen(app: &App, analysis: &Analysis, extra: &Extra) -> Vec<TokenStream
         );));
     }
 
-    // Initialize monotonic's interrupts
-    for (_, monotonic) in app.monotonics.iter()
-    //.map(|(ident, monotonic)| (ident, &monotonic.args.priority, &monotonic.args.binds))
-    {
+    // Initialize monotonic's interrupts and timer queues
+    for (_, monotonic) in &app.monotonics {
         let priority = &monotonic.args.priority;
         let binds = &monotonic.args.binds;
+        let monotonic_name = monotonic.ident.to_string();
+        let tq = util::tq_ident(&monotonic_name);
+        let tq = util::mark_internal_ident(&tq);
+
+        // Initialize timer queues
+        stmts.push(quote!(#tq.as_mut_ptr().write(rtic::export::TimerQueue::new());));
 
         // Compile time assert that this priority is supported by the device
         stmts.push(quote!(let _ = [(); ((1 << #nvic_prio_bits) - #priority as usize)];));
diff --git a/macros/src/codegen/timer_queue.rs b/macros/src/codegen/timer_queue.rs
index 82d0ac981e..0d2c51ead9 100644
--- a/macros/src/codegen/timer_queue.rs
+++ b/macros/src/codegen/timer_queue.rs
@@ -9,6 +9,15 @@ pub fn codegen(app: &App, analysis: &Analysis, _extra: &Extra) -> Vec<TokenStrea
     let mut items = vec![];
 
     if !app.monotonics.is_empty() {
+        // Generate the marker counter used to track for `cancel` and `reschedule`
+        let tq_marker = util::mark_internal_ident(&util::timer_queue_marker_ident());
+        items.push(quote!(
+            // #[doc = #doc]
+            #[doc(hidden)]
+            #[allow(non_camel_case_types)]
+            static mut #tq_marker: u32 = 0;
+        ));
+
         let t = util::schedule_t_ident();
 
         // Enumeration of `schedule`-able tasks
@@ -32,7 +41,7 @@ pub fn codegen(app: &App, analysis: &Analysis, _extra: &Extra) -> Vec<TokenStrea
                 #[doc(hidden)]
                 #[allow(non_camel_case_types)]
                 #[derive(Clone, Copy)]
-                enum #t {
+                pub enum #t {
                     #(#variants,)*
                 }
             ));
@@ -59,15 +68,12 @@ pub fn codegen(app: &App, analysis: &Analysis, _extra: &Extra) -> Vec<TokenStrea
                 .map(|(_name, task)| task.args.capacity)
                 .sum();
             let n = util::capacity_typenum(cap, false);
-            let tq_ty = quote!(rtic::export::TimerQueue<#mono_type, #t, #n>);
+            let tq_ty =
+                quote!(core::mem::MaybeUninit<rtic::export::TimerQueue<#mono_type, #t, #n>>);
 
             items.push(quote!(
                 #[doc(hidden)]
-                static mut #tq: #tq_ty = rtic::export::TimerQueue(
-                    rtic::export::BinaryHeap(
-                        rtic::export::iBinaryHeap::new()
-                    )
-                );
+                static mut #tq: #tq_ty = core::mem::MaybeUninit::uninit();
             ));
 
             let mono = util::monotonic_ident(&monotonic_name);
@@ -129,7 +135,7 @@ pub fn codegen(app: &App, analysis: &Analysis, _extra: &Extra) -> Vec<TokenStrea
 
                     while let Some((task, index)) = rtic::export::interrupt::free(|_|
                         if let Some(mono) = #app_path::#m_ident.as_mut() {
-                            #tq.dequeue(|| #disable_isr, mono)
+                            (&mut *#tq.as_mut_ptr()).dequeue(|| #disable_isr, mono)
                         } else {
                             // We can only use the timer queue if `init` has returned, and it
                             // writes the `Some(monotonic)` we are accessing here.
diff --git a/macros/src/codegen/util.rs b/macros/src/codegen/util.rs
index 6589f62da0..9ccdbf7864 100644
--- a/macros/src/codegen/util.rs
+++ b/macros/src/codegen/util.rs
@@ -89,6 +89,11 @@ pub fn interrupt_ident() -> Ident {
     Ident::new("interrupt", span)
 }
 
+pub fn timer_queue_marker_ident() -> Ident {
+    let span = Span::call_site();
+    Ident::new("TIMER_QUEUE_MARKER", span)
+}
+
 /// Whether `name` is an exception with configurable priority
 pub fn is_exception(name: &Ident) -> bool {
     let s = name.to_string();
diff --git a/src/cyccnt.rs b/src/cyccnt.rs
deleted file mode 100644
index 8e07b00170..0000000000
--- a/src/cyccnt.rs
+++ /dev/null
@@ -1,221 +0,0 @@
-//! Data Watchpoint Trace (DWT) unit's CYCle CouNTer (CYCCNT)
-
-use core::{
-    cmp::Ordering,
-    convert::{Infallible, TryInto},
-    fmt, ops,
-};
-
-use cortex_m::peripheral::DWT;
-
-use crate::Fraction;
-
-/// A measurement of the CYCCNT. Opaque and useful only with `Duration`
-///
-/// This data type is only available on ARMv7-M
-///
-/// # Correctness
-///
-/// Adding or subtracting a `Duration` of more than `(1 << 31)` cycles to an `Instant` effectively
-/// makes it "wrap around" and creates an incorrect value. This is also true if the operation is
-/// done in steps, e.g. `(instant + dur) + dur` where `dur` is `(1 << 30)` ticks.
-#[derive(Clone, Copy, Eq, PartialEq)]
-pub struct Instant {
-    inner: i32,
-}
-
-impl Instant {
-    /// Returns an instant corresponding to "now"
-    ///
-    /// *HEADS UP* this function can, and will, return nonsensical values if called within `init`.
-    /// Only use it in `idle` and tasks. In `init`, use the `init::Context.start` field, or the
-    /// `CYCCNT::zero` function, instead of this function
-    pub fn now() -> Self {
-        Instant {
-            inner: DWT::get_cycle_count() as i32,
-        }
-    }
-
-    /// Returns the amount of time elapsed since this instant was created.
-    pub fn elapsed(&self) -> Duration {
-        let diff = Instant::now().inner.wrapping_sub(self.inner);
-        assert!(diff >= 0, "instant now is earlier than self");
-        Duration { inner: diff as u32 }
-    }
-
-    /// Returns the amount of time elapsed from another instant to this one.
-    pub fn duration_since(&self, earlier: Instant) -> Duration {
-        let diff = self.inner.wrapping_sub(earlier.inner);
-        assert!(diff >= 0, "second instant is later than self");
-        Duration { inner: diff as u32 }
-    }
-}
-
-impl fmt::Debug for Instant {
-    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
-        f.debug_tuple("Instant")
-            .field(&(self.inner as u32))
-            .finish()
-    }
-}
-
-impl ops::AddAssign<Duration> for Instant {
-    fn add_assign(&mut self, dur: Duration) {
-        // NOTE this is a debug assertion because there's no foolproof way to detect a wrap around;
-        // the user may write `(instant + dur) + dur` where `dur` is `(1<<31)-1` ticks.
-        debug_assert!(dur.inner < (1 << 31));
-        self.inner = self.inner.wrapping_add(dur.inner as i32);
-    }
-}
-
-impl ops::Add<Duration> for Instant {
-    type Output = Self;
-
-    fn add(mut self, dur: Duration) -> Self {
-        self += dur;
-        self
-    }
-}
-
-impl ops::SubAssign<Duration> for Instant {
-    fn sub_assign(&mut self, dur: Duration) {
-        // NOTE see the NOTE in `<Instant as AddAssign<Duration>>::add_assign`
-        debug_assert!(dur.inner < (1 << 31));
-        self.inner = self.inner.wrapping_sub(dur.inner as i32);
-    }
-}
-
-impl ops::Sub<Duration> for Instant {
-    type Output = Self;
-
-    fn sub(mut self, dur: Duration) -> Self {
-        self -= dur;
-        self
-    }
-}
-
-impl ops::Sub<Instant> for Instant {
-    type Output = Duration;
-
-    fn sub(self, other: Instant) -> Duration {
-        self.duration_since(other)
-    }
-}
-
-impl Ord for Instant {
-    fn cmp(&self, rhs: &Self) -> Ordering {
-        self.inner.wrapping_sub(rhs.inner).cmp(&0)
-    }
-}
-
-impl PartialOrd for Instant {
-    fn partial_cmp(&self, rhs: &Self) -> Option<Ordering> {
-        Some(self.cmp(rhs))
-    }
-}
-
-/// A `Duration` type to represent a span of time.
-///
-/// This data type is only available on ARMv7-M
-///
-/// # Correctness
-///
-/// This type is *not* appropriate for representing time spans in the order of, or larger than,
-/// seconds because it can hold a maximum of `(1 << 31)` "ticks" where each tick is the inverse of
-/// the CPU frequency, which usually is dozens of MHz.
-#[derive(Clone, Copy, Default, Eq, Ord, PartialEq, PartialOrd)]
-pub struct Duration {
-    inner: u32,
-}
-
-impl Duration {
-    /// Creates a new `Duration` from the specified number of clock cycles
-    pub fn from_cycles(cycles: u32) -> Self {
-        Duration { inner: cycles }
-    }
-
-    /// Returns the total number of clock cycles contained by this `Duration`
-    pub fn as_cycles(&self) -> u32 {
-        self.inner
-    }
-}
-
-impl TryInto<u32> for Duration {
-    type Error = Infallible;
-
-    fn try_into(self) -> Result<u32, Infallible> {
-        Ok(self.as_cycles())
-    }
-}
-
-impl ops::AddAssign for Duration {
-    fn add_assign(&mut self, dur: Duration) {
-        self.inner += dur.inner;
-    }
-}
-
-impl ops::Add<Duration> for Duration {
-    type Output = Self;
-
-    fn add(self, other: Self) -> Self {
-        Duration {
-            inner: self.inner + other.inner,
-        }
-    }
-}
-
-impl ops::SubAssign for Duration {
-    fn sub_assign(&mut self, rhs: Duration) {
-        self.inner -= rhs.inner;
-    }
-}
-
-impl ops::Sub<Duration> for Duration {
-    type Output = Self;
-
-    fn sub(self, rhs: Self) -> Self {
-        Duration {
-            inner: self.inner - rhs.inner,
-        }
-    }
-}
-
-/// Adds the `cycles` method to the `u32` type
-///
-/// This trait is only available on ARMv7-M
-pub trait U32Ext {
-    /// Converts the `u32` value into clock cycles
-    fn cycles(self) -> Duration;
-}
-
-impl U32Ext for u32 {
-    fn cycles(self) -> Duration {
-        Duration { inner: self }
-    }
-}
-
-/// Implementation of the `Monotonic` trait based on CYCle CouNTer
-pub struct CYCCNT;
-
-impl crate::Monotonic for CYCCNT {
-    type Instant = Instant;
-
-    fn ratio() -> Fraction {
-        Fraction {
-            numerator: 1,
-            denominator: 1,
-        }
-    }
-
-    unsafe fn reset() {
-        (0xE0001004 as *mut u32).write_volatile(0)
-    }
-
-    fn now() -> Instant {
-        Instant::now()
-    }
-
-    fn zero() -> Instant {
-        Instant { inner: 0 }
-    }
-}
diff --git a/src/lib.rs b/src/lib.rs
index 822073999f..a4abc4c87b 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -43,6 +43,8 @@ pub use rtic_monotonic::{self, embedded_time as time, Monotonic};
 #[doc(hidden)]
 pub mod export;
 #[doc(hidden)]
+mod linked_list;
+#[doc(hidden)]
 mod tq;
 
 /// Sets the given `interrupt` as pending
diff --git a/src/linked_list.rs b/src/linked_list.rs
new file mode 100644
index 0000000000..6a9836eee2
--- /dev/null
+++ b/src/linked_list.rs
@@ -0,0 +1,599 @@
+use core::fmt;
+use core::marker::PhantomData;
+use core::mem::MaybeUninit;
+use core::ops::{Deref, DerefMut};
+use core::ptr;
+pub use generic_array::ArrayLength;
+use generic_array::GenericArray;
+
+#[derive(Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord)]
+struct LinkedIndex(u16);
+
+impl LinkedIndex {
+    #[inline]
+    const unsafe fn new_unchecked(value: u16) -> Self {
+        LinkedIndex(value)
+    }
+
+    #[inline]
+    const fn none() -> Self {
+        LinkedIndex(u16::MAX)
+    }
+
+    #[inline]
+    const fn option(self) -> Option<u16> {
+        if self.0 == u16::MAX {
+            None
+        } else {
+            Some(self.0)
+        }
+    }
+}
+
+/// A node in the linked list.
+pub struct Node<T> {
+    val: MaybeUninit<T>,
+    next: LinkedIndex,
+}
+
+/// Iterator for the linked list.
+pub struct Iter<'a, T, Kind, N>
+where
+    T: PartialEq + PartialOrd,
+    Kind: kind::Kind,
+    N: ArrayLength<Node<T>>,
+{
+    list: &'a LinkedList<T, Kind, N>,
+    index: LinkedIndex,
+}
+
+impl<'a, T, Kind, N> Iterator for Iter<'a, T, Kind, N>
+where
+    T: PartialEq + PartialOrd,
+    Kind: kind::Kind,
+    N: ArrayLength<Node<T>>,
+{
+    type Item = &'a T;
+
+    fn next(&mut self) -> Option<Self::Item> {
+        let index = self.index.option()?;
+
+        let node = self.list.node_at(index as usize);
+        self.index = node.next;
+
+        Some(self.list.read_data_in_node_at(index as usize))
+    }
+}
+
+/// Comes from [`LinkedList::find_mut`].
+pub struct FindMut<'a, T, Kind, N>
+where
+    T: PartialEq + PartialOrd,
+    Kind: kind::Kind,
+    N: ArrayLength<Node<T>>,
+{
+    list: &'a mut LinkedList<T, Kind, N>,
+    is_head: bool,
+    prev_index: LinkedIndex,
+    index: LinkedIndex,
+    maybe_changed: bool,
+}
+
+impl<'a, T, Kind, N> FindMut<'a, T, Kind, N>
+where
+    T: PartialEq + PartialOrd,
+    Kind: kind::Kind,
+    N: ArrayLength<Node<T>>,
+{
+    fn pop_internal(&mut self) -> T {
+        if self.is_head {
+            // If it is the head element, we can do a normal pop
+            unsafe { self.list.pop_unchecked() }
+        } else {
+            // Somewhere in the list
+
+            // Re-point the previous index
+            self.list.node_at_mut(self.prev_index.0 as usize).next =
+                self.list.node_at_mut(self.index.0 as usize).next;
+
+            // Release the index into the free queue
+            self.list.node_at_mut(self.index.0 as usize).next = self.list.free;
+            self.list.free = self.index;
+
+            self.list.extract_data_in_node_at(self.index.0 as usize)
+        }
+    }
+
+    /// This will pop the element from the list.
+    ///
+    /// Complexity is O(1).
+    #[inline]
+    pub fn pop(mut self) -> T {
+        self.pop_internal()
+    }
+
+    /// This will resort the element into the correct position in the list in needed.
+    /// Same as calling `drop`.
+    ///
+    /// Complexity is worst-case O(N).
+    #[inline]
+    pub fn finish(self) {
+        drop(self)
+    }
+}
+
+impl<T, Kind, N> Drop for FindMut<'_, T, Kind, N>
+where
+    T: PartialEq + PartialOrd,
+    Kind: kind::Kind,
+    N: ArrayLength<Node<T>>,
+{
+    fn drop(&mut self) {
+        // Only resort the list if the element has changed
+        if self.maybe_changed {
+            let val = self.pop_internal();
+            unsafe { self.list.push_unchecked(val) };
+        }
+    }
+}
+
+impl<T, Kind, N> Deref for FindMut<'_, T, Kind, N>
+where
+    T: PartialEq + PartialOrd,
+    Kind: kind::Kind,
+    N: ArrayLength<Node<T>>,
+{
+    type Target = T;
+
+    fn deref(&self) -> &Self::Target {
+        self.list.read_data_in_node_at(self.index.0 as usize)
+    }
+}
+
+impl<T, Kind, N> DerefMut for FindMut<'_, T, Kind, N>
+where
+    T: PartialEq + PartialOrd,
+    Kind: kind::Kind,
+    N: ArrayLength<Node<T>>,
+{
+    fn deref_mut(&mut self) -> &mut Self::Target {
+        self.maybe_changed = true;
+        self.list.read_mut_data_in_node_at(self.index.0 as usize)
+    }
+}
+
+impl<T, Kind, N> fmt::Debug for FindMut<'_, T, Kind, N>
+where
+    T: PartialEq + PartialOrd + core::fmt::Debug,
+    Kind: kind::Kind,
+    N: ArrayLength<Node<T>>,
+{
+    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
+        f.debug_struct("FindMut")
+            .field("prev_index", &self.prev_index)
+            .field("index", &self.index)
+            .field(
+                "prev_value",
+                &self
+                    .list
+                    .read_data_in_node_at(self.prev_index.option().unwrap() as usize),
+            )
+            .field(
+                "value",
+                &self
+                    .list
+                    .read_data_in_node_at(self.index.option().unwrap() as usize),
+            )
+            .finish()
+    }
+}
+
+/// The linked list.
+pub struct LinkedList<T, Kind, N>
+where
+    T: PartialEq + PartialOrd,
+    Kind: kind::Kind,
+    N: ArrayLength<Node<T>>,
+{
+    list: MaybeUninit<GenericArray<Node<T>, N>>,
+    head: LinkedIndex,
+    free: LinkedIndex,
+    _kind: PhantomData<Kind>,
+}
+
+impl<T, Kind, N> LinkedList<T, Kind, N>
+where
+    T: PartialEq + PartialOrd,
+    Kind: kind::Kind,
+    N: ArrayLength<Node<T>>,
+{
+    /// Internal helper to not do pointer arithmetic all over the place.
+    #[inline]
+    fn node_at(&self, index: usize) -> &Node<T> {
+        // Safety: The entire `self.list` is initialized in `new`, which makes this safe.
+        unsafe { &*(self.list.as_ptr() as *const Node<T>).add(index) }
+    }
+
+    /// Internal helper to not do pointer arithmetic all over the place.
+    #[inline]
+    fn node_at_mut(&mut self, index: usize) -> &mut Node<T> {
+        // Safety: The entire `self.list` is initialized in `new`, which makes this safe.
+        unsafe { &mut *(self.list.as_mut_ptr() as *mut Node<T>).add(index) }
+    }
+
+    /// Internal helper to not do pointer arithmetic all over the place.
+    #[inline]
+    fn write_data_in_node_at(&mut self, index: usize, data: T) {
+        unsafe {
+            self.node_at_mut(index).val.as_mut_ptr().write(data);
+        }
+    }
+
+    /// Internal helper to not do pointer arithmetic all over the place.
+    #[inline]
+    fn read_data_in_node_at(&self, index: usize) -> &T {
+        unsafe { &*self.node_at(index).val.as_ptr() }
+    }
+
+    /// Internal helper to not do pointer arithmetic all over the place.
+    #[inline]
+    fn read_mut_data_in_node_at(&mut self, index: usize) -> &mut T {
+        unsafe { &mut *self.node_at_mut(index).val.as_mut_ptr() }
+    }
+
+    /// Internal helper to not do pointer arithmetic all over the place.
+    #[inline]
+    fn extract_data_in_node_at(&mut self, index: usize) -> T {
+        unsafe { self.node_at(index).val.as_ptr().read() }
+    }
+
+    /// Internal helper to not do pointer arithmetic all over the place.
+    /// Safety: This can overwrite existing allocated nodes if used improperly, meaning their
+    /// `Drop` methods won't run.
+    #[inline]
+    unsafe fn write_node_at(&mut self, index: usize, node: Node<T>) {
+        (self.list.as_mut_ptr() as *mut Node<T>)
+            .add(index)
+            .write(node)
+    }
+
+    /// Create a new linked list.
+    pub fn new() -> Self {
+        let mut list = LinkedList {
+            list: MaybeUninit::uninit(),
+            head: LinkedIndex::none(),
+            free: unsafe { LinkedIndex::new_unchecked(0) },
+            _kind: PhantomData,
+        };
+
+        let len = N::U16;
+        let mut free = 0;
+
+        // Initialize indexes
+        while free < len - 1 {
+            unsafe {
+                list.write_node_at(
+                    free as usize,
+                    Node {
+                        val: MaybeUninit::uninit(),
+                        next: LinkedIndex::new_unchecked(free + 1),
+                    },
+                );
+            }
+            free += 1;
+        }
+
+        // Initialize final index
+        unsafe {
+            list.write_node_at(
+                free as usize,
+                Node {
+                    val: MaybeUninit::uninit(),
+                    next: LinkedIndex::none(),
+                },
+            );
+        }
+
+        list
+    }
+
+    /// Push unchecked
+    ///
+    /// Complexity is O(N).
+    ///
+    /// # Safety
+    ///
+    /// Assumes that the list is not full.
+    pub unsafe fn push_unchecked(&mut self, value: T) {
+        let new = self.free.0;
+        // Store the data and update the next free spot
+        self.write_data_in_node_at(new as usize, value);
+        self.free = self.node_at(new as usize).next;
+
+        if let Some(head) = self.head.option() {
+            // Check if we need to replace head
+            if self
+                .read_data_in_node_at(head as usize)
+                .partial_cmp(self.read_data_in_node_at(new as usize))
+                != Kind::ordering()
+            {
+                self.node_at_mut(new as usize).next = self.head;
+                self.head = LinkedIndex::new_unchecked(new);
+            } else {
+                // It's not head, search the list for the correct placement
+                let mut current = head;
+
+                while let Some(next) = self.node_at(current as usize).next.option() {
+                    if self
+                        .read_data_in_node_at(next as usize)
+                        .partial_cmp(self.read_data_in_node_at(new as usize))
+                        != Kind::ordering()
+                    {
+                        break;
+                    }
+
+                    current = next;
+                }
+
+                self.node_at_mut(new as usize).next = self.node_at(current as usize).next;
+                self.node_at_mut(current as usize).next = LinkedIndex::new_unchecked(new);
+            }
+        } else {
+            self.node_at_mut(new as usize).next = self.head;
+            self.head = LinkedIndex::new_unchecked(new);
+        }
+    }
+
+    /// Pushes an element to the linked list and sorts it into place.
+    ///
+    /// Complexity is O(N).
+    pub fn push(&mut self, value: T) -> Result<(), T> {
+        if !self.is_full() {
+            Ok(unsafe { self.push_unchecked(value) })
+        } else {
+            Err(value)
+        }
+    }
+
+    /// Get an iterator over the sorted list.
+    pub fn iter(&self) -> Iter<'_, T, Kind, N> {
+        Iter {
+            list: self,
+            index: self.head,
+        }
+    }
+
+    /// Find an element in the list.
+    pub fn find_mut<F>(&mut self, mut f: F) -> Option<FindMut<'_, T, Kind, N>>
+    where
+        F: FnMut(&T) -> bool,
+    {
+        let head = self.head.option()?;
+
+        // Special-case, first element
+        if f(self.read_data_in_node_at(head as usize)) {
+            return Some(FindMut {
+                is_head: true,
+                prev_index: LinkedIndex::none(),
+                index: self.head,
+                list: self,
+                maybe_changed: false,
+            });
+        }
+
+        let mut current = head;
+
+        while let Some(next) = self.node_at(current as usize).next.option() {
+            if f(self.read_data_in_node_at(next as usize)) {
+                return Some(FindMut {
+                    is_head: false,
+                    prev_index: unsafe { LinkedIndex::new_unchecked(current) },
+                    index: unsafe { LinkedIndex::new_unchecked(next) },
+                    list: self,
+                    maybe_changed: false,
+                });
+            }
+
+            current = next;
+        }
+
+        None
+    }
+
+    /// Peek at the first element.
+    pub fn peek(&self) -> Option<&T> {
+        self.head
+            .option()
+            .map(|head| self.read_data_in_node_at(head as usize))
+    }
+
+    /// Pop unchecked
+    ///
+    /// # Safety
+    ///
+    /// Assumes that the list is not empty.
+    pub unsafe fn pop_unchecked(&mut self) -> T {
+        let head = self.head.0;
+        let current = head;
+        self.head = self.node_at(head as usize).next;
+        self.node_at_mut(current as usize).next = self.free;
+        self.free = LinkedIndex::new_unchecked(current);
+
+        self.extract_data_in_node_at(current as usize)
+    }
+
+    /// Pops the first element in the list.
+    ///
+    /// Complexity is O(1).
+    pub fn pop(&mut self) -> Result<T, ()> {
+        if !self.is_empty() {
+            Ok(unsafe { self.pop_unchecked() })
+        } else {
+            Err(())
+        }
+    }
+
+    /// Checks if the linked list is full.
+    #[inline]
+    pub fn is_full(&self) -> bool {
+        self.free.option().is_none()
+    }
+
+    /// Checks if the linked list is empty.
+    #[inline]
+    pub fn is_empty(&self) -> bool {
+        self.head.option().is_none()
+    }
+}
+
+impl<T, Kind, N> Drop for LinkedList<T, Kind, N>
+where
+    T: PartialEq + PartialOrd,
+    Kind: kind::Kind,
+    N: ArrayLength<Node<T>>,
+{
+    fn drop(&mut self) {
+        let mut index = self.head;
+
+        while let Some(i) = index.option() {
+            let node = self.node_at_mut(i as usize);
+            index = node.next;
+
+            unsafe {
+                ptr::drop_in_place(node.val.as_mut_ptr());
+            }
+        }
+    }
+}
+
+impl<T, Kind, N> fmt::Debug for LinkedList<T, Kind, N>
+where
+    T: PartialEq + PartialOrd + core::fmt::Debug,
+    Kind: kind::Kind,
+    N: ArrayLength<Node<T>>,
+{
+    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
+        f.debug_list().entries(self.iter()).finish()
+    }
+}
+
+/// Min sorted linked list.
+pub struct Min;
+
+/// Max sorted linked list.
+pub struct Max;
+
+/// Sealed traits and implementations for `linked_list`
+pub mod kind {
+    use super::{Max, Min};
+    use core::cmp::Ordering;
+
+    /// The linked list kind: min first or max first
+    pub unsafe trait Kind {
+        #[doc(hidden)]
+        fn ordering() -> Option<Ordering>;
+    }
+
+    unsafe impl Kind for Min {
+        #[inline]
+        fn ordering() -> Option<Ordering> {
+            Some(Ordering::Less)
+        }
+    }
+
+    unsafe impl Kind for Max {
+        #[inline]
+        fn ordering() -> Option<Ordering> {
+            Some(Ordering::Greater)
+        }
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    // Note this useful idiom: importing names from outer (for mod tests) scope.
+    use super::*;
+    use generic_array::typenum::consts::*;
+
+    #[test]
+    fn test_peek() {
+        let mut ll: LinkedList<u32, Max, U3> = LinkedList::new();
+
+        ll.push(1).unwrap();
+        assert_eq!(ll.peek().unwrap(), &1);
+
+        ll.push(2).unwrap();
+        assert_eq!(ll.peek().unwrap(), &2);
+
+        ll.push(3).unwrap();
+        assert_eq!(ll.peek().unwrap(), &3);
+
+        let mut ll: LinkedList<u32, Min, U3> = LinkedList::new();
+
+        ll.push(2).unwrap();
+        assert_eq!(ll.peek().unwrap(), &2);
+
+        ll.push(1).unwrap();
+        assert_eq!(ll.peek().unwrap(), &1);
+
+        ll.push(3).unwrap();
+        assert_eq!(ll.peek().unwrap(), &1);
+    }
+
+    #[test]
+    fn test_full() {
+        let mut ll: LinkedList<u32, Max, U3> = LinkedList::new();
+        ll.push(1).unwrap();
+        ll.push(2).unwrap();
+        ll.push(3).unwrap();
+
+        assert!(ll.is_full())
+    }
+
+    #[test]
+    fn test_empty() {
+        let ll: LinkedList<u32, Max, U3> = LinkedList::new();
+
+        assert!(ll.is_empty())
+    }
+
+    #[test]
+    fn test_rejected_push() {
+        let mut ll: LinkedList<u32, Max, U3> = LinkedList::new();
+        ll.push(1).unwrap();
+        ll.push(2).unwrap();
+        ll.push(3).unwrap();
+
+        // This won't fit
+        let r = ll.push(4);
+
+        assert_eq!(r, Err(4));
+    }
+
+    #[test]
+    fn test_updating() {
+        let mut ll: LinkedList<u32, Max, U3> = LinkedList::new();
+        ll.push(1).unwrap();
+        ll.push(2).unwrap();
+        ll.push(3).unwrap();
+
+        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);
+    }
+}
diff --git a/src/tq.rs b/src/tq.rs
index 063bbd8e4a..3864025924 100644
--- a/src/tq.rs
+++ b/src/tq.rs
@@ -1,22 +1,35 @@
 use crate::{
+    linked_list::{ArrayLength, LinkedList, Min, Node},
     time::{Clock, Instant},
     Monotonic,
 };
 use core::cmp::Ordering;
-use heapless::{binary_heap::Min, ArrayLength, BinaryHeap};
 
-pub struct TimerQueue<Mono, Task, N>(pub BinaryHeap<NotReady<Mono, Task>, N, Min>)
+#[inline(always)]
+fn unwrapper<T, E>(val: Result<T, E>) -> T {
+    if let Ok(v) = val {
+        v
+    } else {
+        unreachable!("Your monotonic is not infallible")
+    }
+}
+
+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
@@ -34,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();
         }
 
@@ -66,17 +73,39 @@ where
         self.0.is_empty()
     }
 
-    #[inline]
-    fn unwrapper<T, E>(val: Result<T, E>) -> T {
-        if let Ok(v) = val {
-            v
+    /// 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 {
-            unreachable!("Your monotonic is not infallible")
+            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(),
@@ -84,7 +113,7 @@ where
         mono.clear_compare_flag();
 
         if let Some(instant) = self.0.peek().map(|p| p.instant) {
-            if instant <= Self::unwrapper(Clock::try_now(mono)) {
+            if instant <= unwrapper(Clock::try_now(mono)) {
                 // task became ready
                 let nr = unsafe { self.0.pop_unchecked() };
 
@@ -97,7 +126,7 @@ where
                 // 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)) {
+                if instant <= unwrapper(Clock::try_now(mono)) {
                     let nr = unsafe { self.0.pop_unchecked() };
 
                     Some((nr.task, nr.index))
@@ -125,6 +154,7 @@ where
     pub index: u8,
     pub instant: Instant<Mono>,
     pub task: Task,
+    pub marker: u32,
 }
 
 impl<Mono, Task> Eq for NotReady<Mono, Task>