[U-Boot] [PATCH v4 05/14] 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.

CC: Marc Zyngier <marc.zyngier at arm.com>
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 315c20b..99ce490 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 9ea3ce8..53d5e3f 100644
--- a/arch/arm/cpu/armv7/sunxi/psci.S
+++ b/arch/arm/cpu/armv7/sunxi/psci.S
@@ -214,6 +214,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:
 	isb
 
 	armv7_get_cpu_id r4
-	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