Merge branch 'main' into u0-lcd

10 files changed, 422 insertions, 769 deletions

diff --git a/embassy-time/src/delay.rs b/embassy-time/src/delay.rs
index f77859d4a..11b54b098 100644
--- a/embassy-time/src/delay.rs
+++ b/embassy-time/src/delay.rs
@@ -1,3 +1,5 @@
+use core::future::Future;
+
 use super::{Duration, Instant};
 use crate::Timer;
 
@@ -13,7 +15,8 @@ pub fn block_for(duration: Duration) {
 /// the amount provided, but accuracy can be affected by many factors, including interrupt usage.
 /// Make sure to use a suitable tick rate for your use case. The tick rate is defined by the currently
 /// active driver.
-#[derive(Clone)]
+#[derive(Clone, Debug)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
 pub struct Delay;
 
 impl embedded_hal_1::delay::DelayNs for Delay {
@@ -31,16 +34,16 @@ impl embedded_hal_1::delay::DelayNs for Delay {
 }
 
 impl embedded_hal_async::delay::DelayNs for Delay {
-    async fn delay_ns(&mut self, ns: u32) {
-        Timer::after_nanos(ns as _).await
+    fn delay_ns(&mut self, ns: u32) -> impl Future<Output = ()> {
+        Timer::after_nanos(ns as _)
     }
 
-    async fn delay_us(&mut self, us: u32) {
-        Timer::after_micros(us as _).await
+    fn delay_us(&mut self, us: u32) -> impl Future<Output = ()> {
+        Timer::after_micros(us as _)
     }
 
-    async fn delay_ms(&mut self, ms: u32) {
-        Timer::after_millis(ms as _).await
+    fn delay_ms(&mut self, ms: u32) -> impl Future<Output = ()> {
+        Timer::after_millis(ms as _)
     }
 }
 
diff --git a/embassy-time/src/driver_mock.rs b/embassy-time/src/driver_mock.rs
index 8587f9172..bcde2a6c9 100644
--- a/embassy-time/src/driver_mock.rs
+++ b/embassy-time/src/driver_mock.rs
@@ -1,7 +1,9 @@
 use core::cell::RefCell;
+use core::task::Waker;
 
 use critical_section::Mutex as CsMutex;
-use embassy_time_driver::{AlarmHandle, Driver};
+use embassy_time_driver::Driver;
+use embassy_time_queue_utils::Queue;
 
 use crate::{Duration, Instant};
 
@@ -26,6 +28,7 @@ use crate::{Duration, Instant};
 ///     assert_eq!(true, has_a_second_passed(reference));
 /// }
 /// ```
+#[derive(Debug)]
 pub struct MockDriver(CsMutex<RefCell<InnerMockDriver>>);
 
 embassy_time_driver::time_driver_impl!(static DRIVER: MockDriver = MockDriver::new());
@@ -52,29 +55,13 @@ impl MockDriver {
     /// Advances the time by the specified [`Duration`].
     /// Calling any alarm callbacks that are due.
     pub fn advance(&self, duration: Duration) {
-        let notify = {
-            critical_section::with(|cs| {
-                let mut inner = self.0.borrow_ref_mut(cs);
-
-                inner.now += duration;
-
-                let now = inner.now.as_ticks();
-
-                inner
-                    .alarm
-                    .as_mut()
-                    .filter(|alarm| alarm.timestamp <= now)
-                    .map(|alarm| {
-                        alarm.timestamp = u64::MAX;
-
-                        (alarm.callback, alarm.ctx)
-                    })
-            })
-        };
+        critical_section::with(|cs| {
+            let inner = &mut *self.0.borrow_ref_mut(cs);
 
-        if let Some((callback, ctx)) = notify {
-            (callback)(ctx);
-        }
+            inner.now += duration;
+            // wake expired tasks.
+            inner.queue.next_expiration(inner.now.as_ticks());
+        })
     }
 }
 
@@ -83,87 +70,38 @@ impl Driver for MockDriver {
         critical_section::with(|cs| self.0.borrow_ref(cs).now).as_ticks()
     }
 
-    unsafe fn allocate_alarm(&self) -> Option<AlarmHandle> {
+    fn schedule_wake(&self, at: u64, waker: &Waker) {
         critical_section::with(|cs| {
-            let mut inner = self.0.borrow_ref_mut(cs);
-
-            if inner.alarm.is_some() {
-                None
-            } else {
-                inner.alarm.replace(AlarmState::new());
-
-                Some(AlarmHandle::new(0))
-            }
-        })
-    }
-
-    fn set_alarm_callback(&self, _alarm: AlarmHandle, callback: fn(*mut ()), ctx: *mut ()) {
-        critical_section::with(|cs| {
-            let mut inner = self.0.borrow_ref_mut(cs);
-
-            let Some(alarm) = inner.alarm.as_mut() else {
-                panic!("Alarm not allocated");
-            };
-
-            alarm.callback = callback;
-            alarm.ctx = ctx;
-        });
-    }
-
-    fn set_alarm(&self, _alarm: AlarmHandle, timestamp: u64) -> bool {
-        critical_section::with(|cs| {
-            let mut inner = self.0.borrow_ref_mut(cs);
-
-            if timestamp <= inner.now.as_ticks() {
-                false
-            } else {
-                let Some(alarm) = inner.alarm.as_mut() else {
-                    panic!("Alarm not allocated");
-                };
-
-                alarm.timestamp = timestamp;
-                true
-            }
+            let inner = &mut *self.0.borrow_ref_mut(cs);
+            // enqueue it
+            inner.queue.schedule_wake(at, waker);
+            // wake it if it's in the past.
+            inner.queue.next_expiration(inner.now.as_ticks());
         })
     }
 }
 
+#[derive(Debug)]
 struct InnerMockDriver {
     now: Instant,
-    alarm: Option<AlarmState>,
+    queue: Queue,
 }
 
 impl InnerMockDriver {
     const fn new() -> Self {
         Self {
             now: Instant::from_ticks(0),
-            alarm: None,
-        }
-    }
-}
-
-struct AlarmState {
-    timestamp: u64,
-    callback: fn(*mut ()),
-    ctx: *mut (),
-}
-
-impl AlarmState {
-    const fn new() -> Self {
-        Self {
-            timestamp: u64::MAX,
-            callback: Self::noop,
-            ctx: core::ptr::null_mut(),
+            queue: Queue::new(),
         }
     }
-
-    fn noop(_ctx: *mut ()) {}
 }
 
-unsafe impl Send for AlarmState {}
-
 #[cfg(test)]
 mod tests {
+    use core::sync::atomic::{AtomicBool, Ordering};
+    use std::sync::Arc;
+    use std::task::Wake;
+
     use serial_test::serial;
 
     use super::*;
@@ -185,37 +123,25 @@ mod tests {
 
     #[test]
     #[serial]
-    fn test_set_alarm_not_in_future() {
+    fn test_schedule_wake() {
         setup();
 
-        let driver = MockDriver::get();
-        let alarm = unsafe { AlarmHandle::new(0) };
-        assert_eq!(false, driver.set_alarm(alarm, driver.now()));
-    }
+        static CALLBACK_CALLED: AtomicBool = AtomicBool::new(false);
 
-    #[test]
-    #[serial]
-    fn test_alarm() {
-        setup();
-
-        let driver = MockDriver::get();
-        let alarm = unsafe { driver.allocate_alarm() }.expect("No alarms available");
-        static mut CALLBACK_CALLED: bool = false;
-        let ctx = &mut () as *mut ();
-        driver.set_alarm_callback(alarm, |_| unsafe { CALLBACK_CALLED = true }, ctx);
-        driver.set_alarm(alarm, driver.now() + 1);
-        assert_eq!(false, unsafe { CALLBACK_CALLED });
-        driver.advance(Duration::from_secs(1));
-        assert_eq!(true, unsafe { CALLBACK_CALLED });
-    }
+        struct MockWaker;
 
-    #[test]
-    #[serial]
-    fn test_allocate_alarm() {
-        setup();
+        impl Wake for MockWaker {
+            fn wake(self: Arc<Self>) {
+                CALLBACK_CALLED.store(true, Ordering::Relaxed);
+            }
+        }
+        let waker = Arc::new(MockWaker).into();
 
         let driver = MockDriver::get();
-        assert!(unsafe { driver.allocate_alarm() }.is_some());
-        assert!(unsafe { driver.allocate_alarm() }.is_none());
+
+        driver.schedule_wake(driver.now() + 1, &waker);
+        assert_eq!(false, CALLBACK_CALLED.load(Ordering::Relaxed));
+        driver.advance(Duration::from_secs(1));
+        assert_eq!(true, CALLBACK_CALLED.load(Ordering::Relaxed));
     }
 }
diff --git a/embassy-time/src/driver_std.rs b/embassy-time/src/driver_std.rs
index d182f8331..0cdb8f4ac 100644
--- a/embassy-time/src/driver_std.rs
+++ b/embassy-time/src/driver_std.rs
@@ -1,170 +1,79 @@
-use core::sync::atomic::{AtomicU8, Ordering};
-use std::cell::{RefCell, UnsafeCell};
-use std::mem::MaybeUninit;
-use std::sync::{Condvar, Mutex, Once};
+use std::sync::{Condvar, Mutex};
+use std::thread;
 use std::time::{Duration as StdDuration, Instant as StdInstant};
-use std::{mem, ptr, thread};
 
-use critical_section::Mutex as CsMutex;
-use embassy_time_driver::{AlarmHandle, Driver};
+use embassy_time_driver::Driver;
+use embassy_time_queue_utils::Queue;
 
-const ALARM_COUNT: usize = 4;
-
-struct AlarmState {
-    timestamp: u64,
-
-    // This is really a Option<(fn(*mut ()), *mut ())>
-    // but fn pointers aren't allowed in const yet
-    callback: *const (),
-    ctx: *mut (),
+#[derive(Debug)]
+struct TimeDriver {
+    signaler: Signaler,
+    inner: Mutex<Inner>,
 }
 
-unsafe impl Send for AlarmState {}
-
-impl AlarmState {
-    const fn new() -> Self {
-        Self {
-            timestamp: u64::MAX,
-            callback: ptr::null(),
-            ctx: ptr::null_mut(),
-        }
-    }
+#[derive(Debug)]
+struct Inner {
+    zero_instant: Option<StdInstant>,
+    queue: Queue,
 }
 
-struct TimeDriver {
-    alarm_count: AtomicU8,
-
-    once: Once,
-    // The STD Driver implementation requires the alarms' mutex to be reentrant, which the STD Mutex isn't
-    // Fortunately, mutexes based on the `critical-section` crate are reentrant, because the critical sections
-    // themselves are reentrant
-    alarms: UninitCell<CsMutex<RefCell<[AlarmState; ALARM_COUNT]>>>,
-    zero_instant: UninitCell<StdInstant>,
-    signaler: UninitCell<Signaler>,
-}
-
-const ALARM_NEW: AlarmState = AlarmState::new();
 embassy_time_driver::time_driver_impl!(static DRIVER: TimeDriver = TimeDriver {
-    alarm_count: AtomicU8::new(0),
-
-    once: Once::new(),
-    alarms: UninitCell::uninit(),
-    zero_instant: UninitCell::uninit(),
-    signaler: UninitCell::uninit(),
+    inner: Mutex::new(Inner{
+        zero_instant: None,
+        queue: Queue::new(),
+    }),
+    signaler: Signaler::new(),
 });
 
-impl TimeDriver {
-    fn init(&self) {
-        self.once.call_once(|| unsafe {
-            self.alarms.write(CsMutex::new(RefCell::new([ALARM_NEW; ALARM_COUNT])));
-            self.zero_instant.write(StdInstant::now());
-            self.signaler.write(Signaler::new());
-
-            thread::spawn(Self::alarm_thread);
-        });
-    }
-
-    fn alarm_thread() {
-        let zero = unsafe { DRIVER.zero_instant.read() };
-        loop {
-            let now = DRIVER.now();
-
-            let next_alarm = critical_section::with(|cs| {
-                let alarms = unsafe { DRIVER.alarms.as_ref() }.borrow(cs);
-                loop {
-                    let pending = alarms
-                        .borrow_mut()
-                        .iter_mut()
-                        .find(|alarm| alarm.timestamp <= now)
-                        .map(|alarm| {
-                            alarm.timestamp = u64::MAX;
-
-                            (alarm.callback, alarm.ctx)
-                        });
-
-                    if let Some((callback, ctx)) = pending {
-                        // safety:
-                        // - we can ignore the possiblity of `f` being unset (null) because of the safety contract of `allocate_alarm`.
-                        // - other than that we only store valid function pointers into alarm.callback
-                        let f: fn(*mut ()) = unsafe { mem::transmute(callback) };
-                        f(ctx);
-                    } else {
-                        // No alarm due
-                        break;
-                    }
-                }
-
-                alarms
-                    .borrow()
-                    .iter()
-                    .map(|alarm| alarm.timestamp)
-                    .min()
-                    .unwrap_or(u64::MAX)
-            });
-
-            // Ensure we don't overflow
-            let until = zero
-                .checked_add(StdDuration::from_micros(next_alarm))
-                .unwrap_or_else(|| StdInstant::now() + StdDuration::from_secs(1));
-
-            unsafe { DRIVER.signaler.as_ref() }.wait_until(until);
-        }
+impl Inner {
+    fn init(&mut self) -> StdInstant {
+        *self.zero_instant.get_or_insert_with(|| {
+            thread::spawn(alarm_thread);
+            StdInstant::now()
+        })
     }
 }
 
 impl Driver for TimeDriver {
     fn now(&self) -> u64 {
-        self.init();
-
-        let zero = unsafe { self.zero_instant.read() };
+        let mut inner = self.inner.lock().unwrap();
+        let zero = inner.init();
         StdInstant::now().duration_since(zero).as_micros() as u64
     }
 
-    unsafe fn allocate_alarm(&self) -> Option<AlarmHandle> {
-        let id = self.alarm_count.fetch_update(Ordering::AcqRel, Ordering::Acquire, |x| {
-            if x < ALARM_COUNT as u8 {
-                Some(x + 1)
-            } else {
-                None
-            }
-        });
-
-        match id {
-            Ok(id) => Some(AlarmHandle::new(id)),
-            Err(_) => None,
+    fn schedule_wake(&self, at: u64, waker: &core::task::Waker) {
+        let mut inner = self.inner.lock().unwrap();
+        inner.init();
+        if inner.queue.schedule_wake(at, waker) {
+            self.signaler.signal();
         }
     }
+}
 
-    fn set_alarm_callback(&self, alarm: AlarmHandle, callback: fn(*mut ()), ctx: *mut ()) {
-        self.init();
-        critical_section::with(|cs| {
-            let mut alarms = unsafe { self.alarms.as_ref() }.borrow_ref_mut(cs);
-            let alarm = &mut alarms[alarm.id() as usize];
-            alarm.callback = callback as *const ();
-            alarm.ctx = ctx;
-        });
-    }
+fn alarm_thread() {
+    let zero = DRIVER.inner.lock().unwrap().zero_instant.unwrap();
+    loop {
+        let now = DRIVER.now();
+
+        let next_alarm = DRIVER.inner.lock().unwrap().queue.next_expiration(now);
 
-    fn set_alarm(&self, alarm: AlarmHandle, timestamp: u64) -> bool {
-        self.init();
-        critical_section::with(|cs| {
-            let mut alarms = unsafe { self.alarms.as_ref() }.borrow_ref_mut(cs);
-            let alarm = &mut alarms[alarm.id() as usize];
-            alarm.timestamp = timestamp;
-            unsafe { self.signaler.as_ref() }.signal();
-        });
+        // Ensure we don't overflow
+        let until = zero
+            .checked_add(StdDuration::from_micros(next_alarm))
+            .unwrap_or_else(|| StdInstant::now() + StdDuration::from_secs(1));
 
-        true
+        DRIVER.signaler.wait_until(until);
     }
 }
 
+#[derive(Debug)]
 struct Signaler {
     mutex: Mutex<bool>,
     condvar: Condvar,
 }
 
 impl Signaler {
-    fn new() -> Self {
+    const fn new() -> Self {
         Self {
             mutex: Mutex::new(false),
             condvar: Condvar::new(),
@@ -196,35 +105,3 @@ impl Signaler {
         self.condvar.notify_one();
     }
 }
-
-pub(crate) struct UninitCell<T>(MaybeUninit<UnsafeCell<T>>);
-unsafe impl<T> Send for UninitCell<T> {}
-unsafe impl<T> Sync for UninitCell<T> {}
-
-impl<T> UninitCell<T> {
-    pub const fn uninit() -> Self {
-        Self(MaybeUninit::uninit())
-    }
-
-    pub unsafe fn as_ptr(&self) -> *const T {
-        (*self.0.as_ptr()).get()
-    }
-
-    pub unsafe fn as_mut_ptr(&self) -> *mut T {
-        (*self.0.as_ptr()).get()
-    }
-
-    pub unsafe fn as_ref(&self) -> &T {
-        &*self.as_ptr()
-    }
-
-    pub unsafe fn write(&self, val: T) {
-        ptr::write(self.as_mut_ptr(), val)
-    }
-}
-
-impl<T: Copy> UninitCell<T> {
-    pub unsafe fn read(&self) -> T {
-        ptr::read(self.as_mut_ptr())
-    }
-}
diff --git a/embassy-time/src/driver_wasm.rs b/embassy-time/src/driver_wasm.rs
index ad884f060..646ce170e 100644
--- a/embassy-time/src/driver_wasm.rs
+++ b/embassy-time/src/driver_wasm.rs
@@ -1,28 +1,19 @@
-use core::sync::atomic::{AtomicU8, Ordering};
-use std::cell::UnsafeCell;
-use std::mem::MaybeUninit;
-use std::ptr;
-use std::sync::{Mutex, Once};
+use std::sync::Mutex;
 
-use embassy_time_driver::{AlarmHandle, Driver};
+use embassy_time_driver::Driver;
+use embassy_time_queue_utils::Queue;
 use wasm_bindgen::prelude::*;
 use wasm_timer::Instant as StdInstant;
 
-const ALARM_COUNT: usize = 4;
-
+#[derive(Debug)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
 struct AlarmState {
     token: Option<f64>,
-    closure: Option<Closure<dyn FnMut() + 'static>>,
 }
 
-unsafe impl Send for AlarmState {}
-
 impl AlarmState {
     const fn new() -> Self {
-        Self {
-            token: None,
-            closure: None,
-        }
+        Self { token: None }
     }
 }
 
@@ -32,68 +23,43 @@ extern "C" {
     fn clearTimeout(token: f64);
 }
 
+#[derive(Debug)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
 struct TimeDriver {
-    alarm_count: AtomicU8,
-
-    once: Once,
-    alarms: UninitCell<Mutex<[AlarmState; ALARM_COUNT]>>,
-    zero_instant: UninitCell<StdInstant>,
+    inner: Mutex<Inner>,
 }
 
-const ALARM_NEW: AlarmState = AlarmState::new();
-embassy_time_driver::time_driver_impl!(static DRIVER: TimeDriver = TimeDriver {
-    alarm_count: AtomicU8::new(0),
-    once: Once::new(),
-    alarms: UninitCell::uninit(),
-    zero_instant: UninitCell::uninit(),
-});
-
-impl TimeDriver {
-    fn init(&self) {
-        self.once.call_once(|| unsafe {
-            self.alarms.write(Mutex::new([ALARM_NEW; ALARM_COUNT]));
-            self.zero_instant.write(StdInstant::now());
-        });
-    }
+#[derive(Debug)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+struct Inner {
+    alarm: AlarmState,
+    zero_instant: Option<StdInstant>,
+    queue: Queue,
+    closure: Option<Closure<dyn FnMut()>>,
 }
 
-impl Driver for TimeDriver {
-    fn now(&self) -> u64 {
-        self.init();
+unsafe impl Send for Inner {}
 
-        let zero = unsafe { self.zero_instant.read() };
-        StdInstant::now().duration_since(zero).as_micros() as u64
-    }
-
-    unsafe fn allocate_alarm(&self) -> Option<AlarmHandle> {
-        let id = self.alarm_count.fetch_update(Ordering::AcqRel, Ordering::Acquire, |x| {
-            if x < ALARM_COUNT as u8 {
-                Some(x + 1)
-            } else {
-                None
-            }
-        });
+embassy_time_driver::time_driver_impl!(static DRIVER: TimeDriver = TimeDriver {
+    inner: Mutex::new(Inner{
+        zero_instant: None,
+        queue: Queue::new(),
+        alarm: AlarmState::new(),
+        closure: None,
+    }),
+});
 
-        match id {
-            Ok(id) => Some(AlarmHandle::new(id)),
-            Err(_) => None,
-        }
+impl Inner {
+    fn init(&mut self) -> StdInstant {
+        *self.zero_instant.get_or_insert_with(StdInstant::now)
     }
 
-    fn set_alarm_callback(&self, alarm: AlarmHandle, callback: fn(*mut ()), ctx: *mut ()) {
-        self.init();
-        let mut alarms = unsafe { self.alarms.as_ref() }.lock().unwrap();
-        let alarm = &mut alarms[alarm.id() as usize];
-        alarm.closure.replace(Closure::new(move || {
-            callback(ctx);
-        }));
+    fn now(&mut self) -> u64 {
+        StdInstant::now().duration_since(self.zero_instant.unwrap()).as_micros() as u64
     }
 
-    fn set_alarm(&self, alarm: AlarmHandle, timestamp: u64) -> bool {
-        self.init();
-        let mut alarms = unsafe { self.alarms.as_ref() }.lock().unwrap();
-        let alarm = &mut alarms[alarm.id() as usize];
-        if let Some(token) = alarm.token {
+    fn set_alarm(&mut self, timestamp: u64) -> bool {
+        if let Some(token) = self.alarm.token {
             clearTimeout(token);
         }
 
@@ -102,40 +68,42 @@ impl Driver for TimeDriver {
             false
         } else {
             let timeout = (timestamp - now) as u32;
-            alarm.token = Some(setTimeout(alarm.closure.as_ref().unwrap(), timeout / 1000));
+            let closure = self.closure.get_or_insert_with(|| Closure::new(dispatch));
+            self.alarm.token = Some(setTimeout(closure, timeout / 1000));
 
             true
         }
     }
 }
 
-pub(crate) struct UninitCell<T>(MaybeUninit<UnsafeCell<T>>);
-unsafe impl<T> Send for UninitCell<T> {}
-unsafe impl<T> Sync for UninitCell<T> {}
-
-impl<T> UninitCell<T> {
-    pub const fn uninit() -> Self {
-        Self(MaybeUninit::uninit())
-    }
-    unsafe fn as_ptr(&self) -> *const T {
-        (*self.0.as_ptr()).get()
-    }
-
-    pub unsafe fn as_mut_ptr(&self) -> *mut T {
-        (*self.0.as_ptr()).get()
-    }
-
-    pub unsafe fn as_ref(&self) -> &T {
-        &*self.as_ptr()
+impl Driver for TimeDriver {
+    fn now(&self) -> u64 {
+        let mut inner = self.inner.lock().unwrap();
+        let zero = inner.init();
+        StdInstant::now().duration_since(zero).as_micros() as u64
     }
 
-    pub unsafe fn write(&self, val: T) {
-        ptr::write(self.as_mut_ptr(), val)
+    fn schedule_wake(&self, at: u64, waker: &core::task::Waker) {
+        let mut inner = self.inner.lock().unwrap();
+        inner.init();
+        if inner.queue.schedule_wake(at, waker) {
+            let now = inner.now();
+            let mut next = inner.queue.next_expiration(now);
+            while !inner.set_alarm(next) {
+                let now = inner.now();
+                next = inner.queue.next_expiration(now);
+            }
+        }
     }
 }
 
-impl<T: Copy> UninitCell<T> {
-    pub unsafe fn read(&self) -> T {
-        ptr::read(self.as_mut_ptr())
+fn dispatch() {
+    let inner = &mut *DRIVER.inner.lock().unwrap();
+
+    let now = inner.now();
+    let mut next = inner.queue.next_expiration(now);
+    while !inner.set_alarm(next) {
+        let now = inner.now();
+        next = inner.queue.next_expiration(now);
     }
 }
diff --git a/embassy-time/src/duration.rs b/embassy-time/src/duration.rs
index 647d208e3..b2bfd6de9 100644
--- a/embassy-time/src/duration.rs
+++ b/embassy-time/src/duration.rs
@@ -37,6 +37,11 @@ impl Duration {
         self.ticks * (1_000_000 / GCD_1M) / (TICK_HZ / GCD_1M)
     }
 
+    /// Convert the `Duration` to nanoseconds, rounding down.
+    pub const fn as_nanos(&self) -> u64 {
+        self.ticks * (1_000_000_000 / GCD_1G) / (TICK_HZ / GCD_1G)
+    }
+
     /// Creates a duration from the specified number of clock ticks
     pub const fn from_ticks(ticks: u64) -> Duration {
         Duration { ticks }
@@ -64,9 +69,9 @@ impl Duration {
 
     /// Creates a duration from the specified number of nanoseconds, rounding up.
     /// NOTE: Delays this small may be inaccurate.
-    pub const fn from_nanos(micros: u64) -> Duration {
+    pub const fn from_nanos(nanoseconds: u64) -> Duration {
         Duration {
-            ticks: div_ceil(micros * (TICK_HZ / GCD_1G), 1_000_000_000 / GCD_1G),
+            ticks: div_ceil(nanoseconds * (TICK_HZ / GCD_1G), 1_000_000_000 / GCD_1G),
         }
     }
 
@@ -90,17 +95,87 @@ impl Duration {
         }
     }
 
+    /// Try to create a duration from the specified number of seconds, rounding up.
+    /// Fails if the number of seconds is too large.
+    pub const fn try_from_secs(secs: u64) -> Option<Duration> {
+        let Some(ticks) = secs.checked_mul(TICK_HZ) else {
+            return None;
+        };
+        Some(Duration { ticks })
+    }
+
+    /// Try to create a duration from the specified number of milliseconds, rounding up.
+    /// Fails if the number of milliseconds is too large.
+    pub const fn try_from_millis(millis: u64) -> Option<Duration> {
+        let Some(value) = millis.checked_mul(TICK_HZ / GCD_1K) else {
+            return None;
+        };
+        Some(Duration {
+            ticks: div_ceil(value, 1000 / GCD_1K),
+        })
+    }
+
+    /// Try to create a duration from the specified number of microseconds, rounding up.
+    /// Fails if the number of microseconds is too large.
+    /// NOTE: Delays this small may be inaccurate.
+    pub const fn try_from_micros(micros: u64) -> Option<Duration> {
+        let Some(value) = micros.checked_mul(TICK_HZ / GCD_1M) else {
+            return None;
+        };
+        Some(Duration {
+            ticks: div_ceil(value, 1_000_000 / GCD_1M),
+        })
+    }
+
+    /// Try to create a duration from the specified number of nanoseconds, rounding up.
+    /// Fails if the number of nanoseconds is too large.
+    /// NOTE: Delays this small may be inaccurate.
+    pub const fn try_from_nanos(nanoseconds: u64) -> Option<Duration> {
+        let Some(value) = nanoseconds.checked_mul(TICK_HZ / GCD_1G) else {
+            return None;
+        };
+        Some(Duration {
+            ticks: div_ceil(value, 1_000_000_000 / GCD_1G),
+        })
+    }
+
+    /// Try to create a duration from the specified number of seconds, rounding down.
+    /// Fails if the number of seconds is too large.
+    pub const fn try_from_secs_floor(secs: u64) -> Option<Duration> {
+        let Some(ticks) = secs.checked_mul(TICK_HZ) else {
+            return None;
+        };
+        Some(Duration { ticks })
+    }
+
+    /// Try to create a duration from the specified number of milliseconds, rounding down.
+    /// Fails if the number of milliseconds is too large.
+    pub const fn try_from_millis_floor(millis: u64) -> Option<Duration> {
+        let Some(value) = millis.checked_mul(TICK_HZ / GCD_1K) else {
+            return None;
+        };
+        Some(Duration {
+            ticks: value / (1000 / GCD_1K),
+        })
+    }
+
+    /// Try to create a duration from the specified number of microseconds, rounding down.
+    /// Fails if the number of microseconds is too large.
+    /// NOTE: Delays this small may be inaccurate.
+    pub const fn try_from_micros_floor(micros: u64) -> Option<Duration> {
+        let Some(value) = micros.checked_mul(TICK_HZ / GCD_1M) else {
+            return None;
+        };
+        Some(Duration {
+            ticks: value / (1_000_000 / GCD_1M),
+        })
+    }
+
     /// Creates a duration corresponding to the specified Hz.
     /// NOTE: Giving this function a hz >= the TICK_HZ of your platform will clamp the Duration to 1
     /// tick. Doing so will not deadlock, but will certainly not produce the desired output.
     pub const fn from_hz(hz: u64) -> Duration {
-        let ticks = {
-            if hz >= TICK_HZ {
-                1
-            } else {
-                (TICK_HZ + hz / 2) / hz
-            }
-        };
+        let ticks = { if hz >= TICK_HZ { 1 } else { (TICK_HZ + hz / 2) / hz } };
         Duration { ticks }
     }
 
@@ -217,3 +292,12 @@ impl From<Duration> for core::time::Duration {
         core::time::Duration::from_micros(value.as_micros())
     }
 }
+
+impl core::iter::Sum for Duration {
+    fn sum<I>(iter: I) -> Self
+    where
+        I: Iterator<Item = Duration>,
+    {
+        Duration::from_ticks(iter.map(|d| d.as_ticks()).sum())
+    }
+}
diff --git a/embassy-time/src/fmt.rs b/embassy-time/src/fmt.rs
index 2ac42c557..8ca61bc39 100644
--- a/embassy-time/src/fmt.rs
+++ b/embassy-time/src/fmt.rs
@@ -6,6 +6,7 @@ use core::fmt::{Debug, Display, LowerHex};
 #[cfg(all(feature = "defmt", feature = "log"))]
 compile_error!("You may not enable both `defmt` and `log` features.");
 
+#[collapse_debuginfo(yes)]
 macro_rules! assert {
     ($($x:tt)*) => {
         {
@@ -17,6 +18,7 @@ macro_rules! assert {
     };
 }
 
+#[collapse_debuginfo(yes)]
 macro_rules! assert_eq {
     ($($x:tt)*) => {
         {
@@ -28,6 +30,7 @@ macro_rules! assert_eq {
     };
 }
 
+#[collapse_debuginfo(yes)]
 macro_rules! assert_ne {
     ($($x:tt)*) => {
         {
@@ -39,6 +42,7 @@ macro_rules! assert_ne {
     };
 }
 
+#[collapse_debuginfo(yes)]
 macro_rules! debug_assert {
     ($($x:tt)*) => {
         {
@@ -50,6 +54,7 @@ macro_rules! debug_assert {
     };
 }
 
+#[collapse_debuginfo(yes)]
 macro_rules! debug_assert_eq {
     ($($x:tt)*) => {
         {
@@ -61,6 +66,7 @@ macro_rules! debug_assert_eq {
     };
 }
 
+#[collapse_debuginfo(yes)]
 macro_rules! debug_assert_ne {
     ($($x:tt)*) => {
         {
@@ -72,6 +78,7 @@ macro_rules! debug_assert_ne {
     };
 }
 
+#[collapse_debuginfo(yes)]
 macro_rules! todo {
     ($($x:tt)*) => {
         {
@@ -83,20 +90,19 @@ macro_rules! todo {
     };
 }
 
-#[cfg(not(feature = "defmt"))]
+#[collapse_debuginfo(yes)]
 macro_rules! unreachable {
     ($($x:tt)*) => {
-        ::core::unreachable!($($x)*)
-    };
-}
-
-#[cfg(feature = "defmt")]
-macro_rules! unreachable {
-    ($($x:tt)*) => {
-        ::defmt::unreachable!($($x)*)
+        {
+            #[cfg(not(feature = "defmt"))]
+            ::core::unreachable!($($x)*);
+            #[cfg(feature = "defmt")]
+            ::defmt::unreachable!($($x)*);
+        }
     };
 }
 
+#[collapse_debuginfo(yes)]
 macro_rules! panic {
     ($($x:tt)*) => {
         {
@@ -108,6 +114,7 @@ macro_rules! panic {
     };
 }
 
+#[collapse_debuginfo(yes)]
 macro_rules! trace {
     ($s:literal $(, $x:expr)* $(,)?) => {
         {
@@ -121,6 +128,7 @@ macro_rules! trace {
     };
 }
 
+#[collapse_debuginfo(yes)]
 macro_rules! debug {
     ($s:literal $(, $x:expr)* $(,)?) => {
         {
@@ -134,6 +142,7 @@ macro_rules! debug {
     };
 }
 
+#[collapse_debuginfo(yes)]
 macro_rules! info {
     ($s:literal $(, $x:expr)* $(,)?) => {
         {
@@ -147,6 +156,7 @@ macro_rules! info {
     };
 }
 
+#[collapse_debuginfo(yes)]
 macro_rules! warn {
     ($s:literal $(, $x:expr)* $(,)?) => {
         {
@@ -160,6 +170,7 @@ macro_rules! warn {
     };
 }
 
+#[collapse_debuginfo(yes)]
 macro_rules! error {
     ($s:literal $(, $x:expr)* $(,)?) => {
         {
@@ -174,6 +185,7 @@ macro_rules! error {
 }
 
 #[cfg(feature = "defmt")]
+#[collapse_debuginfo(yes)]
 macro_rules! unwrap {
     ($($x:tt)*) => {
         ::defmt::unwrap!($($x)*)
@@ -181,6 +193,7 @@ macro_rules! unwrap {
 }
 
 #[cfg(not(feature = "defmt"))]
+#[collapse_debuginfo(yes)]
 macro_rules! unwrap {
     ($arg:expr) => {
         match $crate::fmt::Try::into_result($arg) {
diff --git a/embassy-time/src/instant.rs b/embassy-time/src/instant.rs
index 909f1b173..de5ebebf8 100644
--- a/embassy-time/src/instant.rs
+++ b/embassy-time/src/instant.rs
@@ -1,7 +1,7 @@
 use core::fmt;
 use core::ops::{Add, AddAssign, Sub, SubAssign};
 
-use super::{Duration, GCD_1K, GCD_1M, TICK_HZ};
+use super::{Duration, GCD_1G, GCD_1K, GCD_1M, TICK_HZ};
 
 #[derive(Debug, Copy, Clone, PartialEq, Eq, PartialOrd, Ord)]
 #[cfg_attr(feature = "defmt", derive(defmt::Format))]
@@ -17,6 +17,7 @@ impl Instant {
     pub const MAX: Instant = Instant { ticks: u64::MAX };
 
     /// Returns an Instant representing the current time.
+    #[inline]
     pub fn now() -> Instant {
         Instant {
             ticks: embassy_time_driver::now(),
@@ -28,6 +29,13 @@ impl Instant {
         Self { ticks }
     }
 
+    /// Create an Instant from a nanosecond count since system boot.
+    pub const fn from_nanos(nanos: u64) -> Self {
+        Self {
+            ticks: nanos * (TICK_HZ / GCD_1G) / (1_000_000_000 / GCD_1G),
+        }
+    }
+
     /// Create an Instant from a microsecond count since system boot.
     pub const fn from_micros(micros: u64) -> Self {
         Self {
@@ -49,6 +57,48 @@ impl Instant {
         }
     }
 
+    /// Try to create an Instant from a nanosecond count since system boot.
+    /// Fails if the number of nanoseconds is too large.
+    pub const fn try_from_nanos(nanos: u64) -> Option<Self> {
+        let Some(value) = nanos.checked_mul(TICK_HZ / GCD_1G) else {
+            return None;
+        };
+        Some(Self {
+            ticks: value / (1_000_000_000 / GCD_1G),
+        })
+    }
+
+    /// Try to create an Instant from a microsecond count since system boot.
+    /// Fails if the number of microseconds is too large.
+    pub const fn try_from_micros(micros: u64) -> Option<Self> {
+        let Some(value) = micros.checked_mul(TICK_HZ / GCD_1M) else {
+            return None;
+        };
+        Some(Self {
+            ticks: value / (1_000_000 / GCD_1M),
+        })
+    }
+
+    /// Try to create an Instant from a millisecond count since system boot.
+    /// Fails if the number of milliseconds is too large.
+    pub const fn try_from_millis(millis: u64) -> Option<Self> {
+        let Some(value) = millis.checked_mul(TICK_HZ / GCD_1K) else {
+            return None;
+        };
+        Some(Self {
+            ticks: value / (1000 / GCD_1K),
+        })
+    }
+
+    /// Try to create an Instant from a second count since system boot.
+    /// Fails if the number of seconds is too large.
+    pub const fn try_from_secs(seconds: u64) -> Option<Self> {
+        let Some(ticks) = seconds.checked_mul(TICK_HZ) else {
+            return None;
+        };
+        Some(Self { ticks })
+    }
+
     /// Tick count since system boot.
     pub const fn as_ticks(&self) -> u64 {
         self.ticks
@@ -69,6 +119,11 @@ impl Instant {
         self.ticks * (1_000_000 / GCD_1M) / (TICK_HZ / GCD_1M)
     }
 
+    /// Nanoseconds since system boot.
+    pub const fn as_nanos(&self) -> u64 {
+        self.ticks * (1_000_000_000 / GCD_1G) / (TICK_HZ / GCD_1G)
+    }
+
     /// Duration between this Instant and another Instant
     /// Panics on over/underflow.
     pub fn duration_since(&self, earlier: Instant) -> Duration {
@@ -114,6 +169,18 @@ impl Instant {
     pub fn checked_sub(&self, duration: Duration) -> Option<Instant> {
         self.ticks.checked_sub(duration.ticks).map(|ticks| Instant { ticks })
     }
+
+    /// Adds a Duration to self. In case of overflow, the maximum value is returned.
+    pub fn saturating_add(mut self, duration: Duration) -> Self {
+        self.ticks = self.ticks.saturating_add(duration.ticks);
+        self
+    }
+
+    /// Subtracts a Duration from self. In case of overflow, the minimum value is returned.
+    pub fn saturating_sub(mut self, duration: Duration) -> Self {
+        self.ticks = self.ticks.saturating_sub(duration.ticks);
+        self
+    }
 }
 
 impl Add<Duration> for Instant {
diff --git a/embassy-time/src/lib.rs b/embassy-time/src/lib.rs
index 24ee51be7..e375fe93e 100644
--- a/embassy-time/src/lib.rs
+++ b/embassy-time/src/lib.rs
@@ -1,8 +1,10 @@
 #![cfg_attr(not(any(feature = "std", feature = "wasm", test)), no_std)]
 #![allow(async_fn_in_trait)]
+#![allow(unsafe_op_in_unsafe_fn)]
 #![doc = include_str!("../README.md")]
 #![allow(clippy::new_without_default)]
 #![warn(missing_docs)]
+#![deny(missing_debug_implementations)]
 
 //! ## Feature flags
 #![doc = document_features::document_features!(feature_label = r#"<span class="stab portability"><code>{feature}</code></span>"#)]
@@ -25,21 +27,15 @@ pub use driver_mock::MockDriver;
 mod driver_std;
 #[cfg(feature = "wasm")]
 mod driver_wasm;
-#[cfg(feature = "generic-queue")]
-mod queue_generic;
 
-pub use delay::{block_for, Delay};
+pub use delay::{Delay, block_for};
 pub use duration::Duration;
 pub use embassy_time_driver::TICK_HZ;
 pub use instant::Instant;
-pub use timer::{with_deadline, with_timeout, Ticker, TimeoutError, Timer};
+pub use timer::{Ticker, TimeoutError, Timer, WithTimeout, with_deadline, with_timeout};
 
 const fn gcd(a: u64, b: u64) -> u64 {
-    if b == 0 {
-        a
-    } else {
-        gcd(b, a % b)
-    }
+    if b == 0 { a } else { gcd(b, a % b) }
 }
 
 pub(crate) const GCD_1K: u64 = gcd(TICK_HZ, 1_000);
diff --git a/embassy-time/src/queue_generic.rs b/embassy-time/src/queue_generic.rs
deleted file mode 100644
index 4882afd3e..000000000
--- a/embassy-time/src/queue_generic.rs
+++ /dev/null
@@ -1,348 +0,0 @@
-use core::cell::RefCell;
-use core::cmp::{min, Ordering};
-use core::task::Waker;
-
-use critical_section::Mutex;
-use embassy_time_driver::{allocate_alarm, set_alarm, set_alarm_callback, AlarmHandle};
-use embassy_time_queue_driver::TimerQueue;
-use heapless::Vec;
-
-use crate::Instant;
-
-#[cfg(feature = "generic-queue-8")]
-const QUEUE_SIZE: usize = 8;
-#[cfg(feature = "generic-queue-16")]
-const QUEUE_SIZE: usize = 16;
-#[cfg(feature = "generic-queue-32")]
-const QUEUE_SIZE: usize = 32;
-#[cfg(feature = "generic-queue-64")]
-const QUEUE_SIZE: usize = 64;
-#[cfg(feature = "generic-queue-128")]
-const QUEUE_SIZE: usize = 128;
-#[cfg(not(any(
-    feature = "generic-queue-8",
-    feature = "generic-queue-16",
-    feature = "generic-queue-32",
-    feature = "generic-queue-64",
-    feature = "generic-queue-128"
-)))]
-const QUEUE_SIZE: usize = 64;
-
-#[derive(Debug)]
-struct Timer {
-    at: Instant,
-    waker: Waker,
-}
-
-impl PartialEq for Timer {
-    fn eq(&self, other: &Self) -> bool {
-        self.at == other.at
-    }
-}
-
-impl Eq for Timer {}
-
-impl PartialOrd for Timer {
-    fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
-        self.at.partial_cmp(&other.at)
-    }
-}
-
-impl Ord for Timer {
-    fn cmp(&self, other: &Self) -> Ordering {
-        self.at.cmp(&other.at)
-    }
-}
-
-struct InnerQueue {
-    queue: Vec<Timer, QUEUE_SIZE>,
-    alarm: AlarmHandle,
-}
-
-impl InnerQueue {
-    fn schedule_wake(&mut self, at: Instant, waker: &Waker) {
-        self.queue
-            .iter_mut()
-            .find(|timer| timer.waker.will_wake(waker))
-            .map(|timer| {
-                timer.at = min(timer.at, at);
-            })
-            .unwrap_or_else(|| {
-                let mut timer = Timer {
-                    waker: waker.clone(),
-                    at,
-                };
-
-                loop {
-                    match self.queue.push(timer) {
-                        Ok(()) => break,
-                        Err(e) => timer = e,
-                    }
-
-                    self.queue.pop().unwrap().waker.wake();
-                }
-            });
-
-        // Don't wait for the alarm callback to trigger and directly
-        // dispatch all timers that are already due
-        //
-        // Then update the alarm if necessary
-        self.dispatch();
-    }
-
-    fn dispatch(&mut self) {
-        loop {
-            let now = Instant::now();
-
-            let mut next_alarm = Instant::MAX;
-
-            let mut i = 0;
-            while i < self.queue.len() {
-                let timer = &self.queue[i];
-                if timer.at <= now {
-                    let timer = self.queue.swap_remove(i);
-                    timer.waker.wake();
-                } else {
-                    next_alarm = min(next_alarm, timer.at);
-                    i += 1;
-                }
-            }
-
-            if self.update_alarm(next_alarm) {
-                break;
-            }
-        }
-    }
-
-    fn update_alarm(&mut self, next_alarm: Instant) -> bool {
-        if next_alarm == Instant::MAX {
-            true
-        } else {
-            set_alarm(self.alarm, next_alarm.as_ticks())
-        }
-    }
-
-    fn handle_alarm(&mut self) {
-        self.dispatch();
-    }
-}
-
-struct Queue {
-    inner: Mutex<RefCell<Option<InnerQueue>>>,
-}
-
-impl Queue {
-    const fn new() -> Self {
-        Self {
-            inner: Mutex::new(RefCell::new(None)),
-        }
-    }
-
-    fn schedule_wake(&'static self, at: Instant, waker: &Waker) {
-        critical_section::with(|cs| {
-            let mut inner = self.inner.borrow_ref_mut(cs);
-
-            if inner.is_none() {}
-
-            inner
-                .get_or_insert_with(|| {
-                    let handle = unsafe { allocate_alarm() }.unwrap();
-                    set_alarm_callback(handle, Self::handle_alarm_callback, self as *const _ as _);
-                    InnerQueue {
-                        queue: Vec::new(),
-                        alarm: handle,
-                    }
-                })
-                .schedule_wake(at, waker)
-        });
-    }
-
-    fn handle_alarm(&self) {
-        critical_section::with(|cs| self.inner.borrow_ref_mut(cs).as_mut().unwrap().handle_alarm())
-    }
-
-    fn handle_alarm_callback(ctx: *mut ()) {
-        unsafe { (ctx as *const Self).as_ref().unwrap() }.handle_alarm();
-    }
-}
-
-impl TimerQueue for Queue {
-    fn schedule_wake(&'static self, at: u64, waker: &Waker) {
-        Queue::schedule_wake(self, Instant::from_ticks(at), waker);
-    }
-}
-
-embassy_time_queue_driver::timer_queue_impl!(static QUEUE: Queue = Queue::new());
-
-#[cfg(test)]
-#[cfg(feature = "mock-driver")]
-mod tests {
-    use core::sync::atomic::{AtomicBool, Ordering};
-    use core::task::Waker;
-    use std::sync::Arc;
-    use std::task::Wake;
-
-    use serial_test::serial;
-
-    use crate::driver_mock::MockDriver;
-    use crate::queue_generic::QUEUE;
-    use crate::{Duration, Instant};
-
-    struct TestWaker {
-        pub awoken: AtomicBool,
-    }
-
-    impl Wake for TestWaker {
-        fn wake(self: Arc<Self>) {
-            self.awoken.store(true, Ordering::Relaxed);
-        }
-
-        fn wake_by_ref(self: &Arc<Self>) {
-            self.awoken.store(true, Ordering::Relaxed);
-        }
-    }
-
-    fn test_waker() -> (Arc<TestWaker>, Waker) {
-        let arc = Arc::new(TestWaker {
-            awoken: AtomicBool::new(false),
-        });
-        let waker = Waker::from(arc.clone());
-
-        (arc, waker)
-    }
-
-    fn setup() {
-        MockDriver::get().reset();
-        critical_section::with(|cs| *QUEUE.inner.borrow_ref_mut(cs) = None);
-    }
-
-    fn queue_len() -> usize {
-        critical_section::with(|cs| {
-            QUEUE
-                .inner
-                .borrow_ref(cs)
-                .as_ref()
-                .map(|inner| inner.queue.iter().count())
-                .unwrap_or(0)
-        })
-    }
-
-    #[test]
-    #[serial]
-    fn test_schedule() {
-        setup();
-
-        assert_eq!(queue_len(), 0);
-
-        let (flag, waker) = test_waker();
-
-        QUEUE.schedule_wake(Instant::from_secs(1), &waker);
-
-        assert!(!flag.awoken.load(Ordering::Relaxed));
-        assert_eq!(queue_len(), 1);
-    }
-
-    #[test]
-    #[serial]
-    fn test_schedule_same() {
-        setup();
-
-        let (_flag, waker) = test_waker();
-
-        QUEUE.schedule_wake(Instant::from_secs(1), &waker);
-
-        assert_eq!(queue_len(), 1);
-
-        QUEUE.schedule_wake(Instant::from_secs(1), &waker);
-
-        assert_eq!(queue_len(), 1);
-
-        QUEUE.schedule_wake(Instant::from_secs(100), &waker);
-
-        assert_eq!(queue_len(), 1);
-
-        let (_flag2, waker2) = test_waker();
-
-        QUEUE.schedule_wake(Instant::from_secs(100), &waker2);
-
-        assert_eq!(queue_len(), 2);
-    }
-
-    #[test]
-    #[serial]
-    fn test_trigger() {
-        setup();
-
-        let (flag, waker) = test_waker();
-
-        QUEUE.schedule_wake(Instant::from_secs(100), &waker);
-
-        assert!(!flag.awoken.load(Ordering::Relaxed));
-
-        MockDriver::get().advance(Duration::from_secs(99));
-
-        assert!(!flag.awoken.load(Ordering::Relaxed));
-
-        assert_eq!(queue_len(), 1);
-
-        MockDriver::get().advance(Duration::from_secs(1));
-
-        assert!(flag.awoken.load(Ordering::Relaxed));
-
-        assert_eq!(queue_len(), 0);
-    }
-
-    #[test]
-    #[serial]
-    fn test_immediate_trigger() {
-        setup();
-
-        let (flag, waker) = test_waker();
-
-        QUEUE.schedule_wake(Instant::from_secs(100), &waker);
-
-        MockDriver::get().advance(Duration::from_secs(50));
-
-        let (flag2, waker2) = test_waker();
-
-        QUEUE.schedule_wake(Instant::from_secs(40), &waker2);
-
-        assert!(!flag.awoken.load(Ordering::Relaxed));
-        assert!(flag2.awoken.load(Ordering::Relaxed));
-        assert_eq!(queue_len(), 1);
-    }
-
-    #[test]
-    #[serial]
-    fn test_queue_overflow() {
-        setup();
-
-        for i in 1..super::QUEUE_SIZE {
-            let (flag, waker) = test_waker();
-
-            QUEUE.schedule_wake(Instant::from_secs(310), &waker);
-
-            assert_eq!(queue_len(), i);
-            assert!(!flag.awoken.load(Ordering::Relaxed));
-        }
-
-        let (flag, waker) = test_waker();
-
-        QUEUE.schedule_wake(Instant::from_secs(300), &waker);
-
-        assert_eq!(queue_len(), super::QUEUE_SIZE);
-        assert!(!flag.awoken.load(Ordering::Relaxed));
-
-        let (flag2, waker2) = test_waker();
-
-        QUEUE.schedule_wake(Instant::from_secs(305), &waker2);
-
-        assert_eq!(queue_len(), super::QUEUE_SIZE);
-        assert!(flag.awoken.load(Ordering::Relaxed));
-
-        let (_flag3, waker3) = test_waker();
-        QUEUE.schedule_wake(Instant::from_secs(320), &waker3);
-        assert_eq!(queue_len(), super::QUEUE_SIZE);
-        assert!(flag2.awoken.load(Ordering::Relaxed));
-    }
-}
diff --git a/embassy-time/src/timer.rs b/embassy-time/src/timer.rs
index bc39d8bc7..2f5967c63 100644
--- a/embassy-time/src/timer.rs
+++ b/embassy-time/src/timer.rs
@@ -1,10 +1,9 @@
-use core::future::{poll_fn, Future};
-use core::pin::{pin, Pin};
+use core::future::{Future, poll_fn};
+use core::pin::Pin;
 use core::task::{Context, Poll};
 
-use futures_util::future::{select, Either};
-use futures_util::stream::FusedStream;
-use futures_util::Stream;
+use futures_core::Stream;
+use futures_core::stream::FusedStream;
 
 use crate::{Duration, Instant};
 
@@ -17,11 +16,10 @@ pub struct TimeoutError;
 ///
 /// If the future completes before the timeout, its output is returned. Otherwise, on timeout,
 /// work on the future is stopped (`poll` is no longer called), the future is dropped and `Err(TimeoutError)` is returned.
-pub async fn with_timeout<F: Future>(timeout: Duration, fut: F) -> Result<F::Output, TimeoutError> {
-    let timeout_fut = Timer::after(timeout);
-    match select(pin!(fut), timeout_fut).await {
-        Either::Left((r, _)) => Ok(r),
-        Either::Right(_) => Err(TimeoutError),
+pub fn with_timeout<F: Future>(timeout: Duration, fut: F) -> TimeoutFuture<F> {
+    TimeoutFuture {
+        timer: Timer::after(timeout),
+        fut,
     }
 }
 
@@ -29,16 +27,75 @@ pub async fn with_timeout<F: Future>(timeout: Duration, fut: F) -> Result<F::Out
 ///
 /// If the future completes before the deadline, its output is returned. Otherwise, on timeout,
 /// work on the future is stopped (`poll` is no longer called), the future is dropped and `Err(TimeoutError)` is returned.
-pub async fn with_deadline<F: Future>(at: Instant, fut: F) -> Result<F::Output, TimeoutError> {
-    let timeout_fut = Timer::at(at);
-    match select(pin!(fut), timeout_fut).await {
-        Either::Left((r, _)) => Ok(r),
-        Either::Right(_) => Err(TimeoutError),
+pub fn with_deadline<F: Future>(at: Instant, fut: F) -> TimeoutFuture<F> {
+    TimeoutFuture {
+        timer: Timer::at(at),
+        fut,
+    }
+}
+
+/// Provides functions to run a given future with a timeout or a deadline.
+pub trait WithTimeout: Sized {
+    /// Output type of the future.
+    type Output;
+
+    /// Runs a given future with a timeout.
+    ///
+    /// If the future completes before the timeout, its output is returned. Otherwise, on timeout,
+    /// work on the future is stopped (`poll` is no longer called), the future is dropped and `Err(TimeoutError)` is returned.
+    fn with_timeout(self, timeout: Duration) -> TimeoutFuture<Self>;
+
+    /// Runs a given future with a deadline time.
+    ///
+    /// If the future completes before the deadline, its output is returned. Otherwise, on timeout,
+    /// work on the future is stopped (`poll` is no longer called), the future is dropped and `Err(TimeoutError)` is returned.
+    fn with_deadline(self, at: Instant) -> TimeoutFuture<Self>;
+}
+
+impl<F: Future> WithTimeout for F {
+    type Output = F::Output;
+
+    fn with_timeout(self, timeout: Duration) -> TimeoutFuture<Self> {
+        with_timeout(timeout, self)
+    }
+
+    fn with_deadline(self, at: Instant) -> TimeoutFuture<Self> {
+        with_deadline(at, self)
+    }
+}
+
+/// Future for the [`with_timeout`] and [`with_deadline`] functions.
+#[must_use = "futures do nothing unless you `.await` or poll them"]
+#[derive(Debug)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub struct TimeoutFuture<F> {
+    timer: Timer,
+    fut: F,
+}
+
+impl<F: Unpin> Unpin for TimeoutFuture<F> {}
+
+impl<F: Future> Future for TimeoutFuture<F> {
+    type Output = Result<F::Output, TimeoutError>;
+
+    fn poll(self: Pin<&mut Self>, cx: &mut Context<'_>) -> Poll<Self::Output> {
+        let this = unsafe { self.get_unchecked_mut() };
+        let fut = unsafe { Pin::new_unchecked(&mut this.fut) };
+        let timer = unsafe { Pin::new_unchecked(&mut this.timer) };
+        if let Poll::Ready(x) = fut.poll(cx) {
+            return Poll::Ready(Ok(x));
+        }
+        if let Poll::Ready(_) = timer.poll(cx) {
+            return Poll::Ready(Err(TimeoutError));
+        }
+        Poll::Pending
     }
 }
 
 /// A future that completes at a specified [Instant](struct.Instant.html).
 #[must_use = "futures do nothing unless you `.await` or poll them"]
+#[derive(Debug)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
 pub struct Timer {
     expires_at: Instant,
     yielded_once: bool,
@@ -46,6 +103,7 @@ pub struct Timer {
 
 impl Timer {
     /// Expire at specified [Instant](struct.Instant.html)
+    /// Will expire immediately if the Instant is in the past.
     pub fn at(expires_at: Instant) -> Self {
         Self {
             expires_at,
@@ -127,7 +185,7 @@ impl Future for Timer {
         if self.yielded_once && self.expires_at <= Instant::now() {
             Poll::Ready(())
         } else {
-            embassy_time_queue_driver::schedule_wake(self.expires_at.as_ticks(), cx.waker());
+            embassy_time_driver::schedule_wake(self.expires_at.as_ticks(), cx.waker());
             self.yielded_once = true;
             Poll::Pending
         }
@@ -170,6 +228,12 @@ impl Future for Timer {
 ///     }
 /// }
 /// ```
+///
+/// ## Cancel safety
+/// It is safe to cancel waiting for the next tick,
+/// meaning no tick is lost if the Future is dropped.
+#[derive(Debug)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
 pub struct Ticker {
     expires_at: Instant,
     duration: Duration,
@@ -201,6 +265,9 @@ impl Ticker {
     }
 
     /// Waits for the next tick.
+    ///
+    /// ## Cancel safety
+    /// The produced Future is cancel safe, meaning no tick is lost if the Future is dropped.
     pub fn next(&mut self) -> impl Future<Output = ()> + Send + Sync + '_ {
         poll_fn(|cx| {
             if self.expires_at <= Instant::now() {
@@ -208,7 +275,7 @@ impl Ticker {
                 self.expires_at += dur;
                 Poll::Ready(())
             } else {
-                embassy_time_queue_driver::schedule_wake(self.expires_at.as_ticks(), cx.waker());
+                embassy_time_driver::schedule_wake(self.expires_at.as_ticks(), cx.waker());
                 Poll::Pending
             }
         })
@@ -225,7 +292,7 @@ impl Stream for Ticker {
             self.expires_at += dur;
             Poll::Ready(Some(()))
         } else {
-            embassy_time_queue_driver::schedule_wake(self.expires_at.as_ticks(), cx.waker());
+            embassy_time_driver::schedule_wake(self.expires_at.as_ticks(), cx.waker());
             Poll::Pending
         }
     }