thermosphere: trap refactor WIP

thermosphere/src/traps/hvisor_traps_data_abort.cpp

@@ -0,0 +1,89 @@
+/*
+ * 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_data_abort.hpp"
+
+#include "../hvisor_virtual_gic.hpp"
+
+/*
+#include "../hvisor_core_context.hpp"
+#include "../cpu/hvisor_cpu_sysreg_general.hpp"
+#include "../cpu/hvisor_cpu_instructions.hpp"
+*/
+
+// Lower el
+
+namespace ams::hvisor::traps {
+
+    void DumpUnhandledDataAbort(cpu::DataAbortIss dabtIss, u64 far, const char *msg)
+    {
+        char s1[64], s2[32], s3[64] = "";
+        static_cast<void>(s1);
+        static_cast<void>(s2);
+        static_cast<void>(s3);
+        sprintf(s1, "Unhandled%s %s", msg , dabtIss.wnr ? "write" : "read");
+        if (dabtIss.fnv) {
+            sprintf(s2, "<unk>");
+        } else {
+            sprintf(s2, "%016lx", far);
+        }
+
+        if (dabtIss.isv) {
+            sprintf(s3, ", size %u Rt=%u", BIT(dabtIss.sas), dabtIss.srt);
+        }
+
+        DEBUG("%s at %s%s\n", s1, s2, s3);
+    }
+
+    void HandleLowerElDataAbortException(ExceptionStackFrame *frame, cpu::ExceptionSyndromeRegister esr)
+    {
+        cpu::DataAbortIss dabtIss;
+        u32 iss = esr.iss;
+        std::memcpy(&iss, &dabtIss, 4);
+
+        u64 far = frame->far_el2;
+        u64 farpg = far & ~0xFFFull;
+        size_t off = far & 0xFFF;
+        size_t sz = dabtIss.GetAccessSize();
+
+        // We don't support 8-byte writes for MMIO
+        if (!dabtIss.HasValidFar() || sz > 4) {
+            DumpUnhandledDataAbort(dabtIss, far, "");
+        }
+
+        if (farpg == VirtualGic::gicdPhysicalAddress) {
+            if (VirtualGic::ValidateGicdRegisterAccess(off, sz)) {
+                if (dabtIss.wnr) {
+                    // Register write
+                    u32 reg = frame->ReadFrameRegister<u32>(dabtIss.srt);
+                    VirtualGic::GetInstance().WriteGicdRegister(reg, off, sz);
+                } else {
+                    // Reigster read
+                    frame->WriteFrameRegister(dabtIss.srt, VirtualGic::GetInstance().ReadGicdRegister(off, sz));
+                }
+            } else {
+                // Invalid GICD access
+                DumpUnhandledDataAbort(dabtIss, far, "GICD");
+            }
+        } else {
+            DumpUnhandledDataAbort(dabtIss, far, "");
+        }
+
+        // Skip instruction anyway
+        frame->SkipInstruction(esr.il == 0 ? 2 : 4);
+    }
+
+}

thermosphere/src/traps/hvisor_traps_data_abort.hpp

@@ -0,0 +1,26 @@
+/*
+ * 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/>.
+ */
+
+#pragma once
+
+#include "../hvisor_exception_stack_frame.hpp"
+
+namespace ams::hvisor::traps {
+
+    void DumpUnhandledDataAbort(cpu::DataAbortIss dabtIss, u64 far, const char *msg);
+    void HandleLowerElDataAbort(ExceptionStackFrame *frame, cpu::ExceptionSyndromeRegister esr);
+
+}

thermosphere/src/traps/hvisor_traps_hvc.cpp

@@ -0,0 +1,33 @@
+/*
+ * 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_hvc.hpp"
+#include "../hvisor_exception_dispatcher.hpp"
+
+namespace ams::hvisor::traps {
+
+    void HandleHypercall(ExceptionStackFrame *frame, cpu::ExceptionSyndromeRegister esr)
+    {
+        u32 id = esr.iss;
+        switch (id) {
+            default:
+                DEBUG("Unhandled hypercall: 0x%x.\n");
+                DumpStackFrame(frame, false);
+                break;
+        }
+    }
+
+}

thermosphere/src/traps/hvisor_traps_hvc.hpp

@@ -0,0 +1,25 @@
+/*
+ * 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/>.
+ */
+
+#pragma once
+
+#include "../hvisor_exception_stack_frame.hpp"
+
+namespace ams::hvisor::traps {
+
+    void HandleHypercall(ExceptionStackFrame *frame, cpu::ExceptionSyndromeRegister esr);
+
+}

thermosphere/src/traps/hvisor_traps_single_step.cpp

@@ -0,0 +1,87 @@
+/*
+ * 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_single_step.hpp"
+
+#include "../hvisor_core_context.hpp"
+#include "../cpu/hvisor_cpu_sysreg_general.hpp"
+#include "../cpu/hvisor_cpu_instructions.hpp"
+
+#include "../debug_manager.h"
+
+namespace ams::hvisor::traps {
+
+    SingleStepState GetNextSingleStepState(ExceptionStackFrame *frame)
+    {
+        u64 mdscr = THERMOSPHERE_GET_SYSREG(mdscr_el1);
+        bool mdscrSS = (mdscr & cpu::MDSCR_SS) != 0;
+        bool pstateSS = (frame->spsr_el2 & cpu::PSR_SS) != 0;
+
+        if (!mdscrSS) {
+            return SingleStepState::Inactive;
+        } else {
+            return pstateSS ? SingleStepState::ActiveNotPending : SingleStepState::ActivePending;
+        }
+
+    }
+
+    void SetNextSingleStepState(ExceptionStackFrame *frame, SingleStepState state)
+    {
+        u64 mdscr = THERMOSPHERE_GET_SYSREG(mdscr_el1);
+
+        switch (state) {
+            case SingleStepState::Inactive:
+                // Unset mdscr_el1.ss
+                mdscr &= ~cpu::MDSCR_SS;
+                break;
+            case SingleStepState::ActiveNotPending:
+                // Set mdscr_el1.ss and pstate.ss
+                mdscr |= cpu::MDSCR_SS;
+                frame->spsr_el2 |= cpu::PSR_SS;
+                break;
+            case SingleStepState::ActivePending:
+                // We never use this because pstate.ss is 0 by default...
+                // Set mdscr_el1.ss and unset pstate.ss
+                mdscr |= cpu::MDSCR_SS;
+                frame->spsr_el2 &= cpu::PSR_SS;
+                break;
+            default:
+                break;
+        }
+
+        THERMOSPHERE_SET_SYSREG(mdscr_el1, mdscr);
+        cpu::isb(); // TRM-mandated
+    }
+
+    void HandleSingleStep(ExceptionStackFrame *frame, cpu::ExceptionSyndromeRegister esr)
+    {
+        uintptr_t addr = frame->elr_el2;
+
+        // Stepping range support;
+        auto [startAddr, endAddr] = currentCoreCtx->GetSteppingRange();
+        if (addr >= startAddr && addr < endAddr) {
+            // Reactivate single-step
+            SetNextSingleStepState(frame, SingleStepState::ActiveNotPending);
+        } else {
+            // Disable single-step
+            SetNextSingleStepState(frame, SingleStepState::Inactive);
+            debugManagerReportEvent(DBGEVENT_EXCEPTION);
+        }
+
+        DEBUG("Single-step exeception ELR = 0x%016llx, ISV = %u, EX = %u\n", frame->elr_el2, (esr.iss >> 24) & 1, (esr.iss >> 6) & 1);
+    }
+
+}

thermosphere/src/traps/hvisor_traps_single_step.hpp

@@ -0,0 +1,37 @@
+/*
+ * 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/>.
+ */
+
+#pragma once
+
+#include "../hvisor_exception_stack_frame.hpp"
+
+namespace ams::hvisor::traps {
+
+    enum class SingleStepState {
+        Inactive            = 0, /// Single step disabled OR in the debugger
+        ActiveNotPending    = 1, /// Instruction not yet executed
+        ActivePending       = 2, /// Instruction executed or return-from-trap, single-step exception is going to be generated soon
+    };
+
+    /// Get the single-step state machine state (state after eret)
+    SingleStepState GetNextSingleStepState(ExceptionStackFrame *frame);
+
+    /// Set the single-step state machine state (state after eret). Frame can be nullptr iff new state is "inactive"
+    void SetNextSingleStepState(ExceptionStackFrame *frame, SingleStepState state);
+
+    void HandleSingleStep(ExceptionStackFrame *frame, cpu::ExceptionSyndromeRegister esr);
+
+}

thermosphere/src/traps/smc.c

@@ -0,0 +1,75 @@
+#include <string.h>
+#include "smc.h"
+#include "core_ctx.h"
+#include "caches.h"
+#include "memory_map.h"
+#include "debug_manager.h"
+
+// Currently in exception_vectors.s:
+extern const u32 doSmcIndirectCallImpl[];
+extern const u32 doSmcIndirectCallImplSmcInstructionOffset, doSmcIndirectCallImplSize;
+
+void doSmcIndirectCall(ExceptionStackFrame *frame, u32 smcId)
+{
+    u32 codebuf[doSmcIndirectCallImplSize / 4]; // note: potential VLA
+    memcpy(codebuf, doSmcIndirectCallImpl, doSmcIndirectCallImplSize);
+    codebuf[doSmcIndirectCallImplSmcInstructionOffset / 4] |= smcId << 5;
+
+    cacheHandleSelfModifyingCodePoU(codebuf, doSmcIndirectCallImplSize/4);
+
+    __dsb();
+    __isb();
+    ((void (*)(ExceptionStackFrame *))codebuf)(frame);
+}
+
+static void doCpuOnHook(ExceptionStackFrame *frame, u32 smcId)
+{
+    // Note: preserve contexId which is passed by EL3, we'll save it later
+    u32 cpuId = (u32)frame->x[1]; // note: full affinity mask. Start.s checks that Aff1,2,3 are 0.
+    uintptr_t ep = frame->x[2];
+    // frame->x[3] is contextId
+    if (cpuId < 4) {
+        g_coreCtxs[cpuId].kernelEntrypoint = ep;
+        frame->x[2] = g_loadImageLayout.startPa + 4;
+    }
+}
+
+void handleSmcTrap(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr)
+{
+    // TODO: Arm PSCI 0.2+ stuff
+    u32 smcId = esr.iss;
+
+    // Note: don't differenciate PSCI calls by smc immediate since HOS uses #1 for that
+    // Note: funcId is actually u32 according to Arm PSCI. Not sure what to do if x0>>32 != 0.
+    // NN doesn't truncate, Arm TF does.
+    // Note: clear NN ABI-breaking bits
+    u32 funcId = frame->x[0] & ~0xFF00;
+    switch (funcId) {
+        case 0xC4000001:
+            // CPU_SUSPEND
+            // TODO
+            break;
+        case 0x84000002:
+            // CPU_OFF
+            // TODO
+            debugManagerReportEvent(DBGEVENT_CORE_OFF);
+            break;
+        case 0xC4000003:
+            doCpuOnHook(frame, smcId);
+            break;
+        case 0x84000008:
+            // SYSTEM_OFF, not implemented by Nintendo
+            // TODO
+            break;
+        case 0x84000009:
+            // SYSTEM_RESET, not implemented by Nintendo
+            // TODO
+            break;
+
+        default:
+            // Other unimportant functions we don't care about
+            break;
+    }
+    doSmcIndirectCall(frame, smcId);
+    skipFaultingInstruction(frame, 4);
+}

thermosphere/src/traps/smc.h

@@ -0,0 +1,23 @@
+/*
+ * 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/>.
+ */
+
+#pragma once
+
+#include "traps.h"
+
+void doSmcIndirectCall(ExceptionStackFrame *frame, u32 smcId);
+
+void handleSmcTrap(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr);

thermosphere/src/traps/sysreg_traps.c

@@ -0,0 +1,238 @@
+/*
+ * 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);
+}

thermosphere/src/traps/sysreg_traps.h

@@ -0,0 +1,26 @@
+/*
+ * 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/>.
+ */
+
+#pragma once
+
+#include "traps.h"
+
+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 handleA32CP14Trap(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr);