[U-Boot] [PATCH v3 08/11] ARMv7: PSCI: ls102xa: check target CPU ID before further operations

[macro.wave.z at gmail.com]

From: Hongbo Zhang <hongbo.zhang at 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 at nxp.com>
Signed-off-by: Hongbo Zhang <hongbo.zhang at nxp.com>
---
 arch/arm/cpu/armv7/ls102xa/psci.S | 34 +++++++++++++++++++++++++++++++++-
 1 file changed, 33 insertions(+), 1 deletion(-)

diff --git a/arch/arm/cpu/armv7/ls102xa/psci.S b/arch/arm/cpu/armv7/ls102xa/psci.S
index 47bcb29..9de812f 100644
--- a/arch/arm/cpu/armv7/ls102xa/psci.S
+++ b/arch/arm/cpu/armv7/ls102xa/psci.S
@@ -25,6 +25,34 @@
 #define	ONE_MS		(GENERIC_TIMER_CLK / 1000)
 #define	RESET_WAIT	(30 * ONE_MS)
 
+.globl	psci_check_target_cpu_id
+psci_check_target_cpu_id:
+	@ Get the real CPU number
+	and	r0, r1, #0xff
+
+	@ Verify bit[31:24], bits must be zero.
+	tst	r1, #0xff000000
+	bne	out_psci_invalid_target_cpu_id
+
+	@ Verify Affinity level 2: Cluster, only one cluster in LS1021xa SoC.
+	tst	r1, #0xff0000
+	bne	out_psci_invalid_target_cpu_id
+
+	@ Verify Affinity level 1: Processors, should be in 0xf00 format.
+	lsr	r1, r1, #8
+	teq	r1, #0xf
+	bne	out_psci_invalid_target_cpu_id
+
+	@ Verify Affinity level 0: CPU, only 0, 1 are valid values.
+	cmp	r0, #2
+	bge	out_psci_invalid_target_cpu_id
+
+	bx	lr
+
+out_psci_invalid_target_cpu_id:
+	mov	r0, #PSCI_RET_INVALID_PARAMS
+	bx	lr
+
 	@ r1 = target CPU
 	@ r2 = target PC
 	@ r3 = target Conetxt ID
@@ -34,7 +62,10 @@ psci_cpu_on:
 
 	@ Clear and Get the correct CPU number
 	@ r1 = 0xf01
-	and	r1, r1, #0xff
+	bl	psci_check_target_cpu_id
+	cmp	r0, #PSCI_RET_INVALID_PARAMS
+	beq	out_psci_cpu_on
+	mov	r1, r0
 
 	bl	psci_cpu_on_common
 
@@ -99,6 +130,7 @@ holdoff_release:
 	@ Return
 	mov	r0, #PSCI_RET_SUCCESS
 
+out_psci_cpu_on:
 	pop	{lr}
 	bx	lr
 
-- 
2.1.4