[U-Boot] [PATCH v5 4/8] ARMv7: PSCI: add codes to save context ID for CPU_ON

[macro.wave.z at gmail.com]

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

According to latest PSCI specification, the context ID is needed by CPU_ON.
This patch saves context ID to the second lowest address of the stack (next to
where target PC is saved), and restores it to r0 when needed while target CPU
booting up.

Signed-off-by: Hongbo Zhang <hongbo.zhang at nxp.com>
Signed-off-by: Wang Dongsheng <dongsheng.wang at nxp.com>
---
 arch/arm/cpu/armv7/nonsec_virt.S | 7 +++++++
 arch/arm/cpu/armv7/psci.S        | 4 +++-
 arch/arm/include/asm/psci.h      | 1 +
 3 files changed, 11 insertions(+), 1 deletion(-)

diff --git a/arch/arm/cpu/armv7/nonsec_virt.S b/arch/arm/cpu/armv7/nonsec_virt.S
index b7563ed..6566643 100644
--- a/arch/arm/cpu/armv7/nonsec_virt.S
+++ b/arch/arm/cpu/armv7/nonsec_virt.S
@@ -11,6 +11,7 @@
 #include <asm/gic.h>
 #include <asm/armv7.h>
 #include <asm/proc-armv/ptrace.h>
+#include <asm/psci.h>
 
 .arch_extension sec
 .arch_extension virt
@@ -89,6 +90,12 @@ _secure_monitor:
 	movne	r4, #0
 	mcrrne	p15, 4, r4, r4, c14		@ Reset CNTVOFF to zero
 1:
+#ifdef CONFIG_ARMV7_PSCI
+	bl	psci_get_cpu_id
+	bl	psci_get_cpu_stack_top
+	sub	r0, r0, #PSCI_CONTEXT_ID_OFFSET
+	ldr	r0, [r0]			@ get Context ID in r0
+#endif
 	mov	lr, ip
 	mov	ip, #(F_BIT | I_BIT | A_BIT)	@ Set A, I and F
 	tst	lr, #1				@ Check for Thumb PC
diff --git a/arch/arm/cpu/armv7/psci.S b/arch/arm/cpu/armv7/psci.S
index 5b235df..3ba9e51 100644
--- a/arch/arm/cpu/armv7/psci.S
+++ b/arch/arm/cpu/armv7/psci.S
@@ -253,7 +253,7 @@ ENTRY(psci_enable_smp)
 ENDPROC(psci_enable_smp)
 .weak psci_enable_smp
 
-/* expects target CPU in r1, target PC in r2 */
+/* expects target CPU in r1, target PC in r2, target conetxt ID in r3 */
 ENTRY(psci_cpu_on_common)
 	push	{lr}
 
@@ -261,6 +261,8 @@ ENTRY(psci_cpu_on_common)
 	bl	psci_get_cpu_stack_top		@ get stack top of target CPU
 	sub	r5, r0, #PSCI_TARGET_PC_OFFSET
 	str	r2, [r5]			@ store target PC
+	sub	r5, r0, #PSCI_CONTEXT_ID_OFFSET
+	str	r3, [r5]			@ store target context ID
 	dsb
 
 	pop	{pc}
diff --git a/arch/arm/include/asm/psci.h b/arch/arm/include/asm/psci.h
index cb08544..bedcd30 100644
--- a/arch/arm/include/asm/psci.h
+++ b/arch/arm/include/asm/psci.h
@@ -66,6 +66,7 @@
 /* size of percpu stack, 1kB */
 #define PSCI_PERCPU_STACK_SIZE		0x400
 #define PSCI_TARGET_PC_OFFSET		(PSCI_PERCPU_STACK_SIZE - 4)
+#define PSCI_CONTEXT_ID_OFFSET		(PSCI_PERCPU_STACK_SIZE - 8)
 
 #ifndef __ASSEMBLY__
 int psci_update_dt(void *fdt);
-- 
2.1.4