diff --git a/thermosphere/src/exceptions.c b/thermosphere/src/exceptions.c index 80ba40f50..10d271335 100644 --- a/thermosphere/src/exceptions.c +++ b/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); diff --git a/thermosphere/src/sysreg.h b/thermosphere/src/sysreg.h index 0e75a08fa..ffb7e94ff 100644 --- a/thermosphere/src/sysreg.h +++ b/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_AARCH32_CNTP_CTL (0, 0, 14, 2, 1) -#define TUP_AARCH32_CNTP_CVAL (0, 2, 0, 14, 0) +#define TUP_CNTV_TVAL_EL0 (3, 3, 14, 3, 0) +#define TUP_CNTV_CTL_EL0 (3, 3, 14, 3, 1) +#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)) diff --git a/thermosphere/src/sysreg_traps.c b/thermosphere/src/sysreg_traps.c index 7e9bd0a81..5184ad387 100644 --- a/thermosphere/src/sysreg_traps.c +++ b/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); -} \ No newline at end of file +} + +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); +} diff --git a/thermosphere/src/sysreg_traps.h b/thermosphere/src/sysreg_traps.h index 9ff6cbc6e..9df543ff7 100644 --- a/thermosphere/src/sysreg_traps.h +++ b/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);