From 68a1ce6dd2ae8bc590e812c21b237307705c763f Mon Sep 17 00:00:00 2001 From: TuxSH <1922548+TuxSH@users.noreply.github.com> Date: Sun, 12 Jan 2020 21:51:50 +0000 Subject: [PATCH] thermosphere: properly implement guest timer stuff --- thermosphere/linker.ld | 2 ++ thermosphere/src/core_ctx.h | 5 +-- thermosphere/src/exception_vectors.s | 54 ++++++++++++++++------------ thermosphere/src/exceptions.c | 35 ++++++++++++++++-- thermosphere/src/exceptions.h | 11 +++++- thermosphere/src/initSystem.c | 19 ++++++---- thermosphere/src/irq.c | 31 +++++++++++++--- thermosphere/src/main.c | 41 +++++++++++++++++++-- thermosphere/src/start.s | 5 ++- thermosphere/src/sysreg_traps.c | 11 +++--- thermosphere/src/timer.c | 6 ++-- thermosphere/src/timer.h | 13 ++++--- 12 files changed, 180 insertions(+), 53 deletions(-) diff --git a/thermosphere/linker.ld b/thermosphere/linker.ld index b0cf240b5..05c403ea2 100644 --- a/thermosphere/linker.ld +++ b/thermosphere/linker.ld @@ -23,6 +23,8 @@ SECTIONS . = ALIGN(0x800); __vectors_start__ = ABSOLUTE(.); KEEP(*(.vectors*)); + __vectors_end__ = ABSOLUTE(.); + ASSERT(__vectors_end__ - __vectors_start__ <= 0x800, "Exception vectors section should be max 0x800 in size!"); . = ALIGN(8); } >main :main diff --git a/thermosphere/src/core_ctx.h b/thermosphere/src/core_ctx.h index 04b517754..3b29341b6 100644 --- a/thermosphere/src/core_ctx.h +++ b/thermosphere/src/core_ctx.h @@ -37,11 +37,12 @@ typedef struct CoreCtx { bool executedFunctionSync; // @0x3C // Timer stuff - u64 emulPtimerOffsetThen; // @0x40. When setting cntp_cval_el0 and on interrupt + u64 totalTimeInHypervisor; // @0x40. cntvoff_el2 is updated to that value. + u64 emulPtimerOffsetThen; // @0x48. When setting cntp_cval_el0 and on interrupt } CoreCtx; static_assert(offsetof(CoreCtx, executedFunctionSync) == 0x3C, "Wrong definition for CoreCtx"); -static_assert(offsetof(CoreCtx, emulPtimerOffsetThen) == 0x40, "Wrong definition for CoreCtx"); +static_assert(offsetof(CoreCtx, emulPtimerOffsetThen) == 0x48, "Wrong definition for CoreCtx"); extern CoreCtx g_coreCtxs[4]; register CoreCtx *currentCoreCtx asm("x18"); diff --git a/thermosphere/src/exception_vectors.s b/thermosphere/src/exception_vectors.s index e17679ce5..26ba0598d 100644 --- a/thermosphere/src/exception_vectors.s +++ b/thermosphere/src/exception_vectors.s @@ -6,6 +6,8 @@ */ +#define STACK_FRAME_SIZE 0x140 + /* * Declare the exception vector table, enforcing it is aligned on a * 2KB boundary, as required by the ARMv8 architecture. @@ -50,9 +52,14 @@ .endif .endm -.macro SAVE_ALL_REGISTERS - stp x30, xzr, [sp, #-0x130] - bl _saveAllRegisters +.macro SAVE_MOST_REGISTERS + sub sp, sp, #STACK_FRAME_SIZE + + stp x28, x29, [sp, #-0x20] + stp x30, xzr, [sp, #-0x10] + mrs x28, far_el2 + mrs x29, cntpct_el0 + bl _saveMostRegisters .endm .macro PIVOT_STACK_FOR_CRASH @@ -76,15 +83,23 @@ vector_entry \name PIVOT_STACK_FOR_CRASH .endif - SAVE_ALL_REGISTERS + SAVE_MOST_REGISTERS .if \type == EXCEPTION_TYPE_GUEST - ldp x18, xzr, [sp, #0x120] + ldp x18, xzr, [sp, #STACK_FRAME_SIZE] + mov w1, #1 + .else + mov w1, #0 .endif + + mov x0, sp + bl exceptionEntryPostprocess .endm .macro EXCEPTION_HANDLER_END name, type .if \type != EXCEPTION_TYPE_HOST_CRASH + mov x0, sp + bl exceptionReturnPreprocess b _restoreAllRegisters .else b . @@ -117,8 +132,7 @@ _unknownException: UNKNOWN_EXCEPTION _irqSp0 /* To save space, insert in an unused vector segment. */ -_saveAllRegisters: - sub sp, sp, #0x120 +_saveMostRegisters: stp x0, x1, [sp, #0x00] stp x2, x3, [sp, #0x10] stp x4, x5, [sp, #0x20] @@ -133,22 +147,23 @@ _saveAllRegisters: stp x22, x23, [sp, #0xB0] stp x24, x25, [sp, #0xC0] stp x26, x27, [sp, #0xD0] - stp x28, x29, [sp, #0xE0] - - mov x29, x30 - ldp x30, xzr, [sp, #-0x10] // See SAVE_ALL_REGISTERS macro mrs x20, sp_el1 mrs x21, sp_el0 mrs x22, elr_el2 mrs x23, spsr_el2 - mrs x24, cntvct_el0 + mov x24, x28 // far_el2 + mov x25, x29 // cntpct_el0 - stp x30, x20, [sp, #0xF0] + // See SAVE_MOST_REGISTERS macro + ldp x28, x29, [sp, #-0x20] + ldp x19, xzr, [sp, #-0x10] + + stp x28, x29, [sp, #0xE0] + stp x19, x20, [sp, #0xF0] stp x21, x22, [sp, #0x100] stp x23, x24, [sp, #0x110] - - mov x30, x29 + stp x25, xzr, [sp, #0x120] ret @@ -162,18 +177,13 @@ UNKNOWN_EXCEPTION _fiqSp0 _restoreAllRegisters: ldp x30, x20, [sp, #0xF0] ldp x21, x22, [sp, #0x100] - ldp x23, x24, [sp, #0x110] + ldp x23, xzr, [sp, #0x110] msr sp_el1, x20 msr sp_el0, x21 msr elr_el2, x22 msr spsr_el2, x23 - // Update timer offset: offset = ptimer - vtimer; here the time elapsed is vct(now) - vct(before) - mrs x23, cntvct_el0 - sub x24, x23, x24 - msr cntvoff_el2, x24 - ldp x0, x1, [sp, #0x00] ldp x2, x3, [sp, #0x10] ldp x4, x5, [sp, #0x20] @@ -190,7 +200,7 @@ _restoreAllRegisters: ldp x26, x27, [sp, #0xD0] ldp x28, x29, [sp, #0xE0] - add sp, sp, #0x120 + add sp, sp, #STACK_FRAME_SIZE eret UNKNOWN_EXCEPTION _serrorSp0 diff --git a/thermosphere/src/exceptions.c b/thermosphere/src/exceptions.c index c64146ff1..0fe520d23 100644 --- a/thermosphere/src/exceptions.c +++ b/thermosphere/src/exceptions.c @@ -24,7 +24,7 @@ #include "single_step.h" #include "data_abort.h" -#include "debug_log.h" +#include "timer.h" bool spsrEvaluateConditionCode(u64 spsr, u32 conditionCode) { @@ -60,13 +60,16 @@ void dumpStackFrame(const ExceptionStackFrame *frame, bool sameEl) DEBUG("x30\t\t%016llx\n\n", frame->x[30]); DEBUG("elr_el2\t\t%016llx\n", frame->elr_el2); DEBUG("spsr_el2\t%016llx\n", frame->spsr_el2); + DEBUG("far_el2\t%016llx\n", frame->far_el2); if (sameEl) { DEBUG("sp_el2\t\t%016llx\n", frame->sp_el2); } else { DEBUG("sp_el0\t\t%016llx\n", frame->sp_el0); } DEBUG("sp_el1\t\t%016llx\n", frame->sp_el1); - DEBUG("cntvct_el0\t\t%016llx\n", frame->cntvct_el0); + DEBUG("cntpct_el0\t%016llx\n", frame->cntpct_el0); + DEBUG("cntp_ctl_el0\t%016llx\n", frame->cntp_ctl_el0); + DEBUG("cntv_ctl_el0\t%016llx\n", frame->cntv_ctl_el0); #else (void)frame; (void)sameEl; @@ -92,6 +95,34 @@ void skipFaultingInstruction(ExceptionStackFrame *frame, u32 size) frame->elr_el2 += size; } +void exceptionEnterInterruptibleHypervisorCode(ExceptionStackFrame *frame) +{ + (void)frame; + // We don't want the guest to spam us with its timer interrupts. Disable the timers. + SET_SYSREG(cntp_ctl_el0, 0); + SET_SYSREG(cntv_ctl_el0, 0); +} + +// Called on exception entry (avoids overflowing a vector section) +void exceptionEntryPostprocess(ExceptionStackFrame *frame, bool isLowerEl) +{ + frame->cntp_ctl_el0 = GET_SYSREG(cntp_ctl_el0); + frame->cntv_ctl_el0 = GET_SYSREG(cntv_ctl_el0); +} + +// Called on exception return (avoids overflowing a vector section) +void exceptionReturnPreprocess(ExceptionStackFrame *frame) +{ + // Update virtual counter + currentCoreCtx->totalTimeInHypervisor += timerGetSystemTick() - frame->cntpct_el0; + SET_SYSREG(cntvoff_el2, currentCoreCtx->totalTimeInHypervisor); + //DEBUG("pct %lu - vct %lu = voff %lu\n", timerGetSystemTick() - GET_SYSREG(cntvct_el0), GET_SYSREG(cntvoff_el2)); + + // Restore interrupt mask + SET_SYSREG(cntp_ctl_el0, frame->cntp_ctl_el0); + SET_SYSREG(cntv_ctl_el0, frame->cntv_ctl_el0); +} + void handleLowerElSyncException(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr) { switch (esr.ec) { diff --git a/thermosphere/src/exceptions.h b/thermosphere/src/exceptions.h index 84ab2e082..d43eafd20 100644 --- a/thermosphere/src/exceptions.h +++ b/thermosphere/src/exceptions.h @@ -15,6 +15,7 @@ */ #pragma once +#include #include "utils.h" typedef struct ExceptionStackFrame { @@ -26,9 +27,15 @@ typedef struct ExceptionStackFrame { }; u64 elr_el2; u64 spsr_el2; - u64 cntvct_el0; + u64 far_el2; + u64 cntpct_el0; + u64 cntp_ctl_el0; + u64 cntv_ctl_el0; + u64 reserved; } ExceptionStackFrame; +static_assert(sizeof(ExceptionStackFrame) == 0x140, "Wrong size for ExceptionStackFrame"); + // Adapted from https://developer.arm.com/docs/ddi0596/a/a64-shared-pseudocode-functions/shared-exceptions-pseudocode typedef enum ExceptionClass { Exception_Uncategorized = 0x0, @@ -130,6 +137,8 @@ bool spsrEvaluateConditionCode(u64 spsr, u32 conditionCode); void skipFaultingInstruction(ExceptionStackFrame *frame, u32 size); void dumpStackFrame(const ExceptionStackFrame *frame, bool sameEl); +void exceptionEnterInterruptibleHypervisorCode(ExceptionStackFrame *frame); + void handleLowerElSyncException(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr); void handleSameElSyncException(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr); void handleUnknownException(u32 offset); diff --git a/thermosphere/src/initSystem.c b/thermosphere/src/initSystem.c index e616a3317..9f335e422 100644 --- a/thermosphere/src/initSystem.c +++ b/thermosphere/src/initSystem.c @@ -26,17 +26,22 @@ extern const u32 __vectors_start__[]; static void initSysregs(void) { // Set VBAR - SET_SYSREG(vbar_el2, (uintptr_t)__vectors_start__); + SET_SYSREG(vbar_el2, (uintptr_t)__vectors_start__); // Set system to sane defaults, aarch64 for el1, mmu&caches initially disabled for EL1, etc. - SET_SYSREG(hcr_el2, 0x80000000); - SET_SYSREG(dacr32_el2, 0xFFFFFFFF); // unused - SET_SYSREG(sctlr_el1, 0x00C50838); + SET_SYSREG(hcr_el2, 0x80000000); + SET_SYSREG(dacr32_el2, 0xFFFFFFFF); // unused + SET_SYSREG(sctlr_el1, 0x00C50838); - SET_SYSREG(mdcr_el2, 0x00000000); - SET_SYSREG(mdscr_el1, 0x00000000); + SET_SYSREG(mdcr_el2, 0x00000000); + SET_SYSREG(mdscr_el1, 0x00000000); - SET_SYSREG(cntvoff_el2, 0x00000000); + // Timer stuff + SET_SYSREG(cntvoff_el2, 0x00000000); + SET_SYSREG(cnthctl_el2, 0x00000003); // Don't trap anything for now; event streams disabled + SET_SYSREG(cntkctl_el1, 0x00000003); // Don't trap anything for now; event streams disabled + SET_SYSREG(cntp_ctl_el0, 0x00000000); + SET_SYSREG(cntv_ctl_el0, 0x00000000); } void initSystem(u32 coreId, bool isBootCore, u64 argument) diff --git a/thermosphere/src/irq.c b/thermosphere/src/irq.c index 87ed2b9b0..4ca9189df 100644 --- a/thermosphere/src/irq.c +++ b/thermosphere/src/irq.c @@ -103,7 +103,8 @@ static inline bool checkRescheduleEmulatedPtimer(ExceptionStackFrame *frame) { // Evaluate if the timer really has expired in the PoV of the guest kernel. If not, reschedule (add missed time delta) it & exit early u64 cval = GET_SYSREG(cntp_cval_el0); - if (cval > frame->cntvct_el0) { + u64 vct = frame->cntpct_el0 - currentCoreCtx->totalTimeInHypervisor; + if (cval > vct) { // It has not: reschedule the timer u64 offsetNow = GET_SYSREG(cntvoff_el2); SET_SYSREG(cntp_cval_el0, cval + (offsetNow - currentCoreCtx->emulPtimerOffsetThen)); @@ -115,6 +116,26 @@ static inline bool checkRescheduleEmulatedPtimer(ExceptionStackFrame *frame) return true; } + +static inline bool checkGuestTimerInterrupts(ExceptionStackFrame *frame, bool isLowerEl, u16 irqId) +{ + if (irqId != TIMER_IRQID(NS_VIRT_TIMER) && irqId != TIMER_IRQID(NS_PHYS_TIMER)) { + return true; + } + + // A thing that might have happened is losing the race vs disabling the guest interrupts + // Another thing is that the virtual timer might have fired before us updating voff when executing a top half? + if (!isLowerEl) { + return false; + } else if (irqId == TIMER_IRQID(NS_VIRT_TIMER)) { + u64 cval = GET_SYSREG(cntp_cval_el0); + u64 vct = frame->cntpct_el0 - currentCoreCtx->totalTimeInHypervisor; + return cval <= vct; + } else { + return checkRescheduleEmulatedPtimer(frame); + } +} + static void doConfigureInterrupt(u16 id, u8 prio, bool isLevelSensitive) { volatile ArmGicV2Distributor *gicd = g_irqManager.gic.gicd; @@ -186,7 +207,6 @@ bool irqIsGuest(u16 id) void handleIrqException(ExceptionStackFrame *frame, bool isLowerEl, bool isA32) { - (void)isLowerEl; (void)isA32; volatile ArmGicV2Controller *gicc = g_irqManager.gic.gicc; @@ -195,13 +215,13 @@ void handleIrqException(ExceptionStackFrame *frame, bool isLowerEl, bool isA32) u32 irqId = iar & 0x3FF; u32 srcCore = (iar >> 10) & 7; - //DEBUG("EL2 [core %d]: Received irq %x\n", (int)currentCoreCtx->coreId, irqId); + DEBUG("EL2 [core %d]: Received irq %x\n", (int)currentCoreCtx->coreId, irqId); if (irqId == GIC_IRQID_SPURIOUS) { // Spurious interrupt received return; - } else if (irqId == GIC_IRQID_NS_PHYS_TIMER && !checkRescheduleEmulatedPtimer(frame)) { - // Deactivate the ptimer interrupt, return early + } else if (!checkGuestTimerInterrupts(frame, isLowerEl, irqId)) { + // Deactivate the interrupt, return early gicc->eoir = iar; gicc->dir = iar; return; @@ -254,6 +274,7 @@ void handleIrqException(ExceptionStackFrame *frame, bool isLowerEl, bool isA32) // Bottom half part if (transportIface != NULL) { + exceptionEnterInterruptibleHypervisorCode(frame); unmaskIrq(); transportInterfaceIrqHandlerBottomHalf(transportIface); } diff --git a/thermosphere/src/main.c b/thermosphere/src/main.c index dd957a04a..2b5b22838 100644 --- a/thermosphere/src/main.c +++ b/thermosphere/src/main.c @@ -38,13 +38,50 @@ static void loadKernelViaSemihosting(void) currentCoreCtx->kernelEntrypoint = buf; } -void thermosphereMain(ExceptionStackFrame *frame) + + +#include "platform/uart.h" +typedef struct TestCtx { + char buf[512+1]; +} TestCtx; + +static TestCtx g_testCtx; + +size_t testReceiveCallback(TransportInterface *iface, void *p) +{ + TestCtx *ctx = (TestCtx *)p; + return transportInterfaceReadDataUntil(iface, ctx->buf, 512, '\r'); +} + +void testProcessDataCallback(TransportInterface *iface, void *p, size_t sz) +{ + (void)iface; + (void)sz; + TestCtx *ctx = (TestCtx *)p; + DEBUG("EL2 [core %u]: you typed: %s\n", currentCoreCtx->coreId, ctx->buf); +} + +void test(void) +{ + TransportInterface *iface = transportInterfaceCreate( + TRANSPORT_INTERFACE_TYPE_UART, + DEFAULT_UART, + DEFAULT_UART_FLAGS, + testReceiveCallback, + testProcessDataCallback, + &g_testCtx + ); + transportInterfaceSetInterruptAffinity(iface, BIT(0)); +} + +void thermosphereMain(ExceptionStackFrame *frame, u64 pct) { initIrq(); if (currentCoreCtx->isBootCore) { transportInterfaceInitLayer(); debugLogInit(); + test(); DEBUG("EL2: core %u reached main first!\n", currentCoreCtx->coreId); } @@ -75,5 +112,5 @@ void thermosphereMain(ExceptionStackFrame *frame) frame->spsr_el2 = (0xF << 6) | (1 << 2) | 1; // EL1h+DAIF frame->elr_el2 = currentCoreCtx->kernelEntrypoint; frame->x[0] = currentCoreCtx->kernelArgument; - frame->cntvct_el0 = GET_SYSREG(cntvct_el0); + frame->cntpct_el0 = pct; } diff --git a/thermosphere/src/start.s b/thermosphere/src/start.s index e26091ea8..aec77c1d5 100644 --- a/thermosphere/src/start.s +++ b/thermosphere/src/start.s @@ -39,6 +39,8 @@ _startCommon: msr daifset, 0b1111 msr spsel, #1 + mrs x20, cntpct_el0 + // Set sctlr_el2 ASAP to disable mmu/caching if not already done. mov x1, #0x0838 movk x1, #0x30C5,lsl #16 @@ -69,12 +71,13 @@ _startCommon: // Save x18, reserve space for exception frame stp x18, xzr, [sp, #-0x10]! - sub sp, sp, #0x120 + sub sp, sp, #0x140 dsb sy isb mov x0, sp + mov x1, x20 bl thermosphereMain dsb sy diff --git a/thermosphere/src/sysreg_traps.c b/thermosphere/src/sysreg_traps.c index 43ec33231..1f05866a8 100644 --- a/thermosphere/src/sysreg_traps.c +++ b/thermosphere/src/sysreg_traps.c @@ -27,11 +27,13 @@ static inline u64 doSystemRegisterRead(const ExceptionStackFrame *frame, u32 nor u64 val; switch (normalizedIss) { case ENCODE_SYSREG_ISS(CNTPCT_EL0): { - val = frame->cntvct_el0; + u64 vct = frame->cntpct_el0 - currentCoreCtx->totalTimeInHypervisor; + val = vct; break; } case ENCODE_SYSREG_ISS(CNTP_TVAL_EL0): { - val = (GET_SYSREG(cntp_cval_el0) - frame->cntvct_el0) & 0xFFFFFFFF; + u64 vct = frame->cntpct_el0 - currentCoreCtx->totalTimeInHypervisor; + val = (GET_SYSREG(cntp_cval_el0) - vct) & 0xFFFFFFFF; break; } case ENCODE_SYSREG_ISS(CNTP_CTL_EL0): { @@ -58,7 +60,7 @@ static inline u64 doSystemRegisterRead(const ExceptionStackFrame *frame, u32 nor static inline void writeEmulatedPhysicalCompareValue(u64 val) { - currentCoreCtx->emulPtimerOffsetThen = GET_SYSREG(cntvoff_el2); + currentCoreCtx->emulPtimerOffsetThen = currentCoreCtx->totalTimeInHypervisor; SET_SYSREG(cntp_cval_el0, val); } @@ -69,7 +71,8 @@ static inline void doSystemRegisterWrite(const ExceptionStackFrame *frame, u32 n switch (normalizedIss) { case ENCODE_SYSREG_ISS(CNTP_TVAL_EL0): { // Sign-extend - writeEmulatedPhysicalCompareValue(frame->cntvct_el0 + (u64)(s32)val); + u64 vct = frame->cntpct_el0 - currentCoreCtx->totalTimeInHypervisor; + writeEmulatedPhysicalCompareValue(vct + (u64)(s32)val); break; } case ENCODE_SYSREG_ISS(CNTP_CTL_EL0): { diff --git a/thermosphere/src/timer.c b/thermosphere/src/timer.c index 5fd711357..addaf7060 100644 --- a/thermosphere/src/timer.c +++ b/thermosphere/src/timer.c @@ -21,7 +21,7 @@ u64 g_timerFreq = 0; void timerInit(void) { - timerSetInterruptStatus(false, false); + timerConfigure(false, false); if (currentCoreCtx->isBootCore) { g_timerFreq = GET_SYSREG(cntfrq_el0); } @@ -29,8 +29,8 @@ void timerInit(void) void timerInterruptHandler(void) { - // Disable timer interrupts until reprogrammed - timerSetInterruptStatus(false, false); + // Disable timer programming until reprogrammed + timerConfigure(false, false); // For fun DEBUG("EL2 [core %d]: Timer interrupt at %lums\n", (int)currentCoreCtx->coreId, timerGetSystemTimeMs()); diff --git a/thermosphere/src/timer.h b/thermosphere/src/timer.h index 24a38eea2..c349ecb21 100644 --- a/thermosphere/src/timer.h +++ b/thermosphere/src/timer.h @@ -45,6 +45,10 @@ #define TIMER_CVAL_REG(name) EVAL(TIMER_CVAL_REG_FIELDS name) #define TIMER_TVAL_REG(name) EVAL(TIMER_TVAL_REG_FIELDS name) +#define TIMER_CTL_ISTATUS BITL(2) +#define TIMER_CTL_IMASK BITL(1) +#define TIMER_CTL_ENABLE BITL(0) + #define CURRENT_TIMER NS_PHYS_HYP_TIMER extern u64 g_timerFreq; @@ -67,17 +71,18 @@ static inline u64 timerGetSystemTimeMs(void) return timerGetSystemTick() * SECTOMSECS / g_timerFreq; } -static inline void timerSetInterruptStatus(bool enabled, bool masked) +static inline void timerConfigure(bool enabled, bool interruptMasked) { - u32 ebit = enabled ? BIT(0) : 0; - u32 mbit = masked ? BIT(1) : 0; + u64 ebit = enabled ? TIMER_CTL_ENABLE : 0; + u64 mbit = interruptMasked ? TIMER_CTL_IMASK : 0; SET_SYSREG(TIMER_CTL_REG(CURRENT_TIMER), mbit | ebit); } static inline void timerSetTimeoutTicks(u64 ticks) { + timerConfigure(true, true); SET_SYSREG(TIMER_CVAL_REG(CURRENT_TIMER), timerGetSystemTick() + ticks); - timerSetInterruptStatus(true, false); + timerConfigure(true, false); } static inline void timerSetTimeoutNs(u64 ns)