qemu: Add LOAD_IMAGE_V2 support

The generic LOAD_IMAGE_V2 framework has been merged and enable for almost
all the arm platform. Because qemu platform doesn't share those common
files with arm, QEMU haven't got this support yet.

This patch add all the necessary code the files for adding LOAD_IMAGE_V2
support on QEMU and enable it as default.

Fixes ARM-software/tf-issues#507

Signed-off-by: Fu Wei <fu.wei@linaro.org>

6 files changed, 233 insertions, 7 deletions

diff --git a/plat/qemu/platform.mk b/plat/qemu/platform.mk
index e6d7014f..dc3b5d93 100644
--- a/plat/qemu/platform.mk
+++ b/plat/qemu/platform.mk
@@ -6,6 +6,13 @@
 
 include lib/libfdt/libfdt.mk
 
+# Enable new version of image loading on QEMU platforms
+LOAD_IMAGE_V2		:=	1
+
+ifeq ($(NEED_BL32),yes)
+$(eval $(call add_define,QEMU_LOAD_BL32))
+endif
+
 PLAT_INCLUDES		:=	-Iinclude/plat/arm/common/		\
 				-Iinclude/plat/arm/common/aarch64/	\
 				-Iplat/qemu/include			\
@@ -53,6 +60,11 @@ BL2_SOURCES		+=	drivers/io/io_semihosting.c		\
 				plat/qemu/qemu_bl2_setup.c		\
 				plat/qemu/dt.c				\
 				$(LIBFDT_SRCS)
+ifeq (${LOAD_IMAGE_V2},1)
+BL2_SOURCES		+=	plat/qemu/qemu_bl2_mem_params_desc.c	\
+				plat/qemu/qemu_image_load.c		\
+				common/desc_image_load.c
+endif
 
 BL31_SOURCES		+=	lib/cpus/aarch64/aem_generic.S		\
 				lib/cpus/aarch64/cortex_a53.S		\
diff --git a/plat/qemu/qemu_bl1_setup.c b/plat/qemu/qemu_bl1_setup.c
index 85ff20e4..5a705584 100644
--- a/plat/qemu/qemu_bl1_setup.c
+++ b/plat/qemu/qemu_bl1_setup.c
@@ -35,8 +35,6 @@ meminfo_t *bl1_plat_sec_mem_layout(void)
  ******************************************************************************/
 void bl1_early_platform_setup(void)
 {
-	const size_t bl1_size = BL1_RAM_LIMIT - BL1_RAM_BASE;
-
 	/* Initialize the console to provide early debug support */
 	console_init(PLAT_QEMU_BOOT_UART_BASE, PLAT_QEMU_BOOT_UART_CLK_IN_HZ,
 		     PLAT_QEMU_CONSOLE_BAUDRATE);
@@ -45,11 +43,13 @@ void bl1_early_platform_setup(void)
 	bl1_tzram_layout.total_base = BL_RAM_BASE;
 	bl1_tzram_layout.total_size = BL_RAM_SIZE;
 
+#if !LOAD_IMAGE_V2
 	/* Calculate how much RAM BL1 is using and how much remains free */
 	bl1_tzram_layout.free_base = BL_RAM_BASE;
 	bl1_tzram_layout.free_size = BL_RAM_SIZE;
 	reserve_mem(&bl1_tzram_layout.free_base, &bl1_tzram_layout.free_size,
-		    BL1_RAM_BASE, bl1_size);
+		    BL1_RAM_BASE, BL1_RAM_LIMIT - BL1_RAM_BASE);
+#endif /* !LOAD_IMAGE_V2 */
 }
 
 /******************************************************************************
@@ -70,6 +70,7 @@ void bl1_platform_setup(void)
 	plat_qemu_io_setup();
 }
 
+#if !LOAD_IMAGE_V2
 /*******************************************************************************
  * Function that takes a memory layout into which BL2 has been loaded and
  * populates a new memory layout for BL2 that ensures that BL1's data sections
@@ -110,3 +111,4 @@ void bl1_plat_set_bl2_ep_info(image_info_t *bl2_image,
 	SET_SECURITY_STATE(bl2_ep->h.attr, SECURE);
 	bl2_ep->spsr = SPSR_64(MODE_EL1, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS);
 }
+#endif /* !LOAD_IMAGE_V2 */
diff --git a/plat/qemu/qemu_bl2_mem_params_desc.c b/plat/qemu/qemu_bl2_mem_params_desc.c
new file mode 100644
index 00000000..3396140a
--- /dev/null
+++ b/plat/qemu/qemu_bl2_mem_params_desc.c
@@ -0,0 +1,101 @@
+/*
+ * Copyright (c) 2017, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <desc_image_load.h>
+#include <platform.h>
+#include <platform_def.h>
+
+/*******************************************************************************
+ * Following descriptor provides BL image/ep information that gets used
+ * by BL2 to load the images and also subset of this information is
+ * passed to next BL image. The image loading sequence is managed by
+ * populating the images in required loading order. The image execution
+ * sequence is managed by populating the `next_handoff_image_id` with
+ * the next executable image id.
+ ******************************************************************************/
+static bl_mem_params_node_t bl2_mem_params_descs[] = {
+#ifdef EL3_PAYLOAD_BASE
+	/* Fill EL3 payload related information (BL31 is EL3 payload) */
+	{ .image_id = BL31_IMAGE_ID,
+
+	  SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP, VERSION_2,
+				entry_point_info_t,
+				SECURE | EXECUTABLE | EP_FIRST_EXE),
+	  .ep_info.pc = EL3_PAYLOAD_BASE,
+	  .ep_info.spsr = SPSR_64(MODE_EL3, MODE_SP_ELX,
+				  DISABLE_ALL_EXCEPTIONS),
+
+	  SET_STATIC_PARAM_HEAD(image_info, PARAM_EP, VERSION_2, image_info_t,
+				IMAGE_ATTRIB_PLAT_SETUP | IMAGE_ATTRIB_SKIP_LOADING),
+
+	  .next_handoff_image_id = INVALID_IMAGE_ID,
+	},
+#else /* EL3_PAYLOAD_BASE */
+	/* Fill BL31 related information */
+	{ .image_id = BL31_IMAGE_ID,
+
+	  SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP, VERSION_2,
+				entry_point_info_t,
+				SECURE | EXECUTABLE | EP_FIRST_EXE),
+	  .ep_info.pc = BL31_BASE,
+	  .ep_info.spsr = SPSR_64(MODE_EL3, MODE_SP_ELX,
+				  DISABLE_ALL_EXCEPTIONS),
+# if DEBUG
+	  .ep_info.args.arg1 = QEMU_BL31_PLAT_PARAM_VAL,
+# endif
+	  SET_STATIC_PARAM_HEAD(image_info, PARAM_EP, VERSION_2, image_info_t,
+				IMAGE_ATTRIB_PLAT_SETUP),
+	  .image_info.image_base = BL31_BASE,
+	  .image_info.image_max_size = BL31_LIMIT - BL31_BASE,
+
+# ifdef QEMU_LOAD_BL32
+	  .next_handoff_image_id = BL32_IMAGE_ID,
+# else
+	  .next_handoff_image_id = BL33_IMAGE_ID,
+# endif
+	},
+# ifdef QEMU_LOAD_BL32
+	/* Fill BL32 related information */
+	{ .image_id = BL32_IMAGE_ID,
+
+	  SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP, VERSION_2,
+				entry_point_info_t, SECURE | EXECUTABLE),
+	  .ep_info.pc = BL32_BASE,
+
+	  SET_STATIC_PARAM_HEAD(image_info, PARAM_EP, VERSION_2, image_info_t,
+				0),
+	  .image_info.image_base = BL32_BASE,
+	  .image_info.image_max_size = BL32_LIMIT - BL32_BASE,
+
+	  .next_handoff_image_id = BL33_IMAGE_ID,
+	},
+# endif /* QEMU_LOAD_BL32 */
+
+	/* Fill BL33 related information */
+	{ .image_id = BL33_IMAGE_ID,
+	  SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP, VERSION_2,
+				entry_point_info_t, NON_SECURE | EXECUTABLE),
+# ifdef PRELOADED_BL33_BASE
+	  .ep_info.pc = PRELOADED_BL33_BASE,
+
+	  SET_STATIC_PARAM_HEAD(image_info, PARAM_EP, VERSION_2, image_info_t,
+				IMAGE_ATTRIB_SKIP_LOADING),
+# else /* PRELOADED_BL33_BASE */
+	  .ep_info.pc = NS_IMAGE_OFFSET,
+
+	  SET_STATIC_PARAM_HEAD(image_info, PARAM_EP, VERSION_2, image_info_t,
+				0),
+	  .image_info.image_base = NS_IMAGE_OFFSET,
+	  .image_info.image_max_size = NS_DRAM0_BASE + NS_DRAM0_SIZE -
+				       NS_IMAGE_OFFSET,
+# endif /* !PRELOADED_BL33_BASE */
+
+	  .next_handoff_image_id = INVALID_IMAGE_ID,
+	}
+#endif /* !EL3_PAYLOAD_BASE */
+};
+
+REGISTER_BL_IMAGE_DESCS(bl2_mem_params_descs)
diff --git a/plat/qemu/qemu_bl2_setup.c b/plat/qemu/qemu_bl2_setup.c
index f928b105..1306f34b 100644
--- a/plat/qemu/qemu_bl2_setup.c
+++ b/plat/qemu/qemu_bl2_setup.c
@@ -4,9 +4,11 @@
  * SPDX-License-Identifier: BSD-3-Clause
  */
 #include <arch_helpers.h>
+#include <assert.h>
 #include <bl_common.h>
 #include <console.h>
 #include <debug.h>
+#include <desc_image_load.h>
 #include <libfdt.h>
 #include <platform_def.h>
 #include <string.h>
@@ -22,6 +24,10 @@
 #define BL2_RO_BASE (unsigned long)(&__RO_START__)
 #define BL2_RO_LIMIT (unsigned long)(&__RO_END__)
 
+/* Data structure which holds the extents of the trusted SRAM for BL2 */
+static meminfo_t bl2_tzram_layout __aligned(CACHE_WRITEBACK_GRANULE);
+
+#if !LOAD_IMAGE_V2
 /*******************************************************************************
  * This structure represents the superset of information that is passed to
  * BL3-1, e.g. while passing control to it from BL2, bl31_params
@@ -41,10 +47,6 @@ typedef struct bl2_to_bl31_params_mem {
 static bl2_to_bl31_params_mem_t bl31_params_mem;
 
 
-
-/* Data structure which holds the extents of the trusted SRAM for BL2 */
-static meminfo_t bl2_tzram_layout __aligned(CACHE_WRITEBACK_GRANULE);
-
 meminfo_t *bl2_plat_sec_mem_layout(void)
 {
 	return &bl2_tzram_layout;
@@ -120,6 +122,7 @@ struct entry_point_info *bl2_plat_get_bl31_ep_info(void)
 
 	return &bl31_params_mem.bl31_ep_info;
 }
+#endif /* !LOAD_IMAGE_V2 */
 
 
 
@@ -217,6 +220,41 @@ static uint32_t qemu_get_spsr_for_bl33_entry(void)
 	return spsr;
 }
 
+#if LOAD_IMAGE_V2
+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);
+
+	assert(bl_mem_params);
+
+	switch (image_id) {
+# ifdef AARCH64
+	case BL32_IMAGE_ID:
+		bl_mem_params->ep_info.spsr = qemu_get_spsr_for_bl32_entry();
+		break;
+# endif
+	case BL33_IMAGE_ID:
+		/* BL33 expects to receive the primary CPU MPID (through r0) */
+		bl_mem_params->ep_info.args.arg0 = 0xffff & read_mpidr();
+		bl_mem_params->ep_info.spsr = qemu_get_spsr_for_bl33_entry();
+		break;
+	}
+
+	return err;
+}
+
+/*******************************************************************************
+ * 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)
+{
+	return qemu_bl2_handle_post_image_load(image_id);
+}
+
+#else /* LOAD_IMAGE_V2 */
+
 /*******************************************************************************
  * Before calling this function BL3-1 is loaded in memory and its entrypoint
  * is set by load_image. This is a placeholder for the platform to change
@@ -282,6 +320,7 @@ void bl2_plat_get_bl33_meminfo(meminfo_t *bl33_meminfo)
 	bl33_meminfo->free_base = NS_DRAM0_BASE;
 	bl33_meminfo->free_size = NS_DRAM0_SIZE;
 }
+#endif /* !LOAD_IMAGE_V2 */
 
 unsigned long plat_get_ns_image_entrypoint(void)
 {
diff --git a/plat/qemu/qemu_bl31_setup.c b/plat/qemu/qemu_bl31_setup.c
index 34866f06..5bf45895 100644
--- a/plat/qemu/qemu_bl31_setup.c
+++ b/plat/qemu/qemu_bl31_setup.c
@@ -37,13 +37,49 @@ static entry_point_info_t bl33_image_ep_info;
  * tables. BL2 has flushed this information to memory, so we are guaranteed
  * to pick up good data.
  ******************************************************************************/
+#if LOAD_IMAGE_V2
+void bl31_early_platform_setup(void *from_bl2,
+			       void *plat_params_from_bl2)
+#else
 void bl31_early_platform_setup(bl31_params_t *from_bl2,
 				void *plat_params_from_bl2)
+#endif
 {
 	/* Initialize the console to provide early debug support */
 	console_init(PLAT_QEMU_BOOT_UART_BASE, PLAT_QEMU_BOOT_UART_CLK_IN_HZ,
 			PLAT_QEMU_CONSOLE_BAUDRATE);
 
+#if LOAD_IMAGE_V2
+	/*
+	 * Check params passed from BL2
+	 */
+	bl_params_t *params_from_bl2 = (bl_params_t *)from_bl2;
+
+	assert(params_from_bl2);
+	assert(params_from_bl2->h.type == PARAM_BL_PARAMS);
+	assert(params_from_bl2->h.version >= VERSION_2);
+
+	bl_params_node_t *bl_params = params_from_bl2->head;
+
+	/*
+	 * Copy BL33 and BL32 (if present), entry point information.
+	 * They are stored in Secure RAM, in BL2's address space.
+	 */
+	while (bl_params) {
+		if (bl_params->image_id == BL32_IMAGE_ID)
+			bl32_image_ep_info = *bl_params->ep_info;
+
+		if (bl_params->image_id == BL33_IMAGE_ID)
+			bl33_image_ep_info = *bl_params->ep_info;
+
+		bl_params = bl_params->next_params_info;
+	}
+
+	if (!bl33_image_ep_info.pc)
+		panic();
+
+#else /* LOAD_IMAGE_V2 */
+
 	/*
 	 * Check params passed from BL2 should not be NULL,
 	 */
@@ -65,6 +101,8 @@ void bl31_early_platform_setup(bl31_params_t *from_bl2,
 	if (from_bl2->bl32_ep_info)
 		bl32_image_ep_info = *from_bl2->bl32_ep_info;
 	bl33_image_ep_info = *from_bl2->bl33_ep_info;
+
+#endif /* !LOAD_IMAGE_V2 */
 }
 
 void bl31_plat_arch_setup(void)
diff --git a/plat/qemu/qemu_image_load.c b/plat/qemu/qemu_image_load.c
new file mode 100644
index 00000000..8e246477
--- /dev/null
+++ b/plat/qemu/qemu_image_load.c
@@ -0,0 +1,34 @@
+/*
+ * Copyright (c) 2017, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <desc_image_load.h>
+
+/*******************************************************************************
+ * This function is a wrapper of a common function which flushes the data
+ * structures so that they are visible in memory for the next BL image.
+ ******************************************************************************/
+void plat_flush_next_bl_params(void)
+{
+	flush_bl_params_desc();
+}
+
+/*******************************************************************************
+ * This function is a wrapper of a common function which returns the list of
+ * loadable images.
+ ******************************************************************************/
+bl_load_info_t *plat_get_bl_image_load_info(void)
+{
+	return get_bl_load_info_from_mem_params_desc();
+}
+
+/*******************************************************************************
+ * This function is a wrapper of a common function which returns the data
+ * structures of the next BL image.
+ ******************************************************************************/
+bl_params_t *plat_get_next_bl_params(void)
+{
+	return get_next_bl_params_from_mem_params_desc();
+}