thermosphere: enable EL2 stage1 translation (doesn't take much space)

Identity map using 1GB L1 blocks

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!
 

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);

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

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 <http://www.gnu.org/licenses/>.
+ */
+
+#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 <fenghua@phytium.com.cn>
+ *
+ * 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);
+    }
+}

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 <http://www.gnu.org/licenses/>.
+ */
+
+#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);
+}

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 <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#ifdef PLATFORM_TEGRA
+
+#include "tegra/memory_map.h"
+
+#elif defined(PLATFORM_QEMU)
+
+#include "qemu/memory_map.h"
+
+#endif
+
+void configureMemoryMapEnableMmu(void);

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 <http://www.gnu.org/licenses/>.
+ */
+
+#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;
+}

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 <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "../../types.h"
+
+uintptr_t configureMemoryMap(u32 *addrSpaceSize);

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 <http://www.gnu.org/licenses/>.
+ */
+
+#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;
+}

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 <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "../../types.h"
+
+uintptr_t configureMemoryMap(u32 *addrSpaceSize);

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);
 }

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

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);
 }