thermosphere: refactor exception handlers & add stolen time/emulated ptimer logic

thermosphere/src/core_ctx.h

@@ -15,6 +15,7 @@
  */
 
 #pragma once
+#include <assert.h>
 #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");
 

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
+.macro PIVOT_STACK_FOR_CRASH
-    save_all_regs
-
-    // Reload our x18 value (currentCoreCtx)
-    ldp     x18, xzr, [sp, #0x120]
-.endm
-
-.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
+.global     g_thermosphereVectors
-vector_base thermosphere_vectors
+vector_base g_thermosphereVectors
 
 /* Current EL, SP0 */
 /* Those are unused by us, except on same-EL double-faults. */
-.global unknown_exception
+UNKNOWN_EXCEPTION       _synchSp0
-unknown_exception:
+
-vector_entry synch_sp0
+_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
+UNKNOWN_EXCEPTION       _irqSp0
-    bl      unknown_exception
-    .endfunc
-    .cfi_endproc
-    /* To save space, insert in an unused vector segment. */
-    _save_all_regs:
 
+/* 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
+UNKNOWN_EXCEPTION       _fiqSp0
-    bl      unknown_exception
+
-    .endfunc
+/* To save space, insert in an unused vector segment. */
-    .cfi_endproc
+
-    /* To save space, insert in an unused vector segment. */
+// Accessed by start.s
-    .global _restore_all_regs
+.global     _restoreAllRegisters
-    _restore_all_regs:
+.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
+UNKNOWN_EXCEPTION       _serrorSp0
-    bl      unknown_exception
-    .endfunc
-    .cfi_endproc
-    /* To save space, insert in an unused vector segment. */
 
+/* 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       .
+EXCEPTION_HANDLER_END       _synchSpx, EXCEPTION_TYPE_HOST_CRASH
-    check_vector_size synch_spx
-
-vector_entry irq_spx
-    save_all_regs
 
+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
+UNKNOWN_EXCEPTION           _fiqSpx
-
+UNKNOWN_EXCEPTION           _serrorSpx
-    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
 
 /* 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
+EXCEPTION_HANDLER_START     _irqA64, EXCEPTION_TYPE_GUEST
-    check_vector_size synch_a64
-
-vector_entry irq_a64
-    save_all_regs_reload_x18
-
     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
+UNKNOWN_EXCEPTION           _fiqA64
-    check_vector_size irq_a64
+UNKNOWN_EXCEPTION           _serrorA64
-
-vector_entry fiq_a64
-    bl      unknown_exception
-    check_vector_size fiq_a64
-
-vector_entry serror_a64
-    bl      unknown_exception
-    check_vector_size serror_a64
-
 
 /* Lower EL, A32 */
-vector_entry synch_a32
-    bl      unknown_exception
-    check_vector_size synch_a32
 
-vector_entry irq_a32
+EXCEPTION_HANDLER_START     _synchA32, EXCEPTION_TYPE_GUEST
-    save_all_regs_reload_x18
+    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
+UNKNOWN_EXCEPTION           _fiqA32
-    check_vector_size irq_a32
+UNKNOWN_EXCEPTION           _serrorA32
-
-vector_entry fiq_a32
-    b       fiq_a64
-    check_vector_size fiq_a32
-
-vector_entry serror_a32
-    bl      unknown_exception
-    check_vector_size serror_a32

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);

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

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)

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;

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->spsr_el2     = (0xF << 6) | (1 << 2) | 1; // EL1h+DAIF
-    frame->elr_el2  = currentCoreCtx->kernelEntrypoint;
+    frame->elr_el2      = currentCoreCtx->kernelEntrypoint;
-    frame->x[0]     = currentCoreCtx->kernelArgument;
+    frame->x[0]         = currentCoreCtx->kernelArgument;
-
+    frame->cntvct_el0   = GET_SYSREG(cntvct_el0);
-    setCurrentCoreActive();
 }

thermosphere/src/start.s

@@ -81,6 +81,6 @@ _startCommon:
     isb
 
     // Jump to kernel
-    b       _restore_all_regs
+    b       _restoreAllRegisters
 
 .pool

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 = frame->cntvct_el0;
-            val = GET_SYSREG(cntpct_el0);
             break;
         }
         case ENCODE_SYSREG_ISS(CNTP_TVAL_EL0): {
-            // FIXME too
+            val = (GET_SYSREG(cntp_cval_el0) - frame->cntvct_el0) & 0xFFFFFFFF;
-            val = GET_SYSREG(cntp_tval_el0);
             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
+            // Sign-extend
-            SET_SYSREG(cntp_tval_el0, val);
+            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
+            writeEmulatedPhysicalCompareValue(val);
-            SET_SYSREG(cntp_cval_el0, 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);
 }