Cancellable timers: sorted_list::remove, timer cancel, replace done flag with timer cancellation

firmware/include/dispatch.h

@@ -5,6 +5,7 @@
 #include <optional>
 #include <span>
 #include "wire.h"
+#include "timer_queue.h"
 
 struct responder {
     uint32_t message_id;
@@ -45,5 +46,6 @@ void typed_handler(responder resp, std::span<const uint8_t> payload) {
 }
 
 void dispatch_init();
-void dispatch_schedule_ms(uint32_t ms, std::function<void()> fn);
+timer_handle dispatch_schedule_ms(uint32_t ms, std::function<void()> fn);
+bool dispatch_cancel_timer(timer_handle h);
 [[noreturn]] void dispatch_run(std::span<const handler_entry> handlers);

firmware/include/sorted_list.h

@@ -28,8 +28,8 @@ struct sorted_list {
     T& front() { return head->value(); }
     const T& front() const { return head->value(); }
 
-    void insert(T value) {
-        if (full()) return;
+    node* insert(T value) {
+        if (full()) return nullptr;
         node* n = free_head;
         free_head = n->next;
         new (n->storage) T(std::move(value));
@@ -37,7 +37,7 @@ struct sorted_list {
         if (!head || n->value() < head->value()) {
             n->next = head;
             head = n;
-            return;
+            return n;
         }
 
         node* cur = head;
@@ -45,6 +45,7 @@ struct sorted_list {
             cur = cur->next;
         n->next = cur->next;
         cur->next = n;
+        return n;
     }
 
     void pop_front() {
@@ -55,4 +56,21 @@ struct sorted_list {
         n->next = free_head;
         free_head = n;
     }
+
+    bool remove(node* target) {
+        if (!target || empty()) return false;
+        if (head == target) {
+            pop_front();
+            return true;
+        }
+        node* cur = head;
+        while (cur->next && cur->next != target)
+            cur = cur->next;
+        if (cur->next != target) return false;
+        cur->next = target->next;
+        target->value().~T();
+        target->next = free_head;
+        free_head = target;
+        return true;
+    }
 };

firmware/include/timer_queue.h

@@ -12,18 +12,27 @@ inline bool operator<(const timer_entry& a, const timer_entry& b) {
     return absolute_time_diff_us(b.when, a.when) < 0;
 }
 
+using timer_handle = sorted_list<timer_entry, 16>::node*;
+
 struct timer_queue {
     sorted_list<timer_entry, 16> queue;
     alarm_id_t alarm = -1;
     volatile bool irq_pending = false;
 
-    void schedule(absolute_time_t when, std::function<void()> fn) {
-        queue.insert({when, std::move(fn)});
+    timer_handle schedule(absolute_time_t when, std::function<void()> fn) {
+        auto* n = queue.insert({when, std::move(fn)});
         arm();
+        return n;
     }
 
-    void schedule_ms(uint32_t ms, std::function<void()> fn) {
-        schedule(make_timeout_time_ms(ms), std::move(fn));
+    timer_handle schedule_ms(uint32_t ms, std::function<void()> fn) {
+        return schedule(make_timeout_time_ms(ms), std::move(fn));
+    }
+
+    bool cancel(timer_handle h) {
+        bool removed = queue.remove(h);
+        if (removed) arm();
+        return removed;
     }
 
     void run() {