qemu: Add OP-TEE extra image parsing support

OP-TEE may have extra images to be loaded. Load them one by one and do
the parsing. In this patch, ARM TF need to load up to 3 images for
OP-TEE: header, pager and pages images. Header image is the info about
optee os and images. Pager image include pager code and data.  Paged
image include the paging parts using virtual memory.

Signed-off-by: Jens Wiklander <jens.wiklander@linaro.org>

5 files changed, 111 insertions, 0 deletions

diff --git a/plat/qemu/include/platform_def.h b/plat/qemu/include/platform_def.h
index 85374e8d..e91a7db9 100644
--- a/plat/qemu/include/platform_def.h
+++ b/plat/qemu/include/platform_def.h
@@ -73,6 +73,11 @@
 #define SEC_DRAM_BASE			0x0e100000
 #define SEC_DRAM_SIZE			0x00f00000
 
+/* Load pageable part of OP-TEE at end of secure DRAM */
+#define QEMU_OPTEE_PAGEABLE_LOAD_BASE	(SEC_DRAM_BASE + SEC_DRAM_SIZE - \
+					 QEMU_OPTEE_PAGEABLE_LOAD_SIZE)
+#define QEMU_OPTEE_PAGEABLE_LOAD_SIZE	0x00400000
+
 /*
  * ARM-TF lives in SRAM, partition it here
  */
diff --git a/plat/qemu/platform.mk b/plat/qemu/platform.mk
index 76c7a14f..ed197a1d 100644
--- a/plat/qemu/platform.mk
+++ b/plat/qemu/platform.mk
@@ -65,6 +65,10 @@ BL2_SOURCES		+=	plat/qemu/qemu_bl2_mem_params_desc.c	\
 				plat/qemu/qemu_image_load.c		\
 				common/desc_image_load.c
 endif
+ifeq (${SPD},opteed)
+BL2_SOURCES		+=	lib/optee/optee_utils.c
+endif
+
 
 BL31_SOURCES		+=	lib/cpus/aarch64/aem_generic.S		\
 				lib/cpus/aarch64/cortex_a53.S		\
@@ -79,6 +83,16 @@ BL31_SOURCES		+=	lib/cpus/aarch64/aem_generic.S		\
 				plat/qemu/qemu_bl31_setup.c		\
 				plat/qemu/qemu_gic.c
 
+
+# Add the build options to pack Trusted OS Extra1 and Trusted OS Extra2 images
+# in the FIP if the platform requires.
+ifneq ($(BL32_EXTRA1),)
+$(eval $(call FIP_ADD_IMG,BL32_EXTRA1,--tos-fw-extra1))
+endif
+ifneq ($(BL32_EXTRA2),)
+$(eval $(call FIP_ADD_IMG,BL32_EXTRA2,--tos-fw-extra2))
+endif
+
 # Disable the PSCI platform compatibility layer
 ENABLE_PLAT_COMPAT	:= 	0
 
diff --git a/plat/qemu/qemu_bl2_mem_params_desc.c b/plat/qemu/qemu_bl2_mem_params_desc.c
index 3396140a..47f88acb 100644
--- a/plat/qemu/qemu_bl2_mem_params_desc.c
+++ b/plat/qemu/qemu_bl2_mem_params_desc.c
@@ -72,6 +72,43 @@ static bl_mem_params_node_t bl2_mem_params_descs[] = {
 
 	  .next_handoff_image_id = BL33_IMAGE_ID,
 	},
+
+	/*
+	 * Fill BL32 external 1 related information.
+	 * A typical use for extra1 image is with OP-TEE where it is the
+	 * pager image.
+	 */
+	{ .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),
+	   .image_info.image_base = BL32_BASE,
+	   .image_info.image_max_size = BL32_LIMIT - BL32_BASE,
+
+	   .next_handoff_image_id = INVALID_IMAGE_ID,
+	},
+
+	/*
+	 * Fill BL32 external 2 related information.
+	 * A typical use for extra2 image is with OP-TEE where it is the
+	 * paged image.
+	 */
+	{ .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),
+#ifdef SPD_opteed
+	   .image_info.image_base = QEMU_OPTEE_PAGEABLE_LOAD_BASE,
+	   .image_info.image_max_size = QEMU_OPTEE_PAGEABLE_LOAD_SIZE,
+#endif
+	   .next_handoff_image_id = INVALID_IMAGE_ID,
+	},
 # endif /* QEMU_LOAD_BL32 */
 
 	/* Fill BL33 related information */
diff --git a/plat/qemu/qemu_bl2_setup.c b/plat/qemu/qemu_bl2_setup.c
index 1306f34b..60d96233 100644
--- a/plat/qemu/qemu_bl2_setup.c
+++ b/plat/qemu/qemu_bl2_setup.c
@@ -9,6 +9,9 @@
 #include <console.h>
 #include <debug.h>
 #include <desc_image_load.h>
+#ifdef SPD_opteed
+#include <optee_utils.h>
+#endif
 #include <libfdt.h>
 #include <platform_def.h>
 #include <string.h>
@@ -225,12 +228,36 @@ static int qemu_bl2_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);
+#ifdef SPD_opteed
+	bl_mem_params_node_t *pager_mem_params = NULL;
+	bl_mem_params_node_t *paged_mem_params = NULL;
+#endif
 
 	assert(bl_mem_params);
 
 	switch (image_id) {
 # ifdef AARCH64
 	case BL32_IMAGE_ID:
+#ifdef SPD_opteed
+		pager_mem_params = get_bl_mem_params_node(BL32_EXTRA1_IMAGE_ID);
+		assert(pager_mem_params);
+
+		paged_mem_params = get_bl_mem_params_node(BL32_EXTRA2_IMAGE_ID);
+		assert(paged_mem_params);
+
+		err = parse_optee_header(&bl_mem_params->ep_info,
+					 &pager_mem_params->image_info,
+					 &paged_mem_params->image_info);
+		if (err != 0) {
+			WARN("OPTEE header parse error.\n");
+		}
+
+		/*
+		 * OP-TEE expect to receive DTB address in x2.
+		 * This will be copied into x2 by dispatcher.
+		 */
+		bl_mem_params->ep_info.args.arg3 = PLAT_QEMU_DT_BASE;
+#endif
 		bl_mem_params->ep_info.spsr = qemu_get_spsr_for_bl32_entry();
 		break;
 # endif
diff --git a/plat/qemu/qemu_io_storage.c b/plat/qemu/qemu_io_storage.c
index 19baf21d..e0f7e8ab 100644
--- a/plat/qemu/qemu_io_storage.c
+++ b/plat/qemu/qemu_io_storage.c
@@ -21,6 +21,8 @@
 #define BL2_IMAGE_NAME			"bl2.bin"
 #define BL31_IMAGE_NAME			"bl31.bin"
 #define BL32_IMAGE_NAME			"bl32.bin"
+#define BL32_EXTRA1_IMAGE_NAME		"bl32_extra1.bin"
+#define BL32_EXTRA2_IMAGE_NAME		"bl32_extra2.bin"
 #define BL33_IMAGE_NAME			"bl33.bin"
 
 #if TRUSTED_BOARD_BOOT
@@ -61,6 +63,14 @@ static const io_uuid_spec_t bl32_uuid_spec = {
 	.uuid = UUID_SECURE_PAYLOAD_BL32,
 };
 
+static const io_uuid_spec_t bl32_extra1_uuid_spec = {
+	.uuid = UUID_SECURE_PAYLOAD_BL32_EXTRA1,
+};
+
+static const io_uuid_spec_t bl32_extra2_uuid_spec = {
+	.uuid = UUID_SECURE_PAYLOAD_BL32_EXTRA2,
+};
+
 static const io_uuid_spec_t bl33_uuid_spec = {
 	.uuid = UUID_NON_TRUSTED_FIRMWARE_BL33,
 };
@@ -112,6 +122,14 @@ static const io_file_spec_t sh_file_spec[] = {
 		.path = BL32_IMAGE_NAME,
 		.mode = FOPEN_MODE_RB
 	},
+	[BL32_EXTRA1_IMAGE_ID] = {
+		.path = BL32_EXTRA1_IMAGE_NAME,
+		.mode = FOPEN_MODE_RB
+	},
+	[BL32_EXTRA2_IMAGE_ID] = {
+		.path = BL32_EXTRA2_IMAGE_NAME,
+		.mode = FOPEN_MODE_RB
+	},
 	[BL33_IMAGE_ID] = {
 		.path = BL33_IMAGE_NAME,
 		.mode = FOPEN_MODE_RB
@@ -185,6 +203,16 @@ static const struct plat_io_policy policies[] = {
 		(uintptr_t)&bl32_uuid_spec,
 		open_fip
 	},
+	[BL32_EXTRA1_IMAGE_ID] = {
+		&fip_dev_handle,
+		(uintptr_t)&bl32_extra1_uuid_spec,
+		open_fip
+	},
+	[BL32_EXTRA2_IMAGE_ID] = {
+		&fip_dev_handle,
+		(uintptr_t)&bl32_extra2_uuid_spec,
+		open_fip
+	},
 	[BL33_IMAGE_ID] = {
 		&fip_dev_handle,
 		(uintptr_t)&bl33_uuid_spec,