3 files changed, 67 insertions, 8 deletions

diff --git a/sys/arch/amd64/cpu/idt.S b/sys/arch/amd64/cpu/idt.S
index 729d958..cf31fbb 100644
--- a/sys/arch/amd64/cpu/idt.S
+++ b/sys/arch/amd64/cpu/idt.S
@@ -309,9 +309,12 @@ page_fault:
     .globl lapic_tmr_isr
 lapic_tmr_isr:
     KFENCE
+    subq $8, %rsp
     push_frame 0x81
-    nop
+    mov %rsp, %rdi
+    call mu_process_switch
     pop_frame 0x81
+    add $8, %rsp
     KFENCE
     iretq
 
diff --git a/sys/arch/amd64/cpu/mp.c b/sys/arch/amd64/cpu/mp.c
index 6911a49..ea36829 100644
--- a/sys/arch/amd64/cpu/mp.c
+++ b/sys/arch/amd64/cpu/mp.c
@@ -40,6 +40,7 @@
 #include <md/lapic.h>
 #include <md/msr.h>
 #include <md/cpu.h>
+#include <md/gdt.h>
 #include <mu/cpu.h>
 #include <os/process.h>
 #include <os/sched.h>
@@ -116,10 +117,11 @@ static volatile uint32_t aps_up = 0;
 __section(".trampoline") static char ap_code[4096];
 
 static void
-cpu_idle(void)
+cpu_idle(struct mcb *mcb)
 {
+    lapic_oneshot_usec(mcb, SCHED_QUANTUM);
     for (;;) {
-        __asmv("hlt");
+        __asmv("sti; hlt");
     }
 }
 
@@ -308,7 +310,7 @@ cpu_lm_entry(void)
     );
 
     idt_load();
-    cpu_idle();
+    cpu_idle(&ci->mcb);
     __builtin_unreachable();
 }
 
@@ -391,9 +393,10 @@ static void
 cpu_start_idle(void)
 {
     struct process *p;
+    struct cpu_info *core;
     int error;
 
-    for (size_t i = 0; i < ap_count; ++i) {
+    for (size_t i = 0; i < ap_count + 1; ++i) {
         p = kalloc(sizeof(*p));
         if (p == NULL) {
             panic("mp: could not allocate idle thread\n");
@@ -405,7 +408,8 @@ cpu_start_idle(void)
             panic("mp: could not initialize process\n");
         }
 
-        sched_enqueue_proc(p);
+        core = sched_enqueue_proc(p);
+        core->curproc = NULL;
     }
 }
 
@@ -451,6 +455,7 @@ cpu_start_aps(struct cpu_info *ci)
     /* Copy the bring up code to the BUA */
     bua = AP_BUA_VADDR;
     memcpy(bua, ap_code, AP_BUA_LEN);
+    cpu_start_idle();
 
     /* Start up the APs */
     mcb = &self->mcb;
@@ -471,6 +476,4 @@ cpu_start_aps(struct cpu_info *ci)
     } else {
         dtrace("%d processor(s) up\n", aps_up);
     }
-
-    cpu_start_idle();
 }
diff --git a/sys/arch/amd64/os/os_process.c b/sys/arch/amd64/os/os_process.c
index 21bdd86..60bdcb0 100644
--- a/sys/arch/amd64/os/os_process.c
+++ b/sys/arch/amd64/os/os_process.c
@@ -30,11 +30,64 @@
 #include <sys/errno.h>
 #include <mu/process.h>
 #include <mu/mmu.h>
+#include <mu/cpu.h>
+#include <md/gdt.h>
+#include <md/lapic.h>
+#include <os/process.h>
+#include <os/sched.h>
 #include <vm/phys.h>
+#include <vm/vm.h>
 #include <lib/string.h>
 
 #define STACK_TOP 0xBFFFFFFF
 
+static void
+sched_enter(struct cpu_info *ci)
+{
+    lapic_oneshot_usec(&ci->mcb, SCHED_QUANTUM);
+    for (;;) {
+        __asmv("sti; hlt");
+    }
+}
+
+void
+mu_process_switch(struct trapframe *tf)
+{
+    struct cpu_info *ci = cpu_self();
+    struct process *self, *next;
+    struct pcb *pcb;
+
+    if (ci == NULL) {
+        goto done;
+    }
+
+    if ((self = ci->curproc) == NULL) {
+        ci->curproc = sched_dequeue_proc();
+        goto done;
+    }
+
+    /* Save current process */
+    sched_enqueue_proc(self);
+    pcb = &self->pcb;
+    memcpy(&pcb->tf, tf, sizeof(pcb->tf));
+
+    /* Get the next process */
+    next = sched_dequeue_proc();
+    if (next == NULL) {
+        sched_enter(ci);
+    }
+
+    /* Switch to the next process */
+    pcb = &next->pcb;
+    memcpy(tf, &pcb->tf, sizeof(*tf));
+    ci->curproc = next;
+
+    /* Switch address space and go */
+    mu_pmap_writevas(&pcb->vas);
+done:
+    lapic_eoi(&ci->mcb);
+    lapic_oneshot_usec(&ci->mcb, SCHED_QUANTUM);
+}
 
 int
 mu_process_init(struct process *process, uintptr_t ip, int flags)