[U-Boot] [PATCH v2 09/11] ARMv7: PSCI: ls102xa: check ALREADY_ON or ON_PENDING for CPU_ON

[macro.wave.z at gmail.com]

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

For the robustness of codes, while powering on a CPU, it is better to check
if the target CPU is already on or in the process of power on, if yes the
power on routine shouldn't be executed further and should return with the
corresponding status immediately.

Signed-off-by: Hongbo Zhang <hongbo.zhang at nxp.com>
---
 arch/arm/cpu/armv7/ls102xa/psci.S | 29 +++++++++++++++++++++++++++++
 arch/arm/include/asm/psci.h       |  5 +++++
 2 files changed, 34 insertions(+)

diff --git a/arch/arm/cpu/armv7/ls102xa/psci.S b/arch/arm/cpu/armv7/ls102xa/psci.S
index 9de812f..4cb960a 100644
--- a/arch/arm/cpu/armv7/ls102xa/psci.S
+++ b/arch/arm/cpu/armv7/ls102xa/psci.S
@@ -67,6 +67,22 @@ psci_cpu_on:
 	beq	out_psci_cpu_on
 	mov	r1, r0
 
+	bl	psci_get_cpu_stack_top
+	sub	r0, r0, #PSCI_CPU_STATUS_OFFSET
+	ldr	r5, [r0]
+
+	cmp	r5, #PSCI_CPU_STATUS_ON
+	moveq	r0, #PSCI_RET_ALREADY_ON
+	beq	out_psci_cpu_on
+
+	cmp	r5, #PSCI_CPU_STATUS_ON_PENDING
+	moveq	r0, #PSCI_RET_ON_PENDING
+	beq	out_psci_cpu_on
+
+	mov	r5, #PSCI_CPU_STATUS_ON_PENDING
+	str	r5, [r0]
+	dsb
+
 	bl	psci_cpu_on_common
 
 	@ Get DCFG base address
@@ -124,6 +140,12 @@ holdoff_release:
 	rev	r6, r6
 	str	r6, [r4, #DCFG_CCSR_SCRATCHRW1]
 
+	mov	r0, r1
+	bl	psci_get_cpu_stack_top
+	sub	r0, r0, #PSCI_CPU_STATUS_OFFSET
+	mov	r5, #PSCI_CPU_STATUS_ON
+	str	r5, [r0]
+
 	isb
 	dsb
 
@@ -138,6 +160,13 @@ out_psci_cpu_on:
 psci_cpu_off:
 	bl	psci_cpu_off_common
 
+	bl	psci_get_cpu_id
+	bl	psci_get_cpu_stack_top
+	sub	r0, r0, #PSCI_CPU_STATUS_OFFSET
+	mov	r5, #PSCI_CPU_STATUS_OFF
+	str	r5, [r0]
+	dsb
+
 1:	wfi
 	b	1b
 
diff --git a/arch/arm/include/asm/psci.h b/arch/arm/include/asm/psci.h
index 32ae359..535b77f 100644
--- a/arch/arm/include/asm/psci.h
+++ b/arch/arm/include/asm/psci.h
@@ -22,6 +22,7 @@
 #define PSCI_PERCPU_STACK_SIZE		0x400
 #define PSCI_TARGET_PC_OFFSET		(PSCI_PERCPU_STACK_SIZE - 4)
 #define PSCI_CONTEXT_ID_OFFSET		(PSCI_PERCPU_STACK_SIZE - 8)
+#define PSCI_CPU_STATUS_OFFSET		(PSCI_PERCPU_STACK_SIZE - 12)
 
 /* PSCI interfaces */
 #define PSCI_FN_BASE			0x84000000
@@ -47,6 +48,10 @@
 #define PSCI_FN_STAT_COUNT		PSCI_FN_ID(17)
 
 
+#define PSCI_CPU_STATUS_OFF		0
+#define PSCI_CPU_STATUS_ON		1
+#define PSCI_CPU_STATUS_ON_PENDING	2
+
 /* PSCI return values */
 #define PSCI_RET_SUCCESS		0
 #define PSCI_RET_NOT_SUPPORTED		(-1)
-- 
2.1.4