[U-Boot] [PATCH 5/9] ARM: ARMv7: PSCI: Pass contextID to target CPU

[Dongsheng Wang]

From: Wang Dongsheng <dongsheng.wang at nxp.com>

To follow PSCI, we need to save a "Context ID" in CPU_ON,
and pass it to a CPU when it first enters the OS.

Signed-off-by: Wang Dongsheng <dongsheng.wang at nxp.com>
---
 arch/arm/cpu/armv7/nonsec_virt.S |  4 ++++
 arch/arm/cpu/armv7/psci.S        | 28 ++++++++++++++++++++++++++++
 2 files changed, 32 insertions(+)

diff --git a/arch/arm/cpu/armv7/nonsec_virt.S b/arch/arm/cpu/armv7/nonsec_virt.S
index 31d1c9e..779f25d 100644
--- a/arch/arm/cpu/armv7/nonsec_virt.S
+++ b/arch/arm/cpu/armv7/nonsec_virt.S
@@ -89,6 +89,10 @@ _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_context_id		@ Context ID must 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 c3651bd..a83fa67 100644
--- a/arch/arm/cpu/armv7/psci.S
+++ b/arch/arm/cpu/armv7/psci.S
@@ -21,6 +21,7 @@
 #include <asm/psci.h>
 
 #define SAVE_SPACE_TARGET_PC_OFFSET	0x0
+#define SAVE_SPACE_CONTEXT_ID_OFFSET	0x4
 
 	.pushsection ._secure.text, "ax"
 
@@ -201,6 +202,10 @@ ENTRY(psci_cpu_on_common)
 	@ Save target PC into stack
 	bl	psci_save_target_pc
 
+	@ Save target context into stack
+	mov	r0, r1
+	bl	psci_save_cpu_context_id
+
 	@ Still pass on:
 	@ r1 = target CPU
 	@ r2 = target PC
@@ -275,6 +280,29 @@ ENTRY(psci_save_target_pc)
 	pop	{pc}
 ENDPROC(psci_save_target_pc)
 
+@ Expects cpu ID in r0 and context ID in r3, please ignore the return value.
+ENTRY(psci_save_cpu_context_id)
+	push	{lr}
+
+	@ Save context id
+	bl	psci_get_cpu_save_space
+	str	r3, [r0, #SAVE_SPACE_CONTEXT_ID_OFFSET]
+	dsb
+
+	pop	{pc}
+ENDPROC(psci_save_cpu_context_id)
+
+@ Expect cpu ID in r0, return context ID in r0.
+ENTRY(psci_get_cpu_context_id)
+	push	{lr}
+
+	@ Get context id
+	bl	psci_get_cpu_save_space
+	ldr	r0, [r0, #SAVE_SPACE_CONTEXT_ID_OFFSET]
+
+	pop	{pc}
+ENDPROC(psci_get_cpu_context_id)
+
 ENTRY(psci_cpu_entry)
 	bl	psci_enable_smp
 
-- 
2.1.0.27.g96db324