diff --git a/thermosphere/src/core_ctx.h b/thermosphere/src/core_ctx.h index 959c6d8c0..04b517754 100644 --- a/thermosphere/src/core_ctx.h +++ b/thermosphere/src/core_ctx.h @@ -15,6 +15,7 @@ */ #pragma once +#include #include "utils.h" #include "barrier.h" #include "execute_function.h" @@ -34,8 +35,14 @@ typedef struct CoreCtx { void *executedFunctionArgs; // @0x30 Barrier executedFunctionBarrier; // @0x38 bool executedFunctionSync; // @0x3C + + // Timer stuff + u64 emulPtimerOffsetThen; // @0x40. 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"); + 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 4480606f2..e17679ce5 100644 --- a/thermosphere/src/exception_vectors.s +++ b/thermosphere/src/exception_vectors.s @@ -50,19 +50,12 @@ .endif .endm -.macro save_all_regs +.macro SAVE_ALL_REGISTERS stp x30, xzr, [sp, #-0x130] - bl _save_all_regs + bl _saveAllRegisters .endm -.macro save_all_regs_reload_x18 - save_all_regs - - // Reload our x18 value (currentCoreCtx) - ldp x18, xzr, [sp, #0x120] -.endm - -.macro pivot_stack_for_crash +.macro PIVOT_STACK_FOR_CRASH // Note: x18 assumed uncorrupted // Note: replace sp_el0 with crashing sp str x16, [x18, #0x18] // currentCoreCtx->scratch = x16 @@ -73,30 +66,58 @@ ldr x16, [x18, #0x18] .endm +.equ EXCEPTION_TYPE_HOST, 0 +.equ EXCEPTION_TYPE_GUEST, 1 +.equ EXCEPTION_TYPE_HOST_CRASH, 2 + +.macro EXCEPTION_HANDLER_START name, type +vector_entry \name + .if \type == EXCEPTION_TYPE_HOST_CRASH + PIVOT_STACK_FOR_CRASH + .endif + + SAVE_ALL_REGISTERS + + .if \type == EXCEPTION_TYPE_GUEST + ldp x18, xzr, [sp, #0x120] + .endif +.endm + +.macro EXCEPTION_HANDLER_END name, type + .if \type != EXCEPTION_TYPE_HOST_CRASH + b _restoreAllRegisters + .else + b . + .endif +check_vector_size \name +.endm + +.macro UNKNOWN_EXCEPTION name +vector_entry \name + bl _unknownException +check_vector_size \name +.endm + /* Actual Vectors for Thermosphere. */ -.global thermosphere_vectors -vector_base thermosphere_vectors +.global g_thermosphereVectors +vector_base g_thermosphereVectors /* Current EL, SP0 */ /* Those are unused by us, except on same-EL double-faults. */ -.global unknown_exception -unknown_exception: -vector_entry synch_sp0 +UNKNOWN_EXCEPTION _synchSp0 + +_unknownException: pivot_stack_for_crash mov x0, x30 - adr x1, thermosphere_vectors + 4 + adr x1, g_thermosphereVectors + 4 sub x0, x0, x1 bl handleUnknownException b . - check_vector_size synch_sp0 -vector_entry irq_sp0 - bl unknown_exception - .endfunc - .cfi_endproc - /* To save space, insert in an unused vector segment. */ - _save_all_regs: +UNKNOWN_EXCEPTION _irqSp0 +/* To save space, insert in an unused vector segment. */ +_saveAllRegisters: sub sp, sp, #0x120 stp x0, x1, [sp, #0x00] stp x2, x3, [sp, #0x10] @@ -115,37 +136,44 @@ vector_entry irq_sp0 stp x28, x29, [sp, #0xE0] mov x29, x30 - ldp x30, xzr, [sp, #-0x10] // See save_all_regs macro + 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 stp x30, x20, [sp, #0xF0] stp x21, x22, [sp, #0x100] - stp x23, xzr, [sp, #0x110] + stp x23, x24, [sp, #0x110] mov x30, x29 ret -vector_entry fiq_sp0 - bl unknown_exception - .endfunc - .cfi_endproc - /* To save space, insert in an unused vector segment. */ - .global _restore_all_regs - _restore_all_regs: +UNKNOWN_EXCEPTION _fiqSp0 + +/* To save space, insert in an unused vector segment. */ + +// Accessed by start.s +.global _restoreAllRegisters +.type _restoreAllRegisters, %function +_restoreAllRegisters: ldp x30, x20, [sp, #0xF0] ldp x21, x22, [sp, #0x100] - ldp x23, xzr, [sp, #0x110] + ldp x23, x24, [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] @@ -165,20 +193,20 @@ vector_entry fiq_sp0 add sp, sp, #0x120 eret -vector_entry serror_sp0 - bl unknown_exception - .endfunc - .cfi_endproc - /* To save space, insert in an unused vector segment. */ +UNKNOWN_EXCEPTION _serrorSp0 +/* To save space, insert in an unused vector segment. */ .global semihosting_call .type semihosting_call, %function +.func semihosting_call +.cfi_startproc semihosting_call: hlt #0xF000 ret +.cfi_endproc +.endfunc .global doSmcIndirectCallImpl -//.type doSmcIndirectCallImpl, %function doSmcIndirectCallImpl: stp x19, x20, [sp, #-0x10]! mov x19, x0 @@ -208,89 +236,55 @@ doSmcIndirectCallImplSize: .word _doSmcIndirectCallImplEnd - doSmcIndirectCallImpl /* Current EL, SPx */ -vector_entry synch_spx - /* Only crashes go through there! */ - pivot_stack_for_crash - - save_all_regs +EXCEPTION_HANDLER_START _synchSpx, EXCEPTION_TYPE_HOST_CRASH mov x0, sp mrs x1, esr_el2 bl handleSameElSyncException - b . - check_vector_size synch_spx - -vector_entry irq_spx - save_all_regs +EXCEPTION_HANDLER_END _synchSpx, EXCEPTION_TYPE_HOST_CRASH +EXCEPTION_HANDLER_START _irqSpx, EXCEPTION_TYPE_HOST mov x0, sp mov w1, wzr mov w2, wzr bl handleIrqException +EXCEPTION_HANDLER_END _irqSpx, EXCEPTION_TYPE_HOST - b _restore_all_regs - - check_vector_size irq_spx - -vector_entry fiq_spx - bl unknown_exception - check_vector_size fiq_spx - -vector_entry serror_spx - bl unknown_exception - check_vector_size serror_spx +UNKNOWN_EXCEPTION _fiqSpx +UNKNOWN_EXCEPTION _serrorSpx /* Lower EL, A64 */ -vector_entry synch_a64 - save_all_regs_reload_x18 +EXCEPTION_HANDLER_START _synchA64, EXCEPTION_TYPE_GUEST mov x0, sp mrs x1, esr_el2 bl handleLowerElSyncException +EXCEPTION_HANDLER_END _synchA64, EXCEPTION_TYPE_GUEST - b _restore_all_regs - check_vector_size synch_a64 - -vector_entry irq_a64 - save_all_regs_reload_x18 - +EXCEPTION_HANDLER_START _irqA64, EXCEPTION_TYPE_GUEST mov x0, sp mov w1, #1 - mov w2, wzr + mov w2, #0 bl handleIrqException +EXCEPTION_HANDLER_END _irqA64, EXCEPTION_TYPE_GUEST - b _restore_all_regs - check_vector_size irq_a64 - -vector_entry fiq_a64 - bl unknown_exception - check_vector_size fiq_a64 - -vector_entry serror_a64 - bl unknown_exception - check_vector_size serror_a64 - +UNKNOWN_EXCEPTION _fiqA64 +UNKNOWN_EXCEPTION _serrorA64 /* Lower EL, A32 */ -vector_entry synch_a32 - bl unknown_exception - check_vector_size synch_a32 -vector_entry irq_a32 - save_all_regs_reload_x18 +EXCEPTION_HANDLER_START _synchA32, EXCEPTION_TYPE_GUEST + mov x0, sp + mrs x1, esr_el2 + bl handleLowerElSyncException +EXCEPTION_HANDLER_END _synchA32, EXCEPTION_TYPE_GUEST +EXCEPTION_HANDLER_START _irqA32, EXCEPTION_TYPE_GUEST mov x0, sp mov w1, #1 mov w2, #1 bl handleIrqException +EXCEPTION_HANDLER_END _irqA32, EXCEPTION_TYPE_GUEST - b _restore_all_regs - check_vector_size irq_a32 - -vector_entry fiq_a32 - b fiq_a64 - check_vector_size fiq_a32 - -vector_entry serror_a32 - bl unknown_exception - check_vector_size serror_a32 +UNKNOWN_EXCEPTION _fiqA32 +UNKNOWN_EXCEPTION _serrorA32 diff --git a/thermosphere/src/exceptions.c b/thermosphere/src/exceptions.c index c3e4d81d6..c64146ff1 100644 --- a/thermosphere/src/exceptions.c +++ b/thermosphere/src/exceptions.c @@ -66,6 +66,7 @@ void dumpStackFrame(const ExceptionStackFrame *frame, bool sameEl) 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); #else (void)frame; (void)sameEl; @@ -93,7 +94,6 @@ void skipFaultingInstruction(ExceptionStackFrame *frame, u32 size) void handleLowerElSyncException(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr) { - switch (esr.ec) { case Exception_CP15RTTrap: handleMcrMrcCP15Trap(frame, esr); diff --git a/thermosphere/src/exceptions.h b/thermosphere/src/exceptions.h index e4a607e38..84ab2e082 100644 --- a/thermosphere/src/exceptions.h +++ b/thermosphere/src/exceptions.h @@ -26,6 +26,7 @@ typedef struct ExceptionStackFrame { }; u64 elr_el2; u64 spsr_el2; + u64 cntvct_el0; } ExceptionStackFrame; // Adapted from https://developer.arm.com/docs/ddi0596/a/a64-shared-pseudocode-functions/shared-exceptions-pseudocode diff --git a/thermosphere/src/init.c b/thermosphere/src/initSystem.c similarity index 97% rename from thermosphere/src/init.c rename to thermosphere/src/initSystem.c index 231d256e4..e616a3317 100644 --- a/thermosphere/src/init.c +++ b/thermosphere/src/initSystem.c @@ -35,6 +35,8 @@ static void initSysregs(void) SET_SYSREG(mdcr_el2, 0x00000000); SET_SYSREG(mdscr_el1, 0x00000000); + + SET_SYSREG(cntvoff_el2, 0x00000000); } void initSystem(u32 coreId, bool isBootCore, u64 argument) diff --git a/thermosphere/src/irq.c b/thermosphere/src/irq.c index fa4127a0b..472078ff5 100644 --- a/thermosphere/src/irq.c +++ b/thermosphere/src/irq.c @@ -131,6 +131,22 @@ void initIrq(void) recursiveSpinlockUnlockRestoreIrq(&g_irqManager.lock, flags); } +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) { + // It has not: reschedule the timer + u64 offsetNow = GET_SYSREG(cntvoff_el2); + SET_SYSREG(cntp_cval_el0, cval + (offsetNow - currentCoreCtx->emulPtimerOffsetThen)); + currentCoreCtx->emulPtimerOffsetThen = offsetNow; + + return false; + } + + return true; +} + void handleIrqException(ExceptionStackFrame *frame, bool isLowerEl, bool isA32) { (void)isLowerEl; @@ -147,6 +163,11 @@ void handleIrqException(ExceptionStackFrame *frame, bool isLowerEl, bool isA32) 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 + gicc->eoir = iar; + gicc->dir = iar; + return; } bool isGuestInterrupt = false; diff --git a/thermosphere/src/main.c b/thermosphere/src/main.c index 6dc53be9d..5f055b415 100644 --- a/thermosphere/src/main.c +++ b/thermosphere/src/main.c @@ -64,11 +64,12 @@ void thermosphereMain(ExceptionStackFrame *frame) DEBUG("EL2: core %u reached main!\n", currentCoreCtx->coreId); } + setCurrentCoreActive(); + // Set up exception frame: init regs to 0, set spsr, elr, etc. memset(frame, 0, sizeof(ExceptionStackFrame)); - frame->spsr_el2 = (0xF << 6) | (1 << 2) | 1; // EL1h+DAIF - frame->elr_el2 = currentCoreCtx->kernelEntrypoint; - frame->x[0] = currentCoreCtx->kernelArgument; - - setCurrentCoreActive(); + 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); } diff --git a/thermosphere/src/start.s b/thermosphere/src/start.s index dcb3f6dc7..e26091ea8 100644 --- a/thermosphere/src/start.s +++ b/thermosphere/src/start.s @@ -81,6 +81,6 @@ _startCommon: isb // Jump to kernel - b _restore_all_regs + b _restoreAllRegisters .pool diff --git a/thermosphere/src/sysreg_traps.c b/thermosphere/src/sysreg_traps.c index aeec4fba7..43ec33231 100644 --- a/thermosphere/src/sysreg_traps.c +++ b/thermosphere/src/sysreg_traps.c @@ -20,18 +20,18 @@ #include "debug_log.h" #include "software_breakpoints.h" -static inline u64 doSystemRegisterRead(u32 normalizedIss) +static inline u64 doSystemRegisterRead(const ExceptionStackFrame *frame, u32 normalizedIss) { + // See https://developer.arm.com/architectures/learn-the-architecture/generic-timer/single-page + u64 val; switch (normalizedIss) { case ENCODE_SYSREG_ISS(CNTPCT_EL0): { - // FIXME - val = GET_SYSREG(cntpct_el0); + val = frame->cntvct_el0; break; } case ENCODE_SYSREG_ISS(CNTP_TVAL_EL0): { - // FIXME too - val = GET_SYSREG(cntp_tval_el0); + val = (GET_SYSREG(cntp_cval_el0) - frame->cntvct_el0) & 0xFFFFFFFF; break; } case ENCODE_SYSREG_ISS(CNTP_CTL_EL0): { @@ -56,12 +56,20 @@ static inline u64 doSystemRegisterRead(u32 normalizedIss) return val; } -static inline void doSystemRegisterWrite(u32 normalizedIss, u64 val) +static inline void writeEmulatedPhysicalCompareValue(u64 val) { + currentCoreCtx->emulPtimerOffsetThen = GET_SYSREG(cntvoff_el2); + SET_SYSREG(cntp_cval_el0, val); +} + +static inline void doSystemRegisterWrite(const ExceptionStackFrame *frame, u32 normalizedIss, u64 val) +{ + // See https://developer.arm.com/architectures/learn-the-architecture/generic-timer/single-page + switch (normalizedIss) { case ENCODE_SYSREG_ISS(CNTP_TVAL_EL0): { - // FIXME - SET_SYSREG(cntp_tval_el0, val); + // Sign-extend + writeEmulatedPhysicalCompareValue(frame->cntvct_el0 + (u64)(s32)val); break; } case ENCODE_SYSREG_ISS(CNTP_CTL_EL0): { @@ -70,8 +78,7 @@ static inline void doSystemRegisterWrite(u32 normalizedIss, u64 val) break; } case ENCODE_SYSREG_ISS(CNTP_CVAL_EL0): { - // Passthrough - SET_SYSREG(cntp_cval_el0, val); + writeEmulatedPhysicalCompareValue(val); break; } @@ -85,33 +92,33 @@ static inline void doSystemRegisterWrite(u32 normalizedIss, u64 val) static inline void doMrs(ExceptionStackFrame *frame, u32 normalizedIss, u32 reg) { - writeFrameRegisterZ(frame, reg, doSystemRegisterRead(normalizedIss)); + writeFrameRegisterZ(frame, reg, doSystemRegisterRead(frame, normalizedIss)); skipFaultingInstruction(frame, 4); } static inline void doMsr(ExceptionStackFrame *frame, u32 normalizedIss, u32 reg) { u64 val = readFrameRegisterZ(frame, reg); - doSystemRegisterWrite(normalizedIss, val); + doSystemRegisterWrite(frame, normalizedIss, val); skipFaultingInstruction(frame, 4); } static inline void doMrc(ExceptionStackFrame *frame, u32 normalizedIss, u32 instructionLength, u32 reg) { - writeFrameRegisterZ(frame, reg, doSystemRegisterRead(normalizedIss) & 0xFFFFFFFF); + writeFrameRegisterZ(frame, reg, doSystemRegisterRead(frame, normalizedIss) & 0xFFFFFFFF); skipFaultingInstruction(frame, instructionLength); } static inline void doMcr(ExceptionStackFrame *frame, u32 normalizedIss, u32 instructionLength, u32 reg) { u64 val = readFrameRegisterZ(frame, reg) & 0xFFFFFFFF; - doSystemRegisterWrite(normalizedIss, val); + doSystemRegisterWrite(frame, normalizedIss, val); skipFaultingInstruction(frame, instructionLength); } static inline void doMrrc(ExceptionStackFrame *frame, u32 normalizedIss, u32 instructionLength, u32 reg, u32 reg2) { - u64 val = doSystemRegisterRead(normalizedIss); + u64 val = doSystemRegisterRead(frame, normalizedIss); writeFrameRegister(frame, reg, val & 0xFFFFFFFF); writeFrameRegister(frame, reg2, val >> 32); skipFaultingInstruction(frame, instructionLength); @@ -121,7 +128,7 @@ static inline void doMcrr(ExceptionStackFrame *frame, u32 normalizedIss, u32 ins { u64 valLo = readFrameRegister(frame, reg) & 0xFFFFFFFF; u64 valHi = readFrameRegister(frame, reg2) << 32; - doSystemRegisterWrite(normalizedIss, valHi | valLo); + doSystemRegisterWrite(frame, normalizedIss, valHi | valLo); skipFaultingInstruction(frame, instructionLength); }