nold/Atmosphere
1
0
Fork 0
mirror of https://github.com/Atmosphere-NX/Atmosphere synced 2024-12-22 20:31:14 +00:00
Code Issues Projects Releases Packages Wiki Activity

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

Browse source
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
Show stats Download patch file Download diff file Expand all files Collapse all files

2
thermosphere/src/breakpoints_watchpoints_common.h
View file

@ -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);
}

11
thermosphere/src/exceptions.c
View file

@ -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;

10
thermosphere/src/exceptions.h
View file

@ -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) {

8
thermosphere/src/single_step.c
View file

@ -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:

13
thermosphere/src/sysreg.h
View file

@ -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))

227
thermosphere/src/sysreg_traps.c
View file

@ -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);

8
thermosphere/src/sysreg_traps.h
View file

@ -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);

10
thermosphere/src/traps.c
View file

@ -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();
}

2
thermosphere/src/utils.h
View file

@ -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))