diff --git a/arch/arm/cpu/armv7/ls102xa/psci.S b/arch/arm/cpu/armv7/ls102xa/psci.S
index 0b067d9..3a34064 100644
--- a/arch/arm/cpu/armv7/ls102xa/psci.S
+++ b/arch/arm/cpu/armv7/ls102xa/psci.S
@@ -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)
diff --git a/arch/arm/cpu/armv7/mx7/psci.S b/arch/arm/cpu/armv7/mx7/psci.S
index 34c6ab3..cb39f27 100644
--- a/arch/arm/cpu/armv7/mx7/psci.S
+++ b/arch/arm/cpu/armv7/mx7/psci.S
@@ -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
diff --git a/arch/arm/cpu/armv7/psci.S b/arch/arm/cpu/armv7/psci.S
index 2425f6a..0e0f98e 100644
--- a/arch/arm/cpu/armv7/psci.S
+++ b/arch/arm/cpu/armv7/psci.S
@@ -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)
 
diff --git a/arch/arm/cpu/armv7/sunxi/psci_sun6i.S b/arch/arm/cpu/armv7/sunxi/psci_sun6i.S
index b96c5ef..e8981af 100644
--- a/arch/arm/cpu/armv7/sunxi/psci_sun6i.S
+++ b/arch/arm/cpu/armv7/sunxi/psci_sun6i.S
@@ -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)
diff --git a/arch/arm/cpu/armv7/sunxi/psci_sun7i.S b/arch/arm/cpu/armv7/sunxi/psci_sun7i.S
index 59d7ff0..a1fcc71 100644
--- a/arch/arm/cpu/armv7/sunxi/psci_sun7i.S
+++ b/arch/arm/cpu/armv7/sunxi/psci_sun7i.S
@@ -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)
diff --git a/arch/arm/cpu/armv7/virt-dt.c b/arch/arm/cpu/armv7/virt-dt.c
index f1251d1..5ca353c 100644
--- a/arch/arm/cpu/armv7/virt-dt.c
+++ b/arch/arm/cpu/armv7/virt-dt.c
@@ -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);
diff --git a/arch/arm/include/asm/psci.h b/arch/arm/include/asm/psci.h
index f7b8f65..274dbda 100644
--- a/arch/arm/include/asm/psci.h
+++ b/arch/arm/include/asm/psci.h
@@ -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))
diff --git a/arch/arm/mach-tegra/psci.S b/arch/arm/mach-tegra/psci.S
index 5f326c9..e83566e 100644
--- a/arch/arm/mach-tegra/psci.S
+++ b/arch/arm/mach-tegra/psci.S
@@ -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
