From 8533263c8512d059e546dd1d0032cfecd7b1a0cf Mon Sep 17 00:00:00 2001
From: Alexandru Gagniuc <mr.nuke.me@gmail.com>
Date: Thu, 15 Jul 2021 14:19:26 -0500
Subject: [PATCH] stm32mp1: spl: Configure TrustZone controller for OP-TEE

OP-TEE is very particular about how the TZC should be configured.
When booting an OP-TEE payload, an incorrect TZC configuration will
result in a panic.

Most information can be derived from the SPL devicetree. The only
information we don't have is the split between TZDRAM and shared
memory. This has to be hardcoded. The rest of the configuration is
fairly easy, and only requires 3 TZC regions. Configure them.

Signed-off-by: Alexandru Gagniuc <mr.nuke.me@gmail.com>
---
 arch/arm/mach-stm32mp/spl.c | 92 +++++++++++++++++++++++++++++++++++++
 1 file changed, 92 insertions(+)

diff --git a/arch/arm/mach-stm32mp/spl.c b/arch/arm/mach-stm32mp/spl.c
index b53659a698..405eff68a3 100644
--- a/arch/arm/mach-stm32mp/spl.c
+++ b/arch/arm/mach-stm32mp/spl.c
@@ -11,11 +11,13 @@
 #include <hang.h>
 #include <init.h>
 #include <log.h>
+#include <ram.h>
 #include <spl.h>
 #include <asm/cache.h>
 #include <asm/global_data.h>
 #include <asm/io.h>
 #include <asm/arch/sys_proto.h>
+#include <mach/tzc.h>
 #include <linux/libfdt.h>
 
 u32 spl_boot_device(void)
@@ -92,6 +94,96 @@ __weak int board_early_init_f(void)
 	return 0;
 }
 
+uint32_t stm32mp_get_dram_size(void)
+{
+	struct ram_info ram;
+	struct udevice *dev;
+	int ret;
+
+	if (uclass_get_device(UCLASS_RAM, 0, &dev))
+		return 0;
+
+	ret = ram_get_info(dev, &ram);
+	if (ret)
+		return 0;
+
+	return ram.size;
+}
+
+static int optee_get_reserved_memory(uint32_t *start, uint32_t *size)
+{
+	phys_size_t fdt_mem_size;
+	fdt_addr_t fdt_start;
+	ofnode node;
+
+	node = ofnode_path("/reserved-memory/optee");
+	if (!ofnode_valid(node))
+		return 0;
+
+	fdt_start = ofnode_get_addr_size(node, "reg", &fdt_mem_size);
+	*start = fdt_start;
+	*size = fdt_mem_size;
+	return (fdt_start < 0) ? fdt_start : 0;
+}
+
+#define CFG_SHMEM_SIZE			0x200000
+#define STM32_TZC_NSID_ALL		0xffff
+#define STM32_TZC_FILTER_ALL		3
+
+void stm32_init_tzc_for_optee(void)
+{
+	const uint32_t dram_size = stm32mp_get_dram_size();
+	const uintptr_t dram_top = STM32_DDR_BASE + (dram_size - 1);
+	uint32_t optee_base, optee_size, tee_shmem_base;
+	const uintptr_t tzc = STM32_TZC_BASE;
+	int ret;
+
+	if (dram_size == 0)
+		panic("Cannot determine DRAM size from devicetree\n");
+
+	ret = optee_get_reserved_memory(&optee_base, &optee_size);
+	if (ret < 0 || optee_size <= CFG_SHMEM_SIZE)
+		panic("Invalid OPTEE reserved memory in devicetree\n");
+
+	tee_shmem_base = optee_base + optee_size - CFG_SHMEM_SIZE;
+
+	const struct tzc_region optee_config[] = {
+		{
+			.base = STM32_DDR_BASE,
+			.top = optee_base - 1,
+			.sec_mode = TZC_ATTR_SEC_NONE,
+			.nsec_id = STM32_TZC_NSID_ALL,
+			.filters_mask = STM32_TZC_FILTER_ALL,
+		}, {
+			.base = optee_base,
+			.top = tee_shmem_base - 1,
+			.sec_mode = TZC_ATTR_SEC_RW,
+			.nsec_id = 0,
+			.filters_mask = STM32_TZC_FILTER_ALL,
+		}, {
+			.base = tee_shmem_base,
+			.top = dram_top,
+			.sec_mode = TZC_ATTR_SEC_NONE,
+			.nsec_id = STM32_TZC_NSID_ALL,
+			.filters_mask = STM32_TZC_FILTER_ALL,
+		}, {
+			.top = 0,
+		}
+	};
+
+	flush_dcache_all();
+
+	tzc_configure(tzc, optee_config);
+	tzc_dump_config(tzc);
+
+	dcache_disable();
+}
+
+void spl_board_prepare_for_optee(void *fdt)
+{
+	stm32_init_tzc_for_optee();
+}
+
 void board_init_f(ulong dummy)
 {
 	struct udevice *dev;
-- 
2.39.5