diff --git a/sys/SConscript b/sys/SConscript
index b7d68cf..6574f5b 100644
--- a/sys/SConscript
+++ b/sys/SConscript
@@ -61,6 +61,7 @@ src_arm64 = [
     "arm64/paging.c",
     "arm64/mrt/init.c",
     "arm64/mrt/paging.c",
+    "arm64/mrt/priv.c"
 ]
 
 src_common = [
diff --git a/sys/arm64/include/metal.h b/sys/arm64/include/metal.h
index 9bf0509..602c6f3 100644
--- a/sys/arm64/include/metal.h
+++ b/sys/arm64/include/metal.h
@@ -11,8 +11,6 @@
 #define DECL_MRT(name) void MCODE name(void)
 #endif
 
-#define MGREG_MSTK (MCREG_MG16)
-
 #if defined(IMPL_MRT)
 #undef IMPL_MRT
 #define IMPL_MRT(name) int MCODE _ ## name ## _impl(void); \
@@ -21,38 +19,22 @@
         ".globl " METAL_STR(name) ";" \
         ".balign 16;" \
         METAL_STR(name) ":;" \
-        METAL_STR(RMCR_GAS(MGREG_MSTK, REG_X0)) ";" \
-        "mov x1, sp;" \
-        "str x1, [x0, #-16]!;" \
-        "mov sp, x0;" \
         "mov x0, xzr;" \
         "ldr x0,=_" METAL_STR(name) "_impl;" \
         "blr x0;" \
-        "ldr x1, [sp], #16;" \
-        "mov sp, x1;" \
         METAL_STR(WMR_GAS(MREG_MR0, REG_X0)) ";" \
         METAL_STR(MEXIT_GAS(MREG_MR0)) ";" \
     ); \
 int MCODE _ ## name ## _impl(void)
 #endif
 
-#define IMPL_SHORT_MRT(name) int MCODE _ ## name ## _impl(void); \
-     __asm__ ( \
-        ".section \"mcode\";" \
-        ".globl " METAL_STR(name) ";" \
-        ".balign 16;" \
-        METAL_STR(name) ":;" \
-        "mov x0, xzr;" \
-        "ldr x0,=_" METAL_STR(name) "_impl;" \
-        "blr x0;" \
-        METAL_STR(WMR_GAS(MREG_MR0, REG_X0)) ";" \
-        METAL_STR(MEXIT_GAS(MREG_MR0)) ";" \
-    ); \
-int MCODE _ ## name ## _impl(void)
-
 #define DECL_MVAR(type, name) type MDATA name
 #define DECL_MVAR_ALIGNED(type, name, align) type ALIGNED(align) MDATA name
 
 #define MGREG_MPTB_DMAP (MCREG_MG19)
 #define MGREG_MPTB_XMEM (MCREG_MG18)
-#define MGREG_MPTB_USER (MCREG_MG17)
\ No newline at end of file
+#define MGREG_MPTB_USER (MCREG_MG17)
+#define MGREG_MCPL      (MCREG_MG20)
+#define MGREG_TMPRET    (MCREG_MG21)
+#define MCPL_KERNEL (0)
+#define MCPL_USER (1)
diff --git a/sys/arm64/include/metalasm.h b/sys/arm64/include/metalasm.h
index 968ef2e..cf4f821 100644
--- a/sys/arm64/include/metalasm.h
+++ b/sys/arm64/include/metalasm.h
@@ -121,6 +121,7 @@
 #define MCREG_MEB (MCREG_MG3)
 #define MCREG_MTP (MCREG_MG4)
 #define MCREG_MAR (MCREG_MG5)
+#define MCREG_MSTK (MCREG_MG6)
 
 #define _METAL_STR(x) #x
 #define METAL_STR(x) _METAL_STR(x)
diff --git a/sys/arm64/include/mp.h b/sys/arm64/include/mp.h
index 5bdcabc..598fc42 100644
--- a/sys/arm64/include/mp.h
+++ b/sys/arm64/include/mp.h
@@ -17,12 +17,15 @@ void MP_CrossCallTrap();
 int MP_CrossCall(CrossCallCB cb, void *arg);
 
 struct CPUState {
+    vaddr_t         kstack;
     unsigned int    id;
     uint32_t        mpid;
     uint32_t        gic_redist_id;
     volatile int    state;
 };
 
+_Static_assert(offsetof(struct CPUState, kstack) == 0);
+
 register uint64_t _cpu __asm__("x18");
 static inline unsigned int CPU(void) 
 {
diff --git a/sys/arm64/include/mrt.h b/sys/arm64/include/mrt.h
index a410b03..60d8f43 100644
--- a/sys/arm64/include/mrt.h
+++ b/sys/arm64/include/mrt.h
@@ -38,6 +38,12 @@ DECL_MRT(mrt_set_mtp);
 DECL_MRT(mrt_wfi);
 #define MRT_WFI_IDX (6)
 
+DECL_MRT(mrt_kenter);
+#define MRT_KENTER_IDX (7)
+
+DECL_MRT(mrt_kexit);
+#define MRT_KEXIT_IDX (8)
+
 extern DECL_MVAR(struct mroutine_table, mtl_mrt_tbl);
 extern DECL_MVAR(struct exc_intercept_table, mtl_exc_tbl);
 
diff --git a/sys/arm64/include/thread.h b/sys/arm64/include/thread.h
index 102d6e1..6678ff3 100644
--- a/sys/arm64/include/thread.h
+++ b/sys/arm64/include/thread.h
@@ -6,11 +6,25 @@
 #include <machine/cpuop.h>
 
 typedef struct ThreadArchStackFrame {
+    uint64_t r0;
+    uint64_t _r0;
+    uint64_t r19;
+    uint64_t r20;
+    uint64_t r21;
+    uint64_t r22;
+    uint64_t r23;
+    uint64_t r24;
+    uint64_t r25;
+    uint64_t r26;
+    uint64_t r27;
+    uint64_t r28;
+    uint64_t fp;
+    uint64_t lr;
 } ThreadArchStackFrame;
+_Static_assert(sizeof(ThreadArchStackFrame) % 16 == 0);
 
 typedef struct ThreadArch {
-    bool		useFP;
-    uint64_t		rsp;
+    uint64_t sp;
 } ThreadArch;
 
 #endif /* __MACHINE_THREAD_H__ */
diff --git a/sys/arm64/include/trap.h b/sys/arm64/include/trap.h
index 0acf61b..bcc1e62 100644
--- a/sys/arm64/include/trap.h
+++ b/sys/arm64/include/trap.h
@@ -114,7 +114,12 @@ typedef struct TrapFrame
     uint64_t    r29;
     uint64_t    r30;
     uint64_t    r31;
+
+    uint64_t    pcpl;
+    uint64_t    psp;
 } TrapFrame;
 
+void Trap_Pop(struct TrapFrame * tf);
+
 #endif /* __TRAP_H__ */
 
diff --git a/sys/arm64/machine.c b/sys/arm64/machine.c
index c576071..31c0b05 100644
--- a/sys/arm64/machine.c
+++ b/sys/arm64/machine.c
@@ -71,7 +71,7 @@ Machine_EarlyInit()
 static void
 Machine_IdleThread(UNUSED void *test)
 {
-    while (1) { enable_interrupts(); hlt(); }
+    while (1) { kprintf("Idle Thread!\n"); enable_interrupts(); hlt(); }
 }
 
 /**
@@ -138,12 +138,6 @@ void Machine_Init()
 	    Panic("No boot disk!");
     VFS_MountRoot(root);
 
-    Critical_Exit();
-
-    while(1){
-        hlt();
-    }
-
     /*
      * Create the idle thread
      */
@@ -153,6 +147,12 @@ void Machine_Init()
     }
     Sched_SetRunnable(thr);
 
+    Critical_Exit();
+    
+    while(1) {
+        kprintf("Main thread!\n");
+        hlt();
+    }
     /*
      * Load the init processor
      */
diff --git a/sys/arm64/metal.c b/sys/arm64/metal.c
index 7aec43f..f488157 100644
--- a/sys/arm64/metal.c
+++ b/sys/arm64/metal.c
@@ -2,6 +2,9 @@
 #include <string.h>
 #include <machine/pmap.h>
 
+#define METAL_BOOTSTACK_SZ (4096)
+DECL_MVAR_ALIGNED(static char, mtl_bootstack[METAL_BOOTSTACK_SZ], METAL_BOOTSTACK_SZ);
+
 // early metal init
 void mtl_init(void)
 {
@@ -10,6 +13,10 @@ void mtl_init(void)
 
     mtl_mrt_tbl.entries[MRT_INIT_IDX] = MRT_ENTRY_MAKE(mrt_init, 1);
 
+    // load metal stack for the boot core
+    const regval_t mstk = (regval_t)(mtl_bootstack + METAL_BOOTSTACK_SZ);
+    WMCR(MCREG_MSTK, mstk);
+
     // load mroutine table
     WMCR(MCREG_MBR, &mtl_mrt_tbl);
 
diff --git a/sys/arm64/mrt/init.c b/sys/arm64/mrt/init.c
index 8818875..8702bbe 100644
--- a/sys/arm64/mrt/init.c
+++ b/sys/arm64/mrt/init.c
@@ -6,15 +6,13 @@
 DECL_MVAR(struct mroutine_table, mtl_mrt_tbl);
 DECL_MVAR(struct exc_intercept_table, mtl_exc_tbl);
 DECL_MVAR(struct inst_intercept_table, mtl_inst_tbl);
-#define METAL_BOOTSTACK_SZ (4096)
-DECL_MVAR_ALIGNED(static char, mtl_bootstack[METAL_BOOTSTACK_SZ], METAL_BOOTSTACK_SZ);
 
-IMPL_SHORT_MRT(mrt_dummy)
+IMPL_MRT(mrt_dummy)
 {
     return 0;
 }
 
-IMPL_SHORT_MRT(mrt_wfi)
+IMPL_MRT(mrt_wfi)
 {
     unsigned int times;
     MRT_GETARG(0, times);
@@ -30,7 +28,7 @@ IMPL_SHORT_MRT(mrt_wfi)
     return 0;
 }
 
-IMPL_SHORT_MRT(mrt_init)
+IMPL_MRT(mrt_init)
 {
 	memset(&mtl_mrt_tbl, 0, sizeof(mtl_mrt_tbl));
     memset(&mtl_exc_tbl, 0, sizeof(mtl_exc_tbl));
@@ -44,6 +42,8 @@ IMPL_SHORT_MRT(mrt_init)
     mtl_mrt_tbl.entries[MRT_SET_MPTB_IDX] = MRT_ENTRY_MAKE(mrt_set_mptb, 1);
     mtl_mrt_tbl.entries[MRT_SET_MTP_IDX] = MRT_ENTRY_MAKE(mrt_set_mtp, 1);
     mtl_mrt_tbl.entries[MRT_WFI_IDX] = MRT_ENTRY_MAKE(mrt_wfi, 1);
+    mtl_mrt_tbl.entries[MRT_KENTER_IDX] = MRT_ENTRY_MAKE(mrt_kenter, 1);
+    mtl_mrt_tbl.entries[MRT_KEXIT_IDX] = MRT_ENTRY_MAKE(mrt_kexit, 1);
 
     mtl_exc_tbl.entries[0].esrbits = (0b100100 << 26) | (0b00100); // data abort, lv0 translation
     mtl_exc_tbl.entries[0].esrmask = (0b111111 << 26) | (0b11111);
@@ -111,9 +111,9 @@ IMPL_SHORT_MRT(mrt_init)
     msr |= (1ull << 60);
     WMCR(MCREG_MSR, msr);
 
-    // temporary metal stack
-    regval_t mstk = (regval_t)&mtl_bootstack[METAL_BOOTSTACK_SZ];
-    WMCR(MGREG_MSTK, mstk);
+    // set cpl = 0
+    regval_t zero = 0;
+    WMCR(MGREG_MCPL, zero);
 
     return 0;
 }
\ No newline at end of file
diff --git a/sys/arm64/mrt/priv.c b/sys/arm64/mrt/priv.c
new file mode 100644
index 0000000..1425f3b
--- /dev/null
+++ b/sys/arm64/mrt/priv.c
@@ -0,0 +1,72 @@
+#include <machine/metal.h>
+#include <machine/mrt.h>
+#include <machine/paging.h>
+#include <machine/mp.h>
+#include <sys/thread.h>
+
+extern Thread *curProc[MAX_CPUS];
+
+IMPL_MRT(mrt_kenter)
+{
+    regval_t pcpl;
+    RMCR(MGREG_MCPL, pcpl);
+
+    regval_t kstack_top, psp;
+    psp = SYSREG_GET(sp_el1);
+    if (pcpl == MCPL_USER) {
+        // switch MTP
+        const regval_t kmtp = MTP_KERNEL;
+        WMCR(MCREG_MTP, kmtp);
+
+        // switch to kernel stack
+        struct CPUState * state = (struct CPUState *)SYSREG_GET(tpidr_el1);
+        const uint32_t id = state->id;
+        kstack_top = curProc[id]->kstack + PGSIZE;
+
+        const regval_t kcpl = MCPL_KERNEL;
+        WMCR(MGREG_MCPL, kcpl);
+    } else {
+        // we already in kernel mode
+        kstack_top = SYSREG_GET(sp_el1);
+    }
+
+    // store var onto the current kstack
+    // [kstack top] = pcpl
+    // [kstack top] + 8 = psp
+    kstack_top = kstack_top - 2 * sizeof(regval_t);
+    *(regval_t *)kstack_top = pcpl;
+    *(regval_t *)(kstack_top + sizeof(regval_t)) = psp;
+    SYSREG_SET(sp_el1, kstack_top);
+    
+    return 0;
+}
+
+IMPL_MRT(mrt_kexit)
+{
+    regval_t cpl;
+    RMCR(MGREG_MCPL, cpl);
+    ASSERT(cpl == MCPL_KERNEL);
+
+    // [kstack top] = pcpl
+    // [kstack top] + 8 = psp
+    regval_t kstack_top = SYSREG_GET(sp_el1);
+    const regval_t psp = *(regval_t *)(kstack_top + sizeof(regval_t));
+    const regval_t pcpl = *(regval_t *)(kstack_top);
+    kstack_top += 2 * sizeof(regval_t);
+
+    if (pcpl == MCPL_USER) {
+        // we are going back to usermode
+
+        // switch MTP to user
+        const regval_t kmtp = MTP_USER;
+        WMCR(MCREG_MTP, kmtp);
+
+        const regval_t kcpl = MCPL_USER;
+        WMCR(MGREG_MCPL, kcpl);
+    }
+    
+    // restore previous stack ptr
+    SYSREG_SET(sp_el1, psp);
+
+    return 0;
+}
diff --git a/sys/arm64/paging.c b/sys/arm64/paging.c
index d71ea52..59c270a 100644
--- a/sys/arm64/paging.c
+++ b/sys/arm64/paging.c
@@ -74,6 +74,9 @@ void vm_early_paging_init()
 	SYSREG_SET(ttbr0_el1, zero);
     SYSREG_SET(ttbr1_el1, zero);
 
+    // set MCPL to 0
+    
+
 	// reset page tables
 	flushtlb();
 }
\ No newline at end of file
diff --git a/sys/arm64/pmap.c b/sys/arm64/pmap.c
index 9e7f71c..fbc4f88 100644
--- a/sys/arm64/pmap.c
+++ b/sys/arm64/pmap.c
@@ -124,6 +124,26 @@ PMap_NewAS()
     return as;
 }
 
+static void
+pmap_destroy_vmpt(struct vmpt * const pt)
+{
+    for (size_t i = 0; i < VM_PTBUCKETS; i++) {
+        paddr_t curp = pt->entries[i].first;
+
+        while(curp != 0) {
+            struct vmpd * const cur = (struct vmpd *)DMPA2VA(curp);
+            const paddr_t pnext = cur->next;
+
+            pdcache_free(cur);
+
+            curp = pnext;
+        }
+    }
+
+    // release vmpt
+    PAlloc_Release(pt);
+}
+
 /**
  * PMap_DestroyAS --
  *
@@ -135,13 +155,10 @@ void
 PMap_DestroyAS(AS *space)
 {
     // free usertables
-    UNUSED struct vmpt * pt = space->user_tbl;
-    
+    pmap_destroy_vmpt(space->user_tbl);
+
     // release space itself
     PAlloc_Release(space);
-
-    // XXX: release all pdcaches
-    NOT_IMPLEMENTED();
 }
 
 /**
@@ -205,7 +222,7 @@ PMap_LoadAS(AS *space)
 }
 
 static void
-PMap_MapPage(uint64_t phys, uint64_t virt, uint64_t pages, uint32_t pgshift, struct vmpt * tbl, uint64_t flags)
+PMap_MapPage(paddr_t phys, vaddr_t virt, size_t pages, uint32_t pgshift, struct vmpt * tbl, uint64_t flags)
 {
     unsigned int i = 0;
     while (i < pages) {
@@ -229,6 +246,59 @@ PMap_MapPage(uint64_t phys, uint64_t virt, uint64_t pages, uint32_t pgshift, str
     }
 }
 
+static bool
+PMap_FindVmpd(struct vmpte * pte, vaddr_t virt, uint32_t pgshift, struct vmpd ** curout, struct vmpd ** prevout)
+{
+    paddr_t curp = pte->first;
+    struct vmpd * prev = NULL;
+    while (curp != 0) {
+        struct vmpd * const cur = (struct vmpd *)DMPA2VA(curp);
+        if (vm_get_pfn(cur->vaddr, pgshift) == vm_get_pfn(virt, pgshift)) {
+            if (prevout != NULL) {
+                *prevout = prev;
+            }
+            if (curout != NULL) {
+                *curout = cur;
+            }
+            return true;
+        }
+
+        prev = cur;
+        curp = cur->next;
+    }
+    return false;
+}
+
+static void
+PMap_UnmapPage(vaddr_t virt, size_t pages, uint32_t pgshift, struct vmpt * tbl)
+{
+    ASSERT((virt & ((1ull << pgshift) - 1)) == 0);
+
+    for (uint64_t i = 0; i < pages; i++) {
+        const vaddr_t va = virt + i * (1ull << pgshift);
+        const uint64_t pfn = vm_get_pfn(va, pgshift);
+        const uint64_t hash = vm_vahash(pfn);
+        ASSERT(hash < VM_PTBUCKETS);
+        struct vmpte * const pte = &tbl->entries[hash];
+
+        struct vmpd *found, *prev;
+        if (!PMap_FindVmpd(pte, virt, pgshift, &found, &prev)) {
+            Panic("Unmapping an unmapped vaddr.\n");
+        }
+
+        // detach the node
+        if (prev == NULL) {
+            // first node
+            pte->first = found->next;
+        } else {
+            prev->next = found->next;
+        }
+
+        // free the unmapped pd
+        pdcache_free(found);
+    }
+}
+
 
 /**
  * PMap_Translate --
@@ -239,10 +309,37 @@ PMap_MapPage(uint64_t phys, uint64_t virt, uint64_t pages, uint32_t pgshift, str
  * @param [in] va Virtual address we wish to translate.
  */
 uintptr_t
-PMap_Translate(UNUSED AS *space, UNUSED uintptr_t va)
+PMap_Translate(AS *space, uintptr_t va)
 {
-    NOT_IMPLEMENTED();
-    return 0;
+    struct vmpt * tbl;
+    uint32_t pgshift;
+    if (va >= MEM_USERSPACE_BASE && va < MEM_USERSPACE_TOP) {
+        tbl = space->user_tbl;
+        pgshift = PGSHIFT;
+    } else if (va >= MEM_DIRECTMAP_BASE && va < MEM_DIRECTMAP_TOP) {
+        tbl = space->dmap_tbl;
+        pgshift = LARGE_PGSHIFT;
+    } else if (va >= MEM_XMAP_BASE && va < MEM_XMAP_TOP) {
+        tbl = space->xmem_tbl;
+        pgshift = PGSHIFT;
+    } else {
+        kprintf("Trying to translate non-canonical vaddr 0x%lx.\n", va);
+        return 0;
+    }
+    
+    const uint64_t pfn = vm_get_pfn(va, pgshift);
+    const uint64_t hash = vm_vahash(pfn);
+    ASSERT(hash < VM_PTBUCKETS);
+    struct vmpte * const pte = &tbl->entries[hash];
+    const vaddr_t base = va & ~((1ull << pgshift) - 1);
+
+    struct vmpd * found;
+    if (!PMap_FindVmpd(pte, base, pgshift, &found, NULL)) {
+        kprintf("Trying to translate unmapped vaddr 0x%lx.\n", va);
+        return 0;
+    }
+
+    return found->paddr + (va - base);
 }
 
 /**
@@ -285,9 +382,14 @@ PMap_Map(AS *as, uint64_t phys, uint64_t virt, uint64_t pages, uint64_t flags)
  * @retval false On failure
  */
 bool
-PMap_Unmap(UNUSED AS *as, UNUSED uint64_t va, UNUSED uint64_t pages)
+PMap_Unmap(AS *as, uint64_t va, uint64_t pages)
 {
-    NOT_IMPLEMENTED();
+    // virt address must be within the usermap region
+    if (va < MEM_USERSPACE_BASE || va + pages * PGSIZE > MEM_USERSPACE_TOP) {
+        return false;
+    }
+
+    PMap_UnmapPage(va, pages, PGSHIFT, as->user_tbl);
     return true;
 }
 
@@ -306,9 +408,25 @@ PMap_Unmap(UNUSED AS *as, UNUSED uint64_t va, UNUSED uint64_t pages)
  * @retval false On failure
  */
 bool
-PMap_AllocMap(UNUSED AS *as, UNUSED uint64_t virt, UNUSED uint64_t len, UNUSED uint64_t flags)
+PMap_AllocMap(AS *as, uint64_t virt, uint64_t len, uint64_t flags)
 {
-    NOT_IMPLEMENTED();
+    ASSERT((virt & PGMASK) == 0);
+
+    if (virt + len > MEM_USERSPACE_TOP || virt < MEM_USERSPACE_BASE) {
+        // not in userspace
+        return false;
+    }
+
+    const size_t pages = ROUNDUP(len, PGSIZE) / PGSIZE;
+    const uint64_t padattr = PMapProtToPdattr(flags, false);
+
+    for (unsigned int i = 0; i < pages; i++) {
+        const vaddr_t va = virt + PGSIZE * i;
+
+        const void *pg = PAlloc_AllocPage();
+        PMap_MapPage(DMVA2PA(pg), va, 1, PGSHIFT, as->user_tbl, padattr);
+    }
+
     return true;
 }
 
@@ -387,10 +505,16 @@ PMap_SystemMap(uint64_t phys, uint64_t virt, uint64_t pages, uint64_t flags)
  * We do not currently use this!
  */
 bool
-PMap_SystemUnmap(UNUSED uint64_t virt, UNUSED uint64_t pages)
+PMap_SystemUnmap(uint64_t virt, uint64_t pages)
 {
-    NOT_IMPLEMENTED();
-    return false;
+    // virt address must be within the xmem region
+    if (virt < MEM_XMAP_BASE || virt + pages * PGSIZE > MEM_XMAP_TOP) {
+        return false;
+    }
+
+    PMap_UnmapPage(virt, pages, PGSHIFT, systemAS.xmem_tbl);
+
+    return true;
 }
 
 void
diff --git a/sys/arm64/switch.S b/sys/arm64/switch.S
index 6093111..30829e4 100644
--- a/sys/arm64/switch.S
+++ b/sys/arm64/switch.S
@@ -7,13 +7,30 @@
 .text
 
 # switch(uint64_t *oldsp, uint64_t newsp)
-# %rdi: oldsp
-# %rsi: newsp
+# %x0: oldsp
+# %x1: newsp
 FUNC_BEGIN(switchstack)
     # Save callee saved registers of old thread
+    stp fp, lr, [sp, #-16]!
+    stp x27, x28, [sp, #-16]!
+    stp x25, x26, [sp, #-16]!
+    stp x23, x24, [sp, #-16]!
+    stp x21, x22, [sp, #-16]!
+    stp x19, x20, [sp, #-16]!
+    stp x0, x0, [sp, #-16]!
 
     # Switch stack from old to new thread
+    mov x9, sp
+    str x9, [x0]
+    mov sp, x1
 
     # Restore callee saved registers of new thread
+    ldp x0, x0, [sp], #16
+    ldp x19, x20, [sp], #16
+    ldp x21, x22, [sp], #16
+    ldp x23, x24, [sp], #16
+    ldp x25, x26, [sp], #16
+    ldp x27, x28, [sp], #16
+    ldp fp, lr, [sp], #16
 FUNC_END(switchstack)
 
diff --git a/sys/arm64/thread.c b/sys/arm64/thread.c
index 319756a..9a1df5b 100644
--- a/sys/arm64/thread.c
+++ b/sys/arm64/thread.c
@@ -13,56 +13,85 @@
 #include <machine/cpuop.h>
 #include <machine/trap.h>
 #include <machine/pmap.h>
+#include <machine/metal.h>
 
-extern void ThreadKThreadEntry(TrapFrame *tf);
-extern void switchstack(uint64_t *oldrsp, uint64_t rsp);
+extern void switchstack(uint64_t *oldsp, uint64_t sp);
+extern Spinlock schedLock;
 
 void
 Thread_InitArch(Thread *thr)
 {
-    thr->arch.useFP = true;
 }
 
 void
-Thread_SetupKThread(Thread *thr, UNUSED void (*f)(void *),
-		    UNUSED uintptr_t arg1, UNUSED uintptr_t arg2, UNUSED uintptr_t arg3)
+ThreadKThreadEntry(TrapFrame *tf) __NO_LOCK_ANALYSIS
 {
-    // Initialize stack
+    Spinlock_Unlock(&schedLock);
+    kprintf("tf in entry = 0x%p, elr = 0x%lx\n", tf, tf->elr);
+    Trap_Pop(tf);
+}
+
+#define SPSR_EL1H_DAIF0 (0b101)
+
+void
+Thread_SetupKThread(Thread *thr, kthread_entry f,
+		    uintptr_t arg1,  uintptr_t arg2,  uintptr_t arg3)
+{
+        // Initialize stack
     uint64_t stacktop = thr->kstack + PGSIZE;
     ThreadArchStackFrame *sf;
     TrapFrame *tf;
 
     tf = (TrapFrame *)(stacktop - sizeof(*tf));
     sf = (ThreadArchStackFrame *)(stacktop - sizeof(*tf) - sizeof(*sf));
+    thr->arch.sp = (uint64_t)sf;
 
     memset(tf, 0, sizeof(*tf));
     memset(sf, 0, sizeof(*sf));
 
     // Setup thread exit function on stack
+    sf->lr = (uint64_t)&ThreadKThreadEntry;
+    sf->r0 = (uint64_t)tf;
+    sf->_r0 = (uint64_t)tf;
+    kprintf("tf before entry = 0x%p\n", tf);
 
+    // interrupt enable
+    tf->spsr = SPSR_EL1H_DAIF0; // el1h, enable interrupts
+    tf->elr = (regval_t)f;
+    tf->pcpl = MCPL_KERNEL;
+    tf->psp = stacktop;
+    tf->r0 = (regval_t)arg1;
+    tf->r1 = (regval_t)arg2;
+    tf->r2 = (regval_t)arg3;
 }
 
 static void
-ThreadEnterUserLevelCB(UNUSED void * unused)
+ThreadEnterUserLevelCB(vaddr_t entry, vaddr_t ustack, vaddr_t args)
 {
     TrapFrame tf;
 
     memset(&tf, 0, sizeof(tf));
+    tf.elr = entry;
+    tf.r0 = args;
+    tf.psp = ustack + MEM_USERSPACE_STKLEN - PGSIZE;
+    tf.spsr = SPSR_EL1H_DAIF0;
+    tf.pcpl = MCPL_USER;
 
-    //Trap_Pop(&tf);
+    Trap_Pop(&tf);
 }
 
 void
 Thread_SetupUThread(Thread *thr, uintptr_t rip, uintptr_t arg)
 {
-    Thread_SetupKThread(thr, ThreadEnterUserLevelCB, rip,
+    Thread_SetupKThread(thr, (kthread_entry)ThreadEnterUserLevelCB, rip,
 			thr->ustack, arg);
 }
 
 void
 Thread_SwitchArch(Thread *oldthr, Thread *newthr)
 {
+    kprintf("switch stack! oldthr sp = 0x%lx, newthr sp = 0x%lx\n", oldthr->arch.sp, newthr->arch.sp);
     // Jump to trapframe
-    switchstack(&oldthr->arch.rsp, newthr->arch.rsp);
+    switchstack(&oldthr->arch.sp, newthr->arch.sp);
 }
 
diff --git a/sys/arm64/timer.c b/sys/arm64/timer.c
index 6d846d3..d85aebb 100644
--- a/sys/arm64/timer.c
+++ b/sys/arm64/timer.c
@@ -18,6 +18,8 @@
 // hardcode to 2024/03/14 12:34:56 AM EST
 #define KTIMER_EPOCH (1710390896ull)
 
+extern void Sched_Scheduler(void);
+
 static struct IRQHandler _irqh;
 
 static inline void 
@@ -34,8 +36,9 @@ PTimer_Tick(UNUSED void * arg)
     gic_send_eoi(KTIMER_IRQ);
 
     KTimer_Process();
-
     kprintf("Hardware timer tick, epoch = %lu, tsc = %lu, CVAL = %lu.\n", KTime_GetEpoch(), Time_GetTSC(), SYSREG_GET(CNTP_CVAL_EL0));
+
+    Sched_Scheduler();
 }
 
 void
diff --git a/sys/arm64/trap.c b/sys/arm64/trap.c
index 7c86254..8bd7bfc 100644
--- a/sys/arm64/trap.c
+++ b/sys/arm64/trap.c
@@ -102,6 +102,7 @@ extern void copystr_unsafe_fault(void);
 void
 trap_entry(TrapFrame * tf)
 {
+    // kprintf("Trap: psp = 0x%lx, pcpl = %lu\n", tf->psp, tf->pcpl);
     if (tf->type == T_TYPE_EXC) {
         Panic("Unexpected exception: Type = 0x%lx, IntID = 0x%lx, ESR = 0x%lx, FAR = 0x%lx, ELR = 0x%lx, SPSR = 0x%lx.\n", tf->type, tf->intid, tf->esr, tf->far, tf->elr, tf->spsr);
     } else {
diff --git a/sys/arm64/trapentry.S b/sys/arm64/trapentry.S
index 28d3d5b..eae34af 100644
--- a/sys/arm64/trapentry.S
+++ b/sys/arm64/trapentry.S
@@ -1,4 +1,8 @@
 #include <machine/metalasm.h>
+
+#define MRT_KENTER_IDX (7)
+#define MRT_KEXIT_IDX (8)
+
 /*
  * Trap Handlers
  */
@@ -191,21 +195,36 @@
     // daif bits are stored in PSTATE and are automatically re-enabled by eret
 .endm
 
+.global Trap_Pop
+.text
+Trap_Pop:
+    mov sp, x0
+    trapframe_restore_common
+    MENTER_GAS(MRT_KEXIT_IDX)
+    eret
+
 .extern trap_entry
 .text
-
 interrupt_locore_entry:
+    MENTER_GAS(MRT_KENTER_IDX)
     trapframe_save_common 1
     mov      x0, sp
+    // pcpu for this core
+    mrs     x18, tpidr_el1
     bl       trap_entry
     trapframe_restore_common
+    MENTER_GAS(MRT_KEXIT_IDX)
     eret
 
 exception_locore_entry:
+    MENTER_GAS(MRT_KENTER_IDX)
     trapframe_save_common 0
     mov      x0, sp
+    // pcpu for this core
+    mrs     x18, tpidr_el1
     bl       trap_entry
     trapframe_restore_common
+    MENTER_GAS(MRT_KEXIT_IDX)
     eret
 
 .global _evt
diff --git a/sys/include/thread.h b/sys/include/thread.h
index 04e3d81..23e1b97 100644
--- a/sys/include/thread.h
+++ b/sys/include/thread.h
@@ -92,6 +92,8 @@ typedef struct Process {
     HandleQueue			handles[PROCESS_HANDLE_SLOTS];
 } Process;
 
+typedef void (*kthread_entry)(void *);
+
 // General
 void Thread_Init();
 void Thread_InitAP();
@@ -107,7 +109,7 @@ uint64_t Process_Wait(Process *proc, uint64_t pid);
 
 // Thread functions
 Thread *Thread_Create(Process *proc);
-Thread *Thread_KThreadCreate(void (*f)(void*), void *arg);
+Thread *Thread_KThreadCreate(kthread_entry f, void *arg);
 Thread *Thread_UThreadCreate(Thread *oldThr, uint64_t rip, uint64_t arg);
 Thread *Thread_Lookup(Process *proc, uint64_t tid);
 void Thread_Retain(Thread *thr);
@@ -127,7 +129,7 @@ void Thread_Dump(Thread *thr);
 
 // Platform functions
 void Thread_InitArch(Thread *thr);
-void Thread_SetupKThread(Thread *thr, void (*f)(void *),
+void Thread_SetupKThread(Thread *thr, kthread_entry f,
 			 uintptr_t arg1, uintptr_t arg2, uintptr_t arg3);
 void Thread_SetupUThread(Thread *thr, uint64_t rip, uint64_t arg);
 void Thread_SwitchArch(Thread *oldthr, Thread *newthr);
diff --git a/sys/kern/loader.c b/sys/kern/loader.c
index f42155b..3640c15 100644
--- a/sys/kern/loader.c
+++ b/sys/kern/loader.c
@@ -238,7 +238,6 @@ Loader_LoadInit()
      */
     PMap_LoadAS(thr->space); // Reload CR3
 
-#if defined(__x86_64__)
     /*
      * Pass in zero arguments with null pointers to init
      */
@@ -246,8 +245,9 @@ Loader_LoadInit()
     ap[0] = 0;
     ap[1] = 0;
     ap[2] = 0xDEADBEEF;
-    uintptr_t rsp = MEM_USERSPACE_STKTOP - PGSIZE;
 
+#if defined(__x86_64__)
+    uintptr_t rsp = MEM_USERSPACE_STKTOP - PGSIZE;
     Copy_Out(&ap[0], rsp, sizeof(uintptr_t)*3);
 
     /*
@@ -265,7 +265,19 @@ Loader_LoadInit()
     tf.rdi = rsp;
     Trap_Pop(&tf);
 #elif defined(__aarch64__)
-    NOT_IMPLEMENTED();
+    /*
+     * Pass in zero arguments with null pointers to init
+     */
+    const vaddr_t sp = MEM_USERSPACE_STKTOP - PGSIZE;
+
+    Copy_Out(&ap[0], sp, sizeof(uintptr_t)*3);
+
+    TrapFrame tf;
+    memset(&tf, 0, sizeof(tf));
+    tf.elr = thr->proc->entrypoint;
+    tf.r0 = sp;
+
+    //Trap_Pop(&tf);
 #endif
 
     /*
diff --git a/sys/kern/sched.c b/sys/kern/sched.c
index 269fd47..803eee5 100644
--- a/sys/kern/sched.c
+++ b/sys/kern/sched.c
@@ -214,6 +214,7 @@ Sched_Scheduler()
     curProc[CPU()] = next;
     next->schedState = SCHED_STATE_RUNNING;
     next->ctxSwitches++;
+    kprintf("sched: cur = %lu next = %lu\n", prev->tid, next->tid);
 
     if (prev->schedState == SCHED_STATE_RUNNING) {
 	prev->schedState = SCHED_STATE_RUNNABLE;
diff --git a/sys/kern/thread.c b/sys/kern/thread.c
index 07f0438..a2388be 100644
--- a/sys/kern/thread.c
+++ b/sys/kern/thread.c
@@ -141,7 +141,7 @@ Thread_Create(Process *proc)
 }
 
 Thread *
-Thread_KThreadCreate(void (*f)(void *), void *arg)
+Thread_KThreadCreate(kthread_entry f, void *arg)
 {
     Thread *thr = Thread_Create(kernelProcess);
     if (!thr)