ARMv7: PSCI: ls102xa: check target CPU ID before further operations

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>
Reviewed-by: York Sun <york.sun@nxp.com>

diff --git a/arch/arm/cpu/armv7/ls102xa/psci.S b/arch/arm/cpu/armv7/ls102xa/psci.S
index f9b26b4..cba9c1e 100644 (file)
--- 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 25ea44d..8aefaa7 100644 (file)
--- 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>