4 files changed, 215 insertions, 0 deletions

diff --git a/sim/queue.h b/sim/queue.h
new file mode 100644
index 0000000..890beb6
--- /dev/null
+++ b/sim/queue.h
@@ -0,0 +1,87 @@
+#pragma once
+
+#include <cstdint>
+#include <iterator> // IWYU pragma: keep
+#include <map>
+#include <set>
+
+#include "sim/sim.h"
+#include "util/assert.h"
+
+namespace sim {
+
+    template<typename Element> struct Queue {
+        Scheduler &scheduler;
+
+        Schedulable *reader = nullptr;
+        std::set<Schedulable *> writers;
+
+        unsigned int min_latency;
+
+        std::multimap<std::uint64_t, Element> waiting;
+
+        Queue(Scheduler &scheduler, unsigned int min_latency)
+            : scheduler(scheduler)
+            , min_latency(min_latency)
+        { }
+
+        void add_reader(Schedulable *x)
+        {
+            ASSERT(&x->scheduler == &scheduler, "Reader associated with incorrect Scheduler");
+            ASSERT(!reader, "Already have a reader");
+            reader = x;
+            if (min_latency == 0) {
+                for (const auto &w : writers)
+                    scheduler.constrain(w, reader);
+            }
+        }
+
+        void add_writer(Schedulable *x)
+        {
+            ASSERT(&x->scheduler == &scheduler, "Writer associated with incorrect Scheduler");
+            writers.emplace(x);
+            if (min_latency == 0 && reader)
+                scheduler.constrain(x, reader);
+        }
+
+        void write(Element &&x)
+        {
+            write(std::move(x), min_latency);
+        }
+
+        void write(Element &&x, unsigned int latency)
+        {
+            ASSERT(latency >= min_latency, "Latency too low");
+            ASSERT(!scheduler.current_schedulable || writers.count(scheduler.current_schedulable), "Write lacks permission");
+            waiting.emplace(scheduler.now + min_latency, std::move(x));
+        }
+
+        unsigned int available()
+        {
+            ASSERT(!scheduler.current_schedulable || reader == scheduler.current_schedulable, "Read lacks permission");
+            unsigned int c = 0;
+            for (auto x = waiting.begin(); x != waiting.end() && x->first <= scheduler.now; ++x)
+                ++c;
+            return c;
+        }
+
+        Element &peek(unsigned int i=0)
+        {
+            ASSERT(i < available(), "Peek past available elements");
+            auto x = waiting.begin();
+            std::advance(x, i);
+            return x->second;
+        }
+
+        Element read(unsigned int i=0)
+        {
+            ASSERT(i < available(), "Read past available elements");
+            auto x = waiting.begin();
+            std::advance(x, i);
+            Element e{std::move(x->second)};
+            waiting.erase(x);
+            return e;
+        }
+    };
+
+}
diff --git a/sim/sim.cpp b/sim/sim.cpp
new file mode 100644
index 0000000..e8f84ee
--- /dev/null
+++ b/sim/sim.cpp
@@ -0,0 +1,83 @@
+#include <utility>
+
+#include "sim/sim.h"
+#include "util/assert.h"
+
+namespace sim {
+
+    void Scheduler::add_schedulable(Schedulable *schedulable)
+    {
+        unsorted_schedulables.emplace(schedulable);
+        sort_needed = true;
+    }
+
+    void Scheduler::remove_schedulable(Schedulable *schedulable)
+    {
+        unsorted_schedulables.erase(schedulable);
+        std::erase_if(constraints, [schedulable](const auto &item) { return item.first == schedulable || item.second == schedulable; });
+        sort_needed = true;
+    }
+
+    void Scheduler::constrain(Schedulable *prior, Schedulable *later)
+    {
+        ASSERT(unsorted_schedulables.count(prior), "Constraint prior is not associated with this Scheduler");
+        ASSERT(unsorted_schedulables.count(later), "Constraint later is not associated with this Scheduler");
+        constraints.emplace(later, prior);
+        sort_needed = true;
+    }
+
+    void Scheduler::topo_sort(std::set<Schedulable *> &live, std::set<Schedulable *> &waiting, Schedulable *candidate)
+    {
+        ASSERT(!live.count(candidate), "Dependency loop");
+        for (auto prereq = constraints.find(candidate); prereq != constraints.end() && prereq->first == candidate; ++prereq) {
+            if (prereq->second != candidate && waiting.count(prereq->second)) {
+                live.emplace(candidate);
+                topo_sort(live, waiting, prereq->second);
+            }
+        }
+        sorted_schedulables.emplace_back(candidate);
+        waiting.erase(candidate);
+    }
+
+    void Scheduler::sort()
+    {
+        sorted_schedulables.clear();
+        sorted_schedulables.reserve(unsorted_schedulables.size());
+        auto waiting = unsorted_schedulables;
+        while (!waiting.empty()) {
+            std::set<Schedulable *> live;
+            topo_sort(live, waiting, *waiting.begin());
+        }
+        ASSERT(sorted_schedulables.size() == unsorted_schedulables.size(), "Did not sort every schedulable");
+        sort_needed = false;
+    }
+
+    void Scheduler::clock()
+    {
+        if (sort_needed)
+            sort();
+        for (const auto &s : sorted_schedulables) {
+            current_schedulable = s;
+            s->clock();
+        }
+        current_schedulable = nullptr;
+        ++now;
+    }
+
+    Schedulable::Schedulable(Scheduler &scheduler)
+        : scheduler(scheduler)
+    {
+        scheduler.add_schedulable(this);
+    }
+
+    Schedulable::~Schedulable()
+    {
+        scheduler.remove_schedulable(this);
+    }
+
+    std::uint64_t Schedulable::now()
+    {
+        return scheduler.now;
+    }
+
+}
diff --git a/sim/sim.d b/sim/sim.d
new file mode 100644
index 0000000..d8953fb
--- /dev/null
+++ b/sim/sim.d
@@ -0,0 +1 @@
+sim_SODEPS += build/libutil.so
diff --git a/sim/sim.h b/sim/sim.h
new file mode 100644
index 0000000..399832a
--- /dev/null
+++ b/sim/sim.h
@@ -0,0 +1,44 @@
+#pragma once
+
+#include <cstdint>
+#include <map>
+#include <set>
+#include <vector>
+
+namespace sim {
+
+    struct Schedulable;
+
+    struct Scheduler {
+        std::set<Schedulable *> unsorted_schedulables;
+        std::vector<Schedulable *> sorted_schedulables;
+        bool sort_needed = false;
+
+        std::multimap<Schedulable *, Schedulable *> constraints;
+
+        Schedulable *current_schedulable = nullptr;
+        std::uint64_t now = 0;
+
+        void add_schedulable(Schedulable *schedulable);
+        void remove_schedulable(Schedulable *schedulable);
+
+        void constrain(Schedulable *prior, Schedulable *later);
+
+        void topo_sort(std::set<Schedulable *> &live, std::set<Schedulable *> &waiting, Schedulable *candidate);
+        void sort();
+
+        void clock();
+    };
+
+    struct Schedulable {
+        Scheduler &scheduler;
+
+        Schedulable(Scheduler &scheduler);
+        virtual ~Schedulable();
+
+        virtual void clock() = 0;
+
+        std::uint64_t now();
+    };
+
+}