1 files changed, 44 insertions, 15 deletions

diff --git a/tasking.cpp b/tasking.cpp
index 9e2cd49..5cfe091 100644
--- a/tasking.cpp
+++ b/tasking.cpp
@@ -4,31 +4,56 @@
 
 struct Task
 {
-    Registers regs;
-    bool valid = false;
+    enum class State {
+        Invalid,
+        Staging,
+        Running
+    };
+
+    using enum State;
+
+    std::uint32_t esp;
+    std::uint32_t ebp;
+    State state = State::Invalid;
 };
 
 static std::array<Task, 4> tasks;
 static int current = -1;
 
-void schedule(Registers& regs)
+void schedule(const Registers& regs)
 {
     if (current < 0)
         return;
 
-    tasks[current].regs = regs;
+    asm volatile(R"(
+        mov %%esp, %0
+        mov %%ebp, %1
+    )" : "=m" (tasks[current].esp), "=m" (tasks[current].ebp));
 
     do {
         if (++current >= tasks.size())
             current = 0;
-    } while (!tasks[current].valid);
+    } while (tasks[current].state == Task::Invalid);
 
-    regs = tasks[current].regs;
+    asm volatile(R"(
+        mov %0, %%esp
+        mov %1, %%ebp
+    )" :: "m" (tasks[current].esp), "m" (tasks[current].ebp));
+
+    if (tasks[current].state == Task::Staging) {
+        tasks[current].state = Task::Running;
+        asm volatile(R"(
+            pop %eax
+            popa
+            add $0x4, %esp
+            iret
+        )");
+    }
 }
 
 void tasking_initialize()
 {
-    tasks[0].valid = true;
+    tasks[0].state = Task::Running;
     current = 0;
     asm volatile("int $0x20");
 }
@@ -37,7 +62,7 @@ bool tasking_spawn(void (*entry)(), unsigned ssize)
 {
     int i = -1;
     for (i = 0; i < tasks.size(); ++i) {
-        if (!tasks[i].valid)
+        if (tasks[i].state == Task::Invalid)
             break;
     }
 
@@ -46,15 +71,19 @@ bool tasking_spawn(void (*entry)(), unsigned ssize)
 
     tasks[i] = Task();
 
-    auto& r = tasks[i].regs;
     auto stack = reinterpret_cast<std::uint32_t>(new std::uint8_t[ssize]);
-    r.ebp = stack + ssize;
-    r.esp = r.ebp;
-    r.eip = reinterpret_cast<std::uint32_t>(entry);
-    r.cs = 0x8;
-    r.eflags = tasks[current].regs.eflags;
+    const auto stackend = stack + ssize;
+    const auto regbase = stackend - sizeof(Registers);
+    auto r = reinterpret_cast<Registers *>(regbase);
+    r->ebp = stackend;
+    r->esp = stackend;
+    r->eip = reinterpret_cast<std::uint32_t>(entry);
+    r->cs = 0x8;
+    asm volatile("pushfl; pop %%eax" : "=a"(r->eflags));
 
-    tasks[i].valid = true;
+    tasks[i].esp = regbase;
+    tasks[i].ebp = stackend;
+    tasks[i].state = Task::Staging;
     return true;
 }