initial import from picomap firmware

include/timer_queue.h

@@ -0,0 +1,63 @@
+#pragma once
+#include "pico/time.h"
+#include "callback_list.h"
+
+struct timer_entry {
+    absolute_time_t when;
+    void (*fn)() = nullptr;
+};
+
+using timer_handle = callback_list<timer_entry, 16>::node*;
+
+struct timer_queue {
+    callback_list<timer_entry, 16> list;
+    alarm_id_t alarm = -1;
+    volatile bool irq_pending = false;
+
+    timer_handle schedule(absolute_time_t when, void (*fn)()) {
+        auto* n = list.insert_sorted({when, fn},
+            [](const timer_entry& a, const timer_entry& b) {
+                return absolute_time_diff_us(b.when, a.when) < 0;
+            });
+        arm();
+        return n;
+    }
+
+    timer_handle schedule_ms(uint32_t ms, void (*fn)()) {
+        return schedule(make_timeout_time_ms(ms), fn);
+    }
+
+    bool cancel(timer_handle h) {
+        if (!h) return false;
+        list.remove(h);
+        arm();
+        return true;
+    }
+
+    void run() {
+        if (!irq_pending) return;
+        irq_pending = false;
+        while (auto* n = list.front()) {
+            if (absolute_time_diff_us(get_absolute_time(), n->value.when) > 0) break;
+            auto fn = n->value.fn;
+            list.remove(n);
+            fn();
+        }
+        arm();
+    }
+
+    bool empty() const { return list.empty(); }
+
+private:
+    static int64_t alarm_cb(alarm_id_t, void* user_data) {
+        static_cast<timer_queue*>(user_data)->irq_pending = true;
+        return 0;
+    }
+
+    void arm() {
+        if (alarm >= 0) cancel_alarm(alarm);
+        alarm = -1;
+        if (auto* n = list.front())
+            alarm = add_alarm_at(n->value.when, alarm_cb, this, false);
+    }
+};