STMM: Temporarily move STMM code to LP

This patch moves the STMM specific code into
a separate STMM logical partition file. The future
aim is to add support for the required functionality
direct to the SPMC in the form of FFA ABIs instead
of requiring this functionality to be separated.

Refactor the left behind code to be more generic and
suitable for non STMM partitions as well.

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

3 files changed, 304 insertions, 205 deletions

diff --git a/services/std_svc/spm/common/logical_mm_sp.c b/services/std_svc/spm/common/logical_mm_sp.c
new file mode 100644
index 000000000..3798e62be
--- /dev/null
+++ b/services/std_svc/spm/common/logical_mm_sp.c
@@ -0,0 +1,272 @@
+#include <common/debug.h>
+#include <smccc_helpers.h>
+#include <services/ffa_svc.h>
+#include <services/logical_sp.h>
+
+#include <arch_helpers.h>
+#include <assert.h>
+#include <errno.h>
+#include <libfdt.h>
+
+#include <bl31/bl31.h>
+#include <bl31/ehf.h>
+#include <common/debug.h>
+#include <common/fdt_wrappers.h>
+#include <common/runtime_svc.h>
+#include <dt-bindings/memory/memory.h>
+#include <lib/el3_runtime/context_mgmt.h>
+#include <lib/smccc.h>
+#include <lib/utils.h>
+#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>
+
+
+#define LP_PARTITION_ID 0xC001
+#define LP_UUID {0x0, 0x0, 0x0, 0x2}
+
+static int64_t sp_init(void) {
+        // TODO: Do some initialisation.
+        INFO("LSP: Init function called.\n");
+        return 0;
+}
+
+
+static int get_sp_index(uint16_t sp_id, unsigned int *sp_index)
+{
+	int i;
+
+	for (i = 0; i < SECURE_PARTITION_COUNT; i++) {
+		if (sp_id == spmc_sp_ctx[i].sp_id) {
+			*sp_index = i;
+			return i;
+		}
+	}
+
+	return -EINVAL;
+}
+
+/*******************************************************************************
+ * Function to perform a call to a Secure Partition.
+ * TODO: Update to unwind approach if required.
+ ******************************************************************************/
+static uint64_t spmc_sp_call(unsigned int sp_index, uint32_t smc_fid,
+			     uint64_t comm_buffer_address,
+			     uint64_t comm_size, uint64_t core_pos)
+{
+	uint64_t rc;
+	sp_context_t *sp_ptr = &(spmc_sp_ctx[sp_index].sp_ctx);
+
+	/* Set values for registers on SP entry */
+	cpu_context_t *cpu_ctx = &(sp_ptr->cpu_ctx);
+
+	write_ctx_reg(get_gpregs_ctx(cpu_ctx), CTX_GPREG_X0, smc_fid);
+	write_ctx_reg(get_gpregs_ctx(cpu_ctx), CTX_GPREG_X1, 0);
+	write_ctx_reg(get_gpregs_ctx(cpu_ctx), CTX_GPREG_X2, 0);
+	write_ctx_reg(get_gpregs_ctx(cpu_ctx), CTX_GPREG_X3, comm_buffer_address);
+	write_ctx_reg(get_gpregs_ctx(cpu_ctx), CTX_GPREG_X4, comm_size);
+	write_ctx_reg(get_gpregs_ctx(cpu_ctx), CTX_GPREG_X5, 0);
+	write_ctx_reg(get_gpregs_ctx(cpu_ctx), CTX_GPREG_X6, core_pos);
+
+	/* Jump to the Secure Partition. */
+	rc = spm_sp_synchronous_entry(sp_ptr);
+
+	return rc;
+}
+
+/*******************************************************************************
+ * Return FFA_ERROR with specified error code
+ ******************************************************************************/
+static uint64_t spmc_ffa_error_return(void *handle, int error_code)
+{
+	SMC_RET8(handle, FFA_ERROR,
+		 FFA_TARGET_INFO_MBZ, error_code,
+		 FFA_PARAM_MBZ, FFA_PARAM_MBZ, FFA_PARAM_MBZ,
+		 FFA_PARAM_MBZ, FFA_PARAM_MBZ);
+}
+
+/*******************************************************************************
+ * MM_INTERFACE handler
+ ******************************************************************************/
+static uint64_t spmc_mm_interface_handler(unsigned int sp_index,
+					  uint32_t smc_fid,
+					  uint64_t mm_cookie,
+					  uint64_t comm_buffer_address,
+					  uint64_t comm_size_address,
+					  void *handle)
+{
+	uint64_t rc;
+
+	/*
+	 * The current secure partition design mandates
+	 * - at any point, only a single core can be
+	 *   executing in the secure partition.
+	 * - a core cannot be preempted by an interrupt
+	 *   while executing in secure partition.
+	 * Raise the running priority of the core to the
+	 * interrupt level configured for secure partition
+	 * so as to block any interrupt from preempting this
+	 * core.
+	 */
+	ehf_activate_priority(PLAT_SP_PRI);
+
+	/* Save the Normal world context */
+	cm_el1_sysregs_context_save(NON_SECURE);
+
+	rc = spmc_sp_call(sp_index, smc_fid, comm_buffer_address,
+			  comm_size_address, plat_my_core_pos());
+
+	/* Restore non-secure state */
+	cm_el1_sysregs_context_restore(NON_SECURE);
+	cm_set_next_eret_context(NON_SECURE);
+
+	/*
+	 * Exited from secure partition. This core can take
+	 * interrupts now.
+	 */
+	ehf_deactivate_priority(PLAT_SP_PRI);
+
+	if (rc == 0)
+		SMC_RET2(handle, FFA_SUCCESS_SMC64, 0x7);
+
+	return spmc_ffa_error_return(handle,
+				     FFA_ERROR_NOT_SUPPORTED);
+}
+
+static uint64_t direct_req_secure_smc_handler(uint64_t x1, uint64_t x2,
+					      uint64_t x3, uint64_t x4,
+					      void *cookie, void *handle,
+					      uint64_t flags)
+{
+	uint64_t rc;
+	uint16_t sp_id;
+	unsigned int sp_index;
+	sp_context_t *sp_ctx;
+
+	/* Make next ERET jump to S-EL0 instead of S-EL1. */
+	cm_set_elr_spsr_el3(SECURE, read_elr_el1(), read_spsr_el1());
+
+	switch (x3) {
+	case SP_MEMORY_ATTRIBUTES_GET_AARCH64:
+		INFO("Received SP_MEMORY_ATTRIBUTES_GET_AARCH64 request\n");
+
+		sp_id = STMM_SP_ID;
+		if (get_sp_index(sp_id, &sp_index) < 0) {
+			WARN("Not found the StMM Secure Partition.\n");
+			break;
+		}
+
+		sp_ctx = &(spmc_sp_ctx[sp_index].sp_ctx);
+
+		if (sp_ctx->state != SP_STATE_RESET) {
+			WARN("SP_MEMORY_ATTRIBUTES_GET_AARCH64 is available at boot time\n");
+			return spmc_ffa_error_return(handle,
+						     FFA_ERROR_NOT_SUPPORTED);
+		}
+
+		rc = spm_memory_attributes_get_smc_handler(sp_ctx, x4);
+		if (rc < 0)
+			return spmc_ffa_error_return(handle,
+						     FFA_ERROR_INVALID_PARAMETER);
+
+		SMC_RET4(handle, FFA_MSG_SEND_DIRECT_RESP_SMC64, 0x0, 0x0, rc);
+
+	case SP_MEMORY_ATTRIBUTES_SET_AARCH64:
+		INFO("Received SP_MEMORY_ATTRIBUTES_SET_AARCH64 request\n");
+
+		sp_id = STMM_SP_ID;
+		if (get_sp_index(sp_id, &sp_index) < 0) {
+			WARN("Not found the StMM Secure Partition.\n");
+			break;
+		}
+		sp_ctx = &(spmc_sp_ctx[sp_index].sp_ctx);
+
+		if (sp_ctx->state != SP_STATE_RESET) {
+			WARN("SP_MEMORY_ATTRIBUTES_SET_AARCH64 is available at boot time\n");
+			return spmc_ffa_error_return(handle,
+						     FFA_ERROR_NOT_SUPPORTED);
+		}
+
+		rc =  spm_memory_attributes_set_smc_handler(sp_ctx, x4,
+					SMC_GET_GP(handle, CTX_GPREG_X5),
+					SMC_GET_GP(handle, CTX_GPREG_X6));
+
+		if (rc < 0)
+			return spmc_ffa_error_return(handle,
+						     FFA_ERROR_INVALID_PARAMETER);
+
+		SMC_RET4(handle, FFA_MSG_SEND_DIRECT_RESP_SMC64, 0x0, 0x0, rc);
+
+	default:
+		WARN("Not supported direct request handling for ID=0x%llx\n", x3);
+		break;
+	}
+
+	return spmc_ffa_error_return(handle,
+			FFA_ERROR_NOT_SUPPORTED);
+}
+
+static uint64_t direct_req_non_secure_smc_handler(uint64_t x1,
+						  uint64_t x2,
+						  uint64_t x3,
+						  uint64_t x4,
+						  void *cookie,
+						  void *handle,
+						  uint64_t flags)
+{
+	uint16_t dst_id;
+	unsigned int sp_index;
+
+	switch (x3) {
+	case MM_INTERFACE_ID_AARCH32:
+	case MM_INTERFACE_ID_AARCH64:
+		INFO("MM interface id\n");
+		dst_id = STMM_SP_ID;
+		if (get_sp_index(dst_id, &sp_index) < 0) {
+			WARN("Not found the StMM Secure Partition.\n");
+			break;
+		}
+		return spmc_mm_interface_handler(sp_index, x3, x4,
+				SMC_GET_GP(handle, CTX_GPREG_X5),
+				SMC_GET_GP(handle, CTX_GPREG_X6), handle);
+	default:
+		WARN("Not supported direct request handling for ID=0x%llx\n", x3);
+		break;
+	}
+
+	return spmc_ffa_error_return(handle,
+			FFA_ERROR_NOT_SUPPORTED);
+}
+
+
+
+static uint64_t handle_ffa_direct_request(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) {
+        uint64_t ret;
+
+        if (secure_origin) {
+            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);
+        }
+}
+
+/* Register logical partition  */
+DECLARE_LOGICAL_PARTITION(
+        stmm_lsp,
+        sp_init,                   // Init Function
+        LP_PARTITION_ID,           // FFA Partition ID
+        LP_UUID,                   // UUID
+        0x1,                       // Partition Properties. Can only receive direct messages.
+        handle_ffa_direct_request  // Callback for direct requests.
+);
diff --git a/services/std_svc/spm/common/spm.mk b/services/std_svc/spm/common/spm.mk
index 40ee95d0d..1f2a1beb3 100644
--- a/services/std_svc/spm/common/spm.mk
+++ b/services/std_svc/spm/common/spm.mk
@@ -16,6 +16,7 @@ SPM_SOURCES	:=	$(addprefix services/std_svc/spm/common/,\
 			spm_common.c				\
 			spm_setup.c				\
 			spm_xlat.c)
+			# logical_mm_sp.c)
 
 
 # Let the top-level Makefile know that we intend to include a BL32 image
diff --git a/services/std_svc/spm/spmc/spmc_main.c b/services/std_svc/spm/spmc/spmc_main.c
index 9c697b4f1..f608a6197 100644
--- a/services/std_svc/spm/spmc/spmc_main.c
+++ b/services/std_svc/spm/spmc/spmc_main.c
@@ -31,20 +31,14 @@ static unsigned int schedule_sp_index;
 
 static void *spmc_manifest;
 
-static int get_sp_index(uint16_t sp_id, unsigned int *sp_index)
-{
-	int i;
+el3_lp_desc_t* get_el3_lp_array(void) {
+	el3_lp_desc_t *el3_lp_descs;
+	el3_lp_descs = (el3_lp_desc_t *) EL3_LP_DESCS_START;
 
-	for (i = 0; i < SECURE_PARTITION_COUNT; i++) {
-		if (sp_id == spmc_sp_ctx[i].sp_id) {
-			*sp_index = i;
-			return i;
-		}
-	}
-
-	return -EINVAL;
+	return el3_lp_descs;
 }
 
+
 /*******************************************************************************
  * This function will parse the Secure Partition Manifest for fetching seccure
  * partition specific memory region details. It will find base address, size,
@@ -360,198 +354,9 @@ static int32_t sp_init(void)
 	return !rc;
 }
 
-/*******************************************************************************
- * Function to perform a call to a Secure Partition.
- ******************************************************************************/
-static uint64_t spmc_sp_call(unsigned int sp_index, uint32_t smc_fid,
-			     uint64_t comm_buffer_address,
-			     uint64_t comm_size, uint64_t core_pos)
-{
-	uint64_t rc;
-	sp_context_t *sp_ptr = &(spmc_sp_ctx[sp_index].sp_ctx);
-
-	/* Set values for registers on SP entry */
-	cpu_context_t *cpu_ctx = &(sp_ptr->cpu_ctx);
-
-	write_ctx_reg(get_gpregs_ctx(cpu_ctx), CTX_GPREG_X0, smc_fid);
-	write_ctx_reg(get_gpregs_ctx(cpu_ctx), CTX_GPREG_X1, 0);
-	write_ctx_reg(get_gpregs_ctx(cpu_ctx), CTX_GPREG_X2, 0);
-	write_ctx_reg(get_gpregs_ctx(cpu_ctx), CTX_GPREG_X3, comm_buffer_address);
-	write_ctx_reg(get_gpregs_ctx(cpu_ctx), CTX_GPREG_X4, comm_size);
-	write_ctx_reg(get_gpregs_ctx(cpu_ctx), CTX_GPREG_X5, 0);
-	write_ctx_reg(get_gpregs_ctx(cpu_ctx), CTX_GPREG_X6, core_pos);
-
-	/* Jump to the Secure Partition. */
-	rc = spm_sp_synchronous_entry(sp_ptr);
-
-	return rc;
-}
-
-/*******************************************************************************
- * Return FFA_ERROR with specified error code
- ******************************************************************************/
-static uint64_t spmc_ffa_error_return(void *handle, int error_code)
-{
-	SMC_RET8(handle, FFA_ERROR,
-		 FFA_TARGET_INFO_MBZ, error_code,
-		 FFA_PARAM_MBZ, FFA_PARAM_MBZ, FFA_PARAM_MBZ,
-		 FFA_PARAM_MBZ, FFA_PARAM_MBZ);
-}
-
-/*******************************************************************************
- * MM_INTERFACE handler
- ******************************************************************************/
-static uint64_t spmc_mm_interface_handler(unsigned int sp_index,
-					  uint32_t smc_fid,
-					  uint64_t mm_cookie,
-					  uint64_t comm_buffer_address,
-					  uint64_t comm_size_address,
-					  void *handle)
-{
-	uint64_t rc;
-
-	/*
-	 * The current secure partition design mandates
-	 * - at any point, only a single core can be
-	 *   executing in the secure partition.
-	 * - a core cannot be preempted by an interrupt
-	 *   while executing in secure partition.
-	 * Raise the running priority of the core to the
-	 * interrupt level configured for secure partition
-	 * so as to block any interrupt from preempting this
-	 * core.
-	 */
-	ehf_activate_priority(PLAT_SP_PRI);
-
-	/* Save the Normal world context */
-	cm_el1_sysregs_context_save(NON_SECURE);
-
-	rc = spmc_sp_call(sp_index, smc_fid, comm_buffer_address,
-			  comm_size_address, plat_my_core_pos());
-
-	/* Restore non-secure state */
-	cm_el1_sysregs_context_restore(NON_SECURE);
-	cm_set_next_eret_context(NON_SECURE);
-
-	/*
-	 * Exited from secure partition. This core can take
-	 * interrupts now.
-	 */
-	ehf_deactivate_priority(PLAT_SP_PRI);
-
-	if (rc == 0)
-		SMC_RET2(handle, FFA_SUCCESS_SMC64, 0x7);
-
-	return spmc_ffa_error_return(handle,
-				     FFA_ERROR_NOT_SUPPORTED);
-}
-
-static uint64_t direct_req_secure_smc_handler(uint64_t x1, uint64_t x2,
-					      uint64_t x3, uint64_t x4,
-					      void *cookie, void *handle,
-					      uint64_t flags)
-{
-	uint64_t rc;
-	uint16_t sp_id;
-	unsigned int sp_index;
-	sp_context_t *sp_ctx;
-
-	/* Make next ERET jump to S-EL0 instead of S-EL1. */
-	cm_set_elr_spsr_el3(SECURE, read_elr_el1(), read_spsr_el1());
-
-	switch (x3) {
-	case SP_MEMORY_ATTRIBUTES_GET_AARCH64:
-		INFO("Received SP_MEMORY_ATTRIBUTES_GET_AARCH64 request\n");
-
-		sp_id = STMM_SP_ID;
-		if (get_sp_index(sp_id, &sp_index) < 0) {
-			WARN("Not found the StMM Secure Partition.\n");
-			break;
-		}
-
-		sp_ctx = &(spmc_sp_ctx[sp_index].sp_ctx);
-
-		if (sp_ctx->state != SP_STATE_RESET) {
-			WARN("SP_MEMORY_ATTRIBUTES_GET_AARCH64 is available at boot time\n");
-			return spmc_ffa_error_return(handle,
-						     FFA_ERROR_NOT_SUPPORTED);
-		}
-
-		rc = spm_memory_attributes_get_smc_handler(sp_ctx, x4);
-		if (rc < 0)
-			return spmc_ffa_error_return(handle,
-						     FFA_ERROR_INVALID_PARAMETER);
-
-		SMC_RET4(handle, FFA_MSG_SEND_DIRECT_RESP_SMC64, 0x0, 0x0, rc);
-
-	case SP_MEMORY_ATTRIBUTES_SET_AARCH64:
-		INFO("Received SP_MEMORY_ATTRIBUTES_SET_AARCH64 request\n");
-
-		sp_id = STMM_SP_ID;
-		if (get_sp_index(sp_id, &sp_index) < 0) {
-			WARN("Not found the StMM Secure Partition.\n");
-			break;
-		}
-		sp_ctx = &(spmc_sp_ctx[sp_index].sp_ctx);
-
-		if (sp_ctx->state != SP_STATE_RESET) {
-			WARN("SP_MEMORY_ATTRIBUTES_SET_AARCH64 is available at boot time\n");
-			return spmc_ffa_error_return(handle,
-						     FFA_ERROR_NOT_SUPPORTED);
-		}
-
-		rc =  spm_memory_attributes_set_smc_handler(sp_ctx, x4,
-					SMC_GET_GP(handle, CTX_GPREG_X5),
-					SMC_GET_GP(handle, CTX_GPREG_X6));
-
-		if (rc < 0)
-			return spmc_ffa_error_return(handle,
-						     FFA_ERROR_INVALID_PARAMETER);
-
-		SMC_RET4(handle, FFA_MSG_SEND_DIRECT_RESP_SMC64, 0x0, 0x0, rc);
-
-	default:
-		WARN("Not supported direct request handling for ID=0x%llx\n", x3);
-		break;
-	}
-
-	return spmc_ffa_error_return(handle,
-			FFA_ERROR_NOT_SUPPORTED);
-}
-
-static uint64_t direct_req_non_secure_smc_handler(uint64_t x1,
-						  uint64_t x2,
-						  uint64_t x3,
-						  uint64_t x4,
-						  void *cookie,
-						  void *handle,
-						  uint64_t flags)
-{
-	uint16_t dst_id;
-	unsigned int sp_index;
-
-	switch (x3) {
-	case MM_INTERFACE_ID_AARCH32:
-	case MM_INTERFACE_ID_AARCH64:
-		INFO("MM interface id\n");
-		dst_id = STMM_SP_ID;
-		if (get_sp_index(dst_id, &sp_index) < 0) {
-			WARN("Not found the StMM Secure Partition.\n");
-			break;
-		}
-		return spmc_mm_interface_handler(sp_index, x3, x4,
-				SMC_GET_GP(handle, CTX_GPREG_X5),
-				SMC_GET_GP(handle, CTX_GPREG_X6), handle);
-	default:
-		WARN("Not supported direct request handling for ID=0x%llx\n", x3);
-		break;
-	}
-
-	return spmc_ffa_error_return(handle,
-			FFA_ERROR_NOT_SUPPORTED);
-}
-
-static uint64_t direct_req_smc_handler(uint64_t x1,
+static uint64_t direct_req_smc_handler(uint32_t smc_fid,
+					   bool secure_origin,
+					   uint64_t x1,
 				       uint64_t x2,
 				       uint64_t x3,
 				       uint64_t x4,
@@ -571,8 +376,27 @@ static uint64_t direct_req_smc_handler(uint64_t x1,
 		return direct_req_non_secure_smc_handler(x1, x2, x3, x4, cookie,
 							 handle, flags);
 	}
+	return spmc_smc_return(smc_fid, secure_origin, x1, x2, x3, x4, handle, cookie, flags);
 }
 
+static uint64_t direct_resp_smc_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)
+{
+	/* Don't need to check LPs as they cannot send their own requests. */
+
+	/* TODO: Update state tracking? Clear on-going direct request / current state to idle.  */
+
+	return spmc_smc_return(smc_fid, secure_origin, x1, x2, x3, x4, cookie, handle, flags);
+}
+
+
 void spmc_set_config_addr(uintptr_t soc_fw_config)
 {
 	spmc_manifest = (void *)soc_fw_config;
@@ -625,10 +449,12 @@ uint64_t spmc_smc_handler(uint32_t smc_fid,
 {
 	switch (smc_fid) {
 	case FFA_MSG_SEND_DIRECT_REQ_SMC64:
-		return direct_req_smc_handler(x1, x2, x3, x4, cookie, handle, flags);
+		return direct_req_smc_handler(smc_fid, secure_origin, x1, x2, x3, x4, cookie, handle, flags);
+
 
 	case FFA_MSG_SEND_DIRECT_RESP_SMC64:
-		spm_sp_synchronous_exit(&(spmc_sp_ctx[schedule_sp_index].sp_ctx), x4);
+		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);