[U-Boot] [PATCH v2 2/2] ARM: OMAP5+: Override switch_to_hypervisor function

[Keerthy]

Override the switch_to_hypervisor function to switch cpu to hypervisor
mode using the available ROM code hook early in the boot phase before
the boot loader checks for HYP mode.

Based on the work done by Jonathan Bergsagel jbergsagel at ti.com.

Reviewed-by: Tom Rini <trini at konsulko.com>
Signed-off-by: Keerthy <j-keerthy at ti.com>
---
 arch/arm/cpu/armv7/omap-common/lowlevel_init.S | 21 +++++++++++++++++++++
 1 file changed, 21 insertions(+)

diff --git a/arch/arm/cpu/armv7/omap-common/lowlevel_init.S b/arch/arm/cpu/armv7/omap-common/lowlevel_init.S
index 66a3b3d..1026232 100644
--- a/arch/arm/cpu/armv7/omap-common/lowlevel_init.S
+++ b/arch/arm/cpu/armv7/omap-common/lowlevel_init.S
@@ -24,6 +24,27 @@ ENTRY(save_boot_params)
 	str	r0, [r1]
 	b	save_boot_params_ret
 ENDPROC(save_boot_params)
+
+ENTRY(switch_to_hypervisor)
+/*
+ * Switch to hypervisor mode
+ */
+	adr	r0, save_sp
+	str	sp, [r0]
+	adr	r1, restore_from_hyp
+	ldr	r0, =0x102
+#ifdef CONFIG_TI_SECURE_DEVICE
+	b	omap_smc_sec
+#else
+	b	omap_smc1
+#endif
+restore_from_hyp:
+	adr	r0, save_sp
+	ldr	sp, [r0]
+	b	switch_to_hypervisor_ret
+save_sp:
+	.word	0x0
+ENDPROC(switch_to_hypervisor)
 #endif
 
 ENTRY(omap_smc1)
-- 
1.9.1