@@ -36,9 +36,7 @@ psci_cpu_on:
and r1, r1, #0xff
mov r0, r1
- bl psci_get_cpu_stack_top
- str r2, [r0]
- dsb
+ bl psci_save_target_pc
@ Get DCFG base address
movw r4, #(CONFIG_SYS_FSL_GUTS_ADDR & 0xffff)
@@ -30,9 +30,7 @@ psci_cpu_on:
push {lr}
mov r0, r1
- bl psci_get_cpu_stack_top
- str r2, [r0]
- dsb
+ bl psci_save_target_pc
ldr r2, =psci_cpu_entry
bl imx_cpu_on
@@ -20,6 +20,8 @@
#include <asm/macro.h>
#include <asm/psci.h>
+#define SAVE_SPACE_TARGET_PC_OFFSET 0x0
+
.pushsection ._secure.text, "ax"
.arch_extension sec
@@ -196,27 +198,58 @@ 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
-
+ @ Align psci_text_end minumum page size. Even if page is greater than
+ @ 4K, we still can ensure that data is always safe access by the CPU.
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
+ @ Detect page border
+ lsl r4, r5, #20
+ cmp r4, #0
+ beq out_psci_stack_align
+ add r5, r5, #PSCI_STACK_ALIGN_SIZE
+ lsr r5, r5, #12
lsl r5, r5, #12
- sub r5, r5, #4 @ reserve 1 word for target PC
- sub r0, r5, r0 @ here's our stack!
+
+out_psci_stack_align:
+ mov r4, #PSCI_PERCPU_STACK_SIZE
+ add r0, r0, #1
+ mul r0, r0, r4
+ add r0, r0, r5
bx lr
ENDPROC(psci_get_cpu_stack_top)
+@ Save space in percpu stack tail.
+@ expects CPU ID in r0 and returns save space address in r0.
+ENTRY(psci_get_cpu_save_space)
+ mov r10, lr
+
+ bl psci_get_cpu_stack_top
+ sub r0, r0, #PSCI_PERCPU_STACK_SIZE
+
+ bx r10
+ENDPROC(psci_get_cpu_save_space)
+
+@ Expects cpu ID in r0 and PC in r2, please ignore the return value.
+ENTRY(psci_save_target_pc)
+ push {lr}
+
+ @ Save target PC into stack
+ bl psci_get_cpu_save_space
+ str r2, [r0, #SAVE_SPACE_TARGET_PC_OFFSET]
+ dsb
+
+ pop {lr}
+ bx lr
+ENDPROC(psci_save_target_pc)
+
ENTRY(psci_cpu_entry)
bl psci_enable_smp
bl _nonsec_init
bl psci_get_cpu_id @ CPU ID => r0
- bl psci_get_cpu_stack_top @ stack top => r0
- ldr r0, [r0] @ target PC at stack top
+ bl psci_get_cpu_save_space
+ ldr r0, [r0, #SAVE_SPACE_TARGET_PC_OFFSET]
b _do_nonsec_entry
ENDPROC(psci_cpu_entry)
@@ -135,9 +135,7 @@ psci_cpu_on:
push {lr}
mov r0, r1
- bl psci_get_cpu_stack_top @ get stack top of target CPU
- str r2, [r0] @ store target PC at stack top
- dsb
+ bl psci_save_target_pc
movw r0, #(SUN6I_CPUCFG_BASE & 0xffff)
movt r0, #(SUN6I_CPUCFG_BASE >> 16)
@@ -124,9 +124,7 @@ psci_cpu_on:
push {lr}
mov r0, r1
- bl psci_get_cpu_stack_top @ get stack top of target CPU
- str r2, [r0] @ store target PC at stack top
- dsb
+ bl psci_save_target_pc
movw r0, #(SUN7I_CPUCFG_BASE & 0xffff)
movt r0, #(SUN7I_CPUCFG_BASE >> 16)
@@ -126,7 +126,8 @@ int psci_update_dt(void *fdt)
#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);
+ __secure_end + CONFIG_MAX_CPUS * PSCI_PERCPU_STACK_SIZE
+ + PSCI_STACK_ALIGN_SIZE - __secure_start);
#endif
return fdt_psci(fdt);
@@ -18,6 +18,10 @@
#ifndef __ARM_PSCI_H__
#define __ARM_PSCI_H__
+#define PSCI_STACK_ALIGN_SIZE 0x1000
+
+/* 1kB for percpu stack */
+#define PSCI_PERCPU_STACK_SIZE 0x400
#define PSCI_FN_BASE 0x84000000
#define PSCI_FN_ID(n) (PSCI_FN_BASE + (n))
@@ -91,9 +91,7 @@ ENTRY(psci_cpu_on)
push {lr}
mov r0, r1
- bl psci_get_cpu_stack_top @ get stack top of target CPU
- str r2, [r0] @ store target PC at stack top
- dsb
+ bl psci_save_target_pc
ldr r6, =TEGRA_RESET_EXCEPTION_VECTOR
ldr r5, =psci_cpu_entry