plat-vexpress: spci: add support to register secondary CPU entrypoints using PSCI_CPU_ON

This patch adds support to use the PSCI_CPU_ON function to register the
entry point for each OP-TEE context on a secondary CPU. This function is
invoked on the boot CPU during initialisation. When the physical CPU is
turned on by the Normal world, the SPMD in EL3 arranges for the entry
point to be invoked to perform OP-TEE initialisation.

Acked-by: Etienne Carriere <etienne.carriere@linaro.org>
Signed-off-by: Achin Gupta <achin.gupta@arm.com>
[jw: small edits + AAarch32 support]
Signed-off-by: Jens Wiklander <jens.wiklander@linaro.org>

3 files changed, 87 insertions, 0 deletions

diff --git a/core/arch/arm/include/arm.h b/core/arch/arm/include/arm.h
index 36ca7ec0..0186a79e 100644
--- a/core/arch/arm/include/arm.h
+++ b/core/arch/arm/include/arm.h
@@ -45,6 +45,9 @@
 #define MPIDR_CLUSTER_SHIFT	MPIDR_AFF1_SHIFT
 #define MPIDR_CLUSTER_MASK	MPIDR_AFF1_MASK
 
+#define MPIDR_AARCH32_AFF_MASK	(MPIDR_AFF0_MASK | MPIDR_AFF1_MASK | \
+				 MPIDR_AFF2_MASK)
+
 /* CLIDR definitions */
 #define CLIDR_LOUIS_SHIFT	21
 #define CLIDR_LOC_SHIFT		24
diff --git a/core/arch/arm/include/kernel/boot.h b/core/arch/arm/include/kernel/boot.h
index 5b06507f..53713c3f 100644
--- a/core/arch/arm/include/kernel/boot.h
+++ b/core/arch/arm/include/kernel/boot.h
@@ -88,4 +88,6 @@ void *get_external_dt(void);
 
 unsigned long get_aslr_seed(void *fdt);
 
+void ffa_secondary_cpu_boot_req(vaddr_t secondary_ep, uint64_t cookie);
+
 #endif /* __KERNEL_BOOT_H */
diff --git a/core/arch/arm/plat-vexpress/fvp_spmc_pm.c b/core/arch/arm/plat-vexpress/fvp_spmc_pm.c
new file mode 100644
index 00000000..2f16a968
--- /dev/null
+++ b/core/arch/arm/plat-vexpress/fvp_spmc_pm.c
@@ -0,0 +1,82 @@
+// SPDX-License-Identifier: BSD-2-Clause
+/*
+ * Copyright (c) 2019, Arm Limited
+ */
+
+#include <arm.h>
+#include <ffa.h>
+#include <initcall.h>
+#include <keep.h>
+#include <kernel/boot.h>
+#include <kernel/interrupt.h>
+#include <kernel/misc.h>
+#include <kernel/panic.h>
+#include <mm/core_memprot.h>
+#include <mm/core_mmu.h>
+#include <platform_config.h>
+#include <sm/psci.h>
+#include <stdint.h>
+#include <string.h>
+#include <trace.h>
+
+/*
+ * Lookup table of core and cluster affinities on the FVP. In the absence of a
+ * DT that provides the same information, this table is used to initialise
+ * OP-TEE on secondary cores.
+ */
+static const uint64_t core_clus_aff_array[] = {
+	0x0000,		/* Cluster 0 Cpu 0 */
+	0x0001,		/* Cluster 0 Cpu 1 */
+	0x0002,		/* Cluster 0 Cpu 2 */
+	0x0003,		/* Cluster 0 Cpu 3 */
+	0x0100,		/* Cluster 1 Cpu 0 */
+	0x0101,		/* Cluster 1 Cpu 1 */
+	0x0102,		/* Cluster 1 Cpu 2 */
+	0x0103,		/* Cluster 1 Cpu 3 */
+};
+
+static uint32_t get_cpu_on_fid(void)
+{
+#ifdef ARM64
+	return PSCI_CPU_ON_SMC64;
+#endif
+#ifdef ARM32
+	return PSCI_CPU_ON;
+#endif
+}
+
+void ffa_secondary_cpu_boot_req(vaddr_t secondary_ep, uint64_t cookie)
+{
+	unsigned long mpidr = read_mpidr();
+	unsigned int aff_shift = 0;
+	unsigned long a1 = 0;
+	unsigned int cnt = 0;
+
+	if (mpidr & MPIDR_MT_MASK)
+		aff_shift = MPIDR_CLUSTER_SHIFT;
+
+	for (cnt = 0; cnt < ARRAY_SIZE(core_clus_aff_array); cnt++) {
+		int32_t ret = 0;
+
+		/* Clear out the affinity fields until level 2 */
+		a1 = mpidr & ~(unsigned long)MPIDR_AARCH32_AFF_MASK;
+
+		/* Create an mpidr from core_clus_aff_array */
+		a1 |= core_clus_aff_array[cnt] << aff_shift;
+
+		/* Ignore current cpu */
+		if (a1 == mpidr)
+			continue;
+
+		DMSG("PSCI_CPU_ON op on mpidr 0x%lx", a1);
+
+		/* Invoke the PSCI_CPU_ON function */
+		ret = thread_smc(get_cpu_on_fid(), a1, secondary_ep, cookie);
+
+		if (ret != PSCI_RET_SUCCESS)
+			EMSG("PSCI_CPU_ON op on mpidr 0x%lx failed %"PRId32,
+			     a1, ret);
+		else
+			DMSG("PSCI_CPU_ON op on mpidr 0x%lx done", a1);
+	}
+}