thermosphere: add remaining sysreg passthrough stuff

thermosphere/src/exceptions.c

@@ -15,6 +15,9 @@
  */
 
 #include "hvc.h"
+#include "traps.h"
+#include "sysreg_traps.h"
+
 #include "log.h"
 
 bool spsrEvaluateConditionCode(u64 spsr, u32 conditionCode)
@@ -82,6 +85,20 @@ void handleLowerElSyncException(ExceptionStackFrame *frame, ExceptionSyndromeReg
 {
 
     switch (esr.ec) {
+        case Exception_CP14RTTrap:
+        case Exception_CP15RTTrap:
+            handleMcrMrcTrap(frame, esr);
+            break;
+        case Exception_CP14DTTrap:
+            handleLdcStcTrap(frame, esr);
+            break;
+        case Exception_CP14RRTTrap:
+        case Exception_CP15RRTTrap:
+            handleMcrrMrrcTrap(frame, esr);
+            break;
+        case Exception_SystemRegisterTrap:
+            handleMsrMrsTrap(frame, esr);
+            break;
         case Exception_HypervisorCallA64:
         case Exception_HypervisorCallA32:
             handleHypercall(frame, esr);

thermosphere/src/sysreg.h

@@ -225,14 +225,19 @@
 #define TUP_TPIDRRO_EL0         (3, 3, 13, 0, 3)
 
 #define TUP_CNTFRQ_EL0          (3, 3, 14, 0, 0)
+#define TUP_CNTPCT_EL0          (3, 3, 14, 0, 1)
+#define TUP_CNTVCT_EL0          (3, 3, 14, 0, 2)
 
 #define TUP_CNTP_TVAL_EL0       (3, 3, 14, 2, 0)
 #define TUP_CNTP_CTL_EL0        (3, 3, 14, 2, 1)
 #define TUP_CNTP_CVAL_EL0       (3, 3, 14, 2, 2)
 
-#define TUP_AARCH32_CNTP_TVAL   (0, 0, 14, 2, 0)
+#define TUP_CNTV_TVAL_EL0       (3, 3, 14, 3, 0)
-#define TUP_AARCH32_CNTP_CTL    (0, 0, 14, 2, 1)
+#define TUP_CNTV_CTL_EL0        (3, 3, 14, 3, 1)
-#define TUP_AARCH32_CNTP_CVAL   (0, 2, 0, 14, 0)
+#define TUP_CNTV_CVAL_EL0       (3, 3, 14, 3, 2)
+
+#define TUP_CNTVOFF_EL2         (3, 4, 14, 0, 3)
+#define TUP_CNTHP_CVAL_EL2      (3, 4, 14, 2, 2)
 
 #define __field_PMEV_op2(n)     ((n) & 0x7)
 #define __field__CNTR_CRm(n)    (0x8 | (((n) >> 3) & 0x3))

thermosphere/src/sysreg_traps.c

@@ -172,10 +172,104 @@ void handleMcrMrcTrap(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr)
 
     if (esr.iss & BIT(31)) {
         // Error, we shouldn't have trapped those in first place anyway.
+        skipFaultingInstruction(frame, 4);
         return;
     } else if (!evaluateMcrMrcCondition(frame->spsr_el2, condition, condValid)) {
+        skipFaultingInstruction(frame, 4);
         return;
     }
 
     handleMsrMrsTrap(frame, esr);
-}
+}
+
+void handleMcrrMrrcTrap(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr)
+{
+    u32 iss = esr.iss;
+    u32 coproc = esr.ec == Exception_CP14RRTTrap ? 14 : 15;
+    bool cv = (iss & BIT(24)) != 0;
+    u32 cond = (iss >> 20) & 0xF;
+    u32 opc1 = (iss >> 16) & 0xF;
+    u32 reg2 = (iss >> 10) & 0x1F;
+    u32 reg1 = (iss >>  5) & 0x1F;
+    u32 crm  = (iss >>  1) & 0xF;
+    u32 dir  = iss & 1;
+
+    if (!evaluateMcrMrcCondition(frame->spsr_el2, cond, cv)) {
+        skipFaultingInstruction(frame, 4);
+        return;
+    }
+
+    u32 sysregIss = -1;
+    // No automatic conversion, handle what we potentiall trap
+    if (coproc == 14) {
+        if (crm == 1 && opc1 == 0) {
+            sysregIss = ENCODE_SYSREG_ISS(MDRAR_EL1);
+        } else if (crm == 2 && opc1 == 0) {
+            // DBGSAR, deprecated in Armv8 and no reg mapping,
+            // we won't handle it
+        }
+    } else {
+        switch (crm) {
+            case 2: {
+                switch (opc1) {
+                    case 0:
+                        sysregIss = ENCODE_SYSREG_ISS(TTBR0_EL1);
+                        break;
+                    case 1:
+                        sysregIss = ENCODE_SYSREG_ISS(TTBR1_EL1);
+                        break;
+                    default:
+                        // other regs are el2 ttbr regs, not trapped here
+                        break;
+                }
+                break;
+            }
+
+            case 7:
+                sysregIss = opc1 == 0 ? ENCODE_SYSREG_ISS(PAR_EL1) : -1;
+                break;
+
+            case 14: {
+                switch (opc1) {
+                    case 0:
+                        sysregIss = ENCODE_SYSREG_ISS(CNTPCT_EL0);
+                        break;
+                    case 1:
+                        sysregIss = ENCODE_SYSREG_ISS(CNTVCT_EL0);
+                        break;
+                    case 2:
+                        sysregIss = ENCODE_SYSREG_ISS(CNTP_CVAL_EL0);
+                        break;
+                    case 3:
+                        sysregIss = ENCODE_SYSREG_ISS(CNTV_CVAL_EL0);
+                        break;
+                    case 4:
+                        sysregIss = ENCODE_SYSREG_ISS(CNTVOFF_EL2);
+                        break;
+                    case 6:
+                        sysregIss = ENCODE_SYSREG_ISS(CNTHP_CVAL_EL2);
+                        break;
+                }
+            }
+        }
+    }
+
+    if (esr.iss & BIT(31)) {
+        // Error, we shouldn't have trapped those in first place anyway.
+        skipFaultingInstruction(frame, 4);
+        return;
+    }
+
+    if (dir == 1) {
+        doSystemRegisterRead(frame, sysregIss, reg1, reg2);
+    } else {
+        doSystemRegisterWrite(frame, sysregIss, reg1, reg2);
+    }
+}
+
+void handleLdcStcTrap(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr)
+{
+    // Only used for DBGDTRRXint
+    // Do not execute the read/writes
+    skipFaultingInstruction(frame, esr.il == 0 ? 2 : 4);
+}

thermosphere/src/sysreg_traps.h

@@ -21,3 +21,7 @@
 void doSystemRegisterRead(ExceptionStackFrame *frame, u32 iss, u32 reg1, u32 reg2);
 void doSystemRegisterWrite(ExceptionStackFrame *frame, u32 iss, u32 reg1, u32 reg2);
 
+void handleMsrMrsTrap(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr);
+void handleMcrMrcTrap(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr);
+void handleMcrrMrrcTrap(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr);
+void handleLdcStcTrap(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr);