From d6c95a111c0950757d75496af254e3427e3769b6 Mon Sep 17 00:00:00 2001
From: Vegard Storheil Eriksen <zyp@jvnv.net>
Date: Sat, 16 Apr 2022 20:59:36 +0200
Subject: async: Add preliminary time scheduler.

---
 async/scheduler.h           | 43 +++++++++++++++++++++++++
 async/time_scheduler.h      | 76 +++++++++++++++++++++++++++++++++++++++++++++
 cortex_m/critical_section.h | 21 +++++++++++++
 3 files changed, 140 insertions(+)
 create mode 100644 async/time_scheduler.h
 create mode 100644 cortex_m/critical_section.h

diff --git a/async/scheduler.h b/async/scheduler.h
index 0196aa5..bae4890 100644
--- a/async/scheduler.h
+++ b/async/scheduler.h
@@ -1,6 +1,9 @@
 #pragma once
 
 #include <coroutine>
+#include <optional>
+
+#include <cortex_m/critical_section.h>
 
 struct schedulable {
     schedulable* next = nullptr;
@@ -161,3 +164,43 @@ struct task : public schedulable {
 
     task(std::coroutine_handle<promise_type> h) : schedulable(h) {}
 };
+
+struct async_flag : public schedulable {
+    bool ready;
+
+    async_flag() : ready(false) {}
+
+    bool await_ready() {
+        return ready;
+    }
+
+    bool await_suspend(std::coroutine_handle<> h) {
+        critical_section lock;
+
+        if(ready) {
+            return false;
+        } else {
+            awaiter = h;
+            return true;
+        }
+    }
+
+    void await_resume() {
+        critical_section lock;
+
+        awaiter = nullptr;
+        ready = false;
+    }
+
+    void set() {
+        if(ready) {
+            return;
+        }
+
+        ready = true;
+
+        if(awaiter) {
+            scheduler.schedule(*this);
+        }
+    }
+};
diff --git a/async/time_scheduler.h b/async/time_scheduler.h
new file mode 100644
index 0000000..e09de16
--- /dev/null
+++ b/async/time_scheduler.h
@@ -0,0 +1,76 @@
+#pragma once
+
+#include <chrono>
+
+#include "scheduler.h"
+
+template <typename Duration = std::chrono::milliseconds>
+struct time_scheduler_t {
+    struct wakeup_future : public schedulable {
+        time_scheduler_t& tsched;
+        Duration time;
+        wakeup_future(time_scheduler_t& tsched, Duration time) : tsched(tsched), time(time) {}
+
+        bool await_ready() {
+            return tsched.now >= time;
+        }
+
+        bool await_suspend(std::coroutine_handle<> h) {
+            if(tsched.now >= time) {
+                return false;
+            } else {
+                awaiter = h;
+                tsched.push_wakeup(this);
+                return true;
+            }
+        }
+
+        void await_resume() {}
+
+        void resume() {
+            scheduler.schedule(*this);
+        }
+    };
+
+    Duration now;
+
+    wakeup_future* wakeup_queue;
+
+    void push_wakeup(wakeup_future* fut) {
+        wakeup_future** p = &wakeup_queue;
+
+        while((*p) && (*p)->time <= fut->time) {
+            p = reinterpret_cast<wakeup_future**>(&(*p)->next);
+        }
+
+        fut->next = *p;
+        *p = fut;
+    }
+
+    async_flag tick_flag;
+
+    void tick() {
+        tick_flag.set();
+    }
+
+    task wakeup_task() {
+        while(1) {
+            co_await tick_flag;
+            now++;
+
+            while(wakeup_queue && wakeup_queue->time <= now) {
+                auto fut = wakeup_queue;
+                wakeup_queue = static_cast<wakeup_future*>(fut->next);
+                fut->resume();
+            }
+        }
+    }
+
+    wakeup_future sleep_until(Duration time) {
+        return {*this, time};
+    }
+
+    wakeup_future sleep(Duration duration) {
+        return sleep_until(now + duration);
+    }
+};
diff --git a/cortex_m/critical_section.h b/cortex_m/critical_section.h
new file mode 100644
index 0000000..6b611a8
--- /dev/null
+++ b/cortex_m/critical_section.h
@@ -0,0 +1,21 @@
+#pragma once
+
+#include <cstdint>
+
+struct critical_section {
+    uint32_t primask;
+
+    critical_section() {
+        asm volatile("mrs %0, primask" : "=r" (primask));
+
+        asm volatile("cpsid i");
+
+        asm volatile("dmb");
+    }
+
+    ~critical_section() {
+        asm volatile("dmb");
+
+        asm volatile("msr primask, %0" :: "r" (primask));
+    }
+};
-- 
cgit v1.2.3