From db8c734f4ec560a376194728a54e9115b943ca1e Mon Sep 17 00:00:00 2001 From: quackerd Date: Tue, 19 Nov 2024 14:00:13 -0500 Subject: [PATCH] ctx switch and kenter/kexit --- sys/SConscript | 1 + sys/arm64/include/metal.h | 28 ++----- sys/arm64/include/metalasm.h | 1 + sys/arm64/include/mp.h | 3 + sys/arm64/include/mrt.h | 6 ++ sys/arm64/include/thread.h | 18 +++- sys/arm64/include/trap.h | 5 ++ sys/arm64/machine.c | 14 ++-- sys/arm64/metal.c | 7 ++ sys/arm64/mrt/init.c | 16 ++-- sys/arm64/mrt/priv.c | 72 ++++++++++++++++ sys/arm64/paging.c | 3 + sys/arm64/pmap.c | 156 +++++++++++++++++++++++++++++++---- sys/arm64/switch.S | 21 ++++- sys/arm64/thread.c | 49 ++++++++--- sys/arm64/timer.c | 5 +- sys/arm64/trap.c | 1 + sys/arm64/trapentry.S | 21 ++++- sys/include/thread.h | 6 +- sys/kern/loader.c | 18 +++- sys/kern/sched.c | 1 + sys/kern/thread.c | 2 +- 22 files changed, 378 insertions(+), 76 deletions(-) create mode 100644 sys/arm64/mrt/priv.c 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 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 #include +#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 +#include +#include +#include +#include + +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 #include #include +#include -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 + +#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)