mirror of
https://github.com/Atmosphere-NX/Atmosphere
synced 2024-12-22 20:31:14 +00:00
thermosphere: sysreg traps
This commit is contained in:
parent
797cea0ac8
commit
0437867449
7 changed files with 272 additions and 251 deletions
|
@ -47,3 +47,8 @@
|
||||||
#ifndef ENSURE
|
#ifndef ENSURE
|
||||||
#define ENSURE(...)
|
#define ENSURE(...)
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
//FIXME
|
||||||
|
#ifndef ENSURE2
|
||||||
|
#define ENSURE2(...)
|
||||||
|
#endif
|
||||||
|
|
|
@ -76,7 +76,7 @@ static int GDB_ParseExceptionFrame(char *out, const DebugEventInfo *info, int si
|
||||||
// Dump the GPRs & sp & pc & cpsr (cpsr is 32-bit in the xml desc)
|
// Dump the GPRs & sp & pc & cpsr (cpsr is 32-bit in the xml desc)
|
||||||
// For performance reasons, we don't include the FPU registers here
|
// For performance reasons, we don't include the FPU registers here
|
||||||
for (u32 i = 0; i < 31; i++) {
|
for (u32 i = 0; i < 31; i++) {
|
||||||
n += sprintf(out + n, "%x:%016lx;", i, __builtin_bswap64(readFrameRegister(frame, i)));
|
n += sprintf(out + n, "%x:%016lx;", i, __builtin_bswap64(ReadRegister(frame, i)));
|
||||||
}
|
}
|
||||||
|
|
||||||
n += sprintf(
|
n += sprintf(
|
||||||
|
|
|
@ -101,12 +101,12 @@ namespace ams::hvisor {
|
||||||
}
|
}
|
||||||
|
|
||||||
template<typename T = u64>
|
template<typename T = u64>
|
||||||
constexpr T ReadFrameRegister(u32 id) const
|
constexpr T ReadRegister(u32 id) const
|
||||||
{
|
{
|
||||||
static_assert(std::is_integral_v<T> && std::is_unsigned_v<T>);
|
static_assert(std::is_integral_v<T> && std::is_unsigned_v<T>);
|
||||||
return id == 31 ? static_cast<T>(0u) /* xzr */ : static_cast<T>(x[id]);
|
return id == 31 ? static_cast<T>(0u) /* xzr */ : static_cast<T>(x[id]);
|
||||||
}
|
}
|
||||||
constexpr void WriteFrameRegister(u32 id, u64 val)
|
constexpr void WriteRegister(u32 id, u64 val)
|
||||||
{
|
{
|
||||||
if (id != 31) {
|
if (id != 31) {
|
||||||
// If not xzr
|
// If not xzr
|
||||||
|
|
|
@ -68,11 +68,11 @@ namespace ams::hvisor::traps {
|
||||||
if (VirtualGic::ValidateGicdRegisterAccess(off, sz)) {
|
if (VirtualGic::ValidateGicdRegisterAccess(off, sz)) {
|
||||||
if (dabtIss.wnr) {
|
if (dabtIss.wnr) {
|
||||||
// Register write
|
// Register write
|
||||||
u32 reg = frame->ReadFrameRegister<u32>(dabtIss.srt);
|
u32 reg = frame->ReadRegister<u32>(dabtIss.srt);
|
||||||
VirtualGic::GetInstance().WriteGicdRegister(reg, off, sz);
|
VirtualGic::GetInstance().WriteGicdRegister(reg, off, sz);
|
||||||
} else {
|
} else {
|
||||||
// Reigster read
|
// Reigster read
|
||||||
frame->WriteFrameRegister(dabtIss.srt, VirtualGic::GetInstance().ReadGicdRegister(off, sz));
|
frame->WriteRegister(dabtIss.srt, VirtualGic::GetInstance().ReadGicdRegister(off, sz));
|
||||||
}
|
}
|
||||||
} else {
|
} else {
|
||||||
// Invalid GICD access
|
// Invalid GICD access
|
||||||
|
|
251
thermosphere/src/traps/hvisor_traps_sysreg.cpp
Normal file
251
thermosphere/src/traps/hvisor_traps_sysreg.cpp
Normal file
|
@ -0,0 +1,251 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2019-2020 Atmosphère-NX
|
||||||
|
*
|
||||||
|
* This program is free software; you can redistribute it and/or modify it
|
||||||
|
* under the terms and conditions of the GNU General Public License,
|
||||||
|
* version 2, as published by the Free Software Foundation.
|
||||||
|
*
|
||||||
|
* This program is distributed in the hope it will be useful, but WITHOUT
|
||||||
|
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
|
||||||
|
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
|
||||||
|
* more details.
|
||||||
|
*
|
||||||
|
* You should have received a copy of the GNU General Public License
|
||||||
|
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "hvisor_traps_sysreg.hpp"
|
||||||
|
#include "../hvisor_core_context.hpp"
|
||||||
|
#include "../hvisor_guest_timers.hpp"
|
||||||
|
#include "../cpu/hvisor_cpu_caches.hpp"
|
||||||
|
|
||||||
|
using namespace ams::hvisor;
|
||||||
|
using namespace ams::hvisor::traps;
|
||||||
|
using namespace ams::hvisor::cpu;
|
||||||
|
|
||||||
|
namespace {
|
||||||
|
|
||||||
|
inline u64 DoSystemRegisterRead(const ExceptionStackFrame *frame, u32 normalizedIss)
|
||||||
|
{
|
||||||
|
// See https://developer.arm.com/architectures/learn-the-architecture/generic-timer/single-page
|
||||||
|
|
||||||
|
u64 val;
|
||||||
|
switch (normalizedIss) {
|
||||||
|
case EncodeSysregIss(cntpct_el0): {
|
||||||
|
u64 vct = ComputeCntvct(frame);
|
||||||
|
val = vct;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case EncodeSysregIss(cntp_tval_el0): {
|
||||||
|
u64 vct = frame->cntpct_el0 - currentCoreCtx->GetTotalTimeInHypervisor();
|
||||||
|
u64 cval = currentCoreCtx->GetEmulPtimerCval();
|
||||||
|
val = (cval - vct) & 0xFFFFFFFF;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case EncodeSysregIss(cntp_ctl_el0): {
|
||||||
|
val = frame->cntp_ctl_el0;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case EncodeSysregIss(cntp_cval_el0): {
|
||||||
|
val = currentCoreCtx->GetEmulPtimerCval();
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
// NOTE: We should trap ID_AA64* register to lie to the guest about e.g. MemTag but it would take too much space
|
||||||
|
default: {
|
||||||
|
// We shouldn't have trapped on other registers other than debug regs
|
||||||
|
// and we want the latter as RA0/WI
|
||||||
|
val = 0;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return val;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void DoSystemRegisterWrite(ExceptionStackFrame *frame, u32 normalizedIss, u64 val)
|
||||||
|
{
|
||||||
|
// See https://developer.arm.com/architectures/learn-the-architecture/generic-timer/single-page
|
||||||
|
|
||||||
|
switch (normalizedIss) {
|
||||||
|
case EncodeSysregIss(cntp_tval_el0): {
|
||||||
|
// Sign-extend
|
||||||
|
s32 signedVal = static_cast<s32>(val);
|
||||||
|
u64 vct = ComputeCntvct(frame);
|
||||||
|
WriteEmulatedPhysicalCompareValue(frame, vct + signedVal);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case EncodeSysregIss(cntp_ctl_el0): {
|
||||||
|
frame->cntp_ctl_el0 = val;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case EncodeSysregIss(cntp_cval_el0): {
|
||||||
|
WriteEmulatedPhysicalCompareValue(frame, val);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
// If we didn't trap it, dc isw would behave like dc cisw because stage2 translations are enabled.
|
||||||
|
// Turning dc csw into cisw is also harmless.
|
||||||
|
case EncodeSysregIss(dc_csw):
|
||||||
|
case EncodeSysregIss(dc_isw):
|
||||||
|
case EncodeSysregIss(dc_cisw): {
|
||||||
|
HandleTrappedSetWayOperation(static_cast<u32>(val));
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
default: {
|
||||||
|
// We shouldn't have trapped on other registers other than debug regs
|
||||||
|
// and we want the latter as RA0/WI
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void DoMrs(ExceptionStackFrame *frame, u32 normalizedIss, u32 reg)
|
||||||
|
{
|
||||||
|
frame->WriteRegister(reg, DoSystemRegisterRead(frame, normalizedIss));
|
||||||
|
frame->SkipInstruction(4);
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void DoMsr(ExceptionStackFrame *frame, u32 normalizedIss, u32 reg)
|
||||||
|
{
|
||||||
|
DoSystemRegisterWrite(frame, normalizedIss, frame->ReadRegister(reg));
|
||||||
|
frame->SkipInstruction(4);
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void DoMrc(ExceptionStackFrame *frame, u32 normalizedIss, u32 instructionLength, u32 reg)
|
||||||
|
{
|
||||||
|
frame->WriteRegister(reg, DoSystemRegisterRead(frame, normalizedIss) & 0xFFFFFFFF);
|
||||||
|
frame->SkipInstruction(instructionLength);
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void DoMcr(ExceptionStackFrame *frame, u32 normalizedIss, u32 instructionLength, u32 reg)
|
||||||
|
{
|
||||||
|
DoSystemRegisterWrite(frame, normalizedIss, frame->ReadRegister<u32>(reg));
|
||||||
|
frame->SkipInstruction(instructionLength);
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void DoMrrc(ExceptionStackFrame *frame, u32 normalizedIss, u32 instructionLength, u32 reg, u32 reg2)
|
||||||
|
{
|
||||||
|
u64 val = DoSystemRegisterRead(frame, normalizedIss);
|
||||||
|
frame->WriteRegister(reg, val & 0xFFFFFFFF);
|
||||||
|
frame->WriteRegister(reg2, val >> 32);
|
||||||
|
frame->SkipInstruction(instructionLength);
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void DoMcrr(ExceptionStackFrame *frame, u32 normalizedIss, u32 instructionLength, u32 reg, u32 reg2)
|
||||||
|
{
|
||||||
|
u64 valLo = frame->ReadRegister(reg) & 0xFFFFFFFF;
|
||||||
|
u64 valHi = frame->ReadRegister(reg2) << 32;
|
||||||
|
DoSystemRegisterWrite(frame, normalizedIss, valHi | valLo);
|
||||||
|
frame->SkipInstruction(instructionLength);
|
||||||
|
}
|
||||||
|
|
||||||
|
inline bool EvaluateMcrMrcCondition(const ExceptionStackFrame *frame, u32 condition, bool condValid)
|
||||||
|
{
|
||||||
|
if (!condValid) {
|
||||||
|
// Only T32 instructions can do that
|
||||||
|
u32 it = frame->GetT32ItFlags();
|
||||||
|
return it == 0 || frame->EvaluateConditionCode(it >> 4);
|
||||||
|
} else {
|
||||||
|
return frame->EvaluateConditionCode(condition);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
namespace ams::hvisor::traps {
|
||||||
|
|
||||||
|
void HandleMsrMrsTrap(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr)
|
||||||
|
{
|
||||||
|
u32 iss = esr.iss;
|
||||||
|
u32 reg = (iss >> 5) & 31;
|
||||||
|
bool isRead = (iss & 1) != 0;
|
||||||
|
|
||||||
|
// Clear register field and set direction field to 'normalize' the ISS
|
||||||
|
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;
|
||||||
|
u32 instructionLength = esr.il == 0 ? 2 : 4;
|
||||||
|
|
||||||
|
if (!EvaluateMcrMrcCondition(frame, (iss >> 20) & 0xF, (iss & BIT(24)) != 0)) {
|
||||||
|
// If instruction not valid/condition code says no
|
||||||
|
frame->SkipInstruction(instructionLength);
|
||||||
|
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;
|
||||||
|
|
||||||
|
ENSURE2(
|
||||||
|
opc1 == 0 && CRn == 14 && CRm == 2 && opc2 <= 1,
|
||||||
|
"unexpected cp15 register, instruction: %s p15, #%u, r%u, c%u, c%u, #%u\n",
|
||||||
|
isRead ? "mrc" : "mcr", opc1, Rt, CRn, CRm, opc2
|
||||||
|
);
|
||||||
|
|
||||||
|
iss = opc2 == 0 ? EncodeSysregIss(cntp_tval_el0) : EncodeSysregIss(cntp_ctl_el0);
|
||||||
|
|
||||||
|
if (isRead) {
|
||||||
|
DoMrc(frame, iss, instructionLength, Rt);
|
||||||
|
} else {
|
||||||
|
DoMcr(frame, iss, instructionLength, Rt);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void HandleMcrrMrrcCP15Trap(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr)
|
||||||
|
{
|
||||||
|
u32 iss = esr.iss;
|
||||||
|
u32 instructionLength = esr.il == 0 ? 2 : 4;
|
||||||
|
|
||||||
|
if (!EvaluateMcrMrcCondition(frame, (iss >> 20) & 0xF, (iss & BIT(24)) != 0)) {
|
||||||
|
// If instruction not valid/condition code says no
|
||||||
|
frame->SkipInstruction(instructionLength);
|
||||||
|
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;
|
||||||
|
|
||||||
|
ENSURE2(
|
||||||
|
CRm == 14 && (opc1 == 0 || opc1 == 2),
|
||||||
|
"handleMcrrMrrcTrap: unexpected cp15 register, instruction: %s p15, #%u, r%u, r%u, c%u\n",
|
||||||
|
isRead ? "mrrc" : "mcrr", opc1, Rt, Rt, CRm
|
||||||
|
);
|
||||||
|
|
||||||
|
iss = opc1 == 0 ? EncodeSysregIss(cntpct_el0) : EncodeSysregIss(cntp_cval_el0);
|
||||||
|
|
||||||
|
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, (esr.iss >> 20) & 0xF, (esr.iss & BIT(24)) != 0)) {
|
||||||
|
frame->WriteRegister((esr.iss >> 5) & 0x1F, 0);
|
||||||
|
}
|
||||||
|
frame->SkipInstruction(esr.il == 0 ? 2 : 4);
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
|
@ -1,5 +1,5 @@
|
||||||
/*
|
/*
|
||||||
* Copyright (c) 2019 Atmosphère-NX
|
* Copyright (c) 2019-2020 Atmosphère-NX
|
||||||
*
|
*
|
||||||
* This program is free software; you can redistribute it and/or modify it
|
* This program is free software; you can redistribute it and/or modify it
|
||||||
* under the terms and conditions of the GNU General Public License,
|
* under the terms and conditions of the GNU General Public License,
|
||||||
|
@ -16,11 +16,14 @@
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include "traps.h"
|
#include "../hvisor_exception_stack_frame.hpp"
|
||||||
|
|
||||||
void handleMsrMrsTrap(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr);
|
namespace ams::hvisor::traps {
|
||||||
void handleMcrMrcCP15Trap(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr);
|
|
||||||
void handleMcrrMrrcCP15Trap(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr);
|
|
||||||
void handleA32CP14Trap(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr);
|
|
||||||
|
|
||||||
void handleA32CP14Trap(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr);
|
void HandleMsrMrsTrap(ExceptionStackFrame *frame, cpu::ExceptionSyndromeRegister esr);
|
||||||
|
void HandleMcrMrcCP15Trap(ExceptionStackFrame *frame, cpu::ExceptionSyndromeRegister esr);
|
||||||
|
void HandleMcrrMrrcCP15Trap(ExceptionStackFrame *frame, cpu::ExceptionSyndromeRegister esr);
|
||||||
|
void HandleA32CP14Trap(ExceptionStackFrame *frame, cpu::ExceptionSyndromeRegister esr);
|
||||||
|
void HandleA32CP14Trap(ExceptionStackFrame *frame, cpu::ExceptionSyndromeRegister esr);
|
||||||
|
|
||||||
|
}
|
|
@ -1,238 +0,0 @@
|
||||||
/*
|
|
||||||
* Copyright (c) 2019 Atmosphère-NX
|
|
||||||
*
|
|
||||||
* This program is free software; you can redistribute it and/or modify it
|
|
||||||
* under the terms and conditions of the GNU General Public License,
|
|
||||||
* version 2, as published by the Free Software Foundation.
|
|
||||||
*
|
|
||||||
* This program is distributed in the hope it will be useful, but WITHOUT
|
|
||||||
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
|
|
||||||
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
|
|
||||||
* more details.
|
|
||||||
*
|
|
||||||
* You should have received a copy of the GNU General Public License
|
|
||||||
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include "sysreg_traps.h"
|
|
||||||
#include "guest_timers.h"
|
|
||||||
#include "caches.h"
|
|
||||||
|
|
||||||
static inline u64 doSystemRegisterRead(const ExceptionStackFrame *frame, u32 normalizedIss)
|
|
||||||
{
|
|
||||||
// See https://developer.arm.com/architectures/learn-the-architecture/generic-timer/single-page
|
|
||||||
|
|
||||||
u64 val;
|
|
||||||
switch (normalizedIss) {
|
|
||||||
case ENCODE_SYSREG_ISS(CNTPCT_EL0): {
|
|
||||||
u64 vct = computeCntvct(frame);
|
|
||||||
val = vct;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
case ENCODE_SYSREG_ISS(CNTP_TVAL_EL0): {
|
|
||||||
u64 vct = frame->cntpct_el0 - currentCoreCtx->totalTimeInHypervisor;
|
|
||||||
u64 cval = currentCoreCtx->emulPtimerCval;
|
|
||||||
val = (cval - vct) & 0xFFFFFFFF;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
case ENCODE_SYSREG_ISS(CNTP_CTL_EL0): {
|
|
||||||
val = frame->cntp_ctl_el0;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
case ENCODE_SYSREG_ISS(CNTP_CVAL_EL0): {
|
|
||||||
val = currentCoreCtx->emulPtimerCval;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
// NOTE: We should trap ID_AA64* register to lie to the guest about e.g. MemTag but it would take too much space
|
|
||||||
default: {
|
|
||||||
// We shouldn't have trapped on other registers other than debug regs
|
|
||||||
// and we want the latter as RA0/WI
|
|
||||||
val = 0;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
return val;
|
|
||||||
}
|
|
||||||
|
|
||||||
static inline void doSystemRegisterWrite(ExceptionStackFrame *frame, u32 normalizedIss, u64 val)
|
|
||||||
{
|
|
||||||
// See https://developer.arm.com/architectures/learn-the-architecture/generic-timer/single-page
|
|
||||||
|
|
||||||
switch (normalizedIss) {
|
|
||||||
case ENCODE_SYSREG_ISS(CNTP_TVAL_EL0): {
|
|
||||||
// Sign-extend
|
|
||||||
u64 vct = computeCntvct(frame);
|
|
||||||
writeEmulatedPhysicalCompareValue(frame, vct + (u64)(s32)val);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
case ENCODE_SYSREG_ISS(CNTP_CTL_EL0): {
|
|
||||||
frame->cntp_ctl_el0 = val;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
case ENCODE_SYSREG_ISS(CNTP_CVAL_EL0): {
|
|
||||||
writeEmulatedPhysicalCompareValue(frame, val);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
case ENCODE_SYSREG_ISS(DC_CSW):
|
|
||||||
case ENCODE_SYSREG_ISS(DC_CISW): {
|
|
||||||
cacheHandleTrappedSetWayOperation(false);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
case ENCODE_SYSREG_ISS(DC_ISW): {
|
|
||||||
cacheHandleTrappedSetWayOperation(true);
|
|
||||||
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(frame, normalizedIss));
|
|
||||||
skipFaultingInstruction(frame, 4);
|
|
||||||
}
|
|
||||||
|
|
||||||
static inline void doMsr(ExceptionStackFrame *frame, u32 normalizedIss, u32 reg)
|
|
||||||
{
|
|
||||||
u64 val = readFrameRegisterZ(frame, reg);
|
|
||||||
doSystemRegisterWrite(frame, normalizedIss, val);
|
|
||||||
skipFaultingInstruction(frame, 4);
|
|
||||||
}
|
|
||||||
|
|
||||||
static inline void doMrc(ExceptionStackFrame *frame, u32 normalizedIss, u32 instructionLength, u32 reg)
|
|
||||||
{
|
|
||||||
writeFrameRegisterZ(frame, reg, doSystemRegisterRead(frame, normalizedIss) & 0xFFFFFFFF);
|
|
||||||
skipFaultingInstruction(frame, instructionLength);
|
|
||||||
}
|
|
||||||
|
|
||||||
static inline void doMcr(ExceptionStackFrame *frame, u32 normalizedIss, u32 instructionLength, u32 reg)
|
|
||||||
{
|
|
||||||
u64 val = readFrameRegisterZ(frame, reg) & 0xFFFFFFFF;
|
|
||||||
doSystemRegisterWrite(frame, normalizedIss, val);
|
|
||||||
skipFaultingInstruction(frame, instructionLength);
|
|
||||||
}
|
|
||||||
|
|
||||||
static inline void doMrrc(ExceptionStackFrame *frame, u32 normalizedIss, u32 instructionLength, u32 reg, u32 reg2)
|
|
||||||
{
|
|
||||||
u64 val = doSystemRegisterRead(frame, 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(frame, normalizedIss, valHi | valLo);
|
|
||||||
skipFaultingInstruction(frame, instructionLength);
|
|
||||||
}
|
|
||||||
|
|
||||||
static bool evaluateMcrMrcCondition(u64 spsr, u32 condition, bool condValid)
|
|
||||||
{
|
|
||||||
if (!condValid) {
|
|
||||||
// Only T32 instructions can do that
|
|
||||||
u32 it = spsrGetT32ItFlags(spsr);
|
|
||||||
return it == 0 || spsrEvaluateConditionCode(spsr, it >> 4);
|
|
||||||
} else {
|
|
||||||
return spsrEvaluateConditionCode(spsr, condition);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void handleMsrMrsTrap(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr)
|
|
||||||
{
|
|
||||||
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;
|
|
||||||
|
|
||||||
ENSURE2(
|
|
||||||
opc1 == 0 && CRn == 14 && CRm == 2 && opc2 <= 1,
|
|
||||||
"unexpected cp15 register, instruction: %s p15, #%u, r%u, c%u, c%u, #%u\n",
|
|
||||||
isRead ? "mrc" : "mcr", opc1, Rt, CRn, CRm, opc2
|
|
||||||
);
|
|
||||||
|
|
||||||
iss = opc2 == 0 ? ENCODE_SYSREG_ISS(CNTP_TVAL_EL0) : ENCODE_SYSREG_ISS(CNTP_CTL_EL0);
|
|
||||||
|
|
||||||
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;
|
|
||||||
|
|
||||||
ENSURE2(
|
|
||||||
CRm == 14 && (opc1 == 0 || opc1 == 2),
|
|
||||||
"handleMcrrMrrcTrap: unexpected cp15 register, instruction: %s p15, #%u, r%u, r%u, c%u\n",
|
|
||||||
isRead ? "mrrc" : "mcrr", opc1, Rt, Rt, CRm
|
|
||||||
);
|
|
||||||
|
|
||||||
iss = opc1 == 0 ? ENCODE_SYSREG_ISS(CNTPCT_EL0) : ENCODE_SYSREG_ISS(CNTP_CVAL_EL0);
|
|
||||||
|
|
||||||
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);
|
|
||||||
}
|
|
||||||
skipFaultingInstruction(frame, esr.il == 0 ? 2 : 4);
|
|
||||||
}
|
|
Loading…
Reference in a new issue