From: Hongbo Zhang hongbo.zhang@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.
This patch in current format is for easier review, there are some lines duplication with previous saving target PC codes, e.g. codes of calling the psci_get_cpu_stack_top, this will be optimized by the following patch.
Signed-off-by: Hongbo Zhang hongbo.zhang@nxp.com Signed-off-by: Wang Dongsheng dongsheng.wang@nxp.com --- arch/arm/cpu/armv7/ls102xa/psci.S | 7 +++++++ arch/arm/cpu/armv7/mx7/psci.S | 8 +++++++- arch/arm/cpu/armv7/nonsec_virt.S | 7 +++++++ arch/arm/cpu/armv7/sunxi/psci_sun6i.S | 7 +++++++ arch/arm/cpu/armv7/sunxi/psci_sun7i.S | 8 ++++++++ arch/arm/include/asm/psci.h | 1 + arch/arm/mach-tegra/psci.S | 6 ++++++ 7 files changed, 43 insertions(+), 1 deletion(-)
diff --git a/arch/arm/cpu/armv7/ls102xa/psci.S b/arch/arm/cpu/armv7/ls102xa/psci.S index e1293ed..1303909 100644 --- a/arch/arm/cpu/armv7/ls102xa/psci.S +++ b/arch/arm/cpu/armv7/ls102xa/psci.S @@ -27,6 +27,7 @@
@ r1 = target CPU @ r2 = target PC + @ r3 = target Conetxt ID .globl psci_cpu_on psci_cpu_on: push {lr} @@ -41,6 +42,12 @@ psci_cpu_on: str r2, [r0] dsb
+ mov r0, r1 + bl psci_get_cpu_stack_top + sub r0, r0, #PSCI_CONTEXT_ID_OFFSET + str r3, [r0] + dsb + @ Get DCFG base address movw r4, #(CONFIG_SYS_FSL_GUTS_ADDR & 0xffff) movt r4, #(CONFIG_SYS_FSL_GUTS_ADDR >> 16) diff --git a/arch/arm/cpu/armv7/mx7/psci.S b/arch/arm/cpu/armv7/mx7/psci.S index 02ca076..90b8b9e 100644 --- a/arch/arm/cpu/armv7/mx7/psci.S +++ b/arch/arm/cpu/armv7/mx7/psci.S @@ -24,7 +24,7 @@ psci_arch_init:
@ r1 = target CPU @ r2 = target PC - + @ r3 = target Conetxt ID .globl psci_cpu_on psci_cpu_on: push {lr} @@ -35,6 +35,12 @@ psci_cpu_on: str r2, [r0] dsb
+ mov r0, r1 + bl psci_get_cpu_stack_top + sub r0, r0, #PSCI_CONTEXT_ID_OFFSET + str r3, [r0] + dsb + ldr r2, =psci_cpu_entry bl imx_cpu_on
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/sunxi/psci_sun6i.S b/arch/arm/cpu/armv7/sunxi/psci_sun6i.S index 51241ec..2c9b078 100644 --- a/arch/arm/cpu/armv7/sunxi/psci_sun6i.S +++ b/arch/arm/cpu/armv7/sunxi/psci_sun6i.S @@ -130,6 +130,7 @@ out: mcr p15, 0, r7, c1, c1, 0
@ r1 = target CPU @ r2 = target PC + @ r3 = target Conetxt ID .globl psci_cpu_on psci_cpu_on: push {lr} @@ -140,6 +141,12 @@ psci_cpu_on: str r2, [r0] @ store target PC dsb
+ mov r0, r1 + bl psci_get_cpu_stack_top + sub r0, r0, #PSCI_CONTEXT_ID_OFFSET + str r3, [r0] + dsb + movw r0, #(SUN6I_CPUCFG_BASE & 0xffff) movt r0, #(SUN6I_CPUCFG_BASE >> 16)
diff --git a/arch/arm/cpu/armv7/sunxi/psci_sun7i.S b/arch/arm/cpu/armv7/sunxi/psci_sun7i.S index 50ba355..c1f117a 100644 --- a/arch/arm/cpu/armv7/sunxi/psci_sun7i.S +++ b/arch/arm/cpu/armv7/sunxi/psci_sun7i.S @@ -119,6 +119,7 @@ out: mcr p15, 0, r7, c1, c1, 0
@ r1 = target CPU @ r2 = target PC + @ r3 = target Conetxt ID .globl psci_cpu_on psci_cpu_on: push {lr} @@ -129,6 +130,13 @@ psci_cpu_on: str r2, [r0] @ store target PC dsb
+ + mov r0, r1 + bl psci_get_cpu_stack_top + sub r0, r0, #PSCI_CONTEXT_ID_OFFSET + str r3, [r0] + dsb + movw r0, #(SUN7I_CPUCFG_BASE & 0xffff) movt r0, #(SUN7I_CPUCFG_BASE >> 16)
diff --git a/arch/arm/include/asm/psci.h b/arch/arm/include/asm/psci.h index d0f5d26..d703aeb 100644 --- a/arch/arm/include/asm/psci.h +++ b/arch/arm/include/asm/psci.h @@ -21,6 +21,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)
/* PSCI interfaces */ #define PSCI_FN_BASE 0x84000000 diff --git a/arch/arm/mach-tegra/psci.S b/arch/arm/mach-tegra/psci.S index 037c142..3837d95 100644 --- a/arch/arm/mach-tegra/psci.S +++ b/arch/arm/mach-tegra/psci.S @@ -96,6 +96,12 @@ ENTRY(psci_cpu_on) str r2, [r0] @ store target PC dsb
+ mov r0, r1 + bl psci_get_cpu_stack_top + sub r0, r0, #PSCI_CONTEXT_ID_OFFSET + str r3, [r0] + dsb + ldr r6, =TEGRA_RESET_EXCEPTION_VECTOR ldr r5, =psci_cpu_entry str r5, [r6]