From f9da6271cea7035b2c9f27cfe479aa81889168d1 Mon Sep 17 00:00:00 2001
From: Dario Nieuwenhuis <dirbaio@dirbaio.net>
Date: Wed, 26 Oct 2022 21:00:50 +0200
Subject: [PATCH] time/generic_queue: use Vec instead of SortedLinkedList

---
 embassy-time/src/queue_generic.rs | 46 +++++++++++++++----------------
 1 file changed, 22 insertions(+), 24 deletions(-)

diff --git a/embassy-time/src/queue_generic.rs b/embassy-time/src/queue_generic.rs
index 8a355b32..20ae7e6c 100644
--- a/embassy-time/src/queue_generic.rs
+++ b/embassy-time/src/queue_generic.rs
@@ -4,7 +4,7 @@ use core::task::Waker;
 
 use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex;
 use embassy_sync::blocking_mutex::Mutex;
-use heapless::sorted_linked_list::{LinkedIndexU8, Min, SortedLinkedList};
+use heapless::Vec;
 
 use crate::driver::{allocate_alarm, set_alarm, set_alarm_callback, AlarmHandle};
 use crate::queue::TimerQueue;
@@ -56,18 +56,17 @@ impl Ord for Timer {
 }
 
 struct InnerQueue {
-    queue: SortedLinkedList<Timer, LinkedIndexU8, Min, { QUEUE_SIZE }>,
+    queue: Vec<Timer, QUEUE_SIZE>,
     alarm: AlarmHandle,
-    alarm_at: Instant,
 }
 
 impl InnerQueue {
     fn schedule_wake(&mut self, at: Instant, waker: &Waker) {
         self.queue
-            .find_mut(|timer| timer.waker.will_wake(waker))
+            .iter_mut()
+            .find(|timer| timer.waker.will_wake(waker))
             .map(|mut timer| {
                 timer.at = min(timer.at, at);
-                timer.finish();
             })
             .unwrap_or_else(|| {
                 let mut timer = Timer {
@@ -96,35 +95,35 @@ impl InnerQueue {
         loop {
             let now = Instant::now();
 
-            while self.queue.peek().filter(|timer| timer.at <= now).is_some() {
-                self.queue.pop().unwrap().waker.wake();
+            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() {
+            if self.update_alarm(next_alarm) {
                 break;
             }
         }
     }
 
-    fn update_alarm(&mut self) -> bool {
-        if let Some(timer) = self.queue.peek() {
-            let new_at = timer.at;
-
-            if self.alarm_at != new_at {
-                self.alarm_at = new_at;
-
-                return set_alarm(self.alarm, self.alarm_at.as_ticks());
-            }
+    fn update_alarm(&mut self, next_alarm: Instant) -> bool {
+        if next_alarm == Instant::MAX {
+            true
         } else {
-            self.alarm_at = Instant::MAX;
+            set_alarm(self.alarm, next_alarm.as_ticks())
         }
-
-        true
     }
 
     fn handle_alarm(&mut self) {
-        self.alarm_at = Instant::MAX;
-
         self.dispatch();
     }
 }
@@ -151,9 +150,8 @@ impl Queue {
                     let handle = unsafe { allocate_alarm() }.unwrap();
                     set_alarm_callback(handle, Self::handle_alarm_callback, self as *const _ as _);
                     InnerQueue {
-                        queue: SortedLinkedList::new_u8(),
+                        queue: Vec::new(),
                         alarm: handle,
-                        alarm_at: Instant::MAX,
                     }
                 })
                 .schedule_wake(at, waker)