From fd056425aef09bbc1cf94345c49890aed216d7d3 Mon Sep 17 00:00:00 2001
From: "Chloe M." <chloe@mensia.org>
Date: Tue, 14 Jul 2026 22:14:57 +0000
Subject: [PATCH] ptos/amd64: cpu: Implement context switch groundwork

Signed-off-by: Chloe M. <chloe@mensia.org>
---
 service/ptos/arch/amd64/cpu/vector.S |  5 ++-
 service/ptos/arch/amd64/ps/context.c | 50 +++++++++++++++++++++++++++-
 2 files changed, 51 insertions(+), 4 deletions(-)

diff --git a/service/ptos/arch/amd64/cpu/vector.S b/service/ptos/arch/amd64/cpu/vector.S
index aa5efda..e7c5c5a 100644
--- a/service/ptos/arch/amd64/cpu/vector.S
+++ b/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()
diff --git a/service/ptos/arch/amd64/ps/context.c b/service/ptos/arch/amd64/ps/context.c
index 2919d75..6dd4fcb 100644
--- a/service/ptos/arch/amd64/ps/context.c
+++ b/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);
 }