[PATCH v2 12/18] ARM64: mmu: add dynamic optee memory mapping support

[Marco Felsch <m.felsch@pengutronix.de>]

Use the dynamic optee memory base address for the early mapping if
possible and fallback to the static mapping if the query failed.

Signed-off-by: Marco Felsch <m.felsch@pengutronix.de>
---
 arch/arm/cpu/mmu_64.c      | 14 ++++++++++++--
 arch/arm/mach-imx/esdctl.c |  4 ++++
 common/Makefile            |  2 +-
 3 files changed, 17 insertions(+), 3 deletions(-)

diff --git a/arch/arm/cpu/mmu_64.c b/arch/arm/cpu/mmu_64.c
index 84f45bc2c3c1..27fd15ea1233 100644
--- a/arch/arm/cpu/mmu_64.c
+++ b/arch/arm/cpu/mmu_64.c
@@ -20,6 +20,7 @@
 #include <memory.h>
 #include <asm/system_info.h>
 #include <linux/pagemap.h>
+#include <tee/optee.h>
 
 #include "mmu_64.h"
 
@@ -310,6 +311,7 @@ static void init_range(size_t total_level0_tables)
 void mmu_early_enable(unsigned long membase, unsigned long memsize)
 {
 	int el;
+	u64 optee_membase;
 	unsigned long ttb = arm_mem_ttb(membase + memsize);
 
 	pr_debug("enabling MMU, ttb @ 0x%08lx\n", ttb);
@@ -326,8 +328,16 @@ void mmu_early_enable(unsigned long membase, unsigned long memsize)
 	 * Set 1:1 mapping of VA->PA. So to cover the full 1TB range we need 2 tables.
 	 */
 	init_range(2);
-	early_remap_range(membase, memsize - OPTEE_SIZE, MAP_CACHED);
-	early_remap_range(membase + memsize - OPTEE_SIZE, OPTEE_SIZE, MAP_FAULT);
+
+	if (optee_get_membase(&optee_membase)) {
+		/* Fallback and place OP-TEE at the memory end region */
+		early_remap_range(membase, memsize - OPTEE_SIZE, MAP_CACHED);
+		early_remap_range(membase + memsize - OPTEE_SIZE, OPTEE_SIZE, MAP_FAULT);
+	} else {
+		early_remap_range(membase, memsize, MAP_CACHED);
+		early_remap_range(optee_membase, OPTEE_SIZE, MAP_FAULT);
+	}
+
 	early_remap_range(PAGE_ALIGN_DOWN((uintptr_t)_stext), PAGE_ALIGN(_etext - _stext), MAP_CACHED);
 
 	mmu_enable();
diff --git a/arch/arm/mach-imx/esdctl.c b/arch/arm/mach-imx/esdctl.c
index ac35a2de66f1..47c43d75ec75 100644
--- a/arch/arm/mach-imx/esdctl.c
+++ b/arch/arm/mach-imx/esdctl.c
@@ -29,6 +29,8 @@
 #include <mach/imx/imx8m-regs.h>
 #include <mach/imx/imx7-regs.h>
 #include <mach/imx/imx9-regs.h>
+#include <mach/imx/scratch.h>
+#include <tee/optee.h>
 
 struct imx_esdctl_data {
 	unsigned long base0;
@@ -1004,6 +1006,8 @@ resource_size_t imx8m_barebox_earlymem_size(unsigned buswidth)
 
 static void __noreturn imx8m_barebox_entry(void *boarddata, unsigned buswidth)
 {
+	imx8m_init_scratch_space(buswidth, false);
+	optee_set_membase(imx8m_scratch_get_optee_hdr());
 	barebox_arm_entry(MX8M_DDR_CSD1_BASE_ADDR,
 			  imx8m_barebox_earlymem_size(buswidth), boarddata);
 }
diff --git a/common/Makefile b/common/Makefile
index 7fb864f61480..c31cbab9e48f 100644
--- a/common/Makefile
+++ b/common/Makefile
@@ -80,7 +80,7 @@ obj-$(CONFIG_BAREBOX_UPDATE_IMX_NAND_FCB) += imx-bbu-nand-fcb.o
 obj-$(CONFIG_BOOT)		+= boot.o
 obj-$(CONFIG_SERIAL_DEV_BUS)	+= serdev.o
 obj-$(CONFIG_USBGADGET_START)	+= usbgadget.o
-pbl-$(CONFIG_PBL_OPTEE)		+= optee.o
+obj-pbl-$(CONFIG_PBL_OPTEE)	+= optee.o
 obj-$(CONFIG_BOOTM_OPTEE)	+= optee.o
 obj-$(CONFIG_FASTBOOT_BASE)	+= fastboot.o
 
-- 
2.39.2