[U-Boot] [PATCH 4/9] ARM: ARMv7: PSCI: Factor out reusable psci_cpu_on_common

[Dongsheng Wang]

From: Wang Dongsheng <dongsheng.wang at nxp.com>

Move save target PC codes to a common function.

Signed-off-by: Wang Dongsheng <dongsheng.wang at nxp.com>
---
 arch/arm/cpu/armv7/ls102xa/psci.S     | 20 ++++++++----------
 arch/arm/cpu/armv7/mx7/psci.S         |  3 ---
 arch/arm/cpu/armv7/psci.S             | 39 ++++++++++++++++++++++++++++++++---
 arch/arm/cpu/armv7/sunxi/psci_sun6i.S |  7 +------
 arch/arm/cpu/armv7/sunxi/psci_sun7i.S |  7 +------
 arch/arm/mach-tegra/psci.S            |  7 +------
 6 files changed, 48 insertions(+), 35 deletions(-)

diff --git a/arch/arm/cpu/armv7/ls102xa/psci.S b/arch/arm/cpu/armv7/ls102xa/psci.S
index 3a34064..461b785 100644
--- a/arch/arm/cpu/armv7/ls102xa/psci.S
+++ b/arch/arm/cpu/armv7/ls102xa/psci.S
@@ -25,19 +25,19 @@
 #define	ONE_MS		(GENERIC_TIMER_CLK / 1000)
 #define	RESET_WAIT	(30 * ONE_MS)
 
-	@ r1 = target CPU
-	@ r2 = target PC
-.globl	psci_cpu_on
-psci_cpu_on:
-	push	{lr}
-
+@ Expect target CPU in r1, return the target cpu number in R0
+.globl	psci_get_target_cpu_id
+psci_get_target_cpu_id:
 	@ Clear and Get the correct CPU number
-	@ r1 = 0xf01
 	and	r1, r1, #0xff
-
 	mov	r0, r1
-	bl	psci_save_target_pc
 
+	bx	lr
+
+	@ r1 = target CPU
+	@ r2 = target PC
+.globl	psci_cpu_on
+psci_cpu_on:
 	@ Get DCFG base address
 	movw	r4, #(CONFIG_SYS_FSL_GUTS_ADDR & 0xffff)
 	movt	r4, #(CONFIG_SYS_FSL_GUTS_ADDR >> 16)
@@ -98,8 +98,6 @@ holdoff_release:
 
 	@ Return
 	mov	r0, #PSCI_RET_SUCCESS
-
-	pop	{lr}
 	bx	lr
 
 .globl	psci_cpu_off
diff --git a/arch/arm/cpu/armv7/mx7/psci.S b/arch/arm/cpu/armv7/mx7/psci.S
index cb39f27..716c7ae 100644
--- a/arch/arm/cpu/armv7/mx7/psci.S
+++ b/arch/arm/cpu/armv7/mx7/psci.S
@@ -29,9 +29,6 @@ psci_arch_init:
 psci_cpu_on:
 	push	{lr}
 
-	mov	r0, r1
-	bl	psci_save_target_pc
-
 	ldr	r2, =psci_cpu_entry
 	bl	imx_cpu_on
 
diff --git a/arch/arm/cpu/armv7/psci.S b/arch/arm/cpu/armv7/psci.S
index 0e0f98e..c3651bd 100644
--- a/arch/arm/cpu/armv7/psci.S
+++ b/arch/arm/cpu/armv7/psci.S
@@ -69,7 +69,7 @@ _psci_table:
 	.word	PSCI_FN_CPU_OFF
 	.word	psci_cpu_off
 	.word	PSCI_FN_CPU_ON
-	.word	psci_cpu_on
+	.word	psci_cpu_on_common
 	.word	PSCI_FN_MIGRATE
 	.word	psci_migrate
 	.word	0
@@ -177,6 +177,40 @@ ENTRY(psci_enable_smp)
 ENDPROC(psci_enable_smp)
 .weak psci_enable_smp
 
+@ Expect target CPU in r1, return the target cpu number in r0.
+@ If detect an error, please return #PSCI_RET_INVALID_PARAMS.
+ENTRY(psci_get_target_cpu_id)
+	mov	r0, r1
+	bx	lr
+ENDPROC(psci_get_target_cpu_id)
+.weak psci_get_target_cpu_id
+
+	@ r1 = target CPU
+	@ r2 = target PC
+	@ r3 = target Conetxt ID
+ENTRY(psci_cpu_on_common)
+	push	{lr}
+
+	bl	psci_get_target_cpu_id
+	cmp	r0, #PSCI_RET_INVALID_PARAMS
+	beq	out_psci_cpu_on_common
+
+	@ Update the target CPU ID, translation have been completed.
+	mov	r1, r0
+
+	@ Save target PC into stack
+	bl	psci_save_target_pc
+
+	@ Still pass on:
+	@ r1 = target CPU
+	@ r2 = target PC
+	@ r3 = target Conetxt ID
+	bl	psci_cpu_on
+
+out_psci_cpu_on_common:
+	pop	{pc}
+ENDPROC(psci_cpu_on_common)
+
 ENTRY(psci_cpu_off_common)
 	push	{lr}
 
@@ -238,8 +272,7 @@ ENTRY(psci_save_target_pc)
 	str	r2, [r0, #SAVE_SPACE_TARGET_PC_OFFSET]
 	dsb
 
-	pop	{lr}
-	bx	lr
+	pop	{pc}
 ENDPROC(psci_save_target_pc)
 
 ENTRY(psci_cpu_entry)
diff --git a/arch/arm/cpu/armv7/sunxi/psci_sun6i.S b/arch/arm/cpu/armv7/sunxi/psci_sun6i.S
index e8981af..b64b012 100644
--- a/arch/arm/cpu/armv7/sunxi/psci_sun6i.S
+++ b/arch/arm/cpu/armv7/sunxi/psci_sun6i.S
@@ -132,11 +132,6 @@ out:	mcr	p15, 0, r7, c1, c1, 0
 	@ r2 = target PC
 .globl	psci_cpu_on
 psci_cpu_on:
-	push	{lr}
-
-	mov	r0, r1
-	bl	psci_save_target_pc
-
 	movw	r0, #(SUN6I_CPUCFG_BASE & 0xffff)
 	movt	r0, #(SUN6I_CPUCFG_BASE >> 16)
 
@@ -203,7 +198,7 @@ psci_cpu_on:
 	str	r6, [r0, #0x1e4]
 
 	mov	r0, #PSCI_RET_SUCCESS	@ Return PSCI_RET_SUCCESS
-	pop	{pc}
+	bx	lr
 
 .globl	psci_cpu_off
 psci_cpu_off:
diff --git a/arch/arm/cpu/armv7/sunxi/psci_sun7i.S b/arch/arm/cpu/armv7/sunxi/psci_sun7i.S
index a1fcc71..ed867b8 100644
--- a/arch/arm/cpu/armv7/sunxi/psci_sun7i.S
+++ b/arch/arm/cpu/armv7/sunxi/psci_sun7i.S
@@ -121,11 +121,6 @@ out:	mcr	p15, 0, r7, c1, c1, 0
 	@ r2 = target PC
 .globl	psci_cpu_on
 psci_cpu_on:
-	push	{lr}
-
-	mov	r0, r1
-	bl	psci_save_target_pc
-
 	movw	r0, #(SUN7I_CPUCFG_BASE & 0xffff)
 	movt	r0, #(SUN7I_CPUCFG_BASE >> 16)
 
@@ -178,7 +173,7 @@ psci_cpu_on:
 	str	r6, [r0, #0x1e4]
 
 	mov	r0, #PSCI_RET_SUCCESS	@ Return PSCI_RET_SUCCESS
-	pop	{pc}
+	bx	lr
 
 .globl	psci_cpu_off
 psci_cpu_off:
diff --git a/arch/arm/mach-tegra/psci.S b/arch/arm/mach-tegra/psci.S
index e83566e..278c8ed 100644
--- a/arch/arm/mach-tegra/psci.S
+++ b/arch/arm/mach-tegra/psci.S
@@ -88,11 +88,6 @@ _loop:	wfi
 ENDPROC(psci_cpu_off)
 
 ENTRY(psci_cpu_on)
-	push	{lr}
-
-	mov	r0, r1
-	bl	psci_save_target_pc
-
 	ldr	r6, =TEGRA_RESET_EXCEPTION_VECTOR
 	ldr	r5, =psci_cpu_entry
 	str	r5, [r6]
@@ -104,7 +99,7 @@ ENTRY(psci_cpu_on)
 	str	r5, [r6, r2]
 
 	mov	r0, #PSCI_RET_SUCCESS	@ Return PSCI_RET_SUCCESS
-	pop	{pc}
+	bx	lr
 ENDPROC(psci_cpu_on)
 
 	.globl psci_text_end
-- 
2.1.0.27.g96db324