[U-Boot] [PATCH v4 2/9] ARMv7: PSCI: update function psci_get_cpu_stack_top

[macro.wave.z at gmail.com]

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

There are issues of legacy fuction psci_get_cpu_stack_top:

First, the current algorithm arranges stacks from an fixed adress towards
psci_text_end, if there are more CPUs, the stacks will overlap with psci text
segment and even other segments.
This patch places stacks from psci text segment towards highter address, and
all the stack space is reserved, so overlap can be avoided.

Second, even there is one word reserved in each stack for saving target PC, but
this reserved space isn't used at all, the target PC is still saved to where
the stack top pointer points.
This patch doesn't reserve this word as before, new way of saving target PC
will be introduced in following patch.

Signed-off-by: Hongbo Zhang <hongbo.zhang at nxp.com>
Signed-off-by: Wang Dongsheng <dongsheng.wang at nxp.com>
---
 arch/arm/cpu/armv7/psci.S    | 10 +++++-----
 arch/arm/cpu/armv7/virt-dt.c |  9 +++++++--
 arch/arm/include/asm/psci.h  |  3 +++
 3 files changed, 15 insertions(+), 7 deletions(-)

diff --git a/arch/arm/cpu/armv7/psci.S b/arch/arm/cpu/armv7/psci.S
index 8e25300..91a1dd1 100644
--- a/arch/arm/cpu/armv7/psci.S
+++ b/arch/arm/cpu/armv7/psci.S
@@ -274,16 +274,16 @@ 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
+	mov	r5, #PSCI_PERCPU_STACK_SIZE	@ 1kB of stack per CPU
+	add	r0, r0, #1
+	mul	r0, r0, r5			@ offset of each stack
 
 	ldr	r5, =psci_text_end		@ end of monitor text
-	add	r5, r5, #0x2000			@ Skip two pages
+	add	r5, r5, #0x1000			@ Skip one page
 	lsr	r5, r5, #12			@ Align to start of page
 	lsl	r5, r5, #12
-	sub	r5, r5, #4			@ reserve 1 word for target PC
-	sub	r0, r5, r0			@ here's our stack!
 
+	add	r0, r5, r0			@ here's our stack!
 	bx	lr
 ENDPROC(psci_get_cpu_stack_top)
 
diff --git a/arch/arm/cpu/armv7/virt-dt.c b/arch/arm/cpu/armv7/virt-dt.c
index 5e31891..4fe6f58 100644
--- a/arch/arm/cpu/armv7/virt-dt.c
+++ b/arch/arm/cpu/armv7/virt-dt.c
@@ -127,14 +127,19 @@ int armv7_apply_memory_carveout(u64 *start, u64 *size)
 
 int psci_update_dt(void *fdt)
 {
+	size_t sec_sz = __secure_end - __secure_start;
+#ifdef CONFIG_ARMV7_PSCI
+	sec_sz += CONFIG_MAX_CPUS * PSCI_PERCPU_STACK_SIZE;
+	/* margin to align psci_text_end to page end*/
+	sec_sz += 0x1000;
+#endif
 #ifdef CONFIG_ARMV7_NONSEC
 	if (!armv7_boot_nonsec())
 		return 0;
 #endif
 #ifndef CONFIG_ARMV7_SECURE_BASE
 	/* secure code lives in RAM, keep it alive */
-	fdt_add_mem_rsv(fdt, (unsigned long)__secure_start,
-			__secure_end - __secure_start);
+	fdt_add_mem_rsv(fdt, (unsigned long)__secure_start, sec_sz);
 #endif
 
 	return fdt_psci(fdt);
diff --git a/arch/arm/include/asm/psci.h b/arch/arm/include/asm/psci.h
index 2367ec0..76c3c92 100644
--- a/arch/arm/include/asm/psci.h
+++ b/arch/arm/include/asm/psci.h
@@ -63,6 +63,9 @@
 #define ARM_PSCI_1_0_FN_STAT_RESIDENCY		ARM_PSCI_0_2_FN(16)
 #define ARM_PSCI_1_0_FN_STAT_COUNT		ARM_PSCI_0_2_FN(17)
 
+/* size of percpu stack, 1kB */
+#define PSCI_PERCPU_STACK_SIZE		0x400
+
 #ifndef __ASSEMBLY__
 int psci_update_dt(void *fdt);
 void psci_board_init(void);
-- 
2.1.4