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
diff --git a/services/std_svc/spmd/spmd_main.c b/services/std_svc/spmd/spmd_main.c
index 3050957..edcb5fe 100644
--- a/services/std_svc/spmd/spmd_main.c
+++ b/services/std_svc/spmd/spmd_main.c
@@ -60,6 +60,14 @@
}
/*******************************************************************************
+ * 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 @@
/******************************************************************************
* 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 13c638c..3904196 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 @@
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 @@
******************************************************************************/
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 @@
******************************************************************************/
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 0d78f53..a9dc1d3 100644
--- a/services/std_svc/spmd/spmd_private.h
+++ b/services/std_svc/spmd/spmd_private.h
@@ -78,11 +78,14 @@
/* 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__ */