diff --git a/thermosphere/src/exceptions.c b/thermosphere/src/exceptions.c index e1109e077..4f39c5716 100644 --- a/thermosphere/src/exceptions.c +++ b/thermosphere/src/exceptions.c @@ -68,9 +68,9 @@ void dumpStackFrame(const ExceptionStackFrame *frame, bool sameEl) #endif } -#ifdef A32_SUPPORTED static void advanceItState(ExceptionStackFrame *frame) { + // Just in case EL0 is executing A32 (& not sure if fully supported) if (!spsrIsThumb(frame->spsr_el2) || spsrGetT32ItFlags(frame->spsr_el2) == 0) { return; } @@ -80,13 +80,10 @@ static void advanceItState(ExceptionStackFrame *frame) // Last instruction of the block => wipe, otherwise advance spsrSetT32ItFlags(&frame->spsr_el2, (it & 7) == 0 ? 0 : (it & 0xE0) | ((it << 1) & 0x1F)); } -#endif void skipFaultingInstruction(ExceptionStackFrame *frame, u32 size) { -#ifdef A32_SUPPORTED advanceItState(frame); -#endif frame->elr_el2 += size; } @@ -94,23 +91,15 @@ void handleLowerElSyncException(ExceptionStackFrame *frame, ExceptionSyndromeReg { switch (esr.ec) { - -#ifdef A32_SUPPORTED - case Exception_CP14RTTrap: case Exception_CP15RTTrap: - handleMcrMrcTrap(frame, esr); - break; + case Exception_CP15RRTTrap: + case Exception_CP14RTTrap: case Exception_CP14DTTrap: - handleLdcStcTrap(frame, esr); + case Exception_CP14RRTTrap: { + // A32 stub: Skip instruction, read 0 if necessary (there are debug regs at EL0) + handleSysregAccessA32Stub(frame, esr); break; - case Exception_CP14RRTTrap: - case Exception_CP15RRTTrap: - handleMcrrMrrcTrap(frame, esr); - break; - case Exception_HypervisorCallA32: - handleHypercall(frame, esr); - break; -#endif + } case Exception_HypervisorCallA64: handleHypercall(frame, esr); break; diff --git a/thermosphere/src/sysreg_traps.c b/thermosphere/src/sysreg_traps.c index 76709e3b8..9e5c11425 100644 --- a/thermosphere/src/sysreg_traps.c +++ b/thermosphere/src/sysreg_traps.c @@ -43,50 +43,30 @@ static void doSystemRegisterRwImpl(u64 *val, u32 iss) *val = ((u64 (*)(u64))codebuf)(*val); } -void doSystemRegisterRead(ExceptionStackFrame *frame, u32 iss, u32 reg1, u32 reg2) +void doSystemRegisterRead(ExceptionStackFrame *frame, u32 iss, u32 reg) { - // reg1 != reg2: mrrc/mcrr u64 val = 0; iss &= ~((0x1F << 5) | 1); - doSystemRegisterRwImpl(&val, iss | 1); - if (reg1 == reg2) { - frame->x[reg1] = val; - } else { - if (reg1 != -1) { - frame->x[reg1] = val & 0xFFFFFFFF; - } - if (reg2 != -1) { - frame->x[reg2] = val >> 32; - } + // Hooks go here: + switch (iss) { + default: + break; } + doSystemRegisterRwImpl(&val, iss | 1); + frame->x[reg] = val; + skipFaultingInstruction(frame, 4); } -void doSystemRegisterWrite(ExceptionStackFrame *frame, u32 iss, u32 reg1, u32 reg2) +void doSystemRegisterWrite(ExceptionStackFrame *frame, u32 iss, u32 reg) { - // reg1 != reg2: mrrc/mcrr u64 val = 0; iss &= ~((0x1F << 5) | 1); - if (reg1 == -1 || reg2 == -1) { - doSystemRegisterRwImpl(&val, iss | 1); - if (reg1 == -1) { - val = (frame->x[reg2] << 32) | (val & 0xFFFFFFFF); - } else { - val = ((val >> 32) << 32) | (frame->x[reg1] & 0xFFFFFFFF); - } - } - - else { - if (reg1 != reg2) { - val |= (frame->x[reg2] << 32) | (frame->x[reg1] & 0xFFFFFFFF); - } else { - val = frame->x[reg1]; - } - } + val = frame->x[reg]; // Hooks go here: switch (iss) { @@ -98,117 +78,17 @@ void doSystemRegisterWrite(ExceptionStackFrame *frame, u32 iss, u32 reg1, u32 re skipFaultingInstruction(frame, 4); } -#ifdef A32_SUPPORTED - -// For a32 mcr/mrc => a64 mrs -static u32 convertMcrMrcIss(u32 *outCondition, bool *outCondValid, u32 *outShift, u32 a32Iss, u32 coproc, u32 el) +void handleMsrMrsTrap(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr) { - // NOTE: MCRR / MRRC do NOT map for the most part and need to be handled separately + u32 iss = esr.iss; + u32 reg = (iss >> 5) & 31; + bool isRead = (iss & 1) != 0; - //u32 direction = a32Iss & 1; - - u32 opc2 = (a32Iss >> 17) & 7; - u32 opc1 = (a32Iss >> 14) & 7; - u32 CRn = (a32Iss >> 10) & 15; - //u32 Rt = (a32Iss >> 5) & 31; - u32 CRm = (a32Iss >> 1) & 15; - *outCondValid = (a32Iss & BIT(24)) != 0; - *outCondition = (a32Iss >> 20) & 15; - - *outShift = 0; - - u32 op0 = (16 - coproc) & 3; - u32 op1; - - // Do NOT translate cp15, 0, c7-8 (Cache, and resp. TLB maintenance coproc regs) as they don't map to Aarch64 - if (coproc == 15 && (CRn == 7 || CRn == 8)) { - return -2; + if (isRead) { + doSystemRegisterRead(frame, iss, reg); + } else { + doSystemRegisterWrite(frame, iss, reg); } - - // A few special cases - // We're probably missing some of them - // opc1 Crn Crm opc2 - if (coproc == 15) { - /* don't care // ACTLR2 - if (opc1 == 0 && CRn == 1 && CRm == 0 && opc2 == 3) { - *outShift = 32; - opc2 = 1; - }*/ - - // TTBCR2 - if (opc1 == 0 && CRn == 2 && CRm == 0 && opc2 == 3) { - *outShift = 32; - opc2 = 2; - } - - /* don't care // ERX*2 - if (opc1 == 0 && CRn == 5 && CRm == 4 && opc2 >= 4) { - *outShift = 32; - opc2 &= ~4; - }*/ - - // DFSR -> ESR_EL1 - if (opc1 == 0 && CRn == 5 && CRm == 0 && opc2 == 0) { - CRm = 2; - } - - // IFAR -> high FAR_EL1 - if (opc1 == 0 && CRn == 6 && CRm == 0 && opc2 == 2) { - opc2 = 0; - *outShift = 32; - } - - // MAIR1 - if (opc1 == 0 && CRn == 10 && CRm == 2 && opc2 == 1) { - *outShift = 32; - opc2 = 0; - } - - // AMAIR1 - if (opc1 ==0 && CRn == 10 && CRm == 3 && opc2 == 1) { - *outShift = 32; - opc2 = 0; - } - } - - // The difficult part - switch (opc1) { - case SYSREG_OP1_AARCH32_AUTO: { - switch (el) { - case 0: - op1 = SYSREG_OP1_EL0; - break; - case 1: - op1 = SYSREG_OP1_AARCH64_EL1; - break; - case 2: - op1 = SYSREG_OP1_EL2; - break; - case 3: - op1 = SYSREG_OP1_EL3; - break; - default: - return -1; - } - break; - } - - case SYSREG_OP1_EL0: - case SYSREG_OP1_EL2: - case SYSREG_OP1_EL3: - case SYSREG_OP1_CACHE: - case SYSREG_OP1_CACHESZSEL: - op1 = opc1; - break; - - // We shouldn't even trap those to begin with. - case SYSREG_OP1_AARCH32_JZL: - default: - return -1; - } - - // Everything but op0 is at its correct place & only op1 needs to be replaced - return (a32Iss & ~(MASK2(24, 20) | MASK2(16, 14) | MASK2(19, 17))) | (op0 << 20) | (opc2 << 17) | (op1 << 14); } static bool evaluateMcrMrcCondition(u64 spsr, u32 condition, bool condValid) @@ -222,142 +102,12 @@ static bool evaluateMcrMrcCondition(u64 spsr, u32 condition, bool condValid) } } - -void handleMcrMrcTrap(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr) +void handleSysregAccessA32Stub(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr) { - u32 condition; - bool condValid; - u32 coproc = esr.ec == Exception_CP14RTTrap ? 14 : 15; - u32 shift = 0; + // A32 stub: Skip instruction, read 0 if necessary (there are debug regs at EL0) - // EL0 if User Mode else EL1 - u32 iss = convertMcrMrcIss(&condition, &condValid, &shift, 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. - skipFaultingInstruction(frame, 4); - return; - } else if (!evaluateMcrMrcCondition(frame->spsr_el2, condition, condValid)) { - skipFaultingInstruction(frame, 4); - return; + if (esr.iss & 1 && evaluateMcrMrcCondition(frame->spsr_el2, (esr.iss >> 20) & 0xF, (esr.iss & BIT(24)) != 0)) { + frame->x[(esr.iss >> 5) & 0x1F] = 0; } - - u32 reg = (iss >> 5) & 31; - bool isRead = (iss & 1) != 0; - - u32 reg1 = shift == 32 ? -1 : reg; - u32 reg2 = shift == 32 ? reg : -1; - - if (isRead) { - doSystemRegisterRead(frame, iss, reg1, reg2); - } else { - doSystemRegisterRead(frame, iss, reg1, reg2); - } -} - -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); } - -#endif - -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); - } -} \ No newline at end of file diff --git a/thermosphere/src/sysreg_traps.h b/thermosphere/src/sysreg_traps.h index 9df543ff7..1fbce0617 100644 --- a/thermosphere/src/sysreg_traps.h +++ b/thermosphere/src/sysreg_traps.h @@ -18,10 +18,9 @@ #include "traps.h" -void doSystemRegisterRead(ExceptionStackFrame *frame, u32 iss, u32 reg1, u32 reg2); -void doSystemRegisterWrite(ExceptionStackFrame *frame, u32 iss, u32 reg1, u32 reg2); +void doSystemRegisterRead(ExceptionStackFrame *frame, u32 iss, u32 reg); +void doSystemRegisterWrite(ExceptionStackFrame *frame, u32 iss, u32 reg); void handleMsrMrsTrap(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr); -void handleMcrMrcTrap(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr); -void handleMcrrMrrcTrap(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr); -void handleLdcStcTrap(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr); + +void handleSysregAccessA32Stub(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr);