plat: marvell: armada: fix BL32 extra parameters usage

Update missing code releated to the BL32 payload.

Change-Id: I5cbe71921467c53c45be5510f950cefdacc110e1
Signed-off-by: Marcin Wojtas <mw@semihalf.com>

plat/marvell/armada/common/aarch64/marvell_bl2_mem_params_desc.c

@@ -100,6 +100,45 @@ 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 = MARVELL_OPTEE_PAGEABLE_LOAD_BASE,
+	    .image_info.image_max_size = MARVELL_OPTEE_PAGEABLE_LOAD_SIZE,
+#endif
+	    .next_handoff_image_id = INVALID_IMAGE_ID,
+	},
 # endif /* BL32_BASE */
 
 	/* Fill BL33 related information */

plat/marvell/armada/common/marvell_bl2_setup.c

@@ -17,6 +17,9 @@
 #include <drivers/console.h>
 #include <lib/utils.h>
 
+#ifdef SPD_opteed
+#include <optee_utils.h>
+#endif
 #include <marvell_def.h>
 #include <plat_marvell.h>
 
@@ -97,9 +100,29 @@ int marvell_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 /* SPD_opteed */
 	assert(bl_mem_params);
 
 	switch (image_id) {
+	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");
+#endif /* SPD_opteed */
+		bl_mem_params->ep_info.spsr = marvell_get_spsr_for_bl32_entry();
+		break;
 
 	case BL33_IMAGE_ID:
 		/* BL33 expects to receive the primary CPU MPID (through r0) */

plat/marvell/armada/common/marvell_common.mk

@@ -58,6 +58,10 @@ BL2_SOURCES		+=	drivers/io/io_fip.c					\
 				$(MARVELL_PLAT_BASE)/common/aarch64/marvell_bl2_mem_params_desc.c	\
 				$(MARVELL_PLAT_BASE)/common/marvell_image_load.c
 
+ifeq (${SPD},opteed)
+PLAT_INCLUDES		+=	-Iinclude/lib
+BL2_SOURCES		+=	lib/optee/optee_utils.c
+endif
 
 BL31_SOURCES		+=	$(MARVELL_PLAT_BASE)/common/marvell_bl31_setup.c	\
 				$(MARVELL_PLAT_BASE)/common/marvell_pm.c		\
@@ -69,6 +73,15 @@ BL31_SOURCES		+=	$(MARVELL_PLAT_BASE)/common/marvell_bl31_setup.c	\
 # PSCI functionality
 $(eval $(call add_define,CONFIG_ARM64))
 
+# 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 TOOL_ADD_IMG,bl32_extra1,--tos-fw-extra1))
+endif
+ifneq ($(BL32_EXTRA2),)
+$(eval $(call TOOL_ADD_IMG,bl32_extra2,--tos-fw-extra2))
+endif
+
 # MSS (SCP) build
 ifeq (${MSS_SUPPORT}, 1)
 include $(MARVELL_PLAT_BASE)/common/mss/mss_common.mk

plat/marvell/armada/common/marvell_io_storage.c

@@ -43,6 +43,15 @@ static const io_uuid_spec_t bl31_uuid_spec = {
 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,
 };
@@ -83,6 +92,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,