[U-Boot] [PATCH v3 06/11] ARMv7: PSCI: factor out reusable psci_cpu_on_common

[macro.wave.z at gmail.com]

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

There are codes for saving target PC and target context ID in each platform
psci_cpu_on routines, these can be factored out as psci_cpu_on_common.

Signed-off-by: Hongbo Zhang <hongbo.zhang at nxp.com>
Signed-off-by: Wang Dongsheng <dongsheng.wang at nxp.com>
---
 arch/arm/cpu/armv7/ls102xa/psci.S     | 12 +-----------
 arch/arm/cpu/armv7/mx7/psci.S         | 12 +-----------
 arch/arm/cpu/armv7/psci.S             | 15 +++++++++++++++
 arch/arm/cpu/armv7/sunxi/psci_sun6i.S | 12 +-----------
 arch/arm/cpu/armv7/sunxi/psci_sun7i.S | 13 +------------
 arch/arm/mach-tegra/psci.S            | 12 +-----------
 6 files changed, 20 insertions(+), 56 deletions(-)

diff --git a/arch/arm/cpu/armv7/ls102xa/psci.S b/arch/arm/cpu/armv7/ls102xa/psci.S
index 1303909..47bcb29 100644
--- a/arch/arm/cpu/armv7/ls102xa/psci.S
+++ b/arch/arm/cpu/armv7/ls102xa/psci.S
@@ -36,17 +36,7 @@ psci_cpu_on:
 	@ r1 = 0xf01
 	and	r1, r1, #0xff
 
-	mov	r0, r1
-	bl	psci_get_cpu_stack_top
-	sub	r0, r0, #PSCI_TARGET_PC_OFFSET
-	str	r2, [r0]
-	dsb
-
-	mov	r0, r1
-	bl	psci_get_cpu_stack_top
-	sub	r0, r0, #PSCI_CONTEXT_ID_OFFSET
-	str	r3, [r0]
-	dsb
+	bl	psci_cpu_on_common
 
 	@ Get DCFG base address
 	movw	r4, #(CONFIG_SYS_FSL_GUTS_ADDR & 0xffff)
diff --git a/arch/arm/cpu/armv7/mx7/psci.S b/arch/arm/cpu/armv7/mx7/psci.S
index 90b8b9e..c9a7b00 100644
--- a/arch/arm/cpu/armv7/mx7/psci.S
+++ b/arch/arm/cpu/armv7/mx7/psci.S
@@ -29,17 +29,7 @@ psci_arch_init:
 psci_cpu_on:
 	push	{lr}
 
-	mov	r0, r1
-	bl	psci_get_cpu_stack_top
-	sub	r0, r0, #PSCI_TARGET_PC_OFFSET
-	str	r2, [r0]
-	dsb
-
-	mov	r0, r1
-	bl	psci_get_cpu_stack_top
-	sub	r0, r0, #PSCI_CONTEXT_ID_OFFSET
-	str	r3, [r0]
-	dsb
+	bl	psci_cpu_on_common
 
 	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 0865712..28579d7 100644
--- a/arch/arm/cpu/armv7/psci.S
+++ b/arch/arm/cpu/armv7/psci.S
@@ -175,6 +175,21 @@ ENTRY(psci_enable_smp)
 ENDPROC(psci_enable_smp)
 .weak psci_enable_smp
 
+/* expects target CPU in r1, target PC in r2, target conetxt ID in r3 */
+ENTRY(psci_cpu_on_common)
+	push	{lr}
+
+	mov	r0, r1
+	bl	psci_get_cpu_stack_top		@ get stack top of target CPU
+	sub	r5, r0, #PSCI_TARGET_PC_OFFSET
+	str	r2, [r5]			@ save target PC
+	sub	r5, r0, #PSCI_CONTEXT_ID_OFFSET
+	str	r3, [r5]			@ save target context ID
+	dsb
+
+	pop	{pc}
+ENDPROC(psci_cpu_on_common)
+
 ENTRY(psci_cpu_off_common)
 	push	{lr}
 
diff --git a/arch/arm/cpu/armv7/sunxi/psci_sun6i.S b/arch/arm/cpu/armv7/sunxi/psci_sun6i.S
index 2c9b078..a94a68d 100644
--- a/arch/arm/cpu/armv7/sunxi/psci_sun6i.S
+++ b/arch/arm/cpu/armv7/sunxi/psci_sun6i.S
@@ -135,17 +135,7 @@ out:	mcr	p15, 0, r7, c1, c1, 0
 psci_cpu_on:
 	push	{lr}
 
-	mov	r0, r1
-	bl	psci_get_cpu_stack_top	@ get stack top of target CPU
-	sub	r0, r0, #PSCI_TARGET_PC_OFFSET
-	str	r2, [r0]		@ store target PC
-	dsb
-
-	mov	r0, r1
-	bl	psci_get_cpu_stack_top
-	sub	r0, r0, #PSCI_CONTEXT_ID_OFFSET
-	str	r3, [r0]
-	dsb
+	bl	psci_cpu_on_common
 
 	movw	r0, #(SUN6I_CPUCFG_BASE & 0xffff)
 	movt	r0, #(SUN6I_CPUCFG_BASE >> 16)
diff --git a/arch/arm/cpu/armv7/sunxi/psci_sun7i.S b/arch/arm/cpu/armv7/sunxi/psci_sun7i.S
index c1f117a..b0601a7 100644
--- a/arch/arm/cpu/armv7/sunxi/psci_sun7i.S
+++ b/arch/arm/cpu/armv7/sunxi/psci_sun7i.S
@@ -124,18 +124,7 @@ out:	mcr	p15, 0, r7, c1, c1, 0
 psci_cpu_on:
 	push	{lr}
 
-	mov	r0, r1
-	bl	psci_get_cpu_stack_top	@ get stack top of target CPU
-	sub	r0, r0, #PSCI_TARGET_PC_OFFSET
-	str	r2, [r0]		@ store target PC
-	dsb
-
-
-	mov	r0, r1
-	bl	psci_get_cpu_stack_top
-	sub	r0, r0, #PSCI_CONTEXT_ID_OFFSET
-	str	r3, [r0]
-	dsb
+	bl	psci_cpu_on_common
 
 	movw	r0, #(SUN7I_CPUCFG_BASE & 0xffff)
 	movt	r0, #(SUN7I_CPUCFG_BASE >> 16)
diff --git a/arch/arm/mach-tegra/psci.S b/arch/arm/mach-tegra/psci.S
index 3837d95..8fa90ce 100644
--- a/arch/arm/mach-tegra/psci.S
+++ b/arch/arm/mach-tegra/psci.S
@@ -90,17 +90,7 @@ ENDPROC(psci_cpu_off)
 ENTRY(psci_cpu_on)
 	push	{lr}
 
-	mov	r0, r1
-	bl	psci_get_cpu_stack_top	@ get stack top of target CPU
-	sub	r0, r0, #PSCI_TARGET_PC_OFFSET
-	str	r2, [r0]		@ store target PC
-	dsb
-
-	mov	r0, r1
-	bl	psci_get_cpu_stack_top
-	sub	r0, r0, #PSCI_CONTEXT_ID_OFFSET
-	str	r3, [r0]
-	dsb
+	bl	psci_cpu_on_common
 
 	ldr	r6, =TEGRA_RESET_EXCEPTION_VECTOR
 	ldr	r5, =psci_cpu_entry
-- 
2.1.4