thermosphere: rewrite sysreg trapping code, add skeleton code for timer val trap handling; support A32 EL1 once again

thermosphere/src/breakpoints_watchpoints_common.h

@@ -85,5 +85,5 @@ typedef struct DebugRegisterPair {
 
 static inline void enableBreakpointsAndWatchpoints(void)
 {
-    SET_SYSREG(mdscr_el1, GET_SYSREG(mdscr_el1) | MDSCR_EL1_MDE);
+    SET_SYSREG(mdscr_el1, GET_SYSREG(mdscr_el1) | MDSCR_MDE);
 }

thermosphere/src/exceptions.c

@@ -96,14 +96,17 @@ void handleLowerElSyncException(ExceptionStackFrame *frame, ExceptionSyndromeReg
 
     switch (esr.ec) {
         case Exception_CP15RTTrap:
+            handleMcrMrcCP15Trap(frame, esr);
+            break;
         case Exception_CP15RRTTrap:
+            handleMcrrMrrcCP15Trap(frame, esr);
+            break;
         case Exception_CP14RTTrap:
         case Exception_CP14DTTrap:
-        case Exception_CP14RRTTrap: {
+        case Exception_CP14RRTTrap:
             // A32 stub: Skip instruction, read 0 if necessary (there are debug regs at EL0)
-            handleSysregAccessA32Stub(frame, esr);
+            handleA32CP14Trap(frame, esr);
             break;
-        }
         case Exception_HypervisorCallA64:
             handleHypercall(frame, esr);
             break;

thermosphere/src/exceptions.h

@@ -102,11 +102,21 @@ static inline void spsrSetT32ItFlags(u64 *spsr, u32 itFlags)
     *spsr |= ((itFlags >> 2) & 0x3F) << 10;
 }
 
+static inline u64 readFrameRegister(ExceptionStackFrame *frame, u32 id)
+{
+    return frame->x[id];
+}
+
 static inline u64 readFrameRegisterZ(ExceptionStackFrame *frame, u32 id)
 {
     return id == 31 ? 0 /* xzr */ : frame->x[id];
 }
 
+static inline void writeFrameRegister(ExceptionStackFrame *frame, u32 id, u64 val)
+{
+    frame->x[id] = val;
+}
+
 static inline void writeFrameRegisterZ(ExceptionStackFrame *frame, u32 id, u64 val)
 {
     if (id != 31) {

thermosphere/src/single_step.c

@@ -22,7 +22,7 @@
 SingleStepState singleStepGetNextState(ExceptionStackFrame *frame)
 {
     u64 mdscr = GET_SYSREG(mdscr_el1);
-    bool mdscrSS = (mdscr & MDSCR_EL1_SS) != 0;
+    bool mdscrSS = (mdscr & MDSCR_SS) != 0;
     bool pstateSS = (frame->spsr_el2 & PSTATE_SS) != 0;
 
     if (!mdscrSS) {
@@ -39,16 +39,16 @@ void singleStepSetNextState(ExceptionStackFrame *frame, SingleStepState state)
     switch (state) {
         case SingleStepState_Inactive:
             // Unset mdscr_el1.ss
-            mdscr &= ~MDSCR_EL1_SS;
+            mdscr &= ~MDSCR_SS;
             break;
         case SingleStepState_ActivePending:
             // Set mdscr_el1.ss and pstate.ss
-            mdscr |= MDSCR_EL1_SS;
+            mdscr |= MDSCR_SS;
             frame->spsr_el2 |= PSTATE_SS;
             break;
         case SingleStepState_ActiveNotPending:
             // Set mdscr_el1.ss and unset pstate.ss
-            mdscr |= MDSCR_EL1_SS;
+            mdscr |= MDSCR_SS;
             frame->spsr_el2 |= PSTATE_SS;
             break;
         default:

thermosphere/src/sysreg.h

@@ -237,6 +237,7 @@
 #define TUP_CNTV_CVAL_EL0       (3, 3, 14, 3, 2)
 
 #define TUP_CNTVOFF_EL2         (3, 4, 14, 0, 3)
+#define TUP_CNTHCTL_EL2         (3, 4, 14, 1, 0)
 #define TUP_CNTHP_CVAL_EL2      (3, 4, 14, 2, 2)
 
 #define __field_PMEV_op2(n)     ((n) & 0x7)
@@ -403,8 +404,16 @@
 #define MDCR_EL2_TPMCR      BITL(5)
 #define MDCR_EL2_HPMN_MASK  0x1Full
 
-#define MDSCR_EL1_MDE       BITL(15)
+#define MDSCR_MDE           BITL(15)
-#define MDSCR_EL1_SS        BITL(0)
+#define MDSCR_SS            BITL(0)
+
+// Common CNTHCTL_EL2 flags
+#define CNTHCTL_EVNTI_MASK  0xFll
+#define CNTHCTL_EVNTI_SHIFT 4
+#define CNTHCTL_EVNTDIR     BITL(3)
+#define CNTHCTL_EVNTEN      BITL(2)
+#define CNTHCTL_EL1PCEN     BITL(1)
+#define CNTHCTL_EL1PCTEN    BITL(0)
 
 #define ENCODE_SYSREG_FIELDS_MOV(op0, op1, crn, crm, op2) (((op0) << 19) | ((op1) << 16) | ((crn) << 12) | ((crm) << 8) | ((op2) << 5))
 #define ENCODE_SYSREG_MOV(name) EVAL(ENCODE_SYSREG_FIELDS_MOV CAT(TUP_, name))

thermosphere/src/sysreg_traps.c

@@ -20,76 +20,109 @@
 #include "debug_log.h"
 #include "software_breakpoints.h"
 
-static void doSystemRegisterRwImpl(u64 *val, u32 iss)
+static inline u64 doSystemRegisterRead(u32 normalizedIss)
 {
-    u32 op0  = (iss >> 20) & 3;
+    u64 val;
-    u32 op2  = (iss >> 17) & 7;
+    switch (normalizedIss) {
-    u32 op1  = (iss >> 14) & 7;
+        case ENCODE_SYSREG_ISS(CNTPCT_EL0): {
-    u32 CRn  = (iss >> 10) & 15;
+            // FIXME
-    //u32 Rt   = (iss >>  5) & 31;
+            val = GET_SYSREG(cntpct_el0);
-    u32 CRm  = (iss >>  1) & 15;
+            break;
-    u32 dir  = iss & 1;
-
-    u32 codebuf[] = {
-        0,              // TBD
-        0xD65F03C0,     // ret
-    };
-
-    codebuf[0] = dir ? MAKE_MRS_FROM_FIELDS(op0, op1, CRn, CRm, op2, 0) : MAKE_MSR_FROM_FIELDS(op0, op1, CRn, CRm, op2, 0);
-
-    flush_dcache_range(codebuf, (u8 *)codebuf + sizeof(codebuf));
-    invalidate_icache_all();
-
-    *val = ((u64 (*)(u64))codebuf)(*val);
         }
-
+        case ENCODE_SYSREG_ISS(CNTP_TVAL_EL0): {
-void doSystemRegisterRead(ExceptionStackFrame *frame, u32 iss, u32 reg)
+            // FIXME too
-{
+            val = GET_SYSREG(cntp_tval_el0);
-    u64 val = 0;
+            break;
-
+        }
-    iss &= ~((0x1F << 5) | 1);
+        case ENCODE_SYSREG_ISS(CNTP_CTL_EL0): {
-
+            // Passthrough
-    // Hooks go here:
+            val = GET_SYSREG(cntp_ctl_el0);
-    switch (iss) {
+            break;
-        default:
+        }
+        case ENCODE_SYSREG_ISS(CNTP_CVAL_EL0): {
+            // Passthrough
+            val = GET_SYSREG(cntp_cval_el0);
             break;
         }
 
-    doSystemRegisterRwImpl(&val, iss | 1);
+        default: {
-    writeFrameRegisterZ(frame, reg, val);
+            // We shouldn't have trapped on other registers other than debug regs
-
+            // and we want the latter as RA0/WI
-    skipFaultingInstruction(frame, 4);
+            val = 0;
+            break;
+        }
     }
 
-void doSystemRegisterWrite(ExceptionStackFrame *frame, u32 iss, u32 reg)
+    return val;
+}
+
+static inline void doSystemRegisterWrite(u32 normalizedIss, u64 val)
 {
-    u64 val = 0;
+    switch (normalizedIss) {
-    iss &= ~((0x1F << 5) | 1);
+        case ENCODE_SYSREG_ISS(CNTP_TVAL_EL0): {
-
+            // FIXME
-    val = readFrameRegisterZ(frame, reg);
+            SET_SYSREG(cntp_tval_el0, val);
-
+            break;
-    // Hooks go here:
+        }
-    switch (iss) {
+        case ENCODE_SYSREG_ISS(CNTP_CTL_EL0): {
-        default:
+            // Passthrough
+            SET_SYSREG(cntp_ctl_el0, val);
+            break;
+        }
+        case ENCODE_SYSREG_ISS(CNTP_CVAL_EL0): {
+            // Passthrough
+            SET_SYSREG(cntp_cval_el0, val);
             break;
         }
 
-    doSystemRegisterRwImpl(&val, iss);
+        default: {
+            // We shouldn't have trapped on other registers other than debug regs
+            // and we want the latter as RA0/WI
+            break;
+        }
+    }
+}
 
+static inline void doMrs(ExceptionStackFrame *frame, u32 normalizedIss, u32 reg)
+{
+    writeFrameRegisterZ(frame, reg, doSystemRegisterRead(normalizedIss));
     skipFaultingInstruction(frame, 4);
 }
 
-void handleMsrMrsTrap(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr)
+static inline void doMsr(ExceptionStackFrame *frame, u32 normalizedIss, u32 reg)
 {
-    u32 iss = esr.iss;
+    u64 val = readFrameRegisterZ(frame, reg);
-    u32 reg = (iss >> 5) & 31;
+    doSystemRegisterWrite(normalizedIss, val);
-    bool isRead = (iss & 1) != 0;
+    skipFaultingInstruction(frame, 4);
-
-    if (isRead) {
-        doSystemRegisterRead(frame, iss, reg);
-    } else {
-        doSystemRegisterWrite(frame, iss, reg);
 }
+
+static inline void doMrc(ExceptionStackFrame *frame, u32 normalizedIss, u32 instructionLength, u32 reg)
+{
+    writeFrameRegisterZ(frame, reg, doSystemRegisterRead(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);
+    skipFaultingInstruction(frame, instructionLength);
+}
+
+static inline void doMrrc(ExceptionStackFrame *frame, u32 normalizedIss, u32 instructionLength, u32 reg, u32 reg2)
+{
+    u64 val = doSystemRegisterRead(normalizedIss);
+    writeFrameRegister(frame, reg, val & 0xFFFFFFFF);
+    writeFrameRegister(frame, reg2, val >> 32);
+    skipFaultingInstruction(frame, instructionLength);
+}
+
+static inline void doMcrr(ExceptionStackFrame *frame, u32 normalizedIss, u32 instructionLength, u32 reg, u32 reg2)
+{
+    u64 valLo = readFrameRegister(frame, reg)  & 0xFFFFFFFF;
+    u64 valHi = readFrameRegister(frame, reg2) << 32;
+    doSystemRegisterWrite(normalizedIss, valHi | valLo);
+    skipFaultingInstruction(frame, instructionLength);
 }
 
 static bool evaluateMcrMrcCondition(u64 spsr, u32 condition, bool condValid)
@@ -103,9 +136,87 @@ static bool evaluateMcrMrcCondition(u64 spsr, u32 condition, bool condValid)
     }
 }
 
-void handleSysregAccessA32Stub(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr)
+void handleMsrMrsTrap(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr)
 {
-    // A32 stub: Skip instruction, read 0 if necessary (there are debug regs at EL0)
+    u32 iss = esr.iss;
+    u32 reg = (iss >> 5) & 31;
+    bool isRead = (iss & 1) != 0;
+
+    iss &= ~((0x1F << 5) | 1);
+
+    if (isRead) {
+        doMrs(frame, iss, reg);
+    } else {
+        doMsr(frame, iss, reg);
+    }
+}
+
+void handleMcrMrcCP15Trap(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr)
+{
+    u32 iss = esr.iss;
+
+    if (!evaluateMcrMrcCondition(frame->spsr_el2, (iss >> 20) & 0xF, (iss & BIT(24)) != 0)) {
+        // If instruction not valid/condition code says no
+        skipFaultingInstruction(frame, esr.il == 0 ? 2 : 4);
+        return;
+    }
+
+    u32 opc2 = (iss >> 17) & 7;
+    u32 opc1 = (iss >> 14) & 7;
+    u32 CRn  = (iss >> 10) & 15;
+    u32 Rt   = (iss >>  5) & 31;
+    u32 CRm  = (iss >>  1) & 15;
+    bool isRead = (iss & 1) != 0;
+    u32 instructionLength = esr.il == 0 ? 2 : 4;
+
+    if (LIKELY(opc1 == 0 && CRn == 14 && CRm == 2 && opc2 <= 1)) {
+        iss = opc2 == 0 ? ENCODE_SYSREG_ISS(CNTP_TVAL_EL0) : ENCODE_SYSREG_ISS(CNTP_CTL_EL0);
+    } else {
+        PANIC("handleMcrMrcTrap: unexpected cp15 register, instruction: %s p15, #%u, r%u, c%u, c%u, #%u\n", isRead ? "mrc" : "mcr", opc1, Rt, CRn, CRm, opc2);
+    }
+
+    if (isRead) {
+        doMrc(frame, iss, instructionLength, Rt);
+    } else {
+        doMcr(frame, iss, instructionLength, Rt);
+    }
+}
+
+void handleMcrrMrrcCP15Trap(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr)
+{
+    u32 iss = esr.iss;
+
+    if (!evaluateMcrMrcCondition(frame->spsr_el2, (iss >> 20) & 0xF, (iss & BIT(24)) != 0)) {
+        // If instruction not valid/condition code says no
+        skipFaultingInstruction(frame, esr.il == 0 ? 2 : 4);
+        return;
+    }
+
+    u32 opc1 = (iss >> 16) & 15;
+    u32 Rt2  = (iss >> 10) & 31;
+    u32 Rt   = (iss >>  5) & 31;
+    u32 CRm  = (iss >>  1) & 15;
+
+    bool isRead = (iss & 1) != 0;
+    u32 instructionLength = esr.il == 0 ? 2 : 4;
+
+    if (LIKELY(CRm == 14 && (opc1 == 0 || opc1 == 2))) {
+        iss = opc1 == 0 ? ENCODE_SYSREG_ISS(CNTPCT_EL0) : ENCODE_SYSREG_ISS(CNTP_CVAL_EL0);
+    } else {
+        PANIC("handleMcrrMrrcTrap: unexpected cp15 register, instruction: %s p15, #%u, r%u, r%u, c%u\n", isRead ? "mrrc" : "mcrr", opc1, Rt, Rt, CRm);
+    }
+
+    if (isRead) {
+        doMrrc(frame, iss, instructionLength, Rt, Rt2);
+    } else {
+        doMcrr(frame, iss, instructionLength, Rt, Rt2);
+    }
+}
+
+void handleA32CP14Trap(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr)
+{
+    // LDC/STC: Skip instruction, read 0 if necessary, since only one debug reg can be accessed with it
+    // Other CP14 accesses: do the same thing
 
     if (esr.iss & 1 && evaluateMcrMrcCondition(frame->spsr_el2, (esr.iss >> 20) & 0xF, (esr.iss & BIT(24)) != 0)) {
         writeFrameRegisterZ(frame, (esr.iss >> 5) & 0x1F, 0);

thermosphere/src/sysreg_traps.h

@@ -18,9 +18,9 @@
 
 #include "traps.h"
 
-void doSystemRegisterRead(ExceptionStackFrame *frame, u32 iss, u32 reg);
-void doSystemRegisterWrite(ExceptionStackFrame *frame, u32 iss, u32 reg);
-
 void handleMsrMrsTrap(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr);
+void handleMcrMrcCP15Trap(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr);
+void handleMcrrMrrcCP15Trap(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr);
+void handleA32CP14Trap(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr);
 
-void handleSysregAccessA32Stub(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr);
+void handleA32CP14Trap(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr);

thermosphere/src/traps.c

@@ -17,7 +17,7 @@
 #include "traps.h"
 #include "sysreg.h"
 
-static void enableDebugTraps(void)
+static inline void enableDebugTraps(void)
 {
     u64 mdcr = GET_SYSREG(mdcr_el2);
 
@@ -30,6 +30,13 @@ static void enableDebugTraps(void)
     SET_SYSREG(mdcr_el2, mdcr);
 }
 
+static inline void enableTimerTraps(void)
+{
+    // Disable event streams, trap everything
+    u64 cnthctl = 0;
+    SET_SYSREG(cnthctl_el2, cnthctl);
+}
+
 void enableTraps(void)
 {
     u64 hcr = GET_SYSREG(hcr_el2);
@@ -46,4 +53,5 @@ void enableTraps(void)
     SET_SYSREG(hcr_el2, hcr);
 
     enableDebugTraps();
+    enableTimerTraps();
 }

thermosphere/src/utils.h

@@ -31,6 +31,8 @@
 
 #define ALIGN(m)        __attribute__((aligned(m)))
 #define PACKED          __attribute__((packed))
+#define LIKELY(expr)    __builtin_expect((expr), 1)
+#define UNLIKELY(expr)  __builtin_expect((expr), 0)
 
 #define ALINLINE        __attribute__((always_inline))