From: Wang Dongsheng <dongsheng.w...@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.w...@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
_______________________________________________
U-Boot mailing list
U-Boot@lists.denx.de
http://lists.denx.de/mailman/listinfo/u-boot
