[U-Boot] [PATCH 7/8] ARMv8: PSCI: Setup ARMv8 PSCI

Arnab Basu
2014-08-27T20:30:00Z

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);
}
--
1.7.7.4