Add support for FF-A power mgmt. messages in the EL3 SPMC for a S-EL1 SP

This patch adds support for forwarding the following PSCI messages received by
the SPMC at EL3 to the S-EL1 SP.

1. A PSCI CPU_OFF message in response to a cpu hot unplug request from the OS
2. A message to indicate warm boot of a cpu in response to a cpu hot plug
   request from the OS

This patch also implements the FFA_SECONDARY_EP_REGISTER function to enable the
SP specify its secondary entrypoint.

Signed-off-by: Achin Gupta <achin.gupta@arm.com>
Change-Id: I375d0655b2c6fc27445facc39213d1d0678557f4

5 files changed, 209 insertions, 4 deletions

diff --git a/include/services/ffa_svc.h b/include/services/ffa_svc.h
index d3e5f4aa0..74fef6bf1 100644
--- a/include/services/ffa_svc.h
+++ b/include/services/ffa_svc.h
@@ -73,6 +73,21 @@
 		 (OEN_STD_START << FUNCID_OEN_SHIFT) |	\
 		 ((func_num) << FUNCID_NUM_SHIFT))
 
+/*
+ * Defines for power management framework messages exchanged using direct
+ * messages between the SPMC and SP.
+ */
+#define FFA_DIRECT_FRAMEWORK_MSG_SHIFT	31
+#define FFA_DIRECT_FRAMEWORK_MSG_MASK	(1UL << FFA_DIRECT_FRAMEWORK_MSG_SHIFT)
+
+#define FFA_PM_MSG_MASK			0xFF
+#define FFA_PM_MSG_PSCI_REQ		0x0
+#define FFA_PM_MSG_WB_REQ		0x1		/* Warm boot request */
+#define FFA_PM_MSG_PM_RESP		0x2		/* Response to a PSCI or warmboot request */
+
+#define FFA_WB_TYPE_S2RAM		0
+#define FFA_WB_TYPE_NOTS2RAM		1
+
 /* FFA function numbers */
 #define FFA_FNUM_ERROR				U(0x60)
 #define FFA_FNUM_SUCCESS			U(0x61)
diff --git a/plat/arm/common/arm_bl31_setup.c b/plat/arm/common/arm_bl31_setup.c
index d4ba6da8b..01051e73a 100644
--- a/plat/arm/common/arm_bl31_setup.c
+++ b/plat/arm/common/arm_bl31_setup.c
@@ -189,10 +189,6 @@ void __init arm_bl31_early_platform_setup(void *from_bl2, uintptr_t soc_fw_confi
 	if (bl33_image_ep_info.pc == 0U)
 		panic();
 
-#if SPMC_AT_EL3
-	spmc_set_config_addr(soc_fw_config);
-#endif
-
 #endif /* RESET_TO_BL31 */
 
 # if ARM_LINUX_KERNEL_AS_BL33
diff --git a/services/std_svc/spm/spmc/spmc.h b/services/std_svc/spm/spmc/spmc.h
index fc3d6f851..04908bcc9 100644
--- a/services/std_svc/spm/spmc/spmc.h
+++ b/services/std_svc/spm/spmc/spmc.h
@@ -8,6 +8,8 @@
 #define SPMC_H
 
 #include <stdint.h>
+#include <lib/psci/psci.h>
+
 
 #include "spm_common.h"
 
diff --git a/services/std_svc/spm/spmc/spmc_main.c b/services/std_svc/spm/spmc/spmc_main.c
index 187854530..8b56d73a8 100644
--- a/services/std_svc/spm/spmc/spmc_main.c
+++ b/services/std_svc/spm/spmc/spmc_main.c
@@ -491,6 +491,7 @@ static uint64_t ffa_features_handler(uint32_t smc_fid,
 		case FFA_ERROR:
 		case FFA_SUCCESS_SMC32:
 		case FFA_SUCCESS_SMC64:
+		case FFA_SPM_ID_GET:
 		case FFA_ID_GET:
 		case FFA_FEATURES:
 		case FFA_VERSION:
@@ -1248,6 +1249,9 @@ int32_t spmc_setup(void)
 		return ret;
 	}
 
+	/* Register power management hooks with PSCI */
+	psci_register_spd_pm_hook(&spmc_pm);
+
 	/* Register init function for deferred init.  */
 	bl31_register_bl32_init(&sp_init);
 
@@ -1280,6 +1284,9 @@ uint64_t spmc_smc_handler(uint32_t smc_fid,
 	case FFA_VERSION:
 		return ffa_version_handler(smc_fid, secure_origin, x1, x2, x3, x4, cookie, handle, flags);
 
+	case FFA_SECONDARY_EP_REGISTER_SMC64:
+		return ffa_sec_ep_register_handler(smc_fid, secure_origin, x1, x2, x3, x4, cookie, handle, flags);
+
 	case FFA_MSG_SEND_DIRECT_REQ_SMC32:
 	case FFA_MSG_SEND_DIRECT_REQ_SMC64:
 		return direct_req_smc_handler(smc_fid, secure_origin, x1, x2, x3, x4, cookie, handle, flags);
diff --git a/services/std_svc/spm/spmc/spmc_pm.c b/services/std_svc/spm/spmc/spmc_pm.c
new file mode 100644
index 000000000..b51805150
--- /dev/null
+++ b/services/std_svc/spm/spmc/spmc_pm.c
@@ -0,0 +1,185 @@
+/*
+ * Copyright (c) 2020-2021, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <services/ffa_svc.h>
+#include <lib/el3_runtime/context_mgmt.h>
+#include <lib/spinlock.h>
+#include <platform_def.h>
+#include <plat/common/common_def.h>
+#include <plat/common/platform.h>
+
+#include "spmc.h"
+
+/*******************************************************************************
+ * spmc_build_pm_message
+ *
+ * Builds an SPMC to SP direct message request.
+ ******************************************************************************/
+static void spmc_build_pm_message(gp_regs_t *gpregs,
+				  unsigned long long message,
+				  uint8_t pm_msg_type,
+				  uint16_t sp_id)
+{
+	write_ctx_reg(gpregs, CTX_GPREG_X0, FFA_MSG_SEND_DIRECT_REQ_SMC32);
+	write_ctx_reg(gpregs, CTX_GPREG_X1,
+		      (FFA_SPMC_ID << FFA_DIRECT_MSG_SOURCE_SHIFT) |
+		      sp_id);
+	write_ctx_reg(gpregs, CTX_GPREG_X2, FFA_DIRECT_FRAMEWORK_MSG_MASK |
+		      (pm_msg_type & FFA_PM_MSG_MASK));
+	write_ctx_reg(gpregs, CTX_GPREG_X3, message);
+}
+
+/*******************************************************************************
+ * This CPU has been turned on. Enter the SP to initialise S-EL1.
+ ******************************************************************************/
+static void spmc_cpu_on_finish_handler(u_register_t unused)
+{
+	sp_desc_t *sp = spmc_get_current_sp_ctx();
+	sp_exec_ctx_t *ec;
+	unsigned int linear_id = plat_my_core_pos();
+	entry_point_info_t sec_ec_ep_info = {0};
+	uint64_t rc;
+
+	/* Sanity check for a NULL pointer dereference */
+	assert (NULL != sp);
+
+	/* Do nothing in case of a S-EL0 SP */
+	if (sp-> runtime_el == EL0)
+		return;
+
+	/* Initialize entry point information for the SP */
+	SET_PARAM_HEAD(&sec_ec_ep_info, PARAM_EP, VERSION_1, SECURE | EP_ST_ENABLE);
+
+	/*
+	 * Check if the primary execution context registered an entry point else
+	 * bail out early.
+	 * TODO: Add support for boot reason in manifest to allow jumping to
+	 * entrypoint into the primary execution context.
+	 */
+	spin_lock(&sp->secondary_ep_lock);
+	if (0 == sp->secondary_ep) {
+		WARN("%s: No secondary ep on core%u \n", __func__, linear_id);
+		return;
+	}
+
+	sec_ec_ep_info.pc = sp->secondary_ep;
+	spin_unlock(&sp->secondary_ep_lock);
+
+	/*
+	 * Setup and initialise the SP execution context on this physical cpu.
+	 */
+	spmc_sp_common_setup(sp, &sec_ec_ep_info);
+	spmc_el1_sp_setup(sp, &sec_ec_ep_info);
+
+	/* Obtain a reference to the SP execution context */
+	ec = &sp->ec[get_ec_index(sp)];
+
+	/*
+	 * TODO: Should we do some PM related state tracking of the SP execution
+	 * context here?
+	 */
+
+	/* Update the runtime model and state of the partition */
+	ec->rt_model = RT_MODEL_INIT;
+	ec->rt_state = RT_STATE_RUNNING;
+
+	INFO("SP (0x%x) init start on core%u.\n", sp->sp_id, linear_id);
+
+	rc = spmc_sp_synchronous_entry(ec);
+	if (rc != 0ULL) {
+		ERROR("%s failed (%llu) on CPU%u\n", __func__, rc, linear_id);
+	}
+
+	/* Update the runtime state of the partition */
+	ec->rt_state = RT_STATE_WAITING;
+
+	VERBOSE("CPU %u on!\n", linear_id);
+}
+
+/*******************************************************************************
+ * spmc_cpu_off_handler
+ ******************************************************************************/
+static int32_t spmc_cpu_off_handler(u_register_t unused)
+{
+	sp_desc_t *sp = spmc_get_current_sp_ctx();
+	sp_exec_ctx_t *ec;
+	unsigned int linear_id = plat_my_core_pos();
+	u_register_t resp;
+	uint64_t rc;
+
+	/* Sanity check for a NULL pointer dereference */
+	assert (NULL != sp);
+
+	/* Do nothing in case of a S-EL0 SP */
+	if (sp-> runtime_el == EL0)
+		return 0;
+
+	/* Obtain a reference to the SP execution context */
+	ec = &sp->ec[get_ec_index(sp)];
+
+	/*
+	 * TODO: Should we do some PM related state tracking of the SP execution
+	 * context here?
+	 */
+
+	/* Build an SPMC to SPMC direct message request. */
+	spmc_build_pm_message(get_gpregs_ctx(&ec->cpu_ctx),
+			      PSCI_CPU_OFF,
+			      FFA_PM_MSG_PSCI_REQ,
+			      sp->sp_id);
+
+	/* Sanity check partition state */
+	assert(ec->rt_state == RT_STATE_WAITING);
+
+	/* Update the runtime model and state of the partition */
+	ec->rt_model = RT_MODEL_DIR_REQ;
+	ec->rt_state = RT_STATE_RUNNING;
+
+	rc = spmc_sp_synchronous_entry(ec);
+	if (rc != 0ULL) {
+		ERROR("%s failed (%llu) on CPU%u\n", __func__, rc, linear_id);
+	}
+
+	/* Expect a direct message response from the SP. */
+	resp = read_ctx_reg(get_gpregs_ctx(&ec->cpu_ctx), CTX_GPREG_X0);
+	if (resp != FFA_MSG_SEND_DIRECT_RESP_SMC32) {
+		ERROR("%s invalid SPMC response (%lx).\n", __func__, resp);
+		return -EINVAL;
+	}
+
+	/* Expect a PM message response from the SP. */
+	resp = read_ctx_reg(get_gpregs_ctx(&ec->cpu_ctx), CTX_GPREG_X2);
+	if (!(resp & FFA_DIRECT_FRAMEWORK_MSG_MASK) ||
+	    ((resp & FFA_PM_MSG_MASK) != FFA_PM_MSG_PM_RESP)) {
+		ERROR("%s invalid SPMC response (%lx).\n", __func__, resp);
+		return -EINVAL;
+	}
+
+	/*
+	 * TODO: Check w1 in case the SP has sent a malformed message. If the
+	 * execution context failed to turn off then it might need to be marked
+	 * as such.
+	 */
+
+	/* Update the runtime state of the partition */
+	ec->rt_state = RT_STATE_WAITING;
+
+	VERBOSE("CPU %u off!\n", linear_id);
+
+	/* Return the status code returned by the SP */
+	return read_ctx_reg(get_gpregs_ctx(&ec->cpu_ctx), CTX_GPREG_X3);
+}
+
+/*******************************************************************************
+ * Structure populated by the SPM Core to perform any bookkeeping before
+ * PSCI executes a power mgmt. operation.
+ ******************************************************************************/
+const spd_pm_ops_t spmc_pm = {
+	.svc_on_finish = spmc_cpu_on_finish_handler,
+	.svc_off = spmc_cpu_off_handler
+};