feat(plat/qemu): add SPMD support with SPMC at S-EL1

Adds support for SPMD with SPMC at S-EL1. A new config option SPMC_OPTEE
is added to support loading the special OP-TEE images when configured
with SPD=spmd. With or without SPMC_OPTEE. It should still be possible
to load another BL32 payload implementing a SPMC, provided that entry
point is the same as load address, that is, BL32_BASE.

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

4 files changed, 61 insertions, 9 deletions

diff --git a/plat/qemu/common/qemu_bl2_mem_params_desc.c b/plat/qemu/common/qemu_bl2_mem_params_desc.c
index f8b9066dff..5af3a2264d 100644
--- a/plat/qemu/common/qemu_bl2_mem_params_desc.c
+++ b/plat/qemu/common/qemu_bl2_mem_params_desc.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2017-2021, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -116,7 +116,7 @@ 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_SKIP_LOADING),
-#if defined(SPD_opteed) || defined(AARCH32_SP_OPTEE)
+#if defined(SPD_opteed) || defined(AARCH32_SP_OPTEE) || defined(SPMC_OPTEE)
 	   .image_info.image_base = QEMU_OPTEE_PAGEABLE_LOAD_BASE,
 	   .image_info.image_max_size = QEMU_OPTEE_PAGEABLE_LOAD_SIZE,
 #endif
diff --git a/plat/qemu/common/qemu_bl2_setup.c b/plat/qemu/common/qemu_bl2_setup.c
index 3e289fc6b0..2c0da15b91 100644
--- a/plat/qemu/common/qemu_bl2_setup.c
+++ b/plat/qemu/common/qemu_bl2_setup.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2021, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -144,16 +144,20 @@ 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);
-#if defined(SPD_opteed) || defined(AARCH32_SP_OPTEE)
+#if defined(SPD_opteed) || defined(AARCH32_SP_OPTEE) || defined(SPMC_OPTEE)
 	bl_mem_params_node_t *pager_mem_params = NULL;
 	bl_mem_params_node_t *paged_mem_params = NULL;
 #endif
+#if defined(SPD_spmd)
+	unsigned int mode_rw = MODE_RW_64;
+	uint64_t pagable_part = 0;
+#endif
 
 	assert(bl_mem_params);
 
 	switch (image_id) {
 	case BL32_IMAGE_ID:
-#if defined(SPD_opteed) || defined(AARCH32_SP_OPTEE)
+#if defined(SPD_opteed) || defined(AARCH32_SP_OPTEE) || defined(SPMC_OPTEE)
 		pager_mem_params = get_bl_mem_params_node(BL32_EXTRA1_IMAGE_ID);
 		assert(pager_mem_params);
 
@@ -166,21 +170,30 @@ static int qemu_bl2_handle_post_image_load(unsigned int image_id)
 		if (err != 0) {
 			WARN("OPTEE header parse error.\n");
 		}
+#if defined(SPD_spmd)
+		mode_rw = bl_mem_params->ep_info.args.arg0;
+		pagable_part = bl_mem_params->ep_info.args.arg1;
+#endif
+#endif
 
-#if defined(SPD_opteed)
+#if defined(SPD_spmd)
+		bl_mem_params->ep_info.args.arg0 = ARM_PRELOADED_DTB_BASE;
+		bl_mem_params->ep_info.args.arg1 = pagable_part;
+		bl_mem_params->ep_info.args.arg2 = mode_rw;
+		bl_mem_params->ep_info.args.arg3 = 0;
+#elif defined(SPD_opteed)
 		/*
 		 * OP-TEE expect to receive DTB address in x2.
 		 * This will be copied into x2 by dispatcher.
 		 */
 		bl_mem_params->ep_info.args.arg3 = ARM_PRELOADED_DTB_BASE;
-#else /* case AARCH32_SP_OPTEE */
+#elif defined(AARCH32_SP_OPTEE)
 		bl_mem_params->ep_info.args.arg0 =
 					bl_mem_params->ep_info.args.arg1;
 		bl_mem_params->ep_info.args.arg1 = 0;
 		bl_mem_params->ep_info.args.arg2 = ARM_PRELOADED_DTB_BASE;
 		bl_mem_params->ep_info.args.arg3 = 0;
 #endif
-#endif
 		bl_mem_params->ep_info.spsr = qemu_get_spsr_for_bl32_entry();
 		break;
 
diff --git a/plat/qemu/common/qemu_spmd_manifest.c b/plat/qemu/common/qemu_spmd_manifest.c
new file mode 100644
index 0000000000..fd46e26756
--- /dev/null
+++ b/plat/qemu/common/qemu_spmd_manifest.c
@@ -0,0 +1,31 @@
+/*
+ * Copyright (c) 2021, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+
+#include <services/spm_core_manifest.h>
+
+#include <plat/common/platform.h>
+#include <platform_def.h>
+
+int plat_spm_core_manifest_load(spmc_manifest_attribute_t *manifest,
+				const void *pm_addr)
+{
+	entry_point_info_t *ep_info = bl31_plat_get_next_image_ep_info(SECURE);
+
+	assert(ep_info != NULL);
+	assert(manifest != NULL);
+
+	manifest->major_version = 1;
+	manifest->minor_version = 0;
+	manifest->exec_state = ep_info->args.arg2;
+	manifest->load_address = BL32_BASE;
+	manifest->entrypoint = BL32_BASE;
+	manifest->binary_size = BL32_LIMIT - BL32_BASE;
+	manifest->spmc_id = 0x8000;
+
+	return 0;
+}
diff --git a/plat/qemu/qemu/platform.mk b/plat/qemu/qemu/platform.mk
index a3b353f746..a8f978ae83 100644
--- a/plat/qemu/qemu/platform.mk
+++ b/plat/qemu/qemu/platform.mk
@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2013-2020, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2013-2021, ARM Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -26,6 +26,10 @@ endif
 ifeq ($(AARCH32_SP),optee)
 add-lib-optee 		:= 	yes
 endif
+ifeq ($(SPMC_OPTEE),1)
+$(eval $(call add_define,SPMC_OPTEE))
+add-lib-optee 		:= 	yes
+endif
 
 include lib/libfdt/libfdt.mk
 
@@ -175,6 +179,10 @@ BL31_SOURCES		+=	lib/cpus/aarch64/aem_generic.S		\
 				${PLAT_QEMU_COMMON_PATH}/aarch64/plat_helpers.S	\
 				${PLAT_QEMU_COMMON_PATH}/qemu_bl31_setup.c		\
 				${QEMU_GIC_SOURCES}
+
+ifeq (${SPD},spmd)
+BL31_SOURCES		+=	plat/qemu/common/qemu_spmd_manifest.c
+endif
 endif
 
 # Add the build options to pack Trusted OS Extra1 and Trusted OS Extra2 images