thermosphere: rewrite sysreg trapping code, add skeleton code for timer val trap handling; support A32 EL1 once again

This commit is contained in:
TuxSH 2020-01-07 02:17:05 +00:00
parent d42d9e60b9
commit b9d07fccd6
9 changed files with 217 additions and 74 deletions

View file

@ -85,5 +85,5 @@ typedef struct DebugRegisterPair {
static inline void enableBreakpointsAndWatchpoints(void) 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);
} }

View file

@ -96,14 +96,17 @@ void handleLowerElSyncException(ExceptionStackFrame *frame, ExceptionSyndromeReg
switch (esr.ec) { switch (esr.ec) {
case Exception_CP15RTTrap: case Exception_CP15RTTrap:
case Exception_CP15RRTTrap: handleMcrMrcCP15Trap(frame, esr);
break;
case Exception_CP15RRTTrap:
handleMcrrMrrcCP15Trap(frame, esr);
break;
case Exception_CP14RTTrap: case Exception_CP14RTTrap:
case Exception_CP14DTTrap: case Exception_CP14DTTrap:
case Exception_CP14RRTTrap: { case Exception_CP14RRTTrap:
// A32 stub: Skip instruction, read 0 if necessary (there are debug regs at EL0) // A32 stub: Skip instruction, read 0 if necessary (there are debug regs at EL0)
handleSysregAccessA32Stub(frame, esr); handleA32CP14Trap(frame, esr);
break; break;
}
case Exception_HypervisorCallA64: case Exception_HypervisorCallA64:
handleHypercall(frame, esr); handleHypercall(frame, esr);
break; break;

View file

@ -102,11 +102,21 @@ static inline void spsrSetT32ItFlags(u64 *spsr, u32 itFlags)
*spsr |= ((itFlags >> 2) & 0x3F) << 10; *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) static inline u64 readFrameRegisterZ(ExceptionStackFrame *frame, u32 id)
{ {
return id == 31 ? 0 /* xzr */ : frame->x[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) static inline void writeFrameRegisterZ(ExceptionStackFrame *frame, u32 id, u64 val)
{ {
if (id != 31) { if (id != 31) {

View file

@ -22,7 +22,7 @@
SingleStepState singleStepGetNextState(ExceptionStackFrame *frame) SingleStepState singleStepGetNextState(ExceptionStackFrame *frame)
{ {
u64 mdscr = GET_SYSREG(mdscr_el1); 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; bool pstateSS = (frame->spsr_el2 & PSTATE_SS) != 0;
if (!mdscrSS) { if (!mdscrSS) {
@ -39,16 +39,16 @@ void singleStepSetNextState(ExceptionStackFrame *frame, SingleStepState state)
switch (state) { switch (state) {
case SingleStepState_Inactive: case SingleStepState_Inactive:
// Unset mdscr_el1.ss // Unset mdscr_el1.ss
mdscr &= ~MDSCR_EL1_SS; mdscr &= ~MDSCR_SS;
break; break;
case SingleStepState_ActivePending: case SingleStepState_ActivePending:
// Set mdscr_el1.ss and pstate.ss // Set mdscr_el1.ss and pstate.ss
mdscr |= MDSCR_EL1_SS; mdscr |= MDSCR_SS;
frame->spsr_el2 |= PSTATE_SS; frame->spsr_el2 |= PSTATE_SS;
break; break;
case SingleStepState_ActiveNotPending: case SingleStepState_ActiveNotPending:
// Set mdscr_el1.ss and unset pstate.ss // Set mdscr_el1.ss and unset pstate.ss
mdscr |= MDSCR_EL1_SS; mdscr |= MDSCR_SS;
frame->spsr_el2 |= PSTATE_SS; frame->spsr_el2 |= PSTATE_SS;
break; break;
default: default:

View file

@ -237,6 +237,7 @@
#define TUP_CNTV_CVAL_EL0 (3, 3, 14, 3, 2) #define TUP_CNTV_CVAL_EL0 (3, 3, 14, 3, 2)
#define TUP_CNTVOFF_EL2 (3, 4, 14, 0, 3) #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 TUP_CNTHP_CVAL_EL2 (3, 4, 14, 2, 2)
#define __field_PMEV_op2(n) ((n) & 0x7) #define __field_PMEV_op2(n) ((n) & 0x7)
@ -403,8 +404,16 @@
#define MDCR_EL2_TPMCR BITL(5) #define MDCR_EL2_TPMCR BITL(5)
#define MDCR_EL2_HPMN_MASK 0x1Full #define MDCR_EL2_HPMN_MASK 0x1Full
#define MDSCR_EL1_MDE BITL(15) #define MDSCR_MDE BITL(15)
#define MDSCR_EL1_SS BITL(0) #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_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)) #define ENCODE_SYSREG_MOV(name) EVAL(ENCODE_SYSREG_FIELDS_MOV CAT(TUP_, name))

View file

@ -20,76 +20,109 @@
#include "debug_log.h" #include "debug_log.h"
#include "software_breakpoints.h" #include "software_breakpoints.h"
static void doSystemRegisterRwImpl(u64 *val, u32 iss) static inline u64 doSystemRegisterRead(u32 normalizedIss)
{ {
u32 op0 = (iss >> 20) & 3; u64 val;
u32 op2 = (iss >> 17) & 7; switch (normalizedIss) {
u32 op1 = (iss >> 14) & 7; case ENCODE_SYSREG_ISS(CNTPCT_EL0): {
u32 CRn = (iss >> 10) & 15; // FIXME
//u32 Rt = (iss >> 5) & 31; val = GET_SYSREG(cntpct_el0);
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:
break; 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); return val;
writeFrameRegisterZ(frame, reg, 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); skipFaultingInstruction(frame, 4);
} }
void doSystemRegisterWrite(ExceptionStackFrame *frame, u32 iss, u32 reg) static inline void doMsr(ExceptionStackFrame *frame, u32 normalizedIss, u32 reg)
{ {
u64 val = 0; u64 val = readFrameRegisterZ(frame, reg);
iss &= ~((0x1F << 5) | 1); doSystemRegisterWrite(normalizedIss, val);
val = readFrameRegisterZ(frame, reg);
// Hooks go here:
switch (iss) {
default:
break;
}
doSystemRegisterRwImpl(&val, iss);
skipFaultingInstruction(frame, 4); 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; writeFrameRegisterZ(frame, reg, doSystemRegisterRead(normalizedIss) & 0xFFFFFFFF);
u32 reg = (iss >> 5) & 31; skipFaultingInstruction(frame, instructionLength);
bool isRead = (iss & 1) != 0; }
if (isRead) { static inline void doMcr(ExceptionStackFrame *frame, u32 normalizedIss, u32 instructionLength, u32 reg)
doSystemRegisterRead(frame, iss, reg); {
} else { u64 val = readFrameRegisterZ(frame, reg) & 0xFFFFFFFF;
doSystemRegisterWrite(frame, iss, reg); 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) 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)) { if (esr.iss & 1 && evaluateMcrMrcCondition(frame->spsr_el2, (esr.iss >> 20) & 0xF, (esr.iss & BIT(24)) != 0)) {
writeFrameRegisterZ(frame, (esr.iss >> 5) & 0x1F, 0); writeFrameRegisterZ(frame, (esr.iss >> 5) & 0x1F, 0);

View file

@ -18,9 +18,9 @@
#include "traps.h" #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 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);

View file

@ -17,7 +17,7 @@
#include "traps.h" #include "traps.h"
#include "sysreg.h" #include "sysreg.h"
static void enableDebugTraps(void) static inline void enableDebugTraps(void)
{ {
u64 mdcr = GET_SYSREG(mdcr_el2); u64 mdcr = GET_SYSREG(mdcr_el2);
@ -30,6 +30,13 @@ static void enableDebugTraps(void)
SET_SYSREG(mdcr_el2, mdcr); 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) void enableTraps(void)
{ {
u64 hcr = GET_SYSREG(hcr_el2); u64 hcr = GET_SYSREG(hcr_el2);
@ -46,4 +53,5 @@ void enableTraps(void)
SET_SYSREG(hcr_el2, hcr); SET_SYSREG(hcr_el2, hcr);
enableDebugTraps(); enableDebugTraps();
enableTimerTraps();
} }

View file

@ -31,6 +31,8 @@
#define ALIGN(m) __attribute__((aligned(m))) #define ALIGN(m) __attribute__((aligned(m)))
#define PACKED __attribute__((packed)) #define PACKED __attribute__((packed))
#define LIKELY(expr) __builtin_expect((expr), 1)
#define UNLIKELY(expr) __builtin_expect((expr), 0)
#define ALINLINE __attribute__((always_inline)) #define ALINLINE __attribute__((always_inline))