Merge changes from topic "spm-secondary-cores" into integration

* changes:
  SPMC: embed secondary core ep info into to SPMC context
  SPMC: manifest changes to support multicore boot
  SPMD: secondary cores PM on and off SPD hooks relayed to SPMC
  SPMD: handle SPMC message to register secondary core entry point
  SPMD: introduce SPMC to SPMD messages
  SPMD: register the SPD PM hooks
  SPMD: add generic SPD PM handlers
  SPMD: enhance SPMC internal boot states
  SPMD: entry point info get helper

include/services/ffa_svc.h

@@ -138,4 +138,43 @@
  */
 #define FFA_PARAM_MBZ			U(0x0)
 
+/*
+ * Maximum FF-A endpoint id value
+ */
+#define FFA_ENDPOINT_ID_MAX			U(1 << 16)
+
+/*
+ * Mask for source and destination endpoint id in
+ * a direct message request/response.
+ */
+#define FFA_DIRECT_MSG_ENDPOINT_ID_MASK		U(0xffff)
+
+/*
+ * Bit shift for destination endpoint id in a direct message request/response.
+ */
+#define FFA_DIRECT_MSG_DESTINATION_SHIFT	U(0)
+
+/*
+ * Bit shift for source endpoint id in a direct message request/response.
+ */
+#define FFA_DIRECT_MSG_SOURCE_SHIFT		U(16)
+
+/******************************************************************************
+ * ffa_endpoint_destination
+ *****************************************************************************/
+static inline uint16_t ffa_endpoint_destination(unsigned int ep)
+{
+	return (ep >> FFA_DIRECT_MSG_DESTINATION_SHIFT) &
+		FFA_DIRECT_MSG_ENDPOINT_ID_MASK;
+}
+
+/******************************************************************************
+ * ffa_endpoint_source
+ *****************************************************************************/
+static inline uint16_t ffa_endpoint_source(unsigned int ep)
+{
+	return (ep >> FFA_DIRECT_MSG_SOURCE_SHIFT) &
+		FFA_DIRECT_MSG_ENDPOINT_ID_MASK;
+}
+
 #endif /* FFA_SVC_H */

include/services/spm_core_manifest.h

@@ -44,7 +44,7 @@ typedef struct spm_core_manifest_sect_attribute {
 	uint32_t binary_size;
 
 	/*
-	 * ID of the SPMD (mandatory)
+	 * ID of the SPMC (mandatory)
 	 */
 	uint16_t spmc_id;
 

plat/arm/board/fvp/fdts/fvp_spmc_manifest.dts

@@ -5,6 +5,14 @@
  */
 /dts-v1/;
 
+#define AFF 00
+
+#include "fvp-defs.dtsi"
+#undef POST
+#define POST \
+		enable-method = "psci"; \
+		};
+
 / {
 	compatible = "arm,ffa-core-manifest-1.0";
 	#address-cells = <2>;
@@ -17,6 +25,7 @@
 		exec_state = <0x0>;
 		load_address = <0x0 0x6000000>;
 		entrypoint = <0x0 0x6000000>;
+		binary_size = <0x80000>;
 	};
 
 	chosen {
@@ -51,22 +60,15 @@
 		#address-cells = <0x2>;
 		#size-cells = <0x0>;
 
-		cpu-map {
-			cluster0 {
-				core0 {
-					cpu = <0x2>;
-				};
-			};
-		};
-
-		cpu@0 {
-			device_type = "cpu";
-			compatible = "arm,armv8";
-			reg = <0x0 0x0>;
-			enable-method = "psci";
-			next-level-cache = <0xc>;
-			phandle = <0x2>;
-		};
+		CPU_0
+		/* SPM(Hafnium) requires secondary cpu nodes are declared in descending order */
+		CPU_7
+		CPU_6
+		CPU_5
+		CPU_4
+		CPU_3
+		CPU_2
+		CPU_1
 	};
 
 	memory@60000000 {

services/std_svc/spmd/spmd.mk

@@ -10,6 +10,7 @@ endif
 
 SPMD_SOURCES	+=	$(addprefix services/std_svc/spmd/,	\
 			${ARCH}/spmd_helpers.S			\
+			spmd_pm.c				\
 			spmd_main.c)
 
 # Let the top-level Makefile know that we intend to include a BL32 image

services/std_svc/spmd/spmd_main.c

@@ -41,14 +41,36 @@ 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];
+/*******************************************************************************
+ * SPM Core entry point information get helper.
+ ******************************************************************************/
+entry_point_info_t *spmd_spmc_ep_info_get(void)
+{
+	return spmc_ep_info;
+}
+
+/*******************************************************************************
+ * SPM Core ID getter.
+ ******************************************************************************/
+uint16_t spmd_spmc_id_get(void)
+{
+	return spmc_attrs.spmc_id;
 }
 
 /*******************************************************************************
@@ -125,9 +147,19 @@ static int32_t spmd_init(void)
 {
 	spmd_spm_core_context_t *ctx = spmd_get_context();
 	uint64_t rc;
+	unsigned int linear_id = plat_my_core_pos();
+	unsigned int core_id;
 
 	VERBOSE("SPM Core init start.\n");
-	ctx->state = SPMC_STATE_RESET;
+	ctx->state = SPMC_STATE_ON_PENDING;
+
+	/* Set the SPMC context state on other CPUs to OFF */
+	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;
+		}
+	}
 
 	rc = spmd_spm_core_sync_entry(ctx);
 	if (rc != 0ULL) {
@@ -135,7 +167,8 @@ static int32_t spmd_init(void)
 		return 0;
 	}
 
-	ctx->state = SPMC_STATE_IDLE;
+	ctx->state = SPMC_STATE_ON;
+
 	VERBOSE("SPM Core init end.\n");
 
 	return 1;
@@ -248,6 +281,9 @@ static int spmd_spmc_init(void *pm_addr)
 
 	INFO("SPM Core setup done.\n");
 
+	/* Register power management hooks with PSCI */
+	psci_register_spd_pm_hook(&spmd_pm);
+
 	/* Register init function for deferred init. */
 	bl31_register_bl32_init(&spmd_init);
 
@@ -301,8 +337,8 @@ static uint64_t spmd_smc_forward(uint32_t smc_fid,
 				 uint64_t x4,
 				 void *handle)
 {
-	uint32_t secure_state_in = (secure_origin) ? SECURE : NON_SECURE;
-	uint32_t secure_state_out = (!secure_origin) ? SECURE : NON_SECURE;
+	unsigned int secure_state_in = (secure_origin) ? SECURE : NON_SECURE;
+	unsigned int secure_state_out = (!secure_origin) ? SECURE : NON_SECURE;
 
 	/* Save incoming security state */
 	cm_el1_sysregs_context_save(secure_state_in);
@@ -334,6 +370,46 @@ static uint64_t spmd_ffa_error_return(void *handle, int error_code)
 		 FFA_PARAM_MBZ, FFA_PARAM_MBZ);
 }
 
+/*******************************************************************************
+ * spmd_check_address_in_binary_image
+ ******************************************************************************/
+bool spmd_check_address_in_binary_image(uint64_t address)
+{
+	assert(!check_uptr_overflow(spmc_attrs.load_address, spmc_attrs.binary_size));
+
+	return ((address >= spmc_attrs.load_address) &&
+		(address < (spmc_attrs.load_address + spmc_attrs.binary_size)));
+}
+
+/******************************************************************************
+ * spmd_is_spmc_message
+ *****************************************************************************/
+static bool spmd_is_spmc_message(unsigned int ep)
+{
+	return ((ffa_endpoint_destination(ep) == SPMD_DIRECT_MSG_ENDPOINT_ID)
+		&& (ffa_endpoint_source(ep) == spmc_attrs.spmc_id));
+}
+
+/******************************************************************************
+ * spmd_handle_spmc_message
+ *****************************************************************************/
+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);
+
+	switch (msg) {
+	case SPMD_DIRECT_MSG_SET_ENTRY_POINT:
+		return spmd_pm_secondary_core_set_ep(parm1, parm2, parm3);
+	default:
+		break;
+	}
+
+	return -EINVAL;
+}
+
 /*******************************************************************************
  * This function handles all SMCs in the range reserved for FFA. Each call is
  * either forwarded to the other security state or handled by the SPM dispatcher
@@ -367,7 +443,7 @@ uint64_t spmd_smc_handler(uint32_t smc_fid,
 		 * this CPU. If so, then indicate that the SPM Core initialised
 		 * unsuccessfully.
 		 */
-		if (secure_origin && (ctx->state == SPMC_STATE_RESET)) {
+		if (secure_origin && (ctx->state == SPMC_STATE_ON_PENDING)) {
 			spmd_spm_core_sync_exit(x2);
 		}
 
@@ -451,6 +527,35 @@ uint64_t spmd_smc_handler(uint32_t smc_fid,
 
 		break; /* not reached */
 
+	case FFA_MSG_SEND_DIRECT_REQ_SMC32:
+		if (secure_origin && spmd_is_spmc_message(x1)) {
+			ret = spmd_handle_spmc_message(x3, x4,
+				SMC_GET_GP(handle, CTX_GPREG_X5),
+				SMC_GET_GP(handle, CTX_GPREG_X6),
+				SMC_GET_GP(handle, CTX_GPREG_X7));
+
+			SMC_RET8(handle, FFA_SUCCESS_SMC32,
+				FFA_TARGET_INFO_MBZ, ret,
+				FFA_PARAM_MBZ, FFA_PARAM_MBZ,
+				FFA_PARAM_MBZ, FFA_PARAM_MBZ,
+				FFA_PARAM_MBZ);
+		} else {
+			/* Forward direct message to the other world */
+			return spmd_smc_forward(smc_fid, secure_origin,
+				x1, x2, x3, x4, handle);
+		}
+		break; /* Not reached */
+
+	case FFA_MSG_SEND_DIRECT_RESP_SMC32:
+		if (secure_origin && spmd_is_spmc_message(x1)) {
+			spmd_spm_core_sync_exit(0);
+		} else {
+			/* Forward direct message to the other world */
+			return spmd_smc_forward(smc_fid, secure_origin,
+				x1, x2, x3, x4, handle);
+		}
+		break; /* Not reached */
+
 	case FFA_RX_RELEASE:
 	case FFA_RXTX_MAP_SMC32:
 	case FFA_RXTX_MAP_SMC64:
@@ -466,9 +571,7 @@ uint64_t spmd_smc_handler(uint32_t smc_fid,
 
 	case FFA_PARTITION_INFO_GET:
 	case FFA_MSG_SEND:
-	case FFA_MSG_SEND_DIRECT_REQ_SMC32:
 	case FFA_MSG_SEND_DIRECT_REQ_SMC64:
-	case FFA_MSG_SEND_DIRECT_RESP_SMC32:
 	case FFA_MSG_SEND_DIRECT_RESP_SMC64:
 	case FFA_MEM_DONATE_SMC32:
 	case FFA_MEM_DONATE_SMC64:
@@ -500,7 +603,7 @@ uint64_t spmd_smc_handler(uint32_t smc_fid,
 		 * this CPU from the Secure world. If so, then indicate that the
 		 * SPM Core initialised successfully.
 		 */
-		if (secure_origin && (ctx->state == SPMC_STATE_RESET)) {
+		if (secure_origin && (ctx->state == SPMC_STATE_ON_PENDING)) {
 			spmd_spm_core_sync_exit(0);
 		}
 

services/std_svc/spmd/spmd_pm.c

+/*
+ * Copyright (c) 2020, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <lib/el3_runtime/context_mgmt.h>
+#include "spmd_private.h"
+
+/*******************************************************************************
+ * 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
+ ******************************************************************************/
+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);
+
+	if ((id < 0) || (id >= PLATFORM_CORE_COUNT)) {
+		ERROR("%s inconsistent MPIDR (%llx)\n", __func__, mpidr);
+		return -EINVAL;
+	}
+
+	/*
+	 * Check entry_point address is a PA within
+	 * load_address <= entry_point < load_address + binary_size
+	 */
+	if (!spmd_check_address_in_binary_image(entry_point)) {
+		ERROR("%s entry point is not within image boundaries (%llx)\n",
+		      __func__, 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 */
+	secondary_ep->entry_point = entry_point;
+	secondary_ep->context = context;
+	secondary_ep->locked = true;
+
+	VERBOSE("%s %d %llx %lx %llx\n",
+		__func__, id, mpidr, entry_point, context);
+
+	return 0;
+}
+
+/*******************************************************************************
+ * This CPU has been turned on. Enter SPMC to initialise S-EL1 or S-EL2. As part
+ * of the SPMC initialization path, they will initialize any SPs that they
+ * manage. Entry into SPMC is done after initialising minimal architectural
+ * state that guarantees safe execution.
+ ******************************************************************************/
+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();
+	uint64_t 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 (ctx->secondary_ep.entry_point == 0UL) {
+		goto exit;
+	}
+
+	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,
+		      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 != 0ULL) {
+		ERROR("%s failed (%llu) 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();
+	int64_t rc;
+
+	assert(ctx != NULL);
+	assert(ctx->state != SPMC_STATE_OFF);
+
+	if (ctx->secondary_ep.entry_point == 0UL) {
+		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 != 0ULL) {
+		ERROR("%s failed (%llu) 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;
+}
+
+/*******************************************************************************
+ * Structure populated by the SPM Dispatcher to perform any bookkeeping before
+ * PSCI executes a power mgmt. operation.
+ ******************************************************************************/
+const spd_pm_ops_t spmd_pm = {
+	.svc_on_finish = spmd_cpu_on_finish_handler,
+	.svc_off = spmd_cpu_off_handler
+};

services/std_svc/spmd/spmd_private.h

@@ -30,14 +30,24 @@
 #define SPMD_C_RT_CTX_ENTRIES		(SPMD_C_RT_CTX_SIZE >> DWORD_SHIFT)
 
 #ifndef __ASSEMBLER__
-#include <services/ffa_svc.h>
 #include <stdint.h>
+#include <lib/psci/psci_lib.h>
+#include <plat/common/platform.h>
+#include <services/ffa_svc.h>
 
 typedef enum spmc_state {
 	SPMC_STATE_RESET = 0,
-	SPMC_STATE_IDLE
+	SPMC_STATE_OFF,
+	SPMC_STATE_ON_PENDING,
+	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.
@@ -46,16 +56,20 @@ 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;
 
 /*
  * Reserve ID for NS physical FFA Endpoint.
  */
-#define FFA_NS_ENDPOINT_ID		U(0)
+#define FFA_NS_ENDPOINT_ID			U(0)
 
-/* Mask and shift to check valid secure FFA Endpoint ID. */
-#define SPMC_SECURE_ID_MASK		U(1)
-#define SPMC_SECURE_ID_SHIFT		U(15)
+/* Mask and shift to check valid secure FF-A Endpoint ID. */
+#define SPMC_SECURE_ID_MASK			U(1)
+#define SPMC_SECURE_ID_SHIFT			U(15)
+
+#define SPMD_DIRECT_MSG_ENDPOINT_ID		U(FFA_ENDPOINT_ID_MAX - 1)
+#define SPMD_DIRECT_MSG_SET_ENTRY_POINT		U(1)
 
 /* Functions used to enter/exit SPMC synchronously */
 uint64_t spmd_spm_core_sync_entry(spmd_spm_core_context_t *ctx);
@@ -65,9 +79,25 @@ __dead2 void spmd_spm_core_sync_exit(uint64_t rc);
 uint64_t spmd_spm_core_enter(uint64_t *c_rt_ctx);
 void __dead2 spmd_spm_core_exit(uint64_t c_rt_ctx, uint64_t ret);
 
+/* SPMD SPD power management handlers */
+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 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);
 
+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__ */
 
 #endif /* SPMD_PRIVATE_H */