Setup the ARMv8 PSCI code just before switching to EL2 and jumping to the kernel.
Signed-off-by: Arnab Basu arnab.basu@freescale.com Reviewed-by: Bhupesh Sharma bhupesh.sharma@freescale.com Cc: Marc Zyngier marc.zyngier@arm.com --- arch/arm/cpu/armv8/cpu.c | 23 +++++++++++++++++++++++ arch/arm/cpu/armv8/psci.S | 6 ++++++ arch/arm/include/asm/system.h | 3 +++ arch/arm/lib/bootm.c | 3 +++ 4 files changed, 35 insertions(+), 0 deletions(-)
diff --git a/arch/arm/cpu/armv8/cpu.c b/arch/arm/cpu/armv8/cpu.c index e06c3cc..55d2654 100644 --- a/arch/arm/cpu/armv8/cpu.c +++ b/arch/arm/cpu/armv8/cpu.c @@ -41,3 +41,26 @@ int cleanup_before_linux(void)
return 0; } + +#ifdef CONFIG_ARMV8_PSCI + +static void relocate_secure_section(void) +{ +#ifdef CONFIG_ARMV8_SECURE_BASE + size_t sz = __secure_end - __secure_start; + + memcpy((void *)CONFIG_ARMV8_SECURE_BASE, __secure_start, sz); + flush_dcache_range(CONFIG_ARMV8_SECURE_BASE, + CONFIG_ARMV8_SECURE_BASE + sz + 1); + invalidate_icache_all(); +#endif +} + +void setup_psci(void) +{ + relocate_secure_section(); + fixup_vectors(); + psci_arch_init(); +} + +#endif diff --git a/arch/arm/cpu/armv8/psci.S b/arch/arm/cpu/armv8/psci.S index 5f4e3b2..9eaad3a 100644 --- a/arch/arm/cpu/armv8/psci.S +++ b/arch/arm/cpu/armv8/psci.S @@ -169,3 +169,9 @@ psci_vectors: b default_psci_vector /* Lower EL Error (64b) */
.popsection + +ENTRY(fixup_vectors) + adr x0, psci_vectors + msr vbar_el3, x0 + ret +ENDPROC(fixup_vectors) diff --git a/arch/arm/include/asm/system.h b/arch/arm/include/asm/system.h index a1066d4..214fed3 100644 --- a/arch/arm/include/asm/system.h +++ b/arch/arm/include/asm/system.h @@ -78,6 +78,9 @@ void gic_send_sgi(unsigned long sgino); void wait_for_wakeup(void); void smp_kick_all_cpus(void); #ifdef CONFIG_ARMV8_PSCI +void setup_psci(void); +void fixup_vectors(void); +void psci_arch_init(void); extern char __secure_start[]; extern char __secure_end[]; #endif diff --git a/arch/arm/lib/bootm.c b/arch/arm/lib/bootm.c index 178e8fb..576c7d5 100644 --- a/arch/arm/lib/bootm.c +++ b/arch/arm/lib/bootm.c @@ -251,6 +251,9 @@ static void boot_jump_linux(bootm_headers_t *images, int flag) announce_and_cleanup(fake);
if (!fake) { +#ifdef CONFIG_ARMV8_PSCI + setup_psci(); +#endif do_nonsec_virt_switch(); kernel_entry(images->ft_addr); }