thermosphere: more sysreg code

thermosphere/src/exceptions.c

@@ -17,6 +17,30 @@
 #include "hvc.h"
 #include "log.h"
 
+bool spsrEvaluateConditionCode(u64 spsr, u32 conditionCode)
+{
+    if (conditionCode == 14) {
+        // AL
+        return true;
+    } else if (conditionCode == 15) {
+        // Invalid encoding
+        return false;
+    }
+
+    // NZCV
+    bool n = (spsr & BIT(31)) != 0;
+    bool z = (spsr & BIT(30)) != 0;
+    bool c = (spsr & BIT(29)) != 0;
+    bool v = (spsr & BIT(28)) != 0;
+
+    bool tableHalf[] = {
+        // EQ, CS, MI, VS, HI, GE, GT
+        z, c, n, v, c && !z, n == v, !z && n == v,
+    };
+
+    return (conditionCode & 1) == 0 ? tableHalf[conditionCode / 2] : !tableHalf[conditionCode / 2];
+}
+
 void dumpStackFrame(const ExceptionStackFrame *frame, bool sameEl)
 {
 #ifndef NDEBUG
@@ -36,6 +60,24 @@ void dumpStackFrame(const ExceptionStackFrame *frame, bool sameEl)
 #endif
 }
 
+static void advanceItState(ExceptionStackFrame *frame)
+{
+    if (!spsrIsThumb(frame->spsr_el2) || spsrGetT32ItFlags(frame->spsr_el2) == 0) {
+        return;
+    }
+
+    u32 it = spsrGetT32ItFlags(frame->spsr_el2);
+
+    // Last instruction of the block => wipe, otherwise advance
+    spsrSetT32ItFlags(&frame->spsr_el2, (it & 7) == 0 ? 0 : (it & 0xE0) | ((it << 1) & 0x1F));
+}
+
+void skipFaultingInstruction(ExceptionStackFrame *frame, u32 size)
+{
+    advanceItState(frame);
+    frame->elr_el2 += size;
+}
+
 void handleLowerElSyncException(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr)
 {
 

thermosphere/src/exceptions.h

@@ -79,4 +79,29 @@ typedef struct ExceptionSyndromeRegister {
     u32 res0            : 32;
 } ExceptionSyndromeRegister;
 
+static inline bool spsrIsA32(u64 spsr)
+{
+    return (spsr & 0x10) != 0;
+}
+
+static inline bool spsrIsThumb(u64 spsr)
+{
+    return spsrIsA32(spsr) && (spsr & 0x20) != 0;
+}
+
+static inline u32 spsrGetT32ItFlags(u64 spsr)
+{
+    return (((spsr >> 10) & 0x3F) << 2) | ((spsr >> 25) & 3);
+}
+
+static inline void spsrSetT32ItFlags(u64 *spsr, u32 itFlags)
+{
+    static const u32 itMask = (0x3F << 10) | (3 << 25);
+    *spsr &= ~itMask;
+    *spsr |= (itFlags & 3) << 25;
+    *spsr |= ((itFlags >> 2) & 0x3F) << 10;
+}
+
+bool spsrEvaluateConditionCode(u64 spsr, u32 conditionCode);
+void skipFaultingInstruction(ExceptionStackFrame *frame, u32 size);
 void dumpStackFrame(const ExceptionStackFrame *frame, bool sameEl);

thermosphere/src/sysreg_traps.c

@@ -19,7 +19,7 @@
 #include "sysreg.h"
 
 // For a32 mcr/mrc => a64 mrs
-u32 convertMcrMrcIss(u32 *outCondition, u32 a32Iss, u32 coproc, u32 el)
+static u32 convertMcrMrcIss(u32 *outCondition, bool *outCondValid, u32 a32Iss, u32 coproc, u32 el)
 {
     // NOTE: MCRR / MRRC do NOT map for the most part and need to be handled separately
 
@@ -30,9 +30,8 @@ u32 convertMcrMrcIss(u32 *outCondition, u32 a32Iss, u32 coproc, u32 el)
     u32 CRn  = (a32Iss >> 10) & 15;
     //u32 Rt   = (a32Iss >>  5) & 31;
     //u32 CRm  = (a32Iss >>  1) & 15;
-
-    bool condValid = (a32Iss & BIT(24)) != 0;
-    *outCondition = condValid ? ((a32Iss >> 20) & 15): 14; // use "unconditional" by default
+    *outCondValid = (a32Iss & BIT(24)) != 0;
+    *outCondition = (a32Iss >> 20) & 15;
 
     u32 op0 = (16 - coproc) & 3;
     u32 op1;
@@ -82,6 +81,17 @@ u32 convertMcrMrcIss(u32 *outCondition, u32 a32Iss, u32 coproc, u32 el)
     return (a32Iss & ~(MASK2(24, 20) | MASK2(16, 14))) | (op0 << 20) | (op1 << 14);
 }
 
+static bool evaluateMcrMrcCondition(u64 spsr, u32 condition, bool condValid)
+{
+    if (!condValid) {
+        // Only T32 instructions can do that
+        u32 it = spsrGetT32ItFlags(spsr);
+        return it == 0 || spsrEvaluateConditionCode(spsr, it >> 4);
+    } else {
+        return spsrEvaluateConditionCode(spsr, condition);
+    }
+}
+
 static void doSystemRegisterRwImpl(u64 *val, u32 iss)
 {
     u32 op0  = (iss >> 20) & 3;
@@ -109,7 +119,10 @@ void doSystemRegisterRead(ExceptionStackFrame *frame, u32 iss, u32 reg1, u32 reg
 {
     // reg1 != reg2: mrrc/mcrr
     u64 val = 0;
-    doSystemRegisterRwImpl(&val, iss | 1);
+
+    iss &= ~((0x1F << 5) | 1);
+
+    doSystemRegisterRwImpl(&val, iss);
     if (reg1 == reg2) {
         frame->x[reg1] = val;
     } else {
@@ -117,22 +130,52 @@ void doSystemRegisterRead(ExceptionStackFrame *frame, u32 iss, u32 reg1, u32 reg
         frame->x[reg2] = val >> 32;
     }
 
-    // Sysreg access are always 4 bit in length even for Aarch32
-    frame->elr_el2 += 4;
+    skipFaultingInstruction(frame, 4);
 }
 
 void doSystemRegisterWrite(ExceptionStackFrame *frame, u32 iss, u32 reg1, u32 reg2)
 {
     // reg1 != reg2: mrrc/mcrr
     u64 val = frame->x[reg1];
+    iss &= ~((0x1F << 5) | 1);
 
     if (reg1 != reg2) {
         val  = (val << 32) >> 32;
         val |= frame->x[reg2] << 32;
     }
 
-    doSystemRegisterRwImpl(&val, iss & ~1);
-
-    // Sysreg access are always 4 bit in length even for Aarch32
-    frame->elr_el2 += 4;
+    doSystemRegisterRwImpl(&val, iss);
+    skipFaultingInstruction(frame, 4);
 }
+
+void handleMsrMrsTrap(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr)
+{
+    u32 iss = esr.iss;
+    u32 reg = (iss >> 5) & 31;
+    bool isRead = (iss & 1) != 0;
+
+    if (isRead) {
+        doSystemRegisterRead(frame, iss, reg, reg);
+    } else {
+        doSystemRegisterWrite(frame, iss, reg, reg);
+    }
+}
+
+void handleMcrMrcTrap(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr)
+{
+    u32 condition;
+    bool condValid;
+    u32 coproc = esr.ec == Exception_CP14RTTrap ? 14 : 15;
+
+    // EL0 if User Mode else EL1
+    esr.iss = convertMcrMrcIss(&condition, &condValid, esr.iss, coproc, (frame->spsr_el2 & 0xF) == 0 ? 0 : 1);
+
+    if (esr.iss & BIT(31)) {
+        // Error, we shouldn't have trapped those in first place anyway.
+        return;
+    } else if (!evaluateMcrMrcCondition(frame->spsr_el2, condition, condValid)) {
+        return;
+    }
+
+    handleMsrMrsTrap(frame, esr);
+}