SPMD: secondary cores PM on and off SPD hooks relayed to SPMC

Define SPMD PM hooks for warm boot and off events. svc_on_finish handler
enters the SPMC at the entry point defined by the secondary EP register
service. The svc_off handler notifies the SPMC that a physical core is
being turned off through a notification message.

Signed-off-by: Olivier Deprez <olivier.deprez@arm.com>
Change-Id: I2609a75a0c6ffb9f6313fc09553be2b29a41de59

3 files changed, 94 insertions, 10 deletions

diff --git a/services/std_svc/spmd/spmd_main.c b/services/std_svc/spmd/spmd_main.c
index 30509574b..edcb5feb9 100644
--- a/services/std_svc/spmd/spmd_main.c
+++ b/services/std_svc/spmd/spmd_main.c
@@ -60,6 +60,14 @@ entry_point_info_t *spmd_spmc_ep_info_get(void)
 }
 
 /*******************************************************************************
+ * SPM Core ID getter.
+ ******************************************************************************/
+uint16_t spmd_spmc_id_get(void)
+{
+	return spmc_attrs.spmc_id;
+}
+
+/*******************************************************************************
  * Static function declaration.
  ******************************************************************************/
 static int32_t spmd_init(void);
@@ -378,9 +386,9 @@ static bool spmd_is_spmc_message(unsigned int ep)
 /******************************************************************************
  * spmd_handle_spmc_message
  *****************************************************************************/
-static int32_t spmd_handle_spmc_message(uint64_t msg, uint64_t parm1,
-					uint64_t parm2, uint64_t parm3,
-					uint64_t parm4)
+static int spmd_handle_spmc_message(unsigned long long msg,
+		unsigned long long parm1, unsigned long long parm2,
+		unsigned long long parm3, unsigned long long parm4)
 {
 	VERBOSE("%s %llx %llx %llx %llx %llx\n", __func__,
 		msg, parm1, parm2, parm3, parm4);
diff --git a/services/std_svc/spmd/spmd_pm.c b/services/std_svc/spmd/spmd_pm.c
index 13c638c38..390419673 100644
--- a/services/std_svc/spmd/spmd_pm.c
+++ b/services/std_svc/spmd/spmd_pm.c
@@ -6,6 +6,7 @@
 
 #include <assert.h>
 #include <errno.h>
+#include <lib/el3_runtime/context_mgmt.h>
 #include "spmd_private.h"
 
 struct spmd_pm_secondary_ep_t {
@@ -17,10 +18,25 @@ struct spmd_pm_secondary_ep_t {
 static struct spmd_pm_secondary_ep_t spmd_pm_secondary_ep[PLATFORM_CORE_COUNT];
 
 /*******************************************************************************
+ * spmd_build_spmc_message
+ *
+ * Builds an SPMD to SPMC direct message request.
+ ******************************************************************************/
+static void spmd_build_spmc_message(gp_regs_t *gpregs, unsigned long long message)
+{
+	write_ctx_reg(gpregs, CTX_GPREG_X0, FFA_MSG_SEND_DIRECT_REQ_SMC32);
+	write_ctx_reg(gpregs, CTX_GPREG_X1,
+		(SPMD_DIRECT_MSG_ENDPOINT_ID << FFA_DIRECT_MSG_SOURCE_SHIFT) |
+		spmd_spmc_id_get());
+	write_ctx_reg(gpregs, CTX_GPREG_X2, FFA_PARAM_MBZ);
+	write_ctx_reg(gpregs, CTX_GPREG_X3, message);
+}
+
+/*******************************************************************************
  * spmd_pm_secondary_core_set_ep
  ******************************************************************************/
-int32_t spmd_pm_secondary_core_set_ep(uint64_t mpidr, uintptr_t entry_point,
-				      uint64_t context)
+int spmd_pm_secondary_core_set_ep(unsigned long long mpidr,
+		uintptr_t entry_point, unsigned long long context)
 {
 	int id = plat_core_pos_by_mpidr(mpidr);
 
@@ -63,21 +79,77 @@ int32_t spmd_pm_secondary_core_set_ep(uint64_t mpidr, uintptr_t entry_point,
  ******************************************************************************/
 static void spmd_cpu_on_finish_handler(u_register_t unused)
 {
-	unsigned int linear_id = plat_my_core_pos();
+	entry_point_info_t *spmc_ep_info = spmd_spmc_ep_info_get();
 	spmd_spm_core_context_t *ctx = spmd_get_context();
+	unsigned int linear_id = plat_my_core_pos();
 	int rc;
 
+	assert(ctx != NULL);
 	assert(ctx->state != SPMC_STATE_ON);
+	assert(spmc_ep_info != NULL);
+
+	/*
+	 * TODO: this might require locking the spmc_ep_info structure,
+	 * or provisioning one structure per cpu
+	 */
+	if (spmd_pm_secondary_ep[linear_id].entry_point == 0) {
+		goto exit;
+	}
+
+	spmc_ep_info->pc = spmd_pm_secondary_ep[linear_id].entry_point;
+	cm_setup_context(&ctx->cpu_ctx, spmc_ep_info);
+	write_ctx_reg(get_gpregs_ctx(&ctx->cpu_ctx), CTX_GPREG_X0,
+		      spmd_pm_secondary_ep[linear_id].context);
+
+	/* Mark CPU as initiating ON operation */
+	ctx->state = SPMC_STATE_ON_PENDING;
 
 	rc = spmd_spm_core_sync_entry(ctx);
 	if (rc != 0) {
-		ERROR("SPMC initialisation failed (%d) on CPU%u\n", rc,
-		      linear_id);
+		ERROR("%s failed failed (%d) on CPU%u\n", __func__, rc,
+			linear_id);
 		ctx->state = SPMC_STATE_OFF;
 		return;
 	}
 
+exit:
 	ctx->state = SPMC_STATE_ON;
+
+	VERBOSE("CPU %u on!\n", linear_id);
+}
+
+/*******************************************************************************
+ * spmd_cpu_off_handler
+ ******************************************************************************/
+static int32_t spmd_cpu_off_handler(u_register_t unused)
+{
+	spmd_spm_core_context_t *ctx = spmd_get_context();
+	unsigned int linear_id = plat_my_core_pos();
+	int32_t rc;
+
+	assert(ctx != NULL);
+	assert(ctx->state != SPMC_STATE_OFF);
+
+	if (spmd_pm_secondary_ep[linear_id].entry_point == 0) {
+		goto exit;
+	}
+
+	/* Build an SPMD to SPMC direct message request. */
+	spmd_build_spmc_message(get_gpregs_ctx(&ctx->cpu_ctx), PSCI_CPU_OFF);
+
+	rc = spmd_spm_core_sync_entry(ctx);
+	if (rc != 0) {
+		ERROR("%s failed (%d) on CPU%u\n", __func__, rc, linear_id);
+	}
+
+	/* TODO expect FFA_DIRECT_MSG_RESP returned from SPMC */
+
+exit:
+	ctx->state = SPMC_STATE_OFF;
+
+	VERBOSE("CPU %u off!\n", linear_id);
+
+	return 0;
 }
 
 /*******************************************************************************
@@ -86,4 +158,5 @@ static void spmd_cpu_on_finish_handler(u_register_t unused)
  ******************************************************************************/
 const spd_pm_ops_t spmd_pm = {
 	.svc_on_finish = spmd_cpu_on_finish_handler,
+	.svc_off = spmd_cpu_off_handler
 };
diff --git a/services/std_svc/spmd/spmd_private.h b/services/std_svc/spmd/spmd_private.h
index 0d78f53a4..a9dc1d311 100644
--- a/services/std_svc/spmd/spmd_private.h
+++ b/services/std_svc/spmd/spmd_private.h
@@ -78,11 +78,14 @@ extern const spd_pm_ops_t spmd_pm;
 /* SPMC entry point information helper */
 entry_point_info_t *spmd_spmc_ep_info_get(void);
 
+/* SPMC ID getter */
+uint16_t spmd_spmc_id_get(void);
+
 /* SPMC context on current CPU get helper */
 spmd_spm_core_context_t *spmd_get_context(void);
 
-int32_t spmd_pm_secondary_core_set_ep(uint64_t mpidr, uintptr_t entry_point,
-				      uint64_t context);
+int spmd_pm_secondary_core_set_ep(unsigned long long mpidr,
+		uintptr_t entry_point, unsigned long long context);
 bool spmd_check_address_in_binary_image(uint64_t address);
 
 #endif /* __ASSEMBLER__ */