stm32mp1: add OP-TEE support

Support booting OP-TEE as BL32 boot stage and secure runtime
service.

OP-TEE executes in internal RAM and uses a secure DDR area to store
the pager pagestore. Memory mapping and TZC are configured accordingly
prior OP-TEE boot. OP-TEE image is expected in OP-TEE v2 format where
a header file describes the effective boot images. This change
post processes header file content to get OP-TEE load addresses
and set OP-TEE boot arguments.

Change-Id: I02ef8b915e4be3e95b27029357d799d70e01cd44
Signed-off-by: Yann Gautier <yann.gautier@st.com>
Signed-off-by: Etienne Carriere <etienne.carriere@st.com>

8 files changed, 254 insertions, 2 deletions

diff --git a/plat/st/common/bl2_io_storage.c b/plat/st/common/bl2_io_storage.c
index 1aa3df76..38b2a0bd 100644
--- a/plat/st/common/bl2_io_storage.c
+++ b/plat/st/common/bl2_io_storage.c
@@ -54,10 +54,27 @@ static const io_block_dev_spec_t mmc_block_dev_spec = {
 static uintptr_t storage_dev_handle;
 static const io_dev_connector_t *mmc_dev_con;
 
+#ifdef AARCH32_SP_OPTEE
+static const struct stm32image_part_info optee_header_partition_spec = {
+	.name = OPTEE_HEADER_IMAGE_NAME,
+	.binary_type = OPTEE_HEADER_BINARY_TYPE,
+};
+
+static const struct stm32image_part_info optee_pager_partition_spec = {
+	.name = OPTEE_PAGER_IMAGE_NAME,
+	.binary_type = OPTEE_PAGER_BINARY_TYPE,
+};
+
+static const struct stm32image_part_info optee_paged_partition_spec = {
+	.name = OPTEE_PAGED_IMAGE_NAME,
+	.binary_type = OPTEE_PAGED_BINARY_TYPE,
+};
+#else
 static const io_block_spec_t bl32_block_spec = {
 	.offset = BL32_BASE,
 	.length = STM32MP_BL32_SIZE
 };
+#endif
 
 static const io_block_spec_t bl2_block_spec = {
 	.offset = BL2_BASE,
@@ -71,6 +88,11 @@ static const struct stm32image_part_info bl33_partition_spec = {
 
 enum {
 	IMG_IDX_BL33,
+#ifdef AARCH32_SP_OPTEE
+	IMG_IDX_OPTEE_HEADER,
+	IMG_IDX_OPTEE_PAGER,
+	IMG_IDX_OPTEE_PAGED,
+#endif
 	IMG_IDX_NUM
 };
 
@@ -80,6 +102,20 @@ static struct stm32image_device_info stm32image_dev_info_spec = {
 		.name = BL33_IMAGE_NAME,
 		.binary_type = BL33_BINARY_TYPE,
 	},
+#ifdef AARCH32_SP_OPTEE
+	.part_info[IMG_IDX_OPTEE_HEADER] = {
+		.name = OPTEE_HEADER_IMAGE_NAME,
+		.binary_type = OPTEE_HEADER_BINARY_TYPE,
+	},
+	.part_info[IMG_IDX_OPTEE_PAGER] = {
+		.name = OPTEE_PAGER_IMAGE_NAME,
+		.binary_type = OPTEE_PAGER_BINARY_TYPE,
+	},
+	.part_info[IMG_IDX_OPTEE_PAGED] = {
+		.name = OPTEE_PAGED_IMAGE_NAME,
+		.binary_type = OPTEE_PAGED_BINARY_TYPE,
+	},
+#endif
 };
 
 static io_block_spec_t stm32image_block_spec = {
@@ -105,11 +141,29 @@ static const struct plat_io_policy policies[] = {
 		.image_spec = (uintptr_t)&bl2_block_spec,
 		.check = open_dummy
 	},
+#ifdef AARCH32_SP_OPTEE
+	[BL32_IMAGE_ID] = {
+		.dev_handle = &image_dev_handle,
+		.image_spec = (uintptr_t)&optee_header_partition_spec,
+		.check = open_image
+	},
+	[BL32_EXTRA1_IMAGE_ID] = {
+		.dev_handle = &image_dev_handle,
+		.image_spec = (uintptr_t)&optee_pager_partition_spec,
+		.check = open_image
+	},
+	[BL32_EXTRA2_IMAGE_ID] = {
+		.dev_handle = &image_dev_handle,
+		.image_spec = (uintptr_t)&optee_paged_partition_spec,
+		.check = open_image
+	},
+#else
 	[BL32_IMAGE_ID] = {
 		.dev_handle = &dummy_dev_handle,
 		.image_spec = (uintptr_t)&bl32_block_spec,
 		.check = open_dummy
 	},
+#endif
 	[BL33_IMAGE_ID] = {
 		.dev_handle = &image_dev_handle,
 		.image_spec = (uintptr_t)&bl33_partition_spec,
diff --git a/plat/st/stm32mp1/bl2_plat_setup.c b/plat/st/stm32mp1/bl2_plat_setup.c
index 2477954b..b54486e8 100644
--- a/plat/st/stm32mp1/bl2_plat_setup.c
+++ b/plat/st/stm32mp1/bl2_plat_setup.c
@@ -22,6 +22,7 @@
 #include <drivers/st/stm32mp1_pwr.h>
 #include <drivers/st/stm32mp1_ram.h>
 #include <lib/mmio.h>
+#include <lib/optee_utils.h>
 #include <lib/xlat_tables/xlat_tables_v2.h>
 #include <plat/common/platform.h>
 
@@ -136,7 +137,13 @@ void bl2_platform_setup(void)
 		panic();
 	}
 
+#ifdef AARCH32_SP_OPTEE
+	INFO("BL2 runs OP-TEE setup\n");
+	/* Initialize tzc400 after DDR initialization */
+	stm32mp1_security_setup();
+#else
 	INFO("BL2 runs SP_MIN setup\n");
+#endif
 }
 
 void bl2_el3_plat_arch_setup(void)
@@ -154,11 +161,25 @@ void bl2_el3_plat_arch_setup(void)
 			BL_CODE_END - BL_CODE_BASE,
 			MT_CODE | MT_SECURE);
 
+#ifdef AARCH32_SP_OPTEE
+	/* OP-TEE image needs post load processing: keep RAM read/write */
+	mmap_add_region(STM32MP_DDR_BASE + dt_get_ddr_size() -
+			STM32MP_DDR_S_SIZE - STM32MP_DDR_SHMEM_SIZE,
+			STM32MP_DDR_BASE + dt_get_ddr_size() -
+			STM32MP_DDR_S_SIZE - STM32MP_DDR_SHMEM_SIZE,
+			STM32MP_DDR_S_SIZE,
+			MT_MEMORY | MT_RW | MT_SECURE);
+
+	mmap_add_region(STM32MP_OPTEE_BASE, STM32MP_OPTEE_BASE,
+			STM32MP_OPTEE_SIZE,
+			MT_MEMORY | MT_RW | MT_SECURE);
+#else
 	/* Prevent corruption of preloaded BL32 */
 	mmap_add_region(BL32_BASE, BL32_BASE,
 			BL32_LIMIT - BL32_BASE,
 			MT_MEMORY | MT_RO | MT_SECURE);
 
+#endif
 	/* Map non secure DDR for BL33 load and DDR training area restore */
 	mmap_add_region(STM32MP_DDR_BASE,
 			STM32MP_DDR_BASE,
@@ -261,3 +282,69 @@ skip_console_init:
 
 	stm32mp_io_setup();
 }
+
+#if defined(AARCH32_SP_OPTEE)
+/*******************************************************************************
+ * This function can be used by the platforms to update/use image
+ * information for given `image_id`.
+ ******************************************************************************/
+int bl2_plat_handle_post_image_load(unsigned int image_id)
+{
+	int err = 0;
+	bl_mem_params_node_t *bl_mem_params = get_bl_mem_params_node(image_id);
+	bl_mem_params_node_t *bl32_mem_params;
+	bl_mem_params_node_t *pager_mem_params;
+	bl_mem_params_node_t *paged_mem_params;
+
+	assert(bl_mem_params != NULL);
+
+	switch (image_id) {
+	case BL32_IMAGE_ID:
+		bl_mem_params->ep_info.pc =
+					bl_mem_params->image_info.image_base;
+
+		pager_mem_params = get_bl_mem_params_node(BL32_EXTRA1_IMAGE_ID);
+		assert(pager_mem_params != NULL);
+		pager_mem_params->image_info.image_base = STM32MP_OPTEE_BASE;
+		pager_mem_params->image_info.image_max_size =
+			STM32MP_OPTEE_SIZE;
+
+		paged_mem_params = get_bl_mem_params_node(BL32_EXTRA2_IMAGE_ID);
+		assert(paged_mem_params != NULL);
+		paged_mem_params->image_info.image_base = STM32MP_DDR_BASE +
+			(dt_get_ddr_size() - STM32MP_DDR_S_SIZE -
+			 STM32MP_DDR_SHMEM_SIZE);
+		paged_mem_params->image_info.image_max_size =
+			STM32MP_DDR_S_SIZE;
+
+		err = parse_optee_header(&bl_mem_params->ep_info,
+					 &pager_mem_params->image_info,
+					 &paged_mem_params->image_info);
+		if (err) {
+			ERROR("OPTEE header parse error.\n");
+			panic();
+		}
+
+		/* Set optee boot info from parsed header data */
+		bl_mem_params->ep_info.pc =
+				pager_mem_params->image_info.image_base;
+		bl_mem_params->ep_info.args.arg0 =
+				paged_mem_params->image_info.image_base;
+		bl_mem_params->ep_info.args.arg1 = 0; /* Unused */
+		bl_mem_params->ep_info.args.arg2 = 0; /* No DT supported */
+		break;
+
+	case BL33_IMAGE_ID:
+		bl32_mem_params = get_bl_mem_params_node(BL32_IMAGE_ID);
+		assert(bl32_mem_params != NULL);
+		bl32_mem_params->ep_info.lr_svc = bl_mem_params->ep_info.pc;
+		break;
+
+	default:
+		/* Do nothing in default case */
+		break;
+	}
+
+	return err;
+}
+#endif
diff --git a/plat/st/stm32mp1/include/platform_def.h b/plat/st/stm32mp1/include/platform_def.h
index 5019b1a3..263e6d6e 100644
--- a/plat/st/stm32mp1/include/platform_def.h
+++ b/plat/st/stm32mp1/include/platform_def.h
@@ -25,6 +25,15 @@
 #define PLATFORM_STACK_SIZE		0xC00
 #endif
 
+#ifdef AARCH32_SP_OPTEE
+#define OPTEE_HEADER_IMAGE_NAME		"teeh"
+#define OPTEE_PAGED_IMAGE_NAME		"teed"
+#define OPTEE_PAGER_IMAGE_NAME		"teex"
+#define OPTEE_HEADER_BINARY_TYPE	U(0x20)
+#define OPTEE_PAGER_BINARY_TYPE		U(0x21)
+#define OPTEE_PAGED_BINARY_TYPE		U(0x22)
+#endif
+
 /* SSBL = second stage boot loader */
 #define BL33_IMAGE_NAME			"ssbl"
 #define BL33_BINARY_TYPE		U(0x0)
@@ -57,9 +66,11 @@
 /*******************************************************************************
  * BL32 specific defines.
  ******************************************************************************/
+#ifndef AARCH32_SP_OPTEE
 #define BL32_BASE			STM32MP_BL32_BASE
 #define BL32_LIMIT			(STM32MP_BL32_BASE + \
 					 STM32MP_BL32_SIZE)
+#endif
 
 /*******************************************************************************
  * BL33 specific defines.
diff --git a/plat/st/stm32mp1/plat_bl2_mem_params_desc.c b/plat/st/stm32mp1/plat_bl2_mem_params_desc.c
index 0da93e4f..1d407bb7 100644
--- a/plat/st/stm32mp1/plat_bl2_mem_params_desc.c
+++ b/plat/st/stm32mp1/plat_bl2_mem_params_desc.c
@@ -27,7 +27,9 @@ static bl_mem_params_node_t bl2_mem_params_descs[] = {
 				      VERSION_2, entry_point_info_t,
 				      SECURE | EXECUTABLE | EP_FIRST_EXE),
 
+#if !defined(AARCH32_SP_OPTEE)
 		.ep_info.pc = BL32_BASE,
+#endif
 		.ep_info.spsr = SPSR_MODE32(MODE32_svc, SPSR_T_ARM,
 					    SPSR_E_LITTLE,
 					    DISABLE_ALL_EXCEPTIONS),
@@ -35,13 +37,48 @@ static bl_mem_params_node_t bl2_mem_params_descs[] = {
 		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
 				      VERSION_2, image_info_t,
 				      IMAGE_ATTRIB_PLAT_SETUP),
-
+#if defined(AARCH32_SP_OPTEE)
+		/* optee header is loaded in SYSRAM above BL2 */
+		.image_info.image_base = STM32MP_OPTEE_BASE,
+		.image_info.image_max_size = STM32MP_OPTEE_SIZE,
+#else
 		.image_info.image_base = BL32_BASE,
 		.image_info.image_max_size = BL32_LIMIT - BL32_BASE,
-
+#endif
 		.next_handoff_image_id = BL33_IMAGE_ID,
 	},
 
+#if defined(AARCH32_SP_OPTEE)
+	/* Fill BL32 external 1 image related information */
+	{
+		.image_id = BL32_EXTRA1_IMAGE_ID,
+
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+				      VERSION_2, entry_point_info_t,
+				      SECURE | NON_EXECUTABLE),
+
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+				      VERSION_2, image_info_t,
+				      IMAGE_ATTRIB_SKIP_LOADING),
+
+		.next_handoff_image_id = INVALID_IMAGE_ID,
+	},
+	/* Fill BL32 external 2 image related information */
+	{
+		.image_id = BL32_EXTRA2_IMAGE_ID,
+
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+				      VERSION_2, entry_point_info_t,
+				      SECURE | NON_EXECUTABLE),
+
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+				      VERSION_2, image_info_t,
+				      IMAGE_ATTRIB_SKIP_LOADING),
+
+		.next_handoff_image_id = INVALID_IMAGE_ID,
+	},
+#endif /* AARCH32_SP_OPTEE */
+
 	/* Fill BL33 related information */
 	{
 		.image_id = BL33_IMAGE_ID,
diff --git a/plat/st/stm32mp1/platform.mk b/plat/st/stm32mp1/platform.mk
index 3e346018..4796e91f 100644
--- a/plat/st/stm32mp1/platform.mk
+++ b/plat/st/stm32mp1/platform.mk
@@ -18,7 +18,11 @@ WORKAROUND_CVE_2017_5715:=	0
 # Number of TF-A copies in the device
 STM32_TF_A_COPIES		:=	2
 $(eval $(call add_define,STM32_TF_A_COPIES))
+ifeq ($(AARCH32_SP),optee)
+PLAT_PARTITION_MAX_ENTRIES	:=	$(shell echo $$(($(STM32_TF_A_COPIES) + 4)))
+else
 PLAT_PARTITION_MAX_ENTRIES	:=	$(shell echo $$(($(STM32_TF_A_COPIES) + 1)))
+endif
 $(eval $(call add_define,PLAT_PARTITION_MAX_ENTRIES))
 
 PLAT_INCLUDES		:=	-Iplat/st/common/include/
@@ -84,6 +88,10 @@ BL2_SOURCES		+=	common/desc_image_load.c				\
 				plat/st/stm32mp1/plat_bl2_mem_params_desc.c		\
 				plat/st/stm32mp1/plat_image_load.c
 
+ifeq ($(AARCH32_SP),optee)
+BL2_SOURCES		+=	lib/optee/optee_utils.c
+endif
+
 # Macros and rules to build TF binary
 STM32_TF_ELF_LDFLAGS	:=	--hash-style=gnu --as-needed
 STM32_DT_BASENAME	:=	$(DTB_FILE_NAME:.dtb=)
diff --git a/plat/st/stm32mp1/stm32mp1.ld.S b/plat/st/stm32mp1/stm32mp1.ld.S
index c041fb67..b347badd 100644
--- a/plat/st/stm32mp1/stm32mp1.ld.S
+++ b/plat/st/stm32mp1/stm32mp1.ld.S
@@ -58,6 +58,7 @@ SECTIONS
         *(.bl2_image*)
         __BL2_IMAGE_END__ = .;
 
+#ifndef AARCH32_SP_OPTEE
         /*
          * bl32 will be settled by bl2.
          * The strongest and only alignment constraint is 8 words to simplify
@@ -67,6 +68,7 @@ SECTIONS
         __BL32_IMAGE_START__ = .;
         *(.bl32_image*)
         __BL32_IMAGE_END__ = .;
+#endif
 
         __DATA_END__ = .;
     } >RAM
diff --git a/plat/st/stm32mp1/stm32mp1_def.h b/plat/st/stm32mp1/stm32mp1_def.h
index f6ebeee1..cff7ddbd 100644
--- a/plat/st/stm32mp1/stm32mp1_def.h
+++ b/plat/st/stm32mp1/stm32mp1_def.h
@@ -34,6 +34,10 @@
 /* DDR configuration */
 #define STM32MP_DDR_BASE		U(0xC0000000)
 #define STM32MP_DDR_MAX_SIZE		U(0x40000000)	/* Max 1GB */
+#ifdef AARCH32_SP_OPTEE
+#define STM32MP_DDR_S_SIZE		U(0x01E00000)	/* 30 MB */
+#define STM32MP_DDR_SHMEM_SIZE		U(0x00200000)	/* 2 MB */
+#endif
 
 /* DDR power initializations */
 #ifndef __ASSEMBLY__
@@ -56,21 +60,38 @@ enum ddr_type {
 					 (STM32MP_PARAM_LOAD_SIZE +	\
 					  STM32MP_HEADER_SIZE))
 
+#ifdef AARCH32_SP_OPTEE
+#define STM32MP_BL32_SIZE		U(0)
+
+#define STM32MP_OPTEE_BASE		STM32MP_SYSRAM_BASE
+
+#define STM32MP_OPTEE_SIZE		(STM32MP_DTB_BASE -  \
+					 STM32MP_OPTEE_BASE)
+#else
 #if STACK_PROTECTOR_ENABLED
 #define STM32MP_BL32_SIZE		U(0x00012000)	/* 72 Ko for BL32 */
 #else
 #define STM32MP_BL32_SIZE		U(0x00011000)	/* 68 Ko for BL32 */
 #endif
+#endif
 
 #define STM32MP_BL32_BASE		(STM32MP_SYSRAM_BASE + \
 					 STM32MP_SYSRAM_SIZE - \
 					 STM32MP_BL32_SIZE)
 
+#ifdef AARCH32_SP_OPTEE
+#if STACK_PROTECTOR_ENABLED
+#define STM32MP_BL2_SIZE		U(0x00019000)	/* 100 Ko for BL2 */
+#else
+#define STM32MP_BL2_SIZE		U(0x00017000)	/* 92 Ko for BL2 */
+#endif
+#else
 #if STACK_PROTECTOR_ENABLED
 #define STM32MP_BL2_SIZE		U(0x00015000)	/* 84 Ko for BL2 */
 #else
 #define STM32MP_BL2_SIZE		U(0x00013000)	/* 76 Ko for BL2 */
 #endif
+#endif
 
 #define STM32MP_BL2_BASE		(STM32MP_BL32_BASE - \
 					 STM32MP_BL2_SIZE)
diff --git a/plat/st/stm32mp1/stm32mp1_security.c b/plat/st/stm32mp1/stm32mp1_security.c
index 1cd56c60..61db2e7c 100644
--- a/plat/st/stm32mp1/stm32mp1_security.c
+++ b/plat/st/stm32mp1/stm32mp1_security.c
@@ -42,6 +42,37 @@ static void init_tzc400(void)
 
 	tzc400_disable_filters();
 
+#ifdef AARCH32_SP_OPTEE
+	/*
+	 * Region 1 set to cover all non-secure DRAM at 0xC000_0000. Apply the
+	 * same configuration to all filters in the TZC.
+	 */
+	region_base = ddr_base;
+	region_top = ddr_top - STM32MP_DDR_S_SIZE - STM32MP_DDR_SHMEM_SIZE;
+	tzc400_configure_region(STM32MP1_FILTER_BIT_ALL, 1,
+				region_base,
+				region_top,
+				TZC_REGION_S_NONE,
+				TZC_REGION_NSEC_ALL_ACCESS_RDWR);
+
+	/* Region 2 set to cover all secure DRAM. */
+	region_base = region_top + 1U;
+	region_top = ddr_top - STM32MP_DDR_SHMEM_SIZE;
+	tzc400_configure_region(STM32MP1_FILTER_BIT_ALL, 2,
+				region_base,
+				region_top,
+				TZC_REGION_S_RDWR,
+				0);
+
+	/* Region 3 set to cover non-secure shared memory DRAM. */
+	region_base = region_top + 1U;
+	region_top = ddr_top;
+	tzc400_configure_region(STM32MP1_FILTER_BIT_ALL, 3,
+				region_base,
+				region_top,
+				TZC_REGION_S_NONE,
+				TZC_REGION_NSEC_ALL_ACCESS_RDWR);
+#else
 	/*
 	 * Region 1 set to cover all DRAM at 0xC000_0000. Apply the
 	 * same configuration to all filters in the TZC.
@@ -53,6 +84,7 @@ static void init_tzc400(void)
 				region_top,
 				TZC_REGION_S_NONE,
 				TZC_REGION_NSEC_ALL_ACCESS_RDWR);
+#endif
 
 	/* Raise an exception if a NS device tries to access secure memory */
 	tzc400_set_action(TZC_ACTION_ERR);