4 files changed, 132 insertions, 5 deletions
diff --git a/sys/include/arch/amd64/pcb.h b/sys/include/arch/amd64/pcb.h
new file mode 100644
index 0000000..5d06ade
--- /dev/null
+++ b/sys/include/arch/amd64/pcb.h
@@ -0,0 +1,40 @@
+/*
+ * Copyright (c) 2023-2024 Ian Marco Moffett and the Osmora Team.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. Neither the name of Hyra nor the names of its
+ *    contributors may be used to endorse or promote products derived from
+ *    this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef _MACHINE_PCB_H_
+#define _MACHINE_PCB_H_
+
+#include <sys/types.h>
+#include <vm/pmap.h>
+
+struct pcb {
+    struct vas addrsp;
+};
+
+#endif  /* !_MACHINE_PCB_H_ */
diff --git a/sys/include/sys/proc.h b/sys/include/sys/proc.h
index ba47a5c..b74cb00 100644
--- a/sys/include/sys/proc.h
+++ b/sys/include/sys/proc.h
@@ -32,17 +32,24 @@
 
 #include <sys/types.h>
 #include <sys/spinlock.h>
+#include <sys/queue.h>
 #if defined(_KERNEL)
-#include <machine/cpu.h>
 #include <machine/frame.h>
+#include <machine/pcb.h>
 #endif  /* _KERNEL */
 
 #if defined(_KERNEL)
 
 struct proc {
     pid_t pid;
-    struct cpu_info *cpu;
+    struct trapframe tf;
+    struct pcb pcb;
+    size_t priority;
+    uintptr_t stack_base;
+    TAILQ_ENTRY(proc) link;
 };
 
+struct proc *this_td(void);
+
 #endif  /* _KERNEL */
 #endif  /* !_SYS_PROC_H_ */
diff --git a/sys/include/sys/sched.h b/sys/include/sys/sched.h
index 33d546d..ae59d5f 100644
--- a/sys/include/sys/sched.h
+++ b/sys/include/sys/sched.h
@@ -30,10 +30,13 @@
 #ifndef _SYS_SCHED_H_
 #define _SYS_SCHED_H_
 
+#include <sys/proc.h>
+
 #if defined(_KERNEL)
 
 void sched_init(void);
 void sched_enter(void);
+void sched_enqueue_td(struct proc *td);
 
 #endif  /* _KERNEL */
 #endif  /* !_SYS_SCHED_H_ */
diff --git a/sys/kern/kern_sched.c b/sys/kern/kern_sched.c
index b79d682..c370311 100644
--- a/sys/kern/kern_sched.c
+++ b/sys/kern/kern_sched.c
@@ -33,8 +33,11 @@
 #include <sys/cdefs.h>
 #include <sys/syslog.h>
 #include <machine/frame.h>
+#include <machine/cpu.h>
+#include <vm/pmap.h>
 #include <dev/timer.h>
 #include <assert.h>
+#include <string.h>
 
 #define pr_trace(fmt, ...) kprintf("ksched: " fmt, ##__VA_ARGS__)
 
@@ -49,6 +52,12 @@ static sched_policy_t policy = SCHED_POLICY_RR;
 static struct sched_queue qlist[SCHED_NQUEUE];
 
 /*
+ * Thread queue lock - all operations to `qlist'
+ * must be done with this lock acquired.
+ */
+__cacheline_aligned static struct spinlock tdq_lock = {0};
+
+/*
  * Perform timer oneshot
  */
 static inline void
@@ -64,6 +73,54 @@ sched_oneshot(bool now)
     timer.oneshot_us(usec);
 }
 
+static struct proc *
+sched_dequeue_td(void)
+{
+    struct sched_queue *queue;
+    struct proc *td = NULL;
+
+    spinlock_acquire(&tdq_lock);
+
+    for (size_t i = 0; i < SCHED_NQUEUE; ++i) {
+        queue = &qlist[i];
+        if (!TAILQ_EMPTY(&queue->q)) {
+            td = TAILQ_FIRST(&queue->q);
+            TAILQ_REMOVE(&queue->q, td, link);
+            break;
+        }
+    }
+
+    spinlock_release(&tdq_lock);
+    return td;
+}
+
+/*
+ * Add a thread to the scheduler.
+ */
+void
+sched_enqueue_td(struct proc *td)
+{
+    struct sched_queue *queue;
+
+    spinlock_acquire(&tdq_lock);
+    queue = &qlist[td->priority];
+
+    TAILQ_INSERT_TAIL(&queue->q, td, link);
+    spinlock_release(&tdq_lock);
+}
+
+/*
+ * Return the currently running thread.
+ */
+struct proc *
+this_td(void)
+{
+    struct cpu_info *ci;
+
+    ci = this_cpu();
+    return ci->curtd;
+}
+
 /*
  * Perform a context switch.
  *
@@ -72,10 +129,30 @@ sched_oneshot(bool now)
 void
 sched_switch(struct trapframe *tf)
 {
-    static struct spinlock lock = {0};
+    struct cpu_info *ci;
+    struct pcb *pcbp;
+    struct proc *next_td, *td;
+
+    ci = this_cpu();
+    td = ci->curtd;
+
+    /* Do we have threads to switch to? */
+    if ((next_td = sched_dequeue_td()) == NULL) {
+        sched_oneshot(false);
+        return;
+    }
+
+    /* Re-enqueue the old thread */
+    if (td != NULL) {
+        memcpy(&td->tf, tf, sizeof(td->tf));
+        sched_enqueue_td(td);
+    }
+
+    memcpy(tf, &next_td->tf, sizeof(*tf));
+    ci->curtd = next_td;
+    pcbp = &next_td->pcb;
 
-    spinlock_acquire(&lock);
-    spinlock_release(&lock);
+    pmap_switch_vas(pcbp->addrsp);
     sched_oneshot(false);
 }