Move integrated timer queue into time-queue-driver

2 files changed, 85 insertions, 4 deletions

diff --git a/embassy-time-queue-driver/src/lib.rs b/embassy-time-queue-driver/src/lib.rs
index c5e989854..0c78921ed 100644
--- a/embassy-time-queue-driver/src/lib.rs
+++ b/embassy-time-queue-driver/src/lib.rs
@@ -22,9 +22,9 @@
 //! );
 //! ```
 //!
-//! You can also use the `queue_generic` or the `embassy_executor::raw::timer_queue` modules to
-//! implement your own timer queue. These modules contain queue implementations which you can wrap
-//! and tailor to your needs.
+//! You can also use the `queue_generic` or the `queue_integrated` modules to implement your own
+//! timer queue. These modules contain queue implementations which you can wrap and tailor to
+//! your needs.
 //!
 //! If you are providing an embassy-executor implementation besides a timer queue, you can choose to
 //! expose the `integrated-timers` feature in your implementation. This feature stores timer items
@@ -49,7 +49,10 @@
 //! embassy_time_queue_driver::timer_queue_impl!(static QUEUE: MyTimerQueue = MyTimerQueue{});
 //! ```
 
+#[cfg(not(feature = "integrated-timers"))]
 pub mod queue_generic;
+#[cfg(feature = "integrated-timers")]
+pub mod queue_integrated;
 
 use core::cell::RefCell;
 use core::task::Waker;
@@ -89,7 +92,7 @@ macro_rules! timer_queue_impl {
 }
 
 #[cfg(feature = "integrated-timers")]
-type InnerQueue = embassy_executor::raw::timer_queue::TimerQueue;
+type InnerQueue = queue_integrated::TimerQueue;
 
 #[cfg(not(feature = "integrated-timers"))]
 type InnerQueue = queue_generic::Queue;
diff --git a/embassy-time-queue-driver/src/queue_integrated.rs b/embassy-time-queue-driver/src/queue_integrated.rs
new file mode 100644
index 000000000..cb0f79356
--- /dev/null
+++ b/embassy-time-queue-driver/src/queue_integrated.rs
@@ -0,0 +1,78 @@
+//! Timer queue operations.
+use core::cell::Cell;
+use core::cmp::min;
+
+use embassy_executor::raw::TaskRef;
+
+/// A timer queue, with items integrated into tasks.
+pub struct TimerQueue {
+    head: Cell<Option<TaskRef>>,
+}
+
+impl TimerQueue {
+    /// Creates a new timer queue.
+    pub const fn new() -> Self {
+        Self { head: Cell::new(None) }
+    }
+
+    /// Schedules a task to run at a specific time.
+    ///
+    /// If this function returns `true`, the called should find the next expiration time and set
+    /// a new alarm for that time.
+    pub fn schedule_wake(&mut self, at: u64, p: TaskRef) -> bool {
+        let item = p.timer_queue_item();
+        if item.next.get().is_none() {
+            // If not in the queue, add it and update.
+            let prev = self.head.replace(Some(p));
+            item.next.set(prev);
+        } else if at <= item.expires_at.get() {
+            // If expiration is sooner than previously set, update.
+        } else {
+            // Task does not need to be updated.
+            return false;
+        }
+
+        item.expires_at.set(at);
+        true
+    }
+
+    /// Dequeues expired timers and returns the next alarm time.
+    ///
+    /// The provided callback will be called for each expired task. Tasks that never expire
+    /// will be removed, but the callback will not be called.
+    pub fn next_expiration(&mut self, now: u64) -> u64 {
+        let mut next_expiration = u64::MAX;
+
+        self.retain(|p| {
+            let item = p.timer_queue_item();
+            let expires = item.expires_at.get();
+
+            if expires <= now {
+                // Timer expired, process task.
+                embassy_executor::raw::wake_task(p);
+                false
+            } else {
+                // Timer didn't yet expire, or never expires.
+                next_expiration = min(next_expiration, expires);
+                expires != u64::MAX
+            }
+        });
+
+        next_expiration
+    }
+
+    fn retain(&self, mut f: impl FnMut(TaskRef) -> bool) {
+        let mut prev = &self.head;
+        while let Some(p) = prev.get() {
+            let item = p.timer_queue_item();
+            if f(p) {
+                // Skip to next
+                prev = &item.next;
+            } else {
+                // Remove it
+                prev.set(item.next.get());
+                item.next.set(None);
+            }
+        }
+    }
+}