From ffa216c8c709cfc8853d0328e460b53396d46152 Mon Sep 17 00:00:00 2001
From: TuxSH <1922548+TuxSH@users.noreply.github.com>
Date: Thu, 25 Jul 2019 01:29:17 +0200
Subject: [PATCH] thermosphere: add some basic sysreg trapping code
---
thermosphere/src/synchronization.h | 27 ++++++
thermosphere/src/sysreg.h | 13 +++
thermosphere/src/sysreg_traps.c | 138 +++++++++++++++++++++++++++++
thermosphere/src/sysreg_traps.h | 23 +++++
thermosphere/src/traps.c | 34 ++++++-
thermosphere/src/traps.h | 3 +-
6 files changed, 236 insertions(+), 2 deletions(-)
create mode 100644 thermosphere/src/synchronization.h
create mode 100644 thermosphere/src/sysreg_traps.c
create mode 100644 thermosphere/src/sysreg_traps.h
diff --git a/thermosphere/src/synchronization.h b/thermosphere/src/synchronization.h
new file mode 100644
index 000000000..bb3a0efdf
--- /dev/null
+++ b/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 .
+ */
+
+#pragma once
+
+static inline void __dsb_sy(void)
+{
+ __asm__ __volatile__ ("dsb sy" ::: "memory");
+}
+
+static inline void __isb(void)
+{
+ __asm__ __volatile__ ("isb" ::: "memory");
+}
diff --git a/thermosphere/src/sysreg.h b/thermosphere/src/sysreg.h
index fde662ead..0e75a08fa 100644
--- a/thermosphere/src/sysreg.h
+++ b/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
diff --git a/thermosphere/src/sysreg_traps.c b/thermosphere/src/sysreg_traps.c
new file mode 100644
index 000000000..07180cab7
--- /dev/null
+++ b/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 .
+ */
+
+#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;
+}
diff --git a/thermosphere/src/sysreg_traps.h b/thermosphere/src/sysreg_traps.h
new file mode 100644
index 000000000..9ff6cbc6e
--- /dev/null
+++ b/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 .
+ */
+
+#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);
+
diff --git a/thermosphere/src/traps.c b/thermosphere/src/traps.c
index 12372aa65..d18d86993 100644
--- a/thermosphere/src/traps.c
+++ b/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 .
+ */
+
#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);
-}
\ No newline at end of file
+
+ enableDebugTraps();
+}
diff --git a/thermosphere/src/traps.h b/thermosphere/src/traps.h
index c40609536..cf4a50af3 100644
--- a/thermosphere/src/traps.h
+++ b/thermosphere/src/traps.h
@@ -17,5 +17,6 @@
#pragma once
#include "utils.h"
+#include "exceptions.h"
-void enableTraps(void);
\ No newline at end of file
+void enableTraps(void);