[boot-wrapper PATCH 05/12] Move PSCI triage to C

[Mark Rutland]

There's no reason we need to test the PSCI function IDs in assembly;
move this to C so that it can be shared across AArch64 and AArch32. At
the same time, limit the PSCI_CPU_ON FIDs to match the register width of
the kernel.

Signed-off-by: Mark Rutland <mark.rutland at arm.com>
---
 arch/aarch32/psci.S | 23 +----------------------
 arch/aarch64/psci.S | 21 +--------------------
 psci.c              | 21 +++++++++++++++++++--
 3 files changed, 21 insertions(+), 44 deletions(-)

diff --git a/arch/aarch32/psci.S b/arch/aarch32/psci.S
index 78dca96..0b7663e 100644
--- a/arch/aarch32/psci.S
+++ b/arch/aarch32/psci.S
@@ -33,32 +33,11 @@ handle_smc:
 	@ Follow the SMC32 calling convention: preserve r4 - r14
 	push	{r4 - r12, lr}
 
-	ldr	r4, =PSCI_CPU_ON_32
-	cmp	r4, r0
-	ldr	r4, =psci_cpu_on
-	beq	do_psci_call
-
-	ldr	r4, =PSCI_CPU_OFF
-	cmp	r4, r0
-	ldr	r4, =psci_cpu_off
-	beq	do_psci_call
-
-	adr	r4, psci_invalid
-
-do_psci_call:
-	mov	r0, r1
-	mov	r1, r2
-	mov	r2, r3
-
-	blx	r4
+	blx	psci_call
 
 	pop	{r4 - r12, lr}
 	movs	pc, lr
 
-psci_invalid:
-	mov	r0, #PSCI_RET_NOT_SUPPORTED
-	bx	lr
-
 ENTRY(start_el3)
 	ldr	r0, =smc_vectors
 	blx	setup_vector
diff --git a/arch/aarch64/psci.S b/arch/aarch64/psci.S
index 01ebe7d..6dbca11 100644
--- a/arch/aarch64/psci.S
+++ b/arch/aarch64/psci.S
@@ -51,13 +51,6 @@ vector:
 err_exception:
 	b err_exception
 
-	.macro branch_if val, addr
-	ldr	x7, =\val
-	cmp	x0, x7
-	adr	x7, \addr
-	b.eq	do_call
-	.endm
-
 smc_entry32:
 	/* Clear upper bits */
 	mov	w0, w0
@@ -74,22 +67,10 @@ smc_entry64:
 	// Keep sp aligned to 16 bytes
 	stp	x30, xzr, [sp, #-16]!
 
-	/* If function ID matches, do_call with procedure address in x7 */
-	branch_if PSCI_CPU_ON_32,	psci_cpu_on
-	branch_if PSCI_CPU_ON_64,	psci_cpu_on
-	branch_if PSCI_CPU_OFF,		psci_cpu_off
+	bl	psci_call
 
-	/* Otherwise, return error in x0/w0 */
-	mov	x0, PSCI_RET_NOT_SUPPORTED
 	b	smc_exit
 
-do_call:
-	mov	x0, x1
-	mov	x1, x2
-	mov	x2, x3
-
-	blr	x7
-
 smc_exit:
 	ldp	x30, xzr, [sp], #16
 	ldp	x28, x29, [sp], #16
diff --git a/psci.c b/psci.c
index fad6f5d..e5d54b7 100644
--- a/psci.c
+++ b/psci.c
@@ -31,7 +31,7 @@ static int psci_store_address(unsigned int cpu, unsigned long address)
 	return PSCI_RET_SUCCESS;
 }
 
-int psci_cpu_on(unsigned long target_mpidr, unsigned long address)
+static int psci_cpu_on(unsigned long target_mpidr, unsigned long address)
 {
 	int ret;
 	unsigned int cpu = find_logical_id(target_mpidr);
@@ -47,7 +47,7 @@ int psci_cpu_on(unsigned long target_mpidr, unsigned long address)
 	return ret;
 }
 
-int psci_cpu_off(void)
+static int psci_cpu_off(void)
 {
 	unsigned long mpidr = read_mpidr();
 	unsigned int cpu = find_logical_id(mpidr);
@@ -62,6 +62,23 @@ int psci_cpu_off(void)
 	unreachable();
 }
 
+long psci_call(unsigned long fid, unsigned long arg1, unsigned long arg2)
+{
+	switch (fid) {
+	case PSCI_CPU_OFF:
+		return psci_cpu_off();
+#ifdef KERNEL_32
+	case PSCI_CPU_ON_32:
+		return psci_cpu_on(arg1, arg2);
+#else
+	case PSCI_CPU_ON_64:
+		return psci_cpu_on(arg1, arg2);
+#endif
+	default:
+		return PSCI_RET_NOT_SUPPORTED;
+	}
+}
+
 void __noreturn psci_first_spin(unsigned int cpu)
 {
 	if (cpu == MPIDR_INVALID)
-- 
2.11.0