[U-Boot] [PATCH v5 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 | 15 +++++----------
 2 files changed, 19 insertions(+), 10 deletions(-)

diff --git a/arch/arm/cpu/armv7/psci.S b/arch/arm/cpu/armv7/psci.S
index fc5f9af..090dee1 100644
--- a/arch/arm/cpu/armv7/psci.S
+++ b/arch/arm/cpu/armv7/psci.S
@@ -193,6 +193,20 @@ ENTRY(psci_cpu_off_common)
 	bx	lr
 ENDPROC(psci_cpu_off_common)
 
+@ expects CPU ID in r0 and returns stack top in r0
+ENTRY(psci_get_cpu_stack_top)
+	mov	r5, #0x400			@ 1kB of stack per CPU
+	mul	r0, r0, 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	r0, r5, r0			@ here's our stack!
+
+	bx	lr
+ENDPROC(psci_get_cpu_stack_top)
+
 ENTRY(psci_cpu_entry)
 	bl	psci_enable_smp
 
diff --git a/arch/arm/cpu/armv7/sunxi/psci.S b/arch/arm/cpu/armv7/sunxi/psci.S
index 44a9ad8..6bafaa1 100644
--- a/arch/arm/cpu/armv7/sunxi/psci.S
+++ b/arch/arm/cpu/armv7/sunxi/psci.S
@@ -241,17 +241,12 @@ psci_arch_init:
 	mcr	p15, 0, r5, c1, c1, 0	@ Write SCR
 	isb
 
-	bl	psci_get_cpu_id
-	mov	r5, #0x400		@ 1kB of stack per CPU
-	mul	r0, r0, 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, r0		@ here's our stack!
+	bl	psci_get_cpu_id		@ CPU ID => r0
+	bl	psci_get_cpu_stack_top	@ stack top => r0
+	mov	sp, r0
 
 	bx	r6
 
-text_end:
+	.globl psci_text_end
+psci_text_end:
 	.popsection
-- 
2.1.4