ptos/amd64: cpu: Implement context switch groundwork

Signed-off-by: Chloe M. <chloe@mensia.org>

service/ptos/arch/amd64/cpu/vector.S

@@ -149,6 +149,5 @@ LapicTmrIsr:
     INTR_ENTRY($0x81)
     mov %rsp, %rdi
     call HalContextSwitch
-1:  cli
-    hlt
-    jmp 1b
+    call MdLapicSendEoi
+    INTR_EXIT()

service/ptos/arch/amd64/ps/context.c

@@ -7,10 +7,58 @@
  */
 
 #include <hal/context.h>
+#include <hal/kpcr.h>
+#include <hal/thread.h>
+#include <hal/mmu.h>
+#include <ps/process.h>
+#include <string.h>
+
 #include <ex/trace.h>
 
 VOID
 HalContextSwitch(TRAP_FRAME *Frame)
 {
-    TRACE("CONTEXT SWITCHING IS A TODO\n");
+    THREAD *ThisThread, *NextThread;
+    PROCESS *NextParent;
+    TCB *ThisTcb, *NextTcb;
+    KPCR *Kpcr;
+
+    Kpcr = HalKpcrCurrent();
+    ThisThread = Kpcr->CurrentThread;
+    NextThread = NULL;
+
+    /* If there is no thread, get one */
+    if (ThisThread == NULL) {
+        NextThread = KeSchedDequeue(&Kpcr->SchedQueue);
+        if (NextThread == NULL) {
+            HalPrimeThreadTimer(SCHED_MIN_QUANTUM);
+            return;
+        }
+    }
+
+    /* Nothing to do if there is no next thread */
+    if (NextThread == NULL) {
+        NextThread = KeSchedDequeue(&Kpcr->SchedQueue);
+        if (NextThread == NULL) {
+            HalPrimeThreadTimer(SCHED_MIN_QUANTUM);
+            return;
+        }
+    }
+
+    /* Save the current thread if we have one */
+    if (ThisThread != NULL) {
+        ThisTcb = &ThisThread->Tcb;
+        RtlMemCpy(&ThisTcb->FrameSnapshot, Frame, sizeof(*Frame));
+        KeSchedEnqueue(&Kpcr->SchedQueue, ThisThread);
+    }
+
+    /* Switch the address space */
+    NextParent = NextThread->Parent;
+    HalMmuWriteVas(&NextParent->Vas);
+
+    /* Context switch here */
+    Kpcr->CurrentThread = NextThread;
+    NextTcb = &NextThread->Tcb;
+    RtlMemCpy(Frame, &NextTcb->FrameSnapshot, sizeof(*Frame));
+    HalPrimeThreadTimer(SCHED_MIN_QUANTUM);
 }