Replace sorted_list/vector with callback_list doubly-linked list for timers and frame callbacks

firmware/include/callback_list.h

@@ -0,0 +1,87 @@
+#pragma once
+#include <cstdint>
+#include <new>
+#include <utility>
+
+template <typename T, int N>
+struct callback_list {
+    struct node {
+        alignas(T) uint8_t storage[sizeof(T)];
+        node* prev = nullptr;
+        node* next = nullptr;
+        bool active = false;
+
+        T& value() { return *reinterpret_cast<T*>(storage); }
+        const T& value() const { return *reinterpret_cast<const T*>(storage); }
+    };
+
+    node nodes[N];
+    node* free_head = &nodes[0];
+    node sentinel;
+
+    callback_list() {
+        for (int i = 0; i < N - 1; i++) nodes[i].next = &nodes[i + 1];
+        nodes[N - 1].next = nullptr;
+        sentinel.prev = &sentinel;
+        sentinel.next = &sentinel;
+    }
+
+    bool empty() const { return sentinel.next == &sentinel; }
+
+    node* insert(T value) {
+        if (!free_head) return nullptr;
+        node* n = alloc(std::move(value));
+        link_before(&sentinel, n);
+        return n;
+    }
+
+    template <typename Less>
+    node* insert_sorted(T value, Less&& less) {
+        if (!free_head) return nullptr;
+        node* n = alloc(std::move(value));
+        node* pos = sentinel.next;
+        while (pos != &sentinel && !less(n->value(), pos->value()))
+            pos = pos->next;
+        link_before(pos, n);
+        return n;
+    }
+
+    void remove(node* n) {
+        if (!n || !n->active) return;
+        n->prev->next = n->next;
+        n->next->prev = n->prev;
+        n->active = false;
+        n->value().~T();
+        n->next = free_head;
+        n->prev = nullptr;
+        free_head = n;
+    }
+
+    node* front() { return sentinel.next != &sentinel ? sentinel.next : nullptr; }
+
+    template <typename Fn>
+    void for_each(Fn&& fn) {
+        node* cur = sentinel.next;
+        while (cur != &sentinel) {
+            node* next = cur->next;
+            fn(cur);
+            cur = next;
+        }
+    }
+
+private:
+    node* alloc(T value) {
+        node* n = free_head;
+        free_head = n->next;
+        new (n->storage) T(std::move(value));
+        n->active = true;
+        return n;
+    }
+
+    void link_before(node* pos, node* n) {
+        n->prev = pos->prev;
+        n->next = pos;
+        pos->prev->next = n;
+        pos->prev = n;
+    }
+};

firmware/include/net.h

@@ -6,6 +6,7 @@
 #include "ipv4.h"
 #include "span_writer.h"
 #include "msgpack.h"
+#include "callback_list.h"
 
 struct net_state {
     eth::mac_addr mac;
@@ -15,11 +16,19 @@ struct net_state {
 using net_handler = std::function<void(std::span<const uint8_t> payload,
                                        std::function<void(std::span<const uint8_t>)> send)>;
 
-using net_frame_callback = std::function<void(std::span<const uint8_t> frame)>;
+using net_frame_callback = std::function<bool(std::span<const uint8_t> frame)>;
+
+struct frame_callback_entry {
+    net_frame_callback fn;
+};
+
+using frame_cb_list = callback_list<frame_callback_entry, 16>;
+using frame_cb_handle = frame_cb_list::node*;
 
 bool net_init();
 const net_state& net_get_state();
 void net_set_handler(net_handler handler);
-void net_add_frame_callback(net_frame_callback cb);
+frame_cb_handle net_add_frame_callback(net_frame_callback cb);
+void net_remove_frame_callback(frame_cb_handle h);
 void net_poll(std::span<uint8_t> tx);
 void net_send_raw(std::span<const uint8_t> data);

firmware/include/timer_queue.h

@@ -1,26 +1,25 @@
 #pragma once
 #include <functional>
 #include "pico/time.h"
-#include "sorted_list.h"
+#include "callback_list.h"
 
 struct timer_entry {
     absolute_time_t when;
     std::function<void()> fn;
 };
 
-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*;
+using timer_handle = callback_list<timer_entry, 16>::node*;
 
 struct timer_queue {
-    sorted_list<timer_entry, 16> queue;
+    callback_list<timer_entry, 16> list;
     alarm_id_t alarm = -1;
     volatile bool irq_pending = false;
 
     timer_handle schedule(absolute_time_t when, std::function<void()> fn) {
-        auto* n = queue.insert({when, std::move(fn)});
+        auto* n = list.insert_sorted({when, std::move(fn)},
+            [](const timer_entry& a, const timer_entry& b) {
+                return absolute_time_diff_us(b.when, a.when) < 0;
+            });
         arm();
         return n;
     }
@@ -30,25 +29,25 @@ struct timer_queue {
     }
 
     bool cancel(timer_handle h) {
-        bool removed = queue.remove(h);
-        if (removed) arm();
-        return removed;
+        if (!h || !h->active) return false;
+        list.remove(h);
+        arm();
+        return true;
     }
 
     void run() {
         if (!irq_pending) return;
         irq_pending = false;
-        while (!queue.empty()) {
-            auto& front = queue.front();
-            if (absolute_time_diff_us(get_absolute_time(), front.when) > 0) break;
-            auto fn = std::move(front.fn);
-            queue.pop_front();
+        while (auto* n = list.front()) {
+            if (absolute_time_diff_us(get_absolute_time(), n->value().when) > 0) break;
+            auto fn = std::move(n->value().fn);
+            list.remove(n);
             fn();
         }
         arm();
     }
 
-    bool empty() const { return queue.empty(); }
+    bool empty() const { return list.empty(); }
 
 private:
     static int64_t alarm_cb(alarm_id_t, void* user_data) {
@@ -59,7 +58,7 @@ private:
     void arm() {
         if (alarm >= 0) cancel_alarm(alarm);
         alarm = -1;
-        if (!queue.empty())
-            alarm = add_alarm_at(queue.front().when, alarm_cb, this, false);
+        if (auto* n = list.front())
+            alarm = add_alarm_at(n->value().when, alarm_cb, this, false);
     }
 };