From: Wang Dongsheng dongsheng.wang@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@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