From 6e6622de166f53597172687b7269b07cf48844df Mon Sep 17 00:00:00 2001
From: Chen-Yu Tsai <wens@csie.org>
Date: Sun, 19 Jun 2016 12:38:44 +0800
Subject: ARM: PSCI: Switch to per-CPU target PC storage in secure data section

Now that we have a secure data section and space to store per-CPU target
PC address, switch to it instead of storing the target PC on the stack.

Also save clobbered r4-r7 registers on the stack and restore them on
return in psci_cpu_on for Tegra, i.MX7, and LS102xA platforms.

Signed-off-by: Chen-Yu Tsai <wens@csie.org>
Signed-off-by: Hans de Goede <hdegoede@redhat.com>

diff --git a/arch/arm/cpu/armv7/ls102xa/psci.S b/arch/arm/cpu/armv7/ls102xa/psci.S
index ba043ef..f9b26b4 100644
--- a/arch/arm/cpu/armv7/ls102xa/psci.S
+++ b/arch/arm/cpu/armv7/ls102xa/psci.S
@@ -29,16 +29,16 @@
 	@ r2 = target PC
 .globl	psci_cpu_on
 psci_cpu_on:
-	push	{lr}
+	push	{r4, r5, r6, lr}
 
 	@ Clear and Get the correct CPU number
 	@ r1 = 0xf01
-	and	r1, r1, #0xff
+	and	r4, r1, #0xff
 
-	mov	r0, r1
-	bl	psci_get_cpu_stack_top
-	str	r2, [r0]
-	dsb
+	mov	r0, r4
+	mov	r1, r2
+	bl	psci_save_target_pc
+	mov	r1, r4
 
 	@ Get DCFG base address
 	movw	r4, #(CONFIG_SYS_FSL_GUTS_ADDR & 0xffff)
@@ -101,7 +101,7 @@ holdoff_release:
 	@ Return
 	mov	r0, #ARM_PSCI_RET_SUCCESS
 
-	pop	{lr}
+	pop	{r4, r5, r6, lr}
 	bx	lr
 
 .globl	psci_cpu_off
diff --git a/arch/arm/cpu/armv7/mx7/psci.S b/arch/arm/cpu/armv7/mx7/psci.S
index d9e9fbf..96e88d6 100644
--- a/arch/arm/cpu/armv7/mx7/psci.S
+++ b/arch/arm/cpu/armv7/mx7/psci.S
@@ -11,17 +11,20 @@
 
 .globl psci_cpu_on
 psci_cpu_on:
-	push	{lr}
+	push	{r4, r5, lr}
 
+	mov	r4, r0
+	mov	r5, r1
 	mov	r0, r1
-	bl	psci_get_cpu_stack_top
-	str	r2, [r0]
-	dsb
+	mov	r1, r2
+	bl	psci_save_target_pc
 
+	mov	r0, r4
+	mov	r1, r5
 	ldr	r2, =psci_cpu_entry
 	bl	imx_cpu_on
 
-	pop	{pc}
+	pop	{r4, r5, pc}
 
 .globl psci_cpu_off
 psci_cpu_off:
diff --git a/arch/arm/cpu/armv7/psci.S b/arch/arm/cpu/armv7/psci.S
index bfc4475..50dfddb 100644
--- a/arch/arm/cpu/armv7/psci.S
+++ b/arch/arm/cpu/armv7/psci.S
@@ -245,8 +245,7 @@ ENTRY(psci_cpu_entry)
 	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_target_pc		@ target PC => r0
 	b	_do_nonsec_entry
 ENDPROC(psci_cpu_entry)
 
diff --git a/arch/arm/cpu/armv7/sunxi/psci.c b/arch/arm/cpu/armv7/sunxi/psci.c
index be3a1fb..7ac8406 100644
--- a/arch/arm/cpu/armv7/sunxi/psci.c
+++ b/arch/arm/cpu/armv7/sunxi/psci.c
@@ -209,9 +209,8 @@ int __secure psci_cpu_on(u32 __always_unused unused, u32 mpidr, u32 pc)
 		(struct sunxi_cpucfg_reg *)SUNXI_CPUCFG_BASE;
 	u32 cpu = (mpidr & 0x3);
 
-	/* store target PC at target CPU stack top */
-	writel(pc, psci_get_cpu_stack_top(cpu));
-	DSB;
+	/* store target PC */
+	psci_save_target_pc(cpu, pc);
 
 	/* Set secondary core power on PC */
 	writel((u32)&psci_cpu_entry, &cpucfg->priv0);
diff --git a/arch/arm/mach-tegra/psci.S b/arch/arm/mach-tegra/psci.S
index 85d5b6b..645d08f 100644
--- a/arch/arm/mach-tegra/psci.S
+++ b/arch/arm/mach-tegra/psci.S
@@ -85,12 +85,13 @@ _loop:	wfi
 ENDPROC(psci_cpu_off)
 
 ENTRY(psci_cpu_on)
-	push	{lr}
+	push	{r4, r5, r6, lr}
 
+	mov	r4, r1
 	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
+	mov	r1, r2
+	bl	psci_save_target_pc	@ store target PC
+	mov	r1, r4
 
 	ldr	r6, =TEGRA_RESET_EXCEPTION_VECTOR
 	ldr	r5, =psci_cpu_entry
@@ -103,7 +104,7 @@ ENTRY(psci_cpu_on)
 	str	r5, [r6, r2]
 
 	mov	r0, #ARM_PSCI_RET_SUCCESS	@ Return PSCI_RET_SUCCESS
-	pop	{pc}
+	pop	{r4, r5, r6, pc}
 ENDPROC(psci_cpu_on)
 
 	.popsection
-- 
cgit v0.10.2