[U-Boot] [PATCH v2 03/12] ARM: Factor out reusable psci_get_cpu_stack_top

[Jan Kiszka]

This algorithm will be useful on Tegra as well, plus we will need it for
making _psci_target_pc per-CPU.

Signed-off-by: Jan Kiszka <jan.kiszka at siemens.com>
---
 arch/arm/cpu/armv7/psci.S       | 14 ++++++++++++++
 arch/arm/cpu/armv7/sunxi/psci.S | 17 +++++++----------
 2 files changed, 21 insertions(+), 10 deletions(-)

diff --git a/arch/arm/cpu/armv7/psci.S b/arch/arm/cpu/armv7/psci.S
index e916d71..da47934 100644
--- a/arch/arm/cpu/armv7/psci.S
+++ b/arch/arm/cpu/armv7/psci.S
@@ -170,6 +170,20 @@ ENTRY(psci_cpu_off_common)
 	bx	lr
 ENDPROC(psci_cpu_off_common)
 
+@ expects CPU ID in r4 (will be overwritten) and returns stack top in r5
+ENTRY(psci_get_cpu_stack_top)
+	mov	r5, #0x400			@ 1kB of stack per CPU
+	mul	r4, r4, r5
+
+	ldr	r5, =psci_text_end		@ end of monitor text
+	add	r5, r5, #0x2000			@ Skip two pages
+	lsr	r5, r5, #12			@ Align to start of page
+	lsl	r5, r5, #12
+	sub	r5, r5, r4			@ here's our stack!
+
+	bx	lr
+ENDPROC(psci_get_cpu_stack_top)
+
 ENTRY(psci_cpu_entry)
 	@ Set SMP bit
 	mrc	p15, 0, r0, c1, c0, 1		@ ACTLR
diff --git a/arch/arm/cpu/armv7/sunxi/psci.S b/arch/arm/cpu/armv7/sunxi/psci.S
index c3a8dc1..4372022 100644
--- a/arch/arm/cpu/armv7/sunxi/psci.S
+++ b/arch/arm/cpu/armv7/sunxi/psci.S
@@ -213,6 +213,8 @@ psci_cpu_off:
 
 .globl	psci_arch_init
 psci_arch_init:
+	mov	r6, lr
+
 	movw	r4, #(GICD_BASE & 0xffff)
 	movt	r4, #(GICD_BASE >> 16)
 
@@ -240,16 +242,11 @@ psci_arch_init:
 
 	mrc	p15, 0, r4, c0, c0, 5	@ MPIDR
 	and	r4, r4, #3		@ cpu number in cluster
-	mov	r5, #0x400		@ 1kB of stack per CPU
-	mul	r4, r4, r5
-
-	adr	r5, text_end		@ end of text
-	add	r5, r5, #0x2000		@ Skip two pages
-	lsr	r5, r5, #12		@ Align to start of page
-	lsl	r5, r5, #12
-	sub	sp, r5, r4		@ here's our stack!
+	bl	psci_get_cpu_stack_top
+	mov	sp, r5
 
-	bx	lr
+	bx	r6
 
-text_end:
+	.globl psci_text_end
+psci_text_end:
 	.popsection
-- 
2.1.4