diff --git a/thermosphere/Makefile b/thermosphere/Makefile
index be55d5a50..568599615 100644
--- a/thermosphere/Makefile
+++ b/thermosphere/Makefile
@@ -55,6 +55,7 @@ DEFINES := -D__CCPLEX__ -DATMOSPHERE_GIT_BRANCH=\"$(AMSBRANCH)\" -DATMOSPHERE_GI
CFLAGS := \
-g \
-Os \
+ -ffixed-x18 \
-ffunction-sections \
-fdata-sections \
-fomit-frame-pointer \
diff --git a/thermosphere/src/exceptions.c b/thermosphere/src/exceptions.c
index 10d271335..2ed26798f 100644
--- a/thermosphere/src/exceptions.c
+++ b/thermosphere/src/exceptions.c
@@ -63,6 +63,7 @@ void dumpStackFrame(const ExceptionStackFrame *frame, bool sameEl)
#endif
}
+#ifdef A32_SUPPORTED
static void advanceItState(ExceptionStackFrame *frame)
{
if (!spsrIsThumb(frame->spsr_el2) || spsrGetT32ItFlags(frame->spsr_el2) == 0) {
@@ -74,10 +75,13 @@ static void advanceItState(ExceptionStackFrame *frame)
// Last instruction of the block => wipe, otherwise advance
spsrSetT32ItFlags(&frame->spsr_el2, (it & 7) == 0 ? 0 : (it & 0xE0) | ((it << 1) & 0x1F));
}
+#endif
void skipFaultingInstruction(ExceptionStackFrame *frame, u32 size)
{
+#ifdef A32_SUPPORTED
advanceItState(frame);
+#endif
frame->elr_el2 += size;
}
@@ -85,6 +89,8 @@ void handleLowerElSyncException(ExceptionStackFrame *frame, ExceptionSyndromeReg
{
switch (esr.ec) {
+
+#ifdef A32_SUPPORTED
case Exception_CP14RTTrap:
case Exception_CP15RTTrap:
handleMcrMrcTrap(frame, esr);
@@ -96,11 +102,14 @@ void handleLowerElSyncException(ExceptionStackFrame *frame, ExceptionSyndromeReg
case Exception_CP15RRTTrap:
handleMcrrMrrcTrap(frame, esr);
break;
+ case Exception_HypervisorCallA32:
+ handleHypercall(frame, esr);
+ break;
+#endif
case Exception_SystemRegisterTrap:
handleMsrMrsTrap(frame, esr);
break;
case Exception_HypervisorCallA64:
- case Exception_HypervisorCallA32:
handleHypercall(frame, esr);
break;
default:
diff --git a/thermosphere/src/shadow_page_tables.c b/thermosphere/src/shadow_page_tables.c
new file mode 100644
index 000000000..963a54726
--- /dev/null
+++ b/thermosphere/src/shadow_page_tables.c
@@ -0,0 +1,128 @@
+/*
+ * 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 "shadow_page_tables.h"
+
+#ifdef A32_SUPPORTED
+static void replacePageTableShortL2(u32 *ttbl)
+{
+ u32 inc;
+ for (u32 i = 0; i < BIT(8); i += inc) {
+ u32 type = ttbl[i] & 3;
+ switch (type) {
+ case 0:
+ // Fault
+ inc = 1;
+ break;
+ case 1:
+ // Large page
+ // Nothing to replace at this granularity level
+ inc = 16;
+ break;
+ case 2:
+ case 3:
+ // Section or supersection
+ // Nothing to replace at this granularity level yet
+ // TODO
+ inc = 1;
+ break;
+ }
+ }
+}
+
+void replacePageTableShort(u32 *ttbl, u32 n)
+{
+ //u32 mask = MASK2(31 - n, 20);
+ u32 inc;
+ for (u32 i = 0; i < BIT(12 - n); i += inc) {
+ u32 type = ttbl[i] & 3;
+ switch (type) {
+ case 0:
+ // Fault
+ inc = 1;
+ break;
+ case 1:
+ // L2 tbl
+ replacePageTableShortL2((u32 *)(uintptr_t)(ttbl[i] & ~MASK(10)));
+ inc = 1;
+ break;
+ case 2:
+ case 3:
+ // Section or supersection
+ // Nothing to replace at this granularity level yet
+ inc = (ttbl[i] & BIT(18)) ? 16 : 0;
+ break;
+ }
+ }
+}
+#endif
+
+static void replacePageTableLongImpl(u64 *ttbl, u32 level, u32 nbits)
+{
+ for (u32 i = 0; i < BIT(nbits); i++) {
+ u64 type = ttbl[i] & 3;
+ switch (type) {
+ case 0:
+ case 2:
+ // Fault
+ break;
+ case 1:
+ // Block (L1 or L2) or invalid (L0 or L3)
+ // Nothing to do at this granularity level anyway.
+ break;
+ case 3: {
+ // Lower-level table or page
+ if (level < 3) {
+ uintptr_t addr = ttbl[i] & MASK2L(47, 12);
+ replacePageTableLongImpl((u64 *)addr, level + 1, 9);
+ } else {
+ u64 pa = ttbl[i] & MASK2L(47, 12);
+ // FIXME
+ if (pa == 0x50042000ull) {
+ ttbl[i] = (ttbl[i] & ~MASK2L(47, 12)) | 0x50046000ull;
+ }
+ }
+
+ break;
+ }
+
+ default:
+ break;
+ }
+ }
+}
+
+void replacePageTableLong(u64 *ttbl, u32 txsz)
+{
+ u32 startBit = 63 - txsz;
+
+ // Initial level 3 for 4KB granule: "c. Only available if ARMv8.4-TTST is implemented, while the PE is executing in AArch64 state." (Arm Arm).
+ // This means there is a maximum value for TxSz...
+
+ if (startBit >= 48) {
+ // Invalid
+ return;
+ } else if (startBit >= 39) {
+ replacePageTableLongImpl(ttbl, 0, startBit - 38);
+ } else if (startBit >= 30) {
+ replacePageTableLongImpl(ttbl, 1, startBit - 29);
+ } else if (startBit >= 21) {
+ replacePageTableLongImpl(ttbl, 2, startBit - 20);
+ } else if (startBit >= 12) {
+ replacePageTableLongImpl(ttbl, 3, startBit - 11);
+ }
+}
\ No newline at end of file
diff --git a/thermosphere/src/shadow_page_tables.h b/thermosphere/src/shadow_page_tables.h
new file mode 100644
index 000000000..f4c0def78
--- /dev/null
+++ b/thermosphere/src/shadow_page_tables.h
@@ -0,0 +1,22 @@
+/*
+ * 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 "utils.h"
+
+void replacePageTableLong(u64 *ttbl, u32 txsz);
diff --git a/thermosphere/src/sysreg_traps.c b/thermosphere/src/sysreg_traps.c
index 387513a98..ebd941110 100644
--- a/thermosphere/src/sysreg_traps.c
+++ b/thermosphere/src/sysreg_traps.c
@@ -18,6 +18,79 @@
#include "synchronization.h"
#include "sysreg.h"
+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;
+
+ iss &= ~((0x1F << 5) | 1);
+
+ doSystemRegisterRwImpl(&val, iss | 1);
+ if (reg1 == reg2) {
+ frame->x[reg1] = val;
+ } else {
+ if (reg1 != -1) {
+ frame->x[reg1] = val & 0xFFFFFFFF;
+ }
+ if (reg2 != -1) {
+ frame->x[reg2] = val >> 32;
+ }
+ }
+
+ skipFaultingInstruction(frame, 4);
+}
+
+void doSystemRegisterWrite(ExceptionStackFrame *frame, u32 iss, u32 reg1, u32 reg2)
+{
+ // reg1 != reg2: mrrc/mcrr
+ u64 val = 0;
+ iss &= ~((0x1F << 5) | 1);
+
+ if (reg1 == -1 || reg2 == -1) {
+ doSystemRegisterRwImpl(&val, iss | 1);
+ if (reg1 == -1) {
+ val = (frame->x[reg2] << 32) | (val & 0xFFFFFFFF);
+ } else {
+ val = ((val >> 32) << 32) | (frame->x[reg1] & 0xFFFFFFFF);
+ }
+ }
+
+ else {
+ if (reg1 != reg2) {
+ val |= (frame->x[reg2] << 32) | (frame->x[reg1] & 0xFFFFFFFF);
+ } else {
+ val = frame->x[reg1];
+ }
+ }
+ doSystemRegisterRwImpl(&val, iss);
+ skipFaultingInstruction(frame, 4);
+}
+
+#ifdef A32_SUPPORTED
+
// For a32 mcr/mrc => a64 mrs
static u32 convertMcrMrcIss(u32 *outCondition, bool *outCondValid, u32 *outShift, u32 a32Iss, u32 coproc, u32 el)
{
@@ -140,89 +213,6 @@ static bool evaluateMcrMrcCondition(u64 spsr, u32 condition, bool condValid)
}
}
-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;
-
- iss &= ~((0x1F << 5) | 1);
-
- doSystemRegisterRwImpl(&val, iss | 1);
- if (reg1 == reg2) {
- frame->x[reg1] = val;
- } else {
- if (reg1 != -1) {
- frame->x[reg1] = val & 0xFFFFFFFF;
- }
- if (reg2 != -1) {
- frame->x[reg2] = val >> 32;
- }
- }
-
- skipFaultingInstruction(frame, 4);
-}
-
-void doSystemRegisterWrite(ExceptionStackFrame *frame, u32 iss, u32 reg1, u32 reg2)
-{
- // reg1 != reg2: mrrc/mcrr
- u64 val = 0;
- iss &= ~((0x1F << 5) | 1);
-
- if (reg1 == -1 || reg2 == -1) {
- doSystemRegisterRwImpl(&val, iss | 1);
- if (reg1 == -1) {
- val = (frame->x[reg2] << 32) | (val & 0xFFFFFFFF);
- } else {
- val = ((val >> 32) << 32) | (frame->x[reg1] & 0xFFFFFFFF);
- }
- }
-
- else {
- if (reg1 != reg2) {
- val |= (frame->x[reg2] << 32) | (frame->x[reg1] & 0xFFFFFFFF);
- } else {
- val = frame->x[reg1];
- }
- }
- doSystemRegisterRwImpl(&val, iss);
- skipFaultingInstruction(frame, 4);
-}
-
-void handleMsrMrsTrap(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr)
-{
- u32 iss = esr.iss;
- u32 reg = (iss >> 5) & 31;
- bool isRead = (iss & 1) != 0;
-
- if (isRead) {
- doSystemRegisterRead(frame, iss, reg, reg);
- } else {
- doSystemRegisterWrite(frame, iss, reg, reg);
- }
-}
void handleMcrMrcTrap(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr)
{
@@ -347,3 +337,18 @@ void handleLdcStcTrap(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr)
// Do not execute the read/writes
skipFaultingInstruction(frame, esr.il == 0 ? 2 : 4);
}
+
+#endif
+
+void handleMsrMrsTrap(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr)
+{
+ u32 iss = esr.iss;
+ u32 reg = (iss >> 5) & 31;
+ bool isRead = (iss & 1) != 0;
+
+ if (isRead) {
+ doSystemRegisterRead(frame, iss, reg, reg);
+ } else {
+ doSystemRegisterWrite(frame, iss, reg, reg);
+ }
+}
\ No newline at end of file
diff --git a/thermosphere/src/traps.c b/thermosphere/src/traps.c
index 91f3102c6..33acbd687 100644
--- a/thermosphere/src/traps.c
+++ b/thermosphere/src/traps.c
@@ -42,10 +42,11 @@ void enableTraps(void)
// Trap SMC instructions
hcr |= HCR_TSC;
- // Reroute physical IRQ to EL2
+ // Reroute physical IRQs to EL2
hcr |= HCR_IMO;
- // TODO debug exceptions
+ // Make sure HVC is enabled
+ hcr &= ~HCR_HCD;
SET_SYSREG(hcr_el2, hcr);