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) {
+    node* insert(T value) {
-        if (full()) return;
+        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) {
+    timer_handle schedule(absolute_time_t when, std::function<void()> fn) {
-        queue.insert({when, std::move(fn)});
+        auto* n = queue.insert({when, std::move(fn)});
         arm();
+        return n;
     }
 
-    void schedule_ms(uint32_t ms, std::function<void()> fn) {
+    timer_handle schedule_ms(uint32_t ms, std::function<void()> fn) {
-        schedule(make_timeout_time_ms(ms), std::move(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() {

firmware/lib/dispatch.cpp

@@ -17,8 +17,12 @@ void dispatch_init() {
     dlog("dispatch_init complete");
 }
 
-void dispatch_schedule_ms(uint32_t ms, std::function<void()> fn) {
+timer_handle dispatch_schedule_ms(uint32_t ms, std::function<void()> fn) {
-    timers.schedule_ms(ms, std::move(fn));
+    return timers.schedule_ms(ms, std::move(fn));
+}
+
+bool dispatch_cancel_timer(timer_handle h) {
+    return timers.cancel(h);
 }
 
 [[noreturn]] void dispatch_run(std::span<const handler_entry> handlers) {

firmware/lib/test_handlers.cpp

@@ -30,27 +30,24 @@ static void test_ping(const responder& resp, ipv4::ip4_addr dst_ip) {
 
     ipv4::ip4_addr our_ip = ns.ip;
 
-    auto done = std::make_shared<bool>(false);
+    auto timer = std::make_shared<timer_handle>(nullptr);
     auto cb = std::make_shared<std::function<void(std::span<const uint8_t>)>>();
-    *cb = [resp, ping_id, our_ip, done, cb](std::span<const uint8_t> frame) {
+    *cb = [resp, ping_id, our_ip, timer, cb](std::span<const uint8_t> frame) {
-        if (*done) return;
         ipv4::ip4_addr src_ip;
         if (!icmp::parse_echo_reply(frame, src_ip, ping_id)) {
             net_add_frame_callback(*cb);
             return;
         }
+        dispatch_cancel_timer(*timer);
         if (src_ip == our_ip) {
-            net_add_frame_callback(*cb);
+            resp.respond(ResponseTest{false, {"got reply from self: " + ipv4::to_string(src_ip)}});
             return;
         }
-        *done = true;
         resp.respond(ResponseTest{true, {"reply from " + ipv4::to_string(src_ip)}});
     };
     net_add_frame_callback(*cb);
 
-    dispatch_schedule_ms(5000, [resp, done]() {
+    *timer = dispatch_schedule_ms(5000, [resp]() {
-        if (*done) return;
-        *done = true;
         resp.respond(ResponseTest{false, {"no reply from non-self host within 5s"}});
     });
 }