From 045f556f80471e75383eac7d6a76e5479ee2503e Mon Sep 17 00:00:00 2001 From: TuxSH <1922548+TuxSH@users.noreply.github.com> Date: Thu, 1 Aug 2019 02:58:16 +0200 Subject: [PATCH] thermosphere: enable EL2 stage1 translation (doesn't take much space) Identity map using 1GB L1 blocks --- thermosphere/Makefile | 4 +- thermosphere/src/arm.h | 4 + thermosphere/src/arm.s | 30 ++- thermosphere/src/mmu.h | 191 ++++++++++++++++++ .../src/platform/memory_map_mmu_cfg.c | 52 +++++ .../src/platform/memory_map_mmu_cfg.h | 29 +++ thermosphere/src/platform/qemu/memory_map.c | 46 +++++ thermosphere/src/platform/qemu/memory_map.h | 21 ++ thermosphere/src/platform/tegra/memory_map.c | 48 +++++ thermosphere/src/platform/tegra/memory_map.h | 21 ++ thermosphere/src/smc.c | 9 +- thermosphere/src/start.s | 6 +- thermosphere/src/sysreg_traps.c | 5 +- 13 files changed, 454 insertions(+), 12 deletions(-) create mode 100644 thermosphere/src/mmu.h create mode 100644 thermosphere/src/platform/memory_map_mmu_cfg.c create mode 100644 thermosphere/src/platform/memory_map_mmu_cfg.h create mode 100644 thermosphere/src/platform/qemu/memory_map.c create mode 100644 thermosphere/src/platform/qemu/memory_map.h create mode 100644 thermosphere/src/platform/tegra/memory_map.c create mode 100644 thermosphere/src/platform/tegra/memory_map.h 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); }