thermosphere: properly implement guest timer stuff

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
 

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

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
+.macro  SAVE_MOST_REGISTERS
-    stp     x30, xzr, [sp, #-0x130]
+    sub     sp, sp, #STACK_FRAME_SIZE
-    bl      _saveAllRegisters
+
+    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:
+_saveMostRegisters:
-    sub     sp, sp, #0x120
     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]
-
+    stp     x25, xzr, [sp, #0x120]
-    mov     x30, x29
 
     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

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

thermosphere/src/exceptions.h

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

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(hcr_el2,         0x80000000);
-    SET_SYSREG(dacr32_el2,  0xFFFFFFFF); // unused
+    SET_SYSREG(dacr32_el2,      0xFFFFFFFF);    // unused
-    SET_SYSREG(sctlr_el1,   0x00C50838);
+    SET_SYSREG(sctlr_el1,       0x00C50838);
 
-    SET_SYSREG(mdcr_el2,    0x00000000);
+    SET_SYSREG(mdcr_el2,        0x00000000);
-    SET_SYSREG(mdscr_el1,   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)

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)) {
+    } else if (!checkGuestTimerInterrupts(frame, isLowerEl, irqId)) {
-        // Deactivate the ptimer interrupt, return early
+        // 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);
     }

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

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

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): {

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
+    // Disable timer programming until reprogrammed
-    timerSetInterruptStatus(false, false);
+    timerConfigure(false, false);
 
     // For fun
     DEBUG("EL2 [core %d]: Timer interrupt at %lums\n", (int)currentCoreCtx->coreId, timerGetSystemTimeMs());

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;
+    u64 ebit = enabled          ? TIMER_CTL_ENABLE : 0;
-    u32 mbit = masked  ? BIT(1) : 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)