2 files changed, 67 insertions, 1 deletions
diff --git a/sys/arch/amd64/amd64/lapic_intr.S b/sys/arch/amd64/amd64/lapic_intr.S
index 5ae8f39..1413660 100644
--- a/sys/arch/amd64/amd64/lapic_intr.S
+++ b/sys/arch/amd64/amd64/lapic_intr.S
@@ -33,6 +33,6 @@
     .globl lapic_tmr_isr
 INTRENTRY(lapic_tmr_isr, handle_lapic_tmr)
 handle_lapic_tmr:
-    call sched_switch       // Context switch per every timer IRQ
+    call md_sched_switch    // Context switch per every timer IRQ
     call lapic_eoi          // Done! Signal that we finished to the Local APIC
     retq
diff --git a/sys/arch/amd64/amd64/proc_machdep.c b/sys/arch/amd64/amd64/proc_machdep.c
index 63604a4..72c2b56 100644
--- a/sys/arch/amd64/amd64/proc_machdep.c
+++ b/sys/arch/amd64/amd64/proc_machdep.c
@@ -32,6 +32,8 @@
 #include <sys/param.h>
 #include <sys/errno.h>
 #include <sys/exec.h>
+#include <sys/sched.h>
+#include <sys/schedvar.h>
 #include <machine/frame.h>
 #include <machine/gdt.h>
 #include <machine/cpu.h>
@@ -220,3 +222,67 @@ md_spawn(struct proc *p, struct proc *parent, uintptr_t ip)
     tfp->rsp = ALIGN_DOWN((stack_base + PROC_STACK_SIZE) - 1, 16);
     return 0;
 }
+
+/*
+ * Save thread state and enqueue it back into one
+ * of the ready queues.
+ */
+static void
+sched_save_td(struct proc *td, struct trapframe *tf)
+{
+    /*
+     * Save trapframe to process structure only
+     * if PROC_EXEC is not set.
+     */
+    if (!ISSET(td->flags, PROC_EXEC)) {
+        memcpy(&td->tf, tf, sizeof(td->tf));
+    }
+
+    sched_enqueue_td(td);
+}
+
+static void
+sched_switch_to(struct trapframe *tf, struct proc *td)
+{
+    struct cpu_info *ci;
+    struct pcb *pcbp;
+
+    ci = this_cpu();
+
+    if (tf != NULL) {
+        memcpy(tf, &td->tf, sizeof(*tf));
+    }
+
+    ci->curtd = td;
+    pcbp = &td->pcb;
+    pmap_switch_vas(pcbp->addrsp);
+}
+
+/*
+ * Perform a context switch.
+ */
+void
+md_sched_switch(struct trapframe *tf)
+{
+    struct proc *next_td, *td;
+    struct cpu_info *ci;
+
+    ci = this_cpu();
+    td = ci->curtd;
+    mi_sched_switch(td);
+
+    if (td != NULL) {
+        if (td->pid == 0)
+            return;
+
+        sched_save_td(td, tf);
+    }
+
+    if ((next_td = sched_dequeue_td()) == NULL) {
+        sched_oneshot(false);
+        return;
+    }
+
+    sched_switch_to(tf, next_td);
+    sched_oneshot(false);
+}