thermosphere: add some basic sysreg trapping code

thermosphere/src/synchronization.h

@@ -0,0 +1,27 @@
+/*
+ * 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
+
+static inline void __dsb_sy(void)
+{
+    __asm__ __volatile__ ("dsb sy" ::: "memory");
+}
+
+static inline void __isb(void)
+{
+    __asm__ __volatile__ ("isb" ::: "memory");
+}

thermosphere/src/sysreg.h

@@ -403,6 +403,9 @@
 #define MAKE_MSR(name, Rt)      (0xD5000000 | ENCODE_SYSREG_MOV(name) | ((Rt) & 0x1F))
 #define MAKE_MRS(name, Rt)      (0xD5200000 | ENCODE_SYSREG_MOV(name) | ((Rt) & 0x1F))
 
+#define MAKE_MSR_FROM_FIELDS(op0, op1, crn, crm, op2, Rt)    (0xD5000000 | ENCODE_SYSREG_FIELDS_MOV(op0, op1, crn, crm, op2) | ((Rt) & 0x1F))
+#define MAKE_MRS_FROM_FIELDS(op0, op1, crn, crm, op2, Rt)    (0xD5200000 | ENCODE_SYSREG_FIELDS_MOV(op0, op1, crn, crm, op2) | ((Rt) & 0x1F))
+
 #define ENCODE_SYSREG_FIELDS_ISS(op0, op1, crn, crm, op2) (((op0) << 20) | ((op2) << 17) | ((op1) << 14) | ((crn) << 10) | ((crm) << 1))
 #define ENCODE_SYSREG_ISS(name) EVAL(ENCODE_SYSREG_FIELDS_ISS CAT(TUP_, name))
 
@@ -413,3 +416,13 @@
 })
 
 #define SET_SYSREG(reg, val) do { u64 temp_reg = (val); __asm__ __volatile__ ("msr " #reg ", %0" :: "r"(temp_reg) : "memory"); } while(false)
+
+#define SYSREG_OP1_AARCH32_AUTO 0
+#define SYSREG_OP1_AARCH64_EL1  0
+#define SYSREG_OP1_CACHE        1
+#define SYSREG_OP1_CACHESZSEL   2
+#define SYSREG_OP1_EL0          3
+#define SYSREG_OP1_EL2          4
+#define SYSREG_OP1_EL3          6
+#define SYSREG_OP1_AARCH32_JZL  7
+#define SYSREG_OP1_AARCH64_SEL1 7

thermosphere/src/sysreg_traps.c

@@ -0,0 +1,138 @@
+/*
+ * 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 "synchronization.h"
+#include "sysreg.h"
+
+// For a32 mcr/mrc => a64 mrs
+u32 convertMcrMrcIss(u32 *outCondition, u32 a32Iss, u32 coproc, u32 el)
+{
+    // NOTE: MCRR / MRRC do NOT map for the most part and need to be handled separately
+
+    //u32 direction = a32Iss & 1;
+
+    //u32 opc2 = (a32Iss >> 17) & 7;
+    u32 opc1 = (a32Iss >> 14) & 7;
+    u32 CRn  = (a32Iss >> 10) & 15;
+    //u32 Rt   = (a32Iss >>  5) & 31;
+    //u32 CRm  = (a32Iss >>  1) & 15;
+
+    bool condValid = (a32Iss & BIT(24)) != 0;
+    *outCondition = condValid ? ((a32Iss >> 20) & 15): 14; // use "unconditional" by default
+
+    u32 op0 = (16 - coproc) & 3;
+    u32 op1;
+
+    // Do NOT translate cp15, 0, c7-8 (Cache, and resp. TLB maintenance coproc regs) as they don't map to Aarch64
+    if (coproc == 15 && (CRn == 7 || CRn == 8)) {
+        return -2;
+    }
+
+    // The difficult part
+    switch (opc1) {
+        case SYSREG_OP1_AARCH32_AUTO: {
+            switch (el) {
+                case 0:
+                    op1 = SYSREG_OP1_EL0;
+                    break;
+                case 1:
+                    op1 = SYSREG_OP1_AARCH64_EL1;
+                    break;
+                case 2:
+                    op1 = SYSREG_OP1_EL2;
+                    break;
+                case 3:
+                    op1 = SYSREG_OP1_EL3;
+                    break;
+                default:
+                    return -1;
+            }
+            break;
+        }
+
+        case SYSREG_OP1_EL0:
+        case SYSREG_OP1_EL2:
+        case SYSREG_OP1_EL3:
+        case SYSREG_OP1_CACHE:
+        case SYSREG_OP1_CACHESZSEL:
+            op1 = opc1;
+            break;
+
+        // We shouldn't even trap those to begin with.
+        case SYSREG_OP1_AARCH32_JZL:
+        default:
+            return -1;
+    }
+
+    // Everything but op0 is at its correct place & only op1 needs to be replaced
+    return (a32Iss & ~(MASK2(24, 20) | MASK2(16, 14))) | (op0 << 20) | (op1 << 14);
+}
+
+static void doSystemRegisterRwImpl(u64 *val, u32 iss)
+{
+    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);
+
+    __dsb_sy();
+    __isb();
+
+    *val = ((u64 (*)(u64))codebuf)(*val);
+}
+
+void doSystemRegisterRead(ExceptionStackFrame *frame, u32 iss, u32 reg1, u32 reg2)
+{
+    // reg1 != reg2: mrrc/mcrr
+    u64 val = 0;
+    doSystemRegisterRwImpl(&val, iss | 1);
+    if (reg1 == reg2) {
+        frame->x[reg1] = val;
+    } else {
+        frame->x[reg1] = val & 0xFFFFFFFF;
+        frame->x[reg2] = val >> 32;
+    }
+
+    // Sysreg access are always 4 bit in length even for Aarch32
+    frame->elr_el2 += 4;
+}
+
+void doSystemRegisterWrite(ExceptionStackFrame *frame, u32 iss, u32 reg1, u32 reg2)
+{
+    // reg1 != reg2: mrrc/mcrr
+    u64 val = frame->x[reg1];
+
+    if (reg1 != reg2) {
+        val  = (val << 32) >> 32;
+        val |= frame->x[reg2] << 32;
+    }
+
+    doSystemRegisterRwImpl(&val, iss & ~1);
+
+    // Sysreg access are always 4 bit in length even for Aarch32
+    frame->elr_el2 += 4;
+}

thermosphere/src/sysreg_traps.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 doSystemRegisterRead(ExceptionStackFrame *frame, u32 iss, u32 reg1, u32 reg2);
+void doSystemRegisterWrite(ExceptionStackFrame *frame, u32 iss, u32 reg1, u32 reg2);
+

thermosphere/src/traps.c

@@ -1,5 +1,35 @@
+/*
+ * 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 "traps.h"
 #include "sysreg.h"
+#include "synchronization.h"
+
+static void enableDebugTraps(void)
+{
+    u64 mdcr = GET_SYSREG(mdcr_el2);
+
+    // Trap Debug Exceptions, and accesses to debug registers.
+    mdcr |= MDCR_EL2_TDE;
+
+    // Implied from TDE
+    mdcr |= MDCR_EL2_TDRA | MDCR_EL2_TDOSA | MDCR_EL2_TDA;
+
+    SET_SYSREG(mdcr_el2, mdcr);
+}
 
 void enableTraps(void)
 {
@@ -17,4 +47,6 @@ void enableTraps(void)
     // TODO debug exceptions
 
     SET_SYSREG(hcr_el2, hcr);
+
+    enableDebugTraps();
 }

thermosphere/src/traps.h

@@ -17,5 +17,6 @@
 #pragma once
 
 #include "utils.h"
+#include "exceptions.h"
 
 void enableTraps(void);