[U-Boot] [PATCH v6 2/4] ARMv7: PSCI: ls102xa: check target CPU ID before further operations

[macro.wave.z at gmail.com <macro.wave.z@gmail.com>]

From: Hongbo Zhang <hongbo.zhang@nxp.com>

The input parameter CPU ID needs to be validated before furher oprations such
as CPU_ON, this patch introduces the function to do this.

Signed-off-by: Wang Dongsheng <dongsheng.wang@nxp.com>
Signed-off-by: Hongbo Zhang <hongbo.zhang@nxp.com>
---
 arch/arm/cpu/armv7/ls102xa/psci.S | 35 ++++++++++++++++++++++++++++++++++-
 arch/arm/include/asm/psci.h       |  5 +++++
 2 files changed, 39 insertions(+), 1 deletion(-)

diff --git a/arch/arm/cpu/armv7/ls102xa/psci.S b/arch/arm/cpu/armv7/ls102xa/psci.S
index f9b26b4..cba9c1e 100644
--- a/arch/arm/cpu/armv7/ls102xa/psci.S
+++ b/arch/arm/cpu/armv7/ls102xa/psci.S
@@ -25,6 +25,36 @@
 #define	ONE_MS		(GENERIC_TIMER_CLK / 1000)
 #define	RESET_WAIT	(30 * ONE_MS)
 
+@ r0: return value ARM_PSCI_RET_SUCCESS or ARM_PSCI_RET_INVAL
+@ r1: input target CPU ID in MPIDR format, original value in r1 may be dropped
+@ r4: output validated CPU ID if ARM_PSCI_RET_SUCCESS returns, meaningless for
+@ ARM_PSCI_RET_INVAL,suppose caller saves r4 before calling
+LENTRY(psci_check_target_cpu_id)
+	@ Get the real CPU number
+	and	r4, r1, #0xff
+	mov	r0, #ARM_PSCI_RET_INVAL
+
+	@ Bit[31:24], bits must be zero.
+	tst	r1, #0xff000000
+	bxne	lr
+
+	@ Affinity level 2 - Cluster: only one cluster in LS1021xa.
+	tst	r1, #0xff0000
+	bxne	lr
+
+	@ Affinity level 1 - Processors: should be in 0xf00 format.
+	lsr	r1, r1, #8
+	teq	r1, #0xf
+	bxne	lr
+
+	@ Affinity level 0 - CPU: only 0, 1 are valid in LS1021xa.
+	cmp	r4, #2
+	bxge	lr
+
+	mov	r0, #ARM_PSCI_RET_SUCCESS
+	bx	lr
+ENDPROC(psci_check_target_cpu_id)
+
 	@ r1 = target CPU
 	@ r2 = target PC
 .globl	psci_cpu_on
@@ -33,7 +63,9 @@ psci_cpu_on:
 
 	@ Clear and Get the correct CPU number
 	@ r1 = 0xf01
-	and	r4, r1, #0xff
+	bl	psci_check_target_cpu_id
+	cmp	r0, #ARM_PSCI_RET_INVAL
+	beq	out_psci_cpu_on
 
 	mov	r0, r4
 	mov	r1, r2
@@ -101,6 +133,7 @@ holdoff_release:
 	@ Return
 	mov	r0, #ARM_PSCI_RET_SUCCESS
 
+out_psci_cpu_on:
 	pop	{r4, r5, r6, lr}
 	bx	lr
 
diff --git a/arch/arm/include/asm/psci.h b/arch/arm/include/asm/psci.h
index 58f8b39..67f472a 100644
--- a/arch/arm/include/asm/psci.h
+++ b/arch/arm/include/asm/psci.h
@@ -67,6 +67,11 @@
 #define ARM_PSCI_STACK_SHIFT	10
 #define ARM_PSCI_STACK_SIZE	(1 << ARM_PSCI_STACK_SHIFT)
 
+/* PSCI affinity level state returned by AFFINITY_INFO */
+#define PSCI_AFFINITY_LEVEL_ON		0
+#define PSCI_AFFINITY_LEVEL_OFF		1
+#define PSCI_AFFINITY_LEVEL_ON_PENDING	2
+
 #ifndef __ASSEMBLY__
 #include <asm/types.h>
 
-- 
2.1.4