spl: atf: add support for LOAD_IMAGE_V2

Newer platforms use the LOAD_IMAGE_V2 parameter passing method. Add
support for it.

Signed-off-by: Michael Walle <michael@walle.cc>

2 files changed, 104 insertions, 4 deletions

diff --git a/common/spl/Kconfig b/common/spl/Kconfig
index d8086bd9e8..6d980be0b7 100644
--- a/common/spl/Kconfig
+++ b/common/spl/Kconfig
@@ -1276,6 +1276,15 @@ config SPL_ATF
 	  is loaded by SPL (which is considered as BL2 in ATF terminology).
 	  More detail at: https://github.com/ARM-software/arm-trusted-firmware
 
+config SPL_ATF_LOAD_IMAGE_V2
+	bool "Use the new LOAD_IMAGE_V2 parameter passing"
+	depends on SPL_ATF
+	help
+	  Some platforms use the newer LOAD_IMAGE_V2 parameter passing.
+
+	  If you want to load a bl31 image from the SPL and need the new
+	  method, say Y.
+
 config SPL_ATF_NO_PLATFORM_PARAM
         bool "Pass no platform parameter"
 	depends on SPL_ATF
diff --git a/common/spl/spl_atf.c b/common/spl/spl_atf.c
index 51b45d5dc6..e1b68dd561 100644
--- a/common/spl/spl_atf.c
+++ b/common/spl/spl_atf.c
@@ -29,6 +29,19 @@ struct bl2_to_bl31_params_mem {
 	struct entry_point_info bl31_ep_info;
 };
 
+struct bl2_to_bl31_params_mem_v2 {
+	struct bl_params bl_params;
+	struct bl_params_node bl31_params_node;
+	struct bl_params_node bl32_params_node;
+	struct bl_params_node bl33_params_node;
+	struct atf_image_info bl31_image_info;
+	struct atf_image_info bl32_image_info;
+	struct atf_image_info bl33_image_info;
+	struct entry_point_info bl33_ep_info;
+	struct entry_point_info bl32_ep_info;
+	struct entry_point_info bl31_ep_info;
+};
+
 struct bl31_params *bl2_plat_get_bl31_params_default(uintptr_t bl32_entry,
 						     uintptr_t bl33_entry,
 						     uintptr_t fdt_addr)
@@ -96,6 +109,79 @@ __weak struct bl31_params *bl2_plat_get_bl31_params(uintptr_t bl32_entry,
 						fdt_addr);
 }
 
+struct bl_params *bl2_plat_get_bl31_params_v2_default(uintptr_t bl32_entry,
+						      uintptr_t bl33_entry,
+						      uintptr_t fdt_addr)
+{
+	static struct bl2_to_bl31_params_mem_v2 bl31_params_mem;
+	struct bl_params *bl_params;
+	struct bl_params_node *bl_params_node;
+
+	/*
+	 * Initialise the memory for all the arguments that needs to
+	 * be passed to BL31
+	 */
+	memset(&bl31_params_mem, 0, sizeof(bl31_params_mem));
+
+	/* Assign memory for TF related information */
+	bl_params = &bl31_params_mem.bl_params;
+	SET_PARAM_HEAD(bl_params, ATF_PARAM_BL_PARAMS, ATF_VERSION_2, 0);
+	bl_params->head = &bl31_params_mem.bl31_params_node;
+
+	/* Fill BL31 related information */
+	bl_params_node = &bl31_params_mem.bl31_params_node;
+	bl_params_node->image_id = ATF_BL31_IMAGE_ID;
+	bl_params_node->image_info = &bl31_params_mem.bl31_image_info;
+	bl_params_node->ep_info = &bl31_params_mem.bl31_ep_info;
+	bl_params_node->next_params_info = &bl31_params_mem.bl32_params_node;
+	SET_PARAM_HEAD(bl_params_node->image_info, ATF_PARAM_IMAGE_BINARY,
+		       ATF_VERSION_2, 0);
+
+	/* Fill BL32 related information */
+	bl_params_node = &bl31_params_mem.bl32_params_node;
+	bl_params_node->image_id = ATF_BL32_IMAGE_ID;
+	bl_params_node->image_info = &bl31_params_mem.bl32_image_info;
+	bl_params_node->ep_info = &bl31_params_mem.bl32_ep_info;
+	bl_params_node->next_params_info = &bl31_params_mem.bl33_params_node;
+	SET_PARAM_HEAD(bl_params_node->ep_info, ATF_PARAM_EP,
+		       ATF_VERSION_2, ATF_EP_SECURE);
+
+	/* secure payload is optional, so set pc to 0 if absent */
+	bl_params_node->ep_info->args.arg3 = fdt_addr;
+	bl_params_node->ep_info->pc = bl32_entry ? bl32_entry : 0;
+	bl_params_node->ep_info->spsr = SPSR_64(MODE_EL1, MODE_SP_ELX,
+						DISABLE_ALL_EXECPTIONS);
+	SET_PARAM_HEAD(bl_params_node->image_info, ATF_PARAM_IMAGE_BINARY,
+		       ATF_VERSION_2, 0);
+
+	/* Fill BL33 related information */
+	bl_params_node = &bl31_params_mem.bl33_params_node;
+	bl_params_node->image_id = ATF_BL33_IMAGE_ID;
+	bl_params_node->image_info = &bl31_params_mem.bl33_image_info;
+	bl_params_node->ep_info = &bl31_params_mem.bl33_ep_info;
+	bl_params_node->next_params_info = NULL;
+	SET_PARAM_HEAD(bl_params_node->ep_info, ATF_PARAM_EP,
+		       ATF_VERSION_2, ATF_EP_NON_SECURE);
+
+	/* BL33 expects to receive the primary CPU MPID (through x0) */
+	bl_params_node->ep_info->args.arg0 = 0xffff & read_mpidr();
+	bl_params_node->ep_info->pc = bl33_entry;
+	bl_params_node->ep_info->spsr = SPSR_64(MODE_EL2, MODE_SP_ELX,
+						DISABLE_ALL_EXECPTIONS);
+	SET_PARAM_HEAD(bl_params_node->image_info, ATF_PARAM_IMAGE_BINARY,
+		       ATF_VERSION_2, 0);
+
+	return bl_params;
+}
+
+__weak struct bl_params *bl2_plat_get_bl31_params_v2(uintptr_t bl32_entry,
+						     uintptr_t bl33_entry,
+						     uintptr_t fdt_addr)
+{
+	return bl2_plat_get_bl31_params_v2_default(bl32_entry, bl33_entry,
+						   fdt_addr);
+}
+
 static inline void raw_write_daif(unsigned int daif)
 {
 	__asm__ __volatile__("msr DAIF, %0\n\t" : : "r" (daif) : "memory");
@@ -106,16 +192,21 @@ typedef void (*atf_entry_t)(struct bl31_params *params, void *plat_params);
 static void bl31_entry(uintptr_t bl31_entry, uintptr_t bl32_entry,
 		       uintptr_t bl33_entry, uintptr_t fdt_addr)
 {
-	struct bl31_params *bl31_params;
 	atf_entry_t  atf_entry = (atf_entry_t)bl31_entry;
+	void *bl31_params;
 
-	bl31_params = bl2_plat_get_bl31_params(bl32_entry, bl33_entry,
-					       fdt_addr);
+	if (CONFIG_IS_ENABLED(ATF_LOAD_IMAGE_V2))
+		bl31_params = bl2_plat_get_bl31_params_v2(bl32_entry,
+							  bl33_entry,
+							  fdt_addr);
+	else
+		bl31_params = bl2_plat_get_bl31_params(bl32_entry, bl33_entry,
+						       fdt_addr);
 
 	raw_write_daif(SPSR_EXCEPTION_MASK);
 	dcache_disable();
 
-	atf_entry((void *)bl31_params, (void *)fdt_addr);
+	atf_entry(bl31_params, (void *)fdt_addr);
 }
 
 static int spl_fit_images_find(void *blob, int os)