thermosphere: impl stage2 translation

thermosphere/Makefile

@@ -33,7 +33,7 @@ PLATFORM_DEFINES	:= -DPLATFORM_TEGRA -DPLATFORM_TEGRA_T210_ARM_TF
 
 else
 
-export PLATFORM	:= tegra
+export PLATFORM	:= tegra-t210-nintendo
 
 PLATFORM_SOURCES	:= src/platform/tegra
 PLATFORM_DEFINES	:= -DPLATFORM_TEGRA -D DPLATFORM_TEGRA_T210_NINTENDO

thermosphere/src/arm.h

@@ -12,3 +12,4 @@ void invalidate_icache_all_inner_shareable(void);
 void invalidate_icache_all(void);
 
 void set_memory_registers_enable_mmu(uintptr_t ttbr0, u64 tcr, u64 mair);
+void set_memory_registers_enable_stage2(uintptr_t vttbr, u64 vtcr);

thermosphere/src/arm.s

@@ -237,4 +237,28 @@ set_memory_registers_enable_mmu:
     dsb     sy
     isb
 
+    ret
+
+.section    .text.set_memory_registers_enable_stage2, "ax", %progbits
+.type       set_memory_registers_enable_stage2, %function
+.global     set_memory_registers_enable_stage2
+set_memory_registers_enable_stage2:
+    msr     vttbr_el2, x0
+    msr     vtcr_el2, x1
+
+    dsb     sy
+    isb
+    // Flushes all stage 1&2 entries, EL1
+    tlbi    alle1
+    dsb     sy
+    isb
+
+    // Enable stage2
+    mrs     x0, hcr_el2
+    orr     x0, x0, #1
+    msr     hcr_el2, x0
+
+    dsb     sy
+    isb
+
     ret

thermosphere/src/core_ctx.c

@@ -29,14 +29,14 @@ CoreCtx g_coreCtxs[4] = {
     { .coreId = 3 },
 };
 
-void coreCtxInit(u32 coreId, bool isColdbootCore, u64 argument)
+void coreCtxInit(u32 coreId, bool isBootCore, u64 argument)
 {
     size_t crashStackSize = (__crash_stacks_top__ - __stacks_top__) / 4;
     currentCoreCtx = &g_coreCtxs[coreId];
-    currentCoreCtx->isColdbootCore = isColdbootCore;
+    currentCoreCtx->isBootCore = isBootCore;
     currentCoreCtx->kernelArgument = argument;
     currentCoreCtx->crashStack = __crash_stacks_top__ - crashStackSize * coreId;
-    if (isColdbootCore && currentCoreCtx->kernelEntrypoint == 0) {
+    if (isBootCore && currentCoreCtx->kernelEntrypoint == 0) {
         currentCoreCtx->kernelEntrypoint = g_initialKernelEntrypoint;
     }
 }

thermosphere/src/core_ctx.h

@@ -23,10 +23,10 @@ typedef struct CoreCtx {
     u8 *crashStack;                 // @0x10
     u64 scratch;                    // @0x18
     u32 coreId;                     // @0x20
-    bool isColdbootCore;            // @0x24
+    bool isBootCore;            // @0x24
 } CoreCtx;
 
 extern CoreCtx g_coreCtxs[4];
 register CoreCtx *currentCoreCtx asm("x18");
 
-void coreCtxInit(u32 coreId, bool isColdbootCore, u64 argument);
+void coreCtxInit(u32 coreId, bool isBootCore, u64 argument);

thermosphere/src/main.c

@@ -4,6 +4,7 @@
 #include "platform/uart.h"
 #include "semihosting.h"
 #include "traps.h"
+#include "sysreg.h"
 
 extern const u8 __start__[];
 
@@ -34,7 +35,7 @@ int main(void)
 {
     enableTraps();
 
-    if (currentCoreCtx->isColdbootCore) {
+    if (currentCoreCtx->isBootCore) {
         uartInit(115200);
         DEBUG("EL2: core %u reached main first!\n", currentCoreCtx->coreId);
         if (currentCoreCtx->kernelEntrypoint == 0) {
@@ -49,6 +50,7 @@ int main(void)
     }
     else {
         DEBUG("EL2: core %u reached main!\n", currentCoreCtx->coreId);
+        DEBUG("Test 0x%08llx %016llx\n", get_physical_address_el1_stage12(0x08010000ull), GET_SYSREG(par_el1));
     }
 
     return 0;

thermosphere/src/mmu.h

@@ -90,7 +90,7 @@
 #define MMU_AP_RO                       (3ull << 6)
 
 /*
- * S2AP[2:1] (for stage2 translations; secmon doesn't use it)
+ * S2AP[1:0] (for stage2 translations; secmon doesn't use it)
  */
 #define MMU_S2AP_NONE                   (0ull << 6)
 #define MMU_S2AP_RO                     (1ull << 6)
@@ -103,10 +103,19 @@
 #define MMU_PMD_ATTRINDX(t)     ((uint64_t)((t) << 2))
 #define MMU_PMD_ATTRINDX_MASK   (7ull << 2)
 
+/*
+ * MemAttr[3:0] (stage2)
+ * 
+ */
+#define MMU_MEMATTR(x)                              ((x) << 2)
+#define MMU_MEMATTR_DEVICE_NGNRE                    MMU_MEMATTR(2)
+#define MMU_MEMATTR_NORMAL_CACHEABLE_OR_UNCHANGED   MMU_MEMATTR(0xF)
+
 /*
  * TCR flags.
  */
 #define TCR_T0SZ(x)         ((64 - (x)) << 0)
+#define VTCR_SL0(x)         ((x) << 6)
 #define TCR_IRGN_NC         (0 << 8)
 #define TCR_IRGN_WBWA       (1 << 8)
 #define TCR_IRGN_WT         (2 << 8)
@@ -128,6 +137,7 @@
 
 #define TCR_EL1_RSVD        BIT(31)
 #define TCR_EL2_RSVD        (BIT(31) | BIT(23))
+#define VTCR_EL2_RSVD       BIT(31)
 #define TCR_EL3_RSVD        (BIT(31) | BIT(23))
 
 // We define those:

thermosphere/src/platform/memory_map_mmu_cfg.c

@@ -18,6 +18,7 @@
 #include "../sysreg.h"
 #include "../arm.h"
 #include "../mmu.h"
+#include "../debug_log.h"
 #include "memory_map_mmu_cfg.h"
 
 void configureMemoryMapEnableMmu(void)
@@ -26,16 +27,15 @@ void configureMemoryMapEnableMmu(void)
     uintptr_t ttbr0 = configureMemoryMap(&addrSpaceSize);
 
     u32 ps = GET_SYSREG(id_aa64mmfr0_el1) & 0xF;
-
     /*
         - PA size: from ID_AA64MMFR0_EL1
         - Granule size: 4KB
-        - Shareability attribute for memory associated with translation table walks using TTBR0_EL3: Inner Shareable
+        - Shareability attribute for memory associated with translation table walks using TTBR0_EL2: Inner Shareable
-        - Outer cacheability attribute for memory associated with translation table walks using TTBR0_EL3: Normal memory, Outer Write-Back Read-Allocate Write-Allocate Cacheable
+        - Outer cacheability attribute for memory associated with translation table walks using TTBR0_EL2: Normal memory, Outer Write-Back Read-Allocate Write-Allocate Cacheable
-        - Inner cacheability attribute for memory associated with translation table walks using TTBR0_EL3: Normal memory, Inner Write-Back Read-Allocate Write-Allocate Cacheable
+        - Inner cacheability attribute for memory associated with translation table walks using TTBR0_EL2: Normal memory, Inner Write-Back Read-Allocate Write-Allocate Cacheable
         - T0SZ = from configureMemoryMap
     */
-    u64 tcr = TCR_EL2_RSVD | TCR_PS(ps) | TCR_TG0_4K | TCR_SHARED_INNER | TCR_ORGN_WBWA | TCR_IRGN_WBWA | TCR_T0SZ(64 - addrSpaceSize);
+    u64 tcr = TCR_EL2_RSVD | TCR_PS(ps) | TCR_TG0_4K | TCR_SHARED_INNER | TCR_ORGN_WBWA | TCR_IRGN_WBWA | TCR_T0SZ(addrSpaceSize);
 
 
     /*
@@ -49,4 +49,26 @@ void configureMemoryMapEnableMmu(void)
     invalidate_icache_all();
 
     set_memory_registers_enable_mmu(ttbr0, tcr, mair);
-}
+}
+
+void configureMemoryMapEnableStage2(void)
+{
+    u32 addrSpaceSize;
+    uintptr_t vttbr = configureStage2MemoryMap(&addrSpaceSize);
+
+    u32 ps = GET_SYSREG(id_aa64mmfr0_el1) & 0xF;
+    /*
+        - PA size: from ID_AA64MMFR0_EL1
+        - Granule size: 4KB
+        - Shareability attribute for memory associated with translation table walks using VTTBR_EL2: Inner Shareable
+        - Outer cacheability attribute for memory associated with translation table walks using VTTBR_EL2: Normal memory, Outer Write-Back Read-Allocate Write-Allocate Cacheable
+        - Inner cacheability attribute for memory associated with translation table walks using VTTBR_EL2: Normal memory, Inner Write-Back Read-Allocate Write-Allocate Cacheable
+        - SL0 = start at level 1
+        - T0SZ = from configureMemoryMap
+    */
+    u64 vtcr = VTCR_EL2_RSVD | TCR_PS(ps) | TCR_TG0_4K | TCR_SHARED_INNER | TCR_ORGN_WBWA | TCR_IRGN_WBWA | VTCR_SL0(1) | TCR_T0SZ(addrSpaceSize);
+    flush_dcache_all();
+    invalidate_icache_all();
+
+    set_memory_registers_enable_stage2(vttbr, vtcr);
+}

thermosphere/src/platform/qemu/memory_map.c

@@ -19,28 +19,64 @@
 #include "../../mmu.h"
 #include "../../core_ctx.h"
 
-// Older QEMU have a 4GB RAM limit, let's just assume a 12GB RAM limit/32-bit addr space (even though PASZ corresponds to 1TB)
+// QEMU presently advertises 44-bit PAs we'll only use 39 of them to avoid level 0 tables.
-
+#define ADDRSPACESZ    39
-#define ADDRSPACESZ    32
+#define ADDRSPACESZ2   ADDRSPACESZ
 
 static ALIGN(0x1000) u64 g_ttbl[BIT(ADDRSPACESZ - 30)] = {0};
 
+static ALIGN(0x1000) u64 g_vttbl[BIT(ADDRSPACESZ2 - 30)] = {0};
+static TEMPORARY ALIGN(0x1000) u64 g_vttbl_l2_mmio_0_0[512] = {0};
+static TEMPORARY ALIGN(0x1000) u64 g_vttbl_l3_0[512] = {0};
+
 static inline void identityMapL1(u64 *tbl, uintptr_t addr, size_t size, u64 attribs)
 {
     mmu_map_block_range(1, tbl, addr, addr, size, attribs | MMU_PTE_BLOCK_INNER_SHAREBLE);
 }
 
+static inline void identityMapL2(u64 *tbl, uintptr_t addr, size_t size, u64 attribs)
+{
+    mmu_map_block_range(2, tbl, addr, addr, size, attribs | MMU_PTE_BLOCK_INNER_SHAREBLE);
+}
+
+static inline void identityMapL3(u64 *tbl, uintptr_t addr, size_t size, u64 attribs)
+{
+    mmu_map_block_range(3, tbl, addr, addr, size, attribs | MMU_PTE_BLOCK_INNER_SHAREBLE);
+}
+
 uintptr_t configureMemoryMap(u32 *addrSpaceSize)
 {
     // QEMU virt RAM address space starts at 0x40000000
     *addrSpaceSize = ADDRSPACESZ;
-
+    static bool initialized = false;
-    if (currentCoreCtx->isColdbootCore) {
+    if (currentCoreCtx->isBootCore && !initialized) {
-        identityMapL1(g_ttbl, 0x00000000ull, 1ull << 30, ATTRIB_MEMTYPE_DEVICE);
+        identityMapL1(g_ttbl, 0x00000000ull, BITL(30), ATTRIB_MEMTYPE_DEVICE);
-        identityMapL1(g_ttbl, 0x40000000ull, 1ull << 30, ATTRIB_MEMTYPE_NORMAL);
+        identityMapL1(g_ttbl, 0x40000000ull, (BITL(ADDRSPACESZ - 30) - 1ull) << 30, ATTRIB_MEMTYPE_NORMAL);
-        identityMapL1(g_ttbl, 0x80000000ull, 1ull << 30, ATTRIB_MEMTYPE_NORMAL);
+        initialized = true;
-        identityMapL1(g_ttbl, 0xC0000000ull, 1ull << 30, ATTRIB_MEMTYPE_NORMAL);
     }
 
     return (uintptr_t)g_ttbl;
+}
+
+uintptr_t configureStage2MemoryMap(u32 *addrSpaceSize)
+{
+    *addrSpaceSize = ADDRSPACESZ2;
+    static const u64 devattrs = MMU_S2AP_RW | MMU_MEMATTR_DEVICE_NGNRE;
+    static const u64 unchanged = MMU_S2AP_RW | MMU_MEMATTR_NORMAL_CACHEABLE_OR_UNCHANGED;
+
+    if (currentCoreCtx->isBootCore) {
+        identityMapL1(g_vttbl, 0, 4ull << 30, unchanged);
+        identityMapL1(g_vttbl, 0x40000000ull, (BITL(ADDRSPACESZ2 - 30) - 1ull) << 30, unchanged);
+        mmu_map_table(1, g_vttbl, 0x00000000ull, g_vttbl_l2_mmio_0_0, 0);
+
+        identityMapL2(g_vttbl_l2_mmio_0_0, 0x08000000ull, BITL(30), unchanged);
+        mmu_map_table(2, g_vttbl_l2_mmio_0_0, 0x08000000ull, g_vttbl_l3_0, 0);
+
+        identityMapL3(g_vttbl_l3_0, 0x08000000ull, BITL(21), unchanged);
+
+        // GICv2 CPU -> vCPU interface
+        mmu_map_page_range(g_vttbl_l3_0, 0x08010000ull, 0x08040000ull, 0x10000ull, devattrs);
+    }
+
+    return (uintptr_t)g_vttbl;
 }

thermosphere/src/platform/qemu/memory_map.h

@@ -18,15 +18,5 @@
 
 #include "../../types.h"
 
-static inline u64 transformKernelAddress(u64 pa)
-{
-    switch (pa) {
-        // GICv2 CPU -> vCPU interface
-        case 0x08010000:
-            return 0x08040000;
-        default:
-            return pa;
-    }
-}
-
 uintptr_t configureMemoryMap(u32 *addrSpaceSize);
+uintptr_t configureStage2MemoryMap(u32 *addrSpaceSize);

thermosphere/src/platform/tegra/memory_map.c

@@ -19,30 +19,63 @@
 #include "../../mmu.h"
 #include "../../core_ctx.h"
 
-// Limit ourselves to 34-bit addr space even if the tegra support up to 36 in theory
+// Tegra PA size is 36-bit... should we limit ourselves to 34?
 // i.e. 14GB of dram max
-#define ADDRSPACESZ    34
+#define ADDRSPACESZ    36
+#define ADDRSPACESZ2   ADDRSPACESZ
 
 static ALIGN(0x1000) u64 g_ttbl[BIT(ADDRSPACESZ - 30)] = {0};
 
+static ALIGN(0x1000) u64 g_vttbl[BIT(ADDRSPACESZ2 - 30)] = {0};
+static TEMPORARY ALIGN(0x1000) u64 g_vttbl_l2_mmio_0[512] = {0};
+static TEMPORARY ALIGN(0x1000) u64 g_vttbl_l3_0[512] = {0};
+
 static inline void identityMapL1(u64 *tbl, uintptr_t addr, size_t size, u64 attribs)
 {
     mmu_map_block_range(1, tbl, addr, addr, size, attribs | MMU_PTE_BLOCK_INNER_SHAREBLE);
 }
 
+static inline void identityMapL2(u64 *tbl, uintptr_t addr, size_t size, u64 attribs)
+{
+    mmu_map_block_range(2, tbl, addr, addr, size, attribs | MMU_PTE_BLOCK_INNER_SHAREBLE);
+}
+
+static inline void identityMapL3(u64 *tbl, uintptr_t addr, size_t size, u64 attribs)
+{
+    mmu_map_block_range(3, tbl, addr, addr, size, attribs | MMU_PTE_BLOCK_INNER_SHAREBLE);
+}
+
 uintptr_t configureMemoryMap(u32 *addrSpaceSize)
 {
-    // QEMU virt RAM address space starts at 0x40000000
     *addrSpaceSize = ADDRSPACESZ;
-
+    static bool initialized = false;
-    if (currentCoreCtx->isColdbootCore) {
+    if (currentCoreCtx->isBootCore && !initialized) {
-        identityMapL1(g_ttbl, 0x00000000ull, 1ull << 30, ATTRIB_MEMTYPE_DEVICE);
+        identityMapL1(g_ttbl, 0x00000000ull, 2 * BITL(30), ATTRIB_MEMTYPE_DEVICE);
-        identityMapL1(g_ttbl, 0x40000000ull, 1ull << 30, ATTRIB_MEMTYPE_DEVICE);
+        identityMapL1(g_ttbl, 0x80000000ull, (BITL(ADDRSPACESZ - 30) - 2ull) << 30, ATTRIB_MEMTYPE_NORMAL);
-
+        initialized = true;
-        for (u64 i = 2; i < 16; i++) {
-            identityMapL1(g_ttbl, i << 30, 1ull << 30, ATTRIB_MEMTYPE_NORMAL);
-        }
     }
 
     return (uintptr_t)g_ttbl;
+}
+
+uintptr_t configureStage2MemoryMap(u32 *addrSpaceSize)
+{
+    *addrSpaceSize = ADDRSPACESZ2;
+    static const u64 devattrs = MMU_S2AP_RW | MMU_MEMATTR_DEVICE_NGNRE;
+    static const u64 unchanged = MMU_S2AP_RW | MMU_MEMATTR_NORMAL_CACHEABLE_OR_UNCHANGED;
+    if (currentCoreCtx->isBootCore) {
+        identityMapL1(g_vttbl, 0x00000000ull, BITL(30), unchanged);
+        identityMapL1(g_vttbl, 0x80000000ull, (BITL(ADDRSPACESZ2 - 30) - 2ull) << 30, unchanged);
+        mmu_map_table(1, g_vttbl, 0x40000000ull, g_vttbl_l2_mmio_0, 0);
+
+        identityMapL2(g_vttbl_l2_mmio_0, 0x40000000ull, BITL(30), unchanged);
+        mmu_map_table(2, g_vttbl_l2_mmio_0, 0x50000000ull, g_vttbl_l3_0, 0);
+
+        identityMapL3(g_vttbl_l3_0, 0x00000000ull, BITL(21), unchanged);
+
+        // GICv2 CPU -> vCPU interface
+        mmu_map_page_range(g_vttbl_l3_0, 0x50042000ull, 0x50046000ull, 0x2000ull, devattrs);
+    }
+
+    return (uintptr_t)g_vttbl;
 }

thermosphere/src/platform/tegra/memory_map.h

@@ -18,15 +18,6 @@
 
 #include "../../types.h"
 
-static inline u64 transformKernelAddress(u64 pa)
-{
-    switch (pa) {
-        // GICv2 CPU -> vCPU interface
-        case 0x50042000:
-            return 0x50046000;
-        default:
-            return pa;
-    }
-}
-
 uintptr_t configureMemoryMap(u32 *addrSpaceSize);
+uintptr_t configureStage2MemoryMap(u32 *addrSpaceSize);
+

thermosphere/src/shadow_page_tables.c

@@ -1,138 +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 "shadow_page_tables.h"
-#include "platform/memory_map_mmu_cfg.h"
-#include "debug_log.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);
-                    u64 newPa = transformKernelAddress(pa);
-                    if (pa != newPa) {
-                        // Note: unreachable on QEMU yet
-                        DEBUG("EL1 in-place page-table entry swap: 0x%08llx => 0x%08llx\n", pa, newPa);
-                        ttbl[i] = (ttbl[i] & ~MASK2L(47, 12)) | newPa;
-                    }
-                }
-
-                break;
-            }
-
-            default:
-                __builtin_unreachable();
-                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);
-    }
-}
-
-void replaceKernelPageTables(void)
-{
-
-}

thermosphere/src/shadow_page_tables.h

@@ -1,22 +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/>.
- */
-
-
-#pragma once
-
-#include "utils.h"
-
-void replacePageTableLong(u64 *ttbl, u32 txsz);

thermosphere/src/start.s

@@ -93,7 +93,10 @@ _startCommon:
 
 _enable_mmu:
 
+    // Enable EL2 address translation and caches
     bl      configureMemoryMapEnableMmu
+    // Enable EL1 Stage2 intermediate physical address translation
+    bl      configureMemoryMapEnableStage2
 
     dsb     sy
     isb

thermosphere/src/sysreg_traps.c

@@ -19,7 +19,6 @@
 #include "sysreg.h"
 #include "arm.h"
 #include "debug_log.h"
-#include "shadow_page_tables.h"
 
 static void doSystemRegisterRwImpl(u64 *val, u32 iss)
 {
@@ -91,42 +90,6 @@ void doSystemRegisterWrite(ExceptionStackFrame *frame, u32 iss, u32 reg1, u32 re
 
     // Hooks go here:
     switch (iss) {
-        case ENCODE_SYSREG_ISS(SCTLR_EL1): {
-            DEBUG("Hooked sysreg write: SCTLR_EL1 = %0x016llx\n", val);
-            // Possible MMU (re)-enablement
-            if (val & 1) {
-                u64 tcr = GET_SYSREG(tcr_el1);
-                if (((tcr >> 14) & 3) == 0) {
-                    // 4KB granule
-                    replacePageTableLong((u64 *)(GET_SYSREG(ttbr0_el1) & MASK2L(47, 1)), (u32)(tcr & 0x3F));
-                    replacePageTableLong((u64 *)(GET_SYSREG(ttbr1_el1) & MASK2L(47, 1)), (u32)((tcr >> 16) & 0x3F));
-                }
-            }
-
-            break;
-        }
-        case ENCODE_SYSREG_ISS(TTBR1_EL1): {
-            DEBUG("Hooked sysreg write: TTBR1_EL1 = %0x016llx\n", val);
-            u64 tcr = GET_SYSREG(tcr_el1);
-            // MMU enabled & 4KB granule
-            if ((GET_SYSREG(sctlr_el1) & 1) && ((tcr >> 14) & 3) == 0) {
-                // Note: lack of ttbr0 intentional here
-                replacePageTableLong((u64 *)(val & MASK2L(47, 1)), (u32)((tcr >> 16) & 0x3F));
-            }
-            break;
-        }
-        case ENCODE_SYSREG_ISS(TCR_EL1): {
-            DEBUG("Hooked sysreg write: TCR_EL1 = %0x016llx\n", val);
-            u64 tcr = val;
-            // MMU enabled & 4KB granule
-            if ((GET_SYSREG(sctlr_el1) & 1) && ((tcr >> 14) & 3) == 0) {
-                // Note: lack of ttbr0 intentional here
-                replacePageTableLong((u64 *)(GET_SYSREG(ttbr1_el1) & MASK2L(47, 1)), (u32)((tcr >> 16) & 0x3F));
-            }
-            break;
-        }
-        // Note: TTBR0_EL1 deliberately not hooked
-
         default:
             break;
     }

thermosphere/src/traps.c

@@ -35,10 +35,6 @@ void enableTraps(void)
 {
     u64 hcr = GET_SYSREG(hcr_el2);
 
-    // Trap *writes* to memory control registers
-    // Note: QEMU doesn't support this bit as of August 2019...
-    hcr |= HCR_TVM;
-
     // Trap SMC instructions
     hcr |= HCR_TSC;
 

thermosphere/src/utils.h

@@ -33,21 +33,16 @@
 
 #define ALINLINE        __attribute__((always_inline))
 
+#define TEMPORARY       __attribute__((section(".temp")))
+
 bool overlaps(u64 as, u64 ae, u64 bs, u64 be);
 
-
+static inline uintptr_t get_physical_address_el1_stage12(const uintptr_t el1_vaddr) {
-static inline uintptr_t get_physical_address(const void *vaddr) {
+    // NOTE: interrupt must be disabled when calling this func
     uintptr_t PAR;
-    __asm__ __volatile__ ("at s1e3r, %0" :: "r"(vaddr));
+    __asm__ __volatile__ ("at s12e1r, %0" :: "r"(el1_vaddr));
     __asm__ __volatile__ ("mrs %0, par_el1" : "=r"(PAR));
-    return (PAR & 1) ? 0ull : (PAR & MASK2L(40, 12)) | ((uintptr_t)vaddr & MASKL(12));
+    return (PAR & 1) ? 0ull : (PAR & MASK2L(40, 12)) | ((uintptr_t)el1_vaddr & MASKL(12));
-}
-
-static inline uintptr_t get_physical_address_el0(const uintptr_t el0_vaddr) {
-    uintptr_t PAR;
-    __asm__ __volatile__ ("at s1e0r, %0" :: "r"(el0_vaddr));
-    __asm__ __volatile__ ("mrs %0, par_el1" : "=r"(PAR));
-    return (PAR & 1) ? 0ull : (PAR & MASK2L(40, 12)) | ((uintptr_t)el0_vaddr & MASKL(12));
 }
 
 static inline u32 read32le(const volatile void *dword, size_t offset) {