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

[Arnab Basu]

Setup the ARMv8 PSCI code just before switching to EL2 and jumping
to the kernel.

Signed-off-by: Arnab Basu <arnab_basu at rocketmail.com>
Cc: Bhupesh Sharma <bhupesh.sharma at freescale.com>
Cc: Marc Zyngier <marc.zyngier at 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(+)

diff --git a/arch/arm/cpu/armv8/cpu.c b/arch/arm/cpu/armv8/cpu.c
index 2c8be1c..286ac05 100644
--- a/arch/arm/cpu/armv8/cpu.c
+++ b/arch/arm/cpu/armv8/cpu.c
@@ -154,3 +154,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 6028020..508e6eb 100644
--- a/arch/arm/cpu/armv8/psci.S
+++ b/arch/arm/cpu/armv8/psci.S
@@ -160,3 +160,9 @@ psci_vectors:
 	b	unhandled_exception	/* 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 f3f4ace..a2074a4 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 0c1298a..0430fa5 100644
--- a/arch/arm/lib/bootm.c
+++ b/arch/arm/lib/bootm.c
@@ -275,6 +275,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, NULL, NULL, NULL);
 	}
-- 
1.9.1