diff --git a/thermosphere/Makefile b/thermosphere/Makefile
index b7d655ca2..36898b260 100644
--- a/thermosphere/Makefile
+++ b/thermosphere/Makefile
@@ -42,7 +42,7 @@ endif
#---------------------------------------------------------------------------------
TARGET := $(notdir $(CURDIR))
BUILD := build
-SOURCES := src $(PLATFORM_SOURCES)
+SOURCES := src src/platform $(PLATFORM_SOURCES)
DATA := data
INCLUDES := include ../common/include
@@ -133,7 +133,7 @@ all: $(BUILD)
ifeq ($(PLATFORM), qemu)
QEMUFLAGS := -nographic -machine virt,secure=on,virtualization=on,gic-version=2 -cpu cortex-a57 -smp 4 -m 1024\
- -bios bl1.bin -d unimp -semihosting-config enable,target=native -serial mon:stdio
+ -bios bl1.bin -d unimp,int,mmu -semihosting-config enable,target=native -serial mon:stdio
# NOTE: copy bl1.bin, bl2.bin, bl31.bin from your own build of Arm Trusted Firmware!
diff --git a/thermosphere/src/arm.h b/thermosphere/src/arm.h
index 58e00bae3..8a6a569cd 100644
--- a/thermosphere/src/arm.h
+++ b/thermosphere/src/arm.h
@@ -1,5 +1,7 @@
#pragma once
+#include "types.h"
+
void flush_dcache_all(void);
void invalidate_dcache_all(void);
@@ -8,3 +10,5 @@ void invalidate_dcache_range(const void *start, const void *end);
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);
diff --git a/thermosphere/src/arm.s b/thermosphere/src/arm.s
index 9b8228b98..22bf1cdf6 100644
--- a/thermosphere/src/arm.s
+++ b/thermosphere/src/arm.s
@@ -206,9 +206,35 @@ invalidate_icache_all_inner_shareable:
.type invalidate_icache_all, %function
.global invalidate_icache_all
invalidate_icache_all:
- dsb ish
+ dsb sy
isb
ic iallu
- dsb ish
+ dsb sy
isb
ret
+
+.section .text.set_memory_registers_enable_mmu, "ax", %progbits
+.type set_memory_registers_enable_mmu, %function
+.global set_memory_registers_enable_mmu
+set_memory_registers_enable_mmu:
+ msr ttbr0_el2, x0
+ msr tcr_el2, x1
+ msr mair_el2, x2
+
+ dsb sy
+ isb
+ tlbi alle2
+ dsb sy
+ isb
+
+ // Enable MMU & enable caching
+ mrs x0, sctlr_el2
+ orr x0, x0, #1
+ orr x0, x0, #(1 << 2)
+ orr x0, x0, #(1 << 12)
+ msr sctlr_el2, x0
+
+ dsb sy
+ isb
+
+ ret
\ No newline at end of file
diff --git a/thermosphere/src/mmu.h b/thermosphere/src/mmu.h
new file mode 100644
index 000000000..583535edf
--- /dev/null
+++ b/thermosphere/src/mmu.h
@@ -0,0 +1,191 @@
+/*
+ * Copyright (c) 2018-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"
+
+
+#ifndef MMU_GRANULE_TYPE
+#define MMU_GRANULE_TYPE 0 /* 0: 4KB, 1: 64KB, 2: 16KB. The Switch always uses a 4KB granule size. */
+#endif
+
+#if MMU_GRANULE_TYPE == 0
+#define MMU_Lx_SHIFT(x) (12 + 9 * (3 - (x)))
+#define MMU_Lx_MASK(x) MASKL(9)
+#elif MMU_GRANULE_TYPE == 1
+/* 64 KB, no L0 here */
+#define MMU_Lx_SHIFT(x) (16 + 13 * (3 - (x)))
+#define MMU_Lx_MASK(x) ((x) == 1 ? MASKL(5) : MASKL(13))
+#elif MMU_GRANULE_TYPE == 2
+#define MMU_Lx_SHIFT(x) (14 + 11 * (3 - (x)))
+#define MMU_Lx_MASK(x) ((x) == 0 ? 1 : MASKL(11))
+#endif
+
+/*
+ * The following defines are adapted from uboot:
+ *
+ * (C) Copyright 2013
+ * David Feng
+ *
+ * SPDX-License-Identifier: GPL-2.0+
+ */
+
+/* Memory attributes, see set_memory_registers_enable_mmu */
+#define MMU_MT_NORMAL 0ull
+#define MMU_MT_DEVICE_NGNRE 1ull
+#define MMU_MT_DEVICE_NGNRNE 2ull /* not used, also the same as Attr4-7 */
+
+/*
+ * Hardware page table definitions.
+ *
+ */
+
+#define MMU_PTE_TYPE_MASK 3ull
+#define MMU_PTE_TYPE_FAULT 0ull
+#define MMU_PTE_TYPE_TABLE 3ull
+#define MMU_PTE_TYPE_BLOCK 1ull
+
+/* L3 only */
+#define MMU_PTE_TYPE_PAGE 3ull
+
+#define MMU_PTE_TABLE_PXN BITL(59)
+#define MMU_PTE_TABLE_XN BITL(60)
+#define MMU_PTE_TABLE_AP BITL(61)
+#define MMU_PTE_TABLE_NS BITL(63)
+
+/*
+ * Block
+ */
+#define MMU_PTE_BLOCK_MEMTYPE(x) ((uint64_t)((x) << 2))
+#define MMU_PTE_BLOCK_NS BITL(5)
+#define MMU_PTE_BLOCK_NON_SHAREABLE (0ull << 8)
+#define MMU_PTE_BLOCK_OUTER_SHAREABLE (2ull << 8)
+#define MMU_PTE_BLOCK_INNER_SHAREBLE (3ull << 8)
+#define MMU_PTE_BLOCK_AF BITL(10)
+#define MMU_PTE_BLOCK_NG BITL(11)
+#define MMU_PTE_BLOCK_PXN BITL(53)
+#define MMU_PTE_BLOCK_UXN BITL(54)
+#define MMU_PTE_BLOCK_XN MMU_PTE_BLOCK_UXN
+
+/*
+ * AP[2:1]
+ */
+#define MMU_AP_PRIV_RW (0ull << 6)
+#define MMU_AP_RW (1ull << 6)
+#define MMU_AP_PRIV_RO (2ull << 6)
+#define MMU_AP_RO (3ull << 6)
+
+/*
+ * S2AP[2:1] (for stage2 translations; secmon doesn't use it)
+ */
+#define MMU_S2AP_NONE (0ull << 6)
+#define MMU_S2AP_RO (1ull << 6)
+#define MMU_S2AP_WO (2ull << 6)
+#define MMU_S2AP_RW (3ull << 6)
+
+/*
+ * AttrIndx[2:0]
+ */
+#define MMU_PMD_ATTRINDX(t) ((uint64_t)((t) << 2))
+#define MMU_PMD_ATTRINDX_MASK (7ull << 2)
+
+/*
+ * TCR flags.
+ */
+#define TCR_T0SZ(x) ((64 - (x)) << 0)
+#define TCR_IRGN_NC (0 << 8)
+#define TCR_IRGN_WBWA (1 << 8)
+#define TCR_IRGN_WT (2 << 8)
+#define TCR_IRGN_WBNWA (3 << 8)
+#define TCR_IRGN_MASK (3 << 8)
+#define TCR_ORGN_NC (0 << 10)
+#define TCR_ORGN_WBWA (1 << 10)
+#define TCR_ORGN_WT (2 << 10)
+#define TCR_ORGN_WBNWA (3 << 10)
+#define TCR_ORGN_MASK (3 << 10)
+#define TCR_NOT_SHARED (0 << 12)
+#define TCR_SHARED_OUTER (2 << 12)
+#define TCR_SHARED_INNER (3 << 12)
+#define TCR_TG0_4K (0 << 14)
+#define TCR_TG0_64K (1 << 14)
+#define TCR_TG0_16K (2 << 14)
+#define TCR_PS(x) ((x) << 16)
+#define TCR_EPD1_DISABLE BIT(23)
+
+#define TCR_EL1_RSVD BIT(31)
+#define TCR_EL2_RSVD (BIT(31) | BIT(23))
+#define TCR_EL3_RSVD (BIT(31) | BIT(23))
+
+// We define those:
+#define ATTRIB_MEMTYPE_NORMAL MMU_PTE_BLOCK_MEMTYPE(MMU_MT_NORMAL)
+#define ATTRIB_MEMTYPE_DEVICE MMU_PTE_BLOCK_MEMTYPE(MMU_MT_DEVICE_NGNRE)
+
+static inline void mmu_init_table(uintptr_t *tbl, size_t num_entries) {
+ for(size_t i = 0; i < num_entries; i++) {
+ tbl[i] = MMU_PTE_TYPE_FAULT;
+ }
+}
+
+/*
+ All the functions below assume base_addr is valid.
+ They do not invalidate the TLB, which must be done separately.
+*/
+
+static inline unsigned int mmu_compute_index(unsigned int level, uintptr_t base_addr) {
+ return (base_addr >> MMU_Lx_SHIFT(level)) & MMU_Lx_MASK(level);
+}
+
+static inline void mmu_map_table(unsigned int level, uintptr_t *tbl, uintptr_t base_addr, uintptr_t *next_lvl_tbl_pa, uint64_t attrs) {
+ tbl[mmu_compute_index(level, base_addr)] = (uintptr_t)next_lvl_tbl_pa | attrs | MMU_PTE_TYPE_TABLE;
+}
+
+static inline void mmu_map_block(unsigned int level, uintptr_t *tbl, uintptr_t base_addr, uintptr_t phys_addr, uint64_t attrs) {
+ tbl[mmu_compute_index(level, base_addr)] = phys_addr | attrs | MMU_PTE_BLOCK_AF | MMU_PTE_TYPE_BLOCK;
+}
+
+static inline void mmu_map_page(uintptr_t *tbl, uintptr_t base_addr, uintptr_t phys_addr, uint64_t attrs) {
+ tbl[mmu_compute_index(3, base_addr)] = phys_addr | attrs | MMU_PTE_BLOCK_AF | MMU_PTE_TYPE_PAGE;
+}
+
+static inline void mmu_unmap(unsigned int level, uintptr_t *tbl, uintptr_t base_addr) {
+ tbl[mmu_compute_index(level, base_addr)] = MMU_PTE_TYPE_FAULT;
+}
+
+static inline void mmu_unmap_page(uintptr_t *tbl, uintptr_t base_addr) {
+ tbl[mmu_compute_index(3, base_addr)] = MMU_PTE_TYPE_FAULT;
+}
+
+static inline void mmu_map_block_range(unsigned int level, uintptr_t *tbl, uintptr_t base_addr, uintptr_t phys_addr, size_t size, uint64_t attrs) {
+ size = ((size + (BITL(MMU_Lx_SHIFT(level)) - 1)) >> MMU_Lx_SHIFT(level)) << MMU_Lx_SHIFT(level);
+ for(size_t offset = 0; offset < size; offset += BITL(MMU_Lx_SHIFT(level))) {
+ mmu_map_block(level, tbl, base_addr + offset, phys_addr + offset, attrs);
+ }
+}
+
+static inline void mmu_map_page_range(uintptr_t *tbl, uintptr_t base_addr, uintptr_t phys_addr, size_t size, uint64_t attrs) {
+ size = ((size + (BITL(MMU_Lx_SHIFT(3)) - 1)) >> MMU_Lx_SHIFT(3)) << MMU_Lx_SHIFT(3);
+ for(size_t offset = 0; offset < size; offset += BITL(MMU_Lx_SHIFT(3))) {
+ mmu_map_page(tbl, base_addr + offset, phys_addr + offset, attrs);
+ }
+}
+
+static inline void mmu_unmap_range(unsigned int level, uintptr_t *tbl, uintptr_t base_addr, size_t size) {
+ size = ((size + (BITL(MMU_Lx_SHIFT(level)) - 1)) >> MMU_Lx_SHIFT(level)) << MMU_Lx_SHIFT(level);
+ for(size_t offset = 0; offset < size; offset += BITL(MMU_Lx_SHIFT(level))) {
+ mmu_unmap(level, tbl, base_addr + offset);
+ }
+}
\ No newline at end of file
diff --git a/thermosphere/src/platform/memory_map_mmu_cfg.c b/thermosphere/src/platform/memory_map_mmu_cfg.c
new file mode 100644
index 000000000..21fe01789
--- /dev/null
+++ b/thermosphere/src/platform/memory_map_mmu_cfg.c
@@ -0,0 +1,52 @@
+/*
+ * 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 "../utils.h"
+#include "../sysreg.h"
+#include "../arm.h"
+#include "../mmu.h"
+#include "memory_map_mmu_cfg.h"
+
+void configureMemoryMapEnableMmu(void)
+{
+ u32 addrSpaceSize;
+ 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
+ - Outer cacheability attribute for memory associated with translation table walks using TTBR0_EL3: 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
+ - 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);
+
+
+ /*
+ - Attribute 0: Normal memory, Inner and Outer Write-Back Read-Allocate Write-Allocate Non-transient
+ - Attribute 1: Device-nGnRE memory
+ - Other attributes: Device-nGnRnE memory
+ */
+ u64 mair = 0x4FFull;
+
+ flush_dcache_all();
+ invalidate_icache_all();
+
+ set_memory_registers_enable_mmu(ttbr0, tcr, mair);
+}
\ No newline at end of file
diff --git a/thermosphere/src/platform/memory_map_mmu_cfg.h b/thermosphere/src/platform/memory_map_mmu_cfg.h
new file mode 100644
index 000000000..5e3f51b96
--- /dev/null
+++ b/thermosphere/src/platform/memory_map_mmu_cfg.h
@@ -0,0 +1,29 @@
+/*
+ * 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
+
+#ifdef PLATFORM_TEGRA
+
+#include "tegra/memory_map.h"
+
+#elif defined(PLATFORM_QEMU)
+
+#include "qemu/memory_map.h"
+
+#endif
+
+void configureMemoryMapEnableMmu(void);
diff --git a/thermosphere/src/platform/qemu/memory_map.c b/thermosphere/src/platform/qemu/memory_map.c
new file mode 100644
index 000000000..4207068e3
--- /dev/null
+++ b/thermosphere/src/platform/qemu/memory_map.c
@@ -0,0 +1,46 @@
+/*
+ * 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 "memory_map.h"
+#include "../../utils.h"
+#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)
+
+#define ADDRSPACESZ 32
+
+static ALIGN(0x1000) u64 g_ttbl[BIT(ADDRSPACESZ - 30)] = {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);
+}
+
+uintptr_t configureMemoryMap(u32 *addrSpaceSize)
+{
+ // QEMU virt RAM address space starts at 0x40000000
+ *addrSpaceSize = ADDRSPACESZ;
+
+ if (currentCoreCtx->isColdbootCore) {
+ identityMapL1(g_ttbl, 0x00000000ull, 1ull << 30, ATTRIB_MEMTYPE_DEVICE);
+ identityMapL1(g_ttbl, 0x40000000ull, 1ull << 30, ATTRIB_MEMTYPE_NORMAL);
+ identityMapL1(g_ttbl, 0x80000000ull, 1ull << 30, ATTRIB_MEMTYPE_NORMAL);
+ identityMapL1(g_ttbl, 0xC0000000ull, 1ull << 30, ATTRIB_MEMTYPE_NORMAL);
+ }
+
+ return (uintptr_t)g_ttbl;
+}
\ No newline at end of file
diff --git a/thermosphere/src/platform/qemu/memory_map.h b/thermosphere/src/platform/qemu/memory_map.h
new file mode 100644
index 000000000..331afbee9
--- /dev/null
+++ b/thermosphere/src/platform/qemu/memory_map.h
@@ -0,0 +1,21 @@
+/*
+ * 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 "../../types.h"
+
+uintptr_t configureMemoryMap(u32 *addrSpaceSize);
diff --git a/thermosphere/src/platform/tegra/memory_map.c b/thermosphere/src/platform/tegra/memory_map.c
new file mode 100644
index 000000000..9cf49737e
--- /dev/null
+++ b/thermosphere/src/platform/tegra/memory_map.c
@@ -0,0 +1,48 @@
+/*
+ * 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 "memory_map.h"
+#include "../../utils.h"
+#include "../../mmu.h"
+#include "../../core_ctx.h"
+
+// Limit ourselves to 34-bit addr space even if the tegra support up to 36 in theory
+// i.e. 14GB of dram max
+#define ADDRSPACESZ 34
+
+static ALIGN(0x1000) u64 g_ttbl[BIT(ADDRSPACESZ - 30)] = {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);
+}
+
+uintptr_t configureMemoryMap(u32 *addrSpaceSize)
+{
+ // QEMU virt RAM address space starts at 0x40000000
+ *addrSpaceSize = ADDRSPACESZ;
+
+ if (currentCoreCtx->isColdbootCore) {
+ identityMapL1(g_ttbl, 0x00000000ull, 1ull << 30, ATTRIB_MEMTYPE_DEVICE);
+ identityMapL1(g_ttbl, 0x40000000ull, 1ull << 30, ATTRIB_MEMTYPE_DEVICE);
+
+ for (u64 i = 2; i < 16; i++) {
+ identityMapL1(g_ttbl, i << 30, 1ull << 30, ATTRIB_MEMTYPE_NORMAL);
+ }
+ }
+
+ return (uintptr_t)g_ttbl;
+}
\ No newline at end of file
diff --git a/thermosphere/src/platform/tegra/memory_map.h b/thermosphere/src/platform/tegra/memory_map.h
new file mode 100644
index 000000000..331afbee9
--- /dev/null
+++ b/thermosphere/src/platform/tegra/memory_map.h
@@ -0,0 +1,21 @@
+/*
+ * 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 "../../types.h"
+
+uintptr_t configureMemoryMap(u32 *addrSpaceSize);
diff --git a/thermosphere/src/smc.c b/thermosphere/src/smc.c
index 682803218..21235cb16 100644
--- a/thermosphere/src/smc.c
+++ b/thermosphere/src/smc.c
@@ -2,6 +2,7 @@
#include "smc.h"
#include "synchronization.h"
#include "core_ctx.h"
+#include "arm.h"
// Currently in exception_vectors.s:
extern const u32 doSmcIndirectCallImpl[];
@@ -12,12 +13,12 @@ void start2(u64 contextId);
void doSmcIndirectCall(ExceptionStackFrame *frame, u32 smcId)
{
- u32 codebuf[doSmcIndirectCallImplSize]; // note: potential VLA
+ u32 codebuf[doSmcIndirectCallImplSize / 4]; // note: potential VLA
memcpy(codebuf, doSmcIndirectCallImpl, doSmcIndirectCallImplSize);
- codebuf[doSmcIndirectCallImplSmcInstructionOffset/4] |= smcId << 5;
+ codebuf[doSmcIndirectCallImplSmcInstructionOffset / 4] |= smcId << 5;
- __dsb_sy();
- __isb();
+ flush_dcache_range(codebuf, codebuf + doSmcIndirectCallImplSize/4);
+ invalidate_icache_all();
((void (*)(ExceptionStackFrame *))codebuf)(frame);
}
diff --git a/thermosphere/src/start.s b/thermosphere/src/start.s
index c50d24727..810e9acf8 100644
--- a/thermosphere/src/start.s
+++ b/thermosphere/src/start.s
@@ -82,7 +82,7 @@ _startCommon:
// Don't call init array to save space?
// Clear BSS & call main for the first core executing this code
- cbz x19, _jump_to_main
+ cbz x19, _enable_mmu
adrp x0, __bss_start__
add x0, x0, #:lo12:__bss_start__
mov w1, wzr
@@ -91,7 +91,9 @@ _startCommon:
sub x2, x2, x0
bl memset
-_jump_to_main:
+_enable_mmu:
+
+ bl configureMemoryMapEnableMmu
dsb sy
isb
diff --git a/thermosphere/src/sysreg_traps.c b/thermosphere/src/sysreg_traps.c
index ebd941110..61a339f65 100644
--- a/thermosphere/src/sysreg_traps.c
+++ b/thermosphere/src/sysreg_traps.c
@@ -17,6 +17,7 @@
#include "sysreg_traps.h"
#include "synchronization.h"
#include "sysreg.h"
+#include "arm.h"
static void doSystemRegisterRwImpl(u64 *val, u32 iss)
{
@@ -35,8 +36,8 @@ static void doSystemRegisterRwImpl(u64 *val, u32 iss)
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();
+ flush_dcache_range(codebuf, (u8 *)codebuf + sizeof(codebuf));
+ invalidate_icache_all();
*val = ((u64 (*)(u64))codebuf)(*val);
}