[Test] [PATCH v2 03/11] 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, because the stack grows in descending address order, it is better the
stack starts from page end or some similiar address, but currently the
algorithm is based on address of page start, if so some stack starts at page
start and then grows into the next page only after pushing one word.
This patch recalculate stacks based on address of page end.

Second, 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.

Third, 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    | 13 ++++++-------
 arch/arm/cpu/armv7/virt-dt.c |  9 +++++++--
 arch/arm/include/asm/psci.h  |  2 ++
 3 files changed, 15 insertions(+), 9 deletions(-)

diff --git a/arch/arm/cpu/armv7/psci.S b/arch/arm/cpu/armv7/psci.S
index e1fcb5d..42116e8 100644
--- a/arch/arm/cpu/armv7/psci.S
+++ b/arch/arm/cpu/armv7/psci.S
@@ -196,16 +196,15 @@ 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
-	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!
+	orr	r5, r5, #0xff0			@ align to end of page
+	orr	r5, r5, #0x00c
 
+	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 f1251d1..4953f27 100644
--- a/arch/arm/cpu/armv7/virt-dt.c
+++ b/arch/arm/cpu/armv7/virt-dt.c
@@ -124,9 +124,14 @@ int psci_update_dt(void *fdt)
 		return 0;
 #endif
 #ifndef CONFIG_ARMV7_SECURE_BASE
+	size_t sz = __secure_end - __secure_start;
+#ifdef CONFIG_ARMV7_PSCI
+	sz += CONFIG_MAX_CPUS * PSCI_PERCPU_STACK_SIZE;
+	/* margin to align psci_text_end to page end*/
+	sz += 0x1000;
+#endif
 	/* 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, sz);
 #endif
 
 	return fdt_psci(fdt);
diff --git a/arch/arm/include/asm/psci.h b/arch/arm/include/asm/psci.h
index 7c3450c..47a2ea4 100644
--- a/arch/arm/include/asm/psci.h
+++ b/arch/arm/include/asm/psci.h
@@ -18,6 +18,8 @@
 #ifndef __ARM_PSCI_H__
 #define __ARM_PSCI_H__
 
+/* size of percpu stack, 1kB */
+#define PSCI_PERCPU_STACK_SIZE		0x400
 
 /* PSCI interfaces */
 #define PSCI_FN_BASE			0x84000000
-- 
2.1.4