lsp: Add initial Logical Partition Skeleton Framework

Add the DECLARE_LOGICAL_PARTITION macro to allow
defining a Logical Partition. These are statically
allocated with the descriptors placed in RO memory.

Change-Id: I1c2523e0ad2d9c5d36aeeef6b8bcb1e80db7c443
Signed-off-by: Marc Bonnici <marc.bonnici@arm.com>

7 files changed, 261 insertions, 12 deletions

diff --git a/bl31/bl31.mk b/bl31/bl31.mk
index 22f51ff8a..3c2f7b08b 100644
--- a/bl31/bl31.mk
+++ b/bl31/bl31.mk
@@ -31,6 +31,7 @@ ifeq (${SPMC_AT_EL3},1)
     include services/std_svc/spm/common/spm.mk
     include services/std_svc/spm/spmc/spmc.mk
   endif
+	BL31_SOURCES		+=	common/logical_sp.c
 endif
 
 include lib/psci/psci_lib.mk
diff --git a/bl31/bl31_main.c b/bl31/bl31_main.c
index 44bf32cb7..b1af4404c 100644
--- a/bl31/bl31_main.c
+++ b/bl31/bl31_main.c
@@ -16,6 +16,7 @@
 #include <common/debug.h>
 #include <common/runtime_svc.h>
 #include <drivers/console.h>
+#include <services/logical_sp.h>
 #include <lib/el3_runtime/context_mgmt.h>
 #include <lib/pmf/pmf.h>
 #include <lib/runtime_instr.h>
@@ -128,6 +129,12 @@ void bl31_main(void)
 	INFO("BL31: Initializing runtime services\n");
 	runtime_svc_init();
 
+#if SPMC_AT_EL3
+	/* Initialize logical partition registration. */
+	INFO("BL31: Initializing logical partition structures\n");
+	el3_sp_desc_init();
+#endif
+
 	/*
 	 * All the cold boot actions on the primary cpu are done. We now need to
 	 * decide which is the next image (BL32 or BL33) and how to execute it.
diff --git a/common/logical_sp.c b/common/logical_sp.c
new file mode 100644
index 000000000..56c68f56f
--- /dev/null
+++ b/common/logical_sp.c
@@ -0,0 +1,90 @@
+/*
+ * Copyright (c) 2013-2019, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <string.h>
+
+#include <common/uuid.h>
+#include <common/debug.h>
+#include <common/runtime_svc.h>
+
+#include <services/logical_sp.h>
+
+/*******************************************************************************
+ * The 'logical_partition' array holds the logical partition descriptors exported by
+ * SPs by placing them in the 'logical_partition' linker section.
+ ******************************************************************************/
+
+
+
+/*******************************************************************************
+ * Simple routine to sanity check a logical partition descriptor before using it
+ ******************************************************************************/
+static int32_t validate_logical_partition_struct(const el3_lp_desc_t *desc)
+{
+	if (desc == NULL)
+		return -EINVAL;
+
+	/* Ensue we have setup and direct messaging callback registered */
+	if ((desc->init == NULL) || (desc->direct_req == NULL))
+		return -EINVAL;
+
+	return 0;
+}
+
+/*******************************************************************************
+ * This function validates any logical partition descriptors.
+ * Initalistaion of said partitions will be taken care of during SPMC boot.
+ ******************************************************************************/
+void __init el3_sp_desc_init(void)
+{
+	int rc = 0;
+	uint8_t index, inner_idx;
+	el3_lp_desc_t *logical_partition;
+
+	/* Assert the number of descriptors detected are less than maximum indices */
+	assert(EL3_LP_DESCS_END >= EL3_LP_DESCS_START);
+
+	assert(EL3_LP_DESCS_NUM <= MAX_EL3_LP_DESCS_COUNT);
+
+	/* If no logical partitions are implemented then simply bail out */
+	if (EL3_LP_DESCS_NUM == 0U)
+		return;
+
+	/* Initialise internal variables to invalid state */
+	/* TODO: Do we want to reused indexing mechanism here or just loop as arrary is small? */
+	// (void)memset(logical_partition_indices, -1, sizeof(logical_partition_indices));
+
+	logical_partition = (el3_lp_desc_t *) EL3_LP_DESCS_START;
+	for (index = 0U; index < EL3_LP_DESCS_NUM; index++) {
+		el3_lp_desc_t *lp_descriptor = &logical_partition[index];
+
+		/*
+		 * Validate our logical partition descriptors.
+		 */
+		rc = validate_logical_partition_struct(lp_descriptor);
+		if (rc != 0) {
+			ERROR("Invalid logical partition descriptor %p\n",
+				(void *) lp_descriptor);
+			panic(); // TODO: Should we just continue to load without that partition?
+		}
+
+		/* Check we have a UUID Specified. */
+		if (lp_descriptor->uuid == NULL) {
+			ERROR("Invalid UUID Specified\n");
+		}
+
+		/* Ensure that all partition IDs are unique. */
+		for (inner_idx = index  + 1; inner_idx < EL3_LP_DESCS_NUM; inner_idx++) {
+			el3_lp_desc_t *lp_descriptor_other = &logical_partition[index];
+			if (lp_descriptor->sp_id == lp_descriptor_other->sp_id) {
+				ERROR("Duplicate Partition ID Detected 0x%x\n", lp_descriptor->sp_id);
+				panic();
+			}
+		}
+	}
+}
diff --git a/include/common/bl_common.h b/include/common/bl_common.h
index e33840c9d..ac3e238a6 100644
--- a/include/common/bl_common.h
+++ b/include/common/bl_common.h
@@ -81,6 +81,10 @@
 #define __RODATA_END__			Load$$__RODATA_EPILOGUE__$$Base
 #define __RT_SVC_DESCS_START__		Load$$__RT_SVC_DESCS__$$Base
 #define __RT_SVC_DESCS_END__		Load$$__RT_SVC_DESCS__$$Limit
+#if SPMC_AT_EL3
+#define __EL3_LP_DESCS_START__		Load$$__EL3_LP_DESCS__$$Base
+#define __EL3_LP_DESCS_END__		Load$$__EL3_LP_DESCS__$$Limit
+#endif
 #define __RW_START__			Load$$LR$$LR_RW_DATA$$Base
 #define __RW_END__			Load$$LR$$LR_END$$Base
 #define __SPM_SHIM_EXCEPTIONS_START__	Load$$__SPM_SHIM_EXCEPTIONS__$$Base
diff --git a/include/common/bl_common.ld.h b/include/common/bl_common.ld.h
index 5147e3765..994910d27 100644
--- a/include/common/bl_common.ld.h
+++ b/include/common/bl_common.ld.h
@@ -39,6 +39,16 @@
 	KEEP(*(rt_svc_descs))				\
 	__RT_SVC_DESCS_END__ = .;
 
+#if SPMC_AT_EL3
+#define EL3_LP_DESCS					\
+	. = ALIGN(STRUCT_ALIGN);			\
+	__EL3_LP_DESCS_START__ = .;			\
+	KEEP(*(el3_lp_descs))				\
+	__EL3_LP_DESCS_END__ = .;
+#else
+#define EL3_LP_DESCS
+#endif
+
 #define PMF_SVC_DESCS					\
 	. = ALIGN(STRUCT_ALIGN);			\
 	__PMF_SVC_DESCS_START__ = .;			\
@@ -80,6 +90,7 @@
 #define BASE_XLAT_TABLE_BSS		BASE_XLAT_TABLE
 #endif
 
+
 #define RODATA_COMMON					\
 	RT_SVC_DESCS					\
 	FCONF_POPULATOR					\
@@ -87,7 +98,8 @@
 	PARSER_LIB_DESCS				\
 	CPU_OPS						\
 	GOT						\
-	BASE_XLAT_TABLE_RO
+	BASE_XLAT_TABLE_RO		\
+	EL3_LP_DESCS
 
 /*
  * .data must be placed at a lower address than the stacks if the stack
diff --git a/include/services/logical_sp.h b/include/services/logical_sp.h
new file mode 100644
index 000000000..1852a4036
--- /dev/null
+++ b/include/services/logical_sp.h
@@ -0,0 +1,66 @@
+/*
+ * Copyright (c) 2013-2019, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#ifndef El3_SP_H
+#define El3_SP_H
+
+
+#include <common/uuid.h>
+#include <common/bl_common.h>
+#include <lib/cassert.h>
+
+/*******************************************************************************
+ * Structure definition, typedefs & constants for the Logical SPs
+ ******************************************************************************/
+
+#define MAX_EL3_LP_DESCS_COUNT U(2)
+#define EL3_LP_ID_RANGE_START 0xC0000
+#define EL3_LP_ID_RANGE_END (EL3_LP_ID_RANGE_START + MAX_EL3_LP_DESCS_COUNT)
+
+typedef uint64_t (*direct_msg_handler)(uint32_t smc_fid, bool secure_origin, uint64_t x1, uint64_t x2,
+					uint64_t x3, uint64_t x4, void *cookie, void *handle, uint64_t flags);
+
+/* Prototype for logical partition initializing function */
+typedef int64_t (*ffa_partition_init_t)(void);
+
+/* Logical Partition Descriptor. */
+typedef struct el3_lp_desc {
+    ffa_partition_init_t init;
+	uint16_t sp_id;
+	uint32_t properties;
+	uint32_t uuid[4]; // Little Endian
+    direct_msg_handler direct_req;
+	const char *debug_name;
+} el3_lp_desc_t;
+
+/*
+ * Convenience macros to declare a logical partition descriptor
+ */
+#define DECLARE_LOGICAL_PARTITION(_name, _init, _sp_id, _uuid, _properties, \
+				  _direct_req)				    \
+	static const el3_lp_desc_t __partition_desc_ ## _name		    \
+		__section("el3_lp_descs") __used = {			    \
+			.debug_name = #_name,				    \
+			.init = (_init),				    \
+			.sp_id = (_sp_id),				    \
+			.uuid = _uuid,					    \
+			.properties = (_properties),			    \
+			.direct_req = (_direct_req),			    \
+		}
+
+
+/*******************************************************************************
+ * Function & variable prototypes
+ ******************************************************************************/
+void el3_sp_desc_init(void);
+uintptr_t handle_el3_sp(uint32_t smc_fid, void *cookie, void *handle,
+						unsigned int flags);
+IMPORT_SYM(uintptr_t, __EL3_LP_DESCS_START__,	EL3_LP_DESCS_START);
+IMPORT_SYM(uintptr_t, __EL3_LP_DESCS_END__,		EL3_LP_DESCS_END);
+
+#define EL3_LP_DESCS_NUM	((EL3_LP_DESCS_END - EL3_LP_DESCS_START)\
+					/ sizeof(el3_lp_desc_t))
+
+#endif /* El3_SP_H */
diff --git a/services/std_svc/spm/spmc/spmc_main.c b/services/std_svc/spm/spmc/spmc_main.c
index f608a6197..0b4d9356e 100644
--- a/services/std_svc/spm/spmc/spmc_main.c
+++ b/services/std_svc/spm/spmc/spmc_main.c
@@ -21,6 +21,12 @@
 #include <plat/common/platform.h>
 #include <services/ffa_svc.h>
 #include <services/spmc_svc.h>
+#include <services/spmd_svc.h>
+#include <services/logical_sp.h>
+
+
+#include <plat/arm/common/plat_arm.h>
+#include <platform_def.h>
 
 #include "spmc.h"
 #include "spm_shim_private.h"
@@ -334,6 +340,25 @@ static int find_and_prepare_sp_context(void)
 	return 0;
 }
 
+
+static int32_t logical_sp_init(void)
+{
+	uint64_t rc = 0;
+
+	el3_lp_desc_t *el3_lp_descs;
+	el3_lp_descs = get_el3_lp_array();
+
+	INFO("Logical Secure Partition init start.\n");
+	/* TODO: do some initialistion. */
+	for (int i = 0; i < EL3_LP_DESCS_NUM; i++) {
+		el3_lp_descs[i].init();
+	}
+
+	INFO("Secure Partition initialized.\n");
+
+	return rc;
+}
+
 static int32_t sp_init(void)
 {
 	uint64_t rc;
@@ -354,6 +379,47 @@ static int32_t sp_init(void)
 	return !rc;
 }
 
+
+/*******************************************************************************
+ * This function returns either forwards the request to the other world or returns
+ * with an ERET depending on the source of the call.
+ * Assuming if call is for a logical SP it has already been taken care of.
+ ******************************************************************************/
+
+static uint64_t spmc_smc_return(uint32_t smc_fid,
+ 				 bool secure_origin,
+				 uint64_t x1,
+				 uint64_t x2,
+				 uint64_t x3,
+				 uint64_t x4,
+				 void *handle,
+				 void *cookie,
+				 uint64_t flags) {
+
+	unsigned int cs;
+
+	cs = is_caller_secure(flags);
+
+	/* If the destination is in the normal world always go via the SPMD. */
+	if (ffa_is_normal_world_id(FFA_RECEIVER(x1))) {
+		return spmd_smc_handler(smc_fid, x1, x2, x3, x4, cookie, handle, flags);
+	}
+	/* If the caller is secure and we want to return to the secure world, ERET directly. */
+	else if (cs && ffa_is_secure_world_id(FFA_RECEIVER(x1))) {
+		SMC_RET5(handle, smc_fid, x1, x2, x3, x4);
+	}
+	/* If we originated in the normal world then switch contexts. */
+	else if (!cs && ffa_is_secure_world_id(FFA_RECEIVER(x1))) {
+		return ffa_smc_forward(smc_fid, secure_origin, x1, x2, x3, x4, cookie, handle, flags);
+	}
+	else {
+		/* Unknown State. */
+		panic();
+	}
+	/* Shouldn't be Reached */
+	return 0;
+}
+
 static uint64_t direct_req_smc_handler(uint32_t smc_fid,
 					   bool secure_origin,
 					   uint64_t x1,
@@ -364,17 +430,16 @@ static uint64_t direct_req_smc_handler(uint32_t smc_fid,
 				       void *handle,
 				       uint64_t flags)
 {
-	unsigned int ns;
 
-	ns = is_caller_non_secure(flags);
-	if (ns == SMC_FROM_SECURE) {
-		assert(handle == cm_get_context(SECURE));
-		return direct_req_secure_smc_handler(x1, x2, x3, x4, cookie,
-						     handle, flags);
-	} else {
-		assert(handle == cm_get_context(NON_SECURE));
-		return direct_req_non_secure_smc_handler(x1, x2, x3, x4, cookie,
-							 handle, flags);
+	el3_lp_desc_t *el3_lp_descs;
+	el3_lp_descs = get_el3_lp_array();
+
+	/* Handle is destined for a Logical Partition. */
+	uint16_t dst_id = FFA_RECEIVER(x1);
+	for (int i = 0; i < MAX_EL3_LP_DESCS_COUNT; i++) {
+		if (el3_lp_descs[i].sp_id == dst_id) {
+			return el3_lp_descs[i].direct_req(smc_fid, secure_origin, x1, x2, x3, x4, cookie, handle, flags);
+		}
 	}
 	return spmc_smc_return(smc_fid, secure_origin, x1, x2, x3, x4, handle, cookie, flags);
 }
@@ -414,6 +479,11 @@ int32_t spmc_setup(void)
 {
 	int32_t ret;
 
+	/* Setup logical SPs. */
+	logical_sp_init();
+
+	/* Perform physical SP setup. */
+
 	/* Disable MMU at EL1 (initialized by BL2) */
 	disable_mmu_icache_el1();
 
@@ -454,7 +524,6 @@ uint64_t spmc_smc_handler(uint32_t smc_fid,
 
 	case FFA_MSG_SEND_DIRECT_RESP_SMC64:
 		return direct_resp_smc_handler(smc_fid, secure_origin, x1, x2, x3, x4, cookie, handle, flags);
-		// spm_sp_synchronous_exit(&(spmc_sp_ctx[schedule_sp_index].sp_ctx), x4);
 
 	default:
 		WARN("Not Supported 0x%x FFA Request ID\n", smc_fid);