SPM: Fix secondary core init for FVP platform

When using FF-A on FVP platform the secondary core init was done by
invoking PSCI_CPU_ON function from OP-TEE. However, this relied on
changes in TF-A which were available on a prototype branch, but were
not merged into upstream TF-A.

This commit changes the secondary core init method to get aligned with
upstream TF-A v2.4. The new method is to invoke FFA_MSG_SEND_DIRECT_REQ
to send the entry point information to the SPMD.

Change-Id: I7952bd09fe82d2c080bb859faf3d8326a39e3bcc
Signed-off-by: Balint Dobszay <balint.dobszay@arm.com>

2 files changed, 47 insertions, 20 deletions

diff --git a/core/arch/arm/kernel/thread_spmc_a64.S b/core/arch/arm/kernel/thread_spmc_a64.S
index 40edb2b3a..e1a0f0954 100644
--- a/core/arch/arm/kernel/thread_spmc_a64.S
+++ b/core/arch/arm/kernel/thread_spmc_a64.S
@@ -195,3 +195,33 @@ FUNC thread_foreign_intr_exit , :
 	mov	w7, w0
 	b	ffa_msg_send_direct_resp
 END_FUNC thread_foreign_intr_exit
+
+/*
+ * void spmc_send_spmd_msg(uint64_t msg, uint64_t a1, uint64_t a2, uint64_t a3,
+ *			   uint64_t a4, uint32_t *ffa_res, int32_t *spmd_res)
+ */
+FUNC spmc_send_spmd_msg , :
+	/* Store addresses of return values */
+	push	x5, x6
+
+	/* Move function arguments to correct registers */
+	mov	x3, x0
+	mov	x4, x1
+	mov	x5, x2
+	mov	x6, x3
+	mov	x7, x4
+
+	/* Set FFA parameters */
+	mov_imm	x0, FFA_MSG_SEND_DIRECT_REQ_32
+	movz	x1, #0x8000, LSL #16	/* SPMC ID */
+	orr	x1, x1, #0xffff		/* SPMD ID */
+	mov	x2, #FFA_PARAM_MBZ
+
+	smc	#0
+
+	/* Store return values */
+	pop	x5, x6
+	str	w0, [x5]
+	str	w2, [x6]
+	ret
+END_FUNC spmc_send_spmd_msg
diff --git a/core/arch/arm/plat-vexpress/fvp_spmc_pm.c b/core/arch/arm/plat-vexpress/fvp_spmc_pm.c
index 2f16a968b..a2d61352b 100644
--- a/core/arch/arm/plat-vexpress/fvp_spmc_pm.c
+++ b/core/arch/arm/plat-vexpress/fvp_spmc_pm.c
@@ -1,6 +1,6 @@
 // SPDX-License-Identifier: BSD-2-Clause
 /*
- * Copyright (c) 2019, Arm Limited
+ * Copyright (c) 2019-2020, Arm Limited
  */
 
 #include <arm.h>
@@ -19,6 +19,12 @@
 #include <string.h>
 #include <trace.h>
 
+#define SPMD_MSG_SET_ENTRY_POINT	U(1)
+
+/* Prototype for assembly function */
+void spmc_send_spmd_msg(uint64_t msg, uint64_t a1, uint64_t a2, uint64_t a3,
+			uint64_t a4, uint32_t *ffa_res, int32_t *spmd_res);
+
 /*
  * 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
@@ -35,18 +41,10 @@ static const uint64_t core_clus_aff_array[] = {
 	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)
 {
+	uint32_t ffa_res = FFA_ERROR;
+	int32_t spmd_res = 0;
 	unsigned long mpidr = read_mpidr();
 	unsigned int aff_shift = 0;
 	unsigned long a1 = 0;
@@ -56,8 +54,6 @@ void ffa_secondary_cpu_boot_req(vaddr_t secondary_ep, uint64_t cookie)
 		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;
 
@@ -68,15 +64,16 @@ void ffa_secondary_cpu_boot_req(vaddr_t secondary_ep, uint64_t cookie)
 		if (a1 == mpidr)
 			continue;
 
-		DMSG("PSCI_CPU_ON op on mpidr 0x%lx", a1);
+		DMSG("SET_ENTRY_POINT op on mpidr 0x%lx", a1);
 
-		/* Invoke the PSCI_CPU_ON function */
-		ret = thread_smc(get_cpu_on_fid(), a1, secondary_ep, cookie);
+		spmc_send_spmd_msg(SPMD_MSG_SET_ENTRY_POINT,
+				   a1, secondary_ep, cookie, 0,
+				   &ffa_res, &spmd_res);
 
-		if (ret != PSCI_RET_SUCCESS)
-			EMSG("PSCI_CPU_ON op on mpidr 0x%lx failed %"PRId32,
-			     a1, ret);
+		if (ffa_res == FFA_SUCCESS_32 && spmd_res == 0)
+			DMSG("SET_ENTRY_POINT op mpidr 0x%lx done", a1);
 		else
-			DMSG("PSCI_CPU_ON op on mpidr 0x%lx done", a1);
+			EMSG("SET_ENTRY_POINT op mpidr 0x%lx failed 0x%x %d",
+			     a1, ffa_res, spmd_res);
 	}
 }