From b9d07fccd69b34dc3d4a4606e12936edd20b80a1 Mon Sep 17 00:00:00 2001 From: TuxSH <1922548+TuxSH@users.noreply.github.com> Date: Tue, 7 Jan 2020 02:17:05 +0000 Subject: [PATCH] thermosphere: rewrite sysreg trapping code, add skeleton code for timer val trap handling; support A32 EL1 once again --- .../src/breakpoints_watchpoints_common.h | 2 +- thermosphere/src/exceptions.c | 11 +- thermosphere/src/exceptions.h | 10 + thermosphere/src/single_step.c | 8 +- thermosphere/src/sysreg.h | 13 +- thermosphere/src/sysreg_traps.c | 227 +++++++++++++----- thermosphere/src/sysreg_traps.h | 8 +- thermosphere/src/traps.c | 10 +- thermosphere/src/utils.h | 2 + 9 files changed, 217 insertions(+), 74 deletions(-) diff --git a/thermosphere/src/breakpoints_watchpoints_common.h b/thermosphere/src/breakpoints_watchpoints_common.h index 1757d1642..577ca8934 100644 --- a/thermosphere/src/breakpoints_watchpoints_common.h +++ b/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); } diff --git a/thermosphere/src/exceptions.c b/thermosphere/src/exceptions.c index 29f2182ac..c3e4d81d6 100644 --- a/thermosphere/src/exceptions.c +++ b/thermosphere/src/exceptions.c @@ -96,14 +96,17 @@ void handleLowerElSyncException(ExceptionStackFrame *frame, ExceptionSyndromeReg switch (esr.ec) { case Exception_CP15RTTrap: - case Exception_CP15RRTTrap: + 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; diff --git a/thermosphere/src/exceptions.h b/thermosphere/src/exceptions.h index 99862b981..e4a607e38 100644 --- a/thermosphere/src/exceptions.h +++ b/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) { diff --git a/thermosphere/src/single_step.c b/thermosphere/src/single_step.c index 7f7ae3670..4e647aaa7 100644 --- a/thermosphere/src/single_step.c +++ b/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: diff --git a/thermosphere/src/sysreg.h b/thermosphere/src/sysreg.h index 69f77dfd3..c079d1e2b 100644 --- a/thermosphere/src/sysreg.h +++ b/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_EL1_SS BITL(0) +#define MDSCR_MDE BITL(15) +#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)) diff --git a/thermosphere/src/sysreg_traps.c b/thermosphere/src/sysreg_traps.c index 64510a516..aeec4fba7 100644 --- a/thermosphere/src/sysreg_traps.c +++ b/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; - u32 op2 = (iss >> 17) & 7; - u32 op1 = (iss >> 14) & 7; - u32 CRn = (iss >> 10) & 15; - //u32 Rt = (iss >> 5) & 31; - u32 CRm = (iss >> 1) & 15; - 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); -} - -void doSystemRegisterRead(ExceptionStackFrame *frame, u32 iss, u32 reg) -{ - u64 val = 0; - - iss &= ~((0x1F << 5) | 1); - - // Hooks go here: - switch (iss) { - default: + u64 val; + switch (normalizedIss) { + case ENCODE_SYSREG_ISS(CNTPCT_EL0): { + // FIXME + val = GET_SYSREG(cntpct_el0); break; + } + case ENCODE_SYSREG_ISS(CNTP_TVAL_EL0): { + // FIXME too + val = GET_SYSREG(cntp_tval_el0); + break; + } + case ENCODE_SYSREG_ISS(CNTP_CTL_EL0): { + // Passthrough + val = GET_SYSREG(cntp_ctl_el0); + break; + } + case ENCODE_SYSREG_ISS(CNTP_CVAL_EL0): { + // Passthrough + val = GET_SYSREG(cntp_cval_el0); + break; + } + + default: { + // We shouldn't have trapped on other registers other than debug regs + // and we want the latter as RA0/WI + val = 0; + break; + } } - doSystemRegisterRwImpl(&val, iss | 1); - writeFrameRegisterZ(frame, reg, val); + return val; +} +static inline void doSystemRegisterWrite(u32 normalizedIss, u64 val) +{ + switch (normalizedIss) { + case ENCODE_SYSREG_ISS(CNTP_TVAL_EL0): { + // FIXME + SET_SYSREG(cntp_tval_el0, val); + break; + } + case ENCODE_SYSREG_ISS(CNTP_CTL_EL0): { + // Passthrough + SET_SYSREG(cntp_ctl_el0, val); + break; + } + case ENCODE_SYSREG_ISS(CNTP_CVAL_EL0): { + // Passthrough + SET_SYSREG(cntp_cval_el0, val); + break; + } + + 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 doSystemRegisterWrite(ExceptionStackFrame *frame, u32 iss, u32 reg) +static inline void doMsr(ExceptionStackFrame *frame, u32 normalizedIss, u32 reg) { - u64 val = 0; - iss &= ~((0x1F << 5) | 1); - - val = readFrameRegisterZ(frame, reg); - - // Hooks go here: - switch (iss) { - default: - break; - } - - doSystemRegisterRwImpl(&val, iss); - + u64 val = readFrameRegisterZ(frame, reg); + doSystemRegisterWrite(normalizedIss, val); skipFaultingInstruction(frame, 4); } -void handleMsrMrsTrap(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr) +static inline void doMrc(ExceptionStackFrame *frame, u32 normalizedIss, u32 instructionLength, u32 reg) { - u32 iss = esr.iss; - u32 reg = (iss >> 5) & 31; - bool isRead = (iss & 1) != 0; + writeFrameRegisterZ(frame, reg, doSystemRegisterRead(normalizedIss) & 0xFFFFFFFF); + skipFaultingInstruction(frame, instructionLength); +} - if (isRead) { - doSystemRegisterRead(frame, iss, reg); - } else { - doSystemRegisterWrite(frame, iss, reg); - } +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); diff --git a/thermosphere/src/sysreg_traps.h b/thermosphere/src/sysreg_traps.h index 1fbce0617..a32cea375 100644 --- a/thermosphere/src/sysreg_traps.h +++ b/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); diff --git a/thermosphere/src/traps.c b/thermosphere/src/traps.c index 9b015cb64..f0534debf 100644 --- a/thermosphere/src/traps.c +++ b/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(); } diff --git a/thermosphere/src/utils.h b/thermosphere/src/utils.h index 8f3e07693..8c7543abc 100644 --- a/thermosphere/src/utils.h +++ b/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))