SPMC: embed secondary core ep info into to SPMC context

Signed-off-by: Olivier Deprez <olivier.deprez@arm.com>
Signed-off-by: Max Shvetsov <maksims.svecovs@arm.com>
Change-Id: Icdb15b8664fb3467ffd55b44d1f0660457192586

services/std_svc/spmd/spmd_main.c

@@ -41,14 +41,20 @@ static spmc_manifest_attribute_t spmc_attrs;
  ******************************************************************************/
 static entry_point_info_t *spmc_ep_info;
 
+/*******************************************************************************
+ * SPM Core context on CPU based on mpidr.
+ ******************************************************************************/
+spmd_spm_core_context_t *spmd_get_context_by_mpidr(uint64_t mpidr)
+{
+	return &spm_core_context[plat_core_pos_by_mpidr(mpidr)];
+}
+
 /*******************************************************************************
  * SPM Core context on current CPU get helper.
  ******************************************************************************/
 spmd_spm_core_context_t *spmd_get_context(void)
 {
-	unsigned int linear_id = plat_my_core_pos();
+	return spmd_get_context_by_mpidr(read_mpidr());
-
-	return &spm_core_context[linear_id];
 }
 
 /*******************************************************************************
@@ -151,6 +157,7 @@ static int32_t spmd_init(void)
 	for (core_id = 0U; core_id < PLATFORM_CORE_COUNT; core_id++) {
 		if (core_id != linear_id) {
 			spm_core_context[core_id].state = SPMC_STATE_OFF;
+			spm_core_context[core_id].secondary_ep.entry_point = 0UL;
 		}
 	}
 

services/std_svc/spmd/spmd_pm.c

@@ -9,14 +9,6 @@
 #include <lib/el3_runtime/context_mgmt.h>
 #include "spmd_private.h"
 
-struct spmd_pm_secondary_ep_t {
-	uintptr_t entry_point;
-	uintptr_t context;
-	bool locked;
-};
-
-static struct spmd_pm_secondary_ep_t spmd_pm_secondary_ep[PLATFORM_CORE_COUNT];
-
 /*******************************************************************************
  * spmd_build_spmc_message
  *
@@ -45,11 +37,6 @@ int spmd_pm_secondary_core_set_ep(unsigned long long mpidr,
 		return -EINVAL;
 	}
 
-	if (spmd_pm_secondary_ep[id].locked) {
-		ERROR("%s entry locked (%llx)\n", __func__, mpidr);
-		return -EINVAL;
-	}
-
 	/*
 	 * Check entry_point address is a PA within
 	 * load_address <= entry_point < load_address + binary_size
@@ -60,10 +47,17 @@ int spmd_pm_secondary_core_set_ep(unsigned long long mpidr,
 		return -EINVAL;
 	}
 
+	spmd_spm_core_context_t *ctx = spmd_get_context_by_mpidr(mpidr);
+	spmd_pm_secondary_ep_t *secondary_ep = &ctx->secondary_ep;
+	if (secondary_ep->locked) {
+		ERROR("%s entry locked (%llx)\n", __func__, mpidr);
+		return -EINVAL;
+	}
+
 	/* Fill new entry to corresponding secondary core id and lock it */
-	spmd_pm_secondary_ep[id].entry_point = entry_point;
+	secondary_ep->entry_point = entry_point;
-	spmd_pm_secondary_ep[id].context = context;
+	secondary_ep->context = context;
-	spmd_pm_secondary_ep[id].locked = true;
+	secondary_ep->locked = true;
 
 	VERBOSE("%s %d %llx %lx %llx\n",
 		__func__, id, mpidr, entry_point, context);
@@ -82,7 +76,7 @@ static void spmd_cpu_on_finish_handler(u_register_t unused)
 	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;
+	uint64_t rc;
 
 	assert(ctx != NULL);
 	assert(ctx->state != SPMC_STATE_ON);
@@ -92,21 +86,21 @@ static void spmd_cpu_on_finish_handler(u_register_t unused)
 	 * 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) {
+	if (ctx->secondary_ep.entry_point == 0UL) {
 		goto exit;
 	}
 
-	spmc_ep_info->pc = spmd_pm_secondary_ep[linear_id].entry_point;
+	spmc_ep_info->pc = ctx->secondary_ep.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);
+		      ctx->secondary_ep.context);
 
 	/* Mark CPU as initiating ON operation */
 	ctx->state = SPMC_STATE_ON_PENDING;
 
 	rc = spmd_spm_core_sync_entry(ctx);
-	if (rc != 0) {
+	if (rc != 0ULL) {
-		ERROR("%s failed failed (%d) on CPU%u\n", __func__, rc,
+		ERROR("%s failed (%llu) on CPU%u\n", __func__, rc,
 			linear_id);
 		ctx->state = SPMC_STATE_OFF;
 		return;
@@ -125,12 +119,12 @@ 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;
+	int64_t rc;
 
 	assert(ctx != NULL);
 	assert(ctx->state != SPMC_STATE_OFF);
 
-	if (spmd_pm_secondary_ep[linear_id].entry_point == 0) {
+	if (ctx->secondary_ep.entry_point == 0UL) {
 		goto exit;
 	}
 
@@ -138,8 +132,8 @@ static int32_t spmd_cpu_off_handler(u_register_t unused)
 	spmd_build_spmc_message(get_gpregs_ctx(&ctx->cpu_ctx), PSCI_CPU_OFF);
 
 	rc = spmd_spm_core_sync_entry(ctx);
-	if (rc != 0) {
+	if (rc != 0ULL) {
-		ERROR("%s failed (%d) on CPU%u\n", __func__, rc, linear_id);
+		ERROR("%s failed (%llu) on CPU%u\n", __func__, rc, linear_id);
 	}
 
 	/* TODO expect FFA_DIRECT_MSG_RESP returned from SPMC */

services/std_svc/spmd/spmd_private.h

@@ -42,6 +42,12 @@ typedef enum spmc_state {
 	SPMC_STATE_ON
 } spmc_state_t;
 
+typedef struct spmd_pm_secondary_ep {
+	uintptr_t entry_point;
+	uintptr_t context;
+	bool locked;
+} spmd_pm_secondary_ep_t;
+
 /*
  * Data structure used by the SPM dispatcher (SPMD) in EL3 to track context of
  * the SPM core (SPMC) at the next lower EL.
@@ -50,6 +56,7 @@ typedef struct spmd_spm_core_context {
 	uint64_t c_rt_ctx;
 	cpu_context_t cpu_ctx;
 	spmc_state_t state;
+	spmd_pm_secondary_ep_t secondary_ep;
 } spmd_spm_core_context_t;
 
 /*
@@ -81,6 +88,9 @@ entry_point_info_t *spmd_spmc_ep_info_get(void);
 /* SPMC ID getter */
 uint16_t spmd_spmc_id_get(void);
 
+/* SPMC context on CPU based on mpidr */
+spmd_spm_core_context_t *spmd_get_context_by_mpidr(uint64_t mpidr);
+
 /* SPMC context on current CPU get helper */
 spmd_spm_core_context_t *spmd_get_context(void);