[U-Boot] [PATCH] HACK: enable HYP mode on OMAP5

[Alexander Tarasikov]

---
 arch/arm/lib/bootm.c | 89 ++++++++++++++++++++++++++++++++++++++++++++++++++--
 1 file changed, 87 insertions(+), 2 deletions(-)

diff --git a/arch/arm/lib/bootm.c b/arch/arm/lib/bootm.c
index eefb456..63395c3 100644
--- a/arch/arm/lib/bootm.c
+++ b/arch/arm/lib/bootm.c
@@ -8,6 +8,9 @@
  * Marius Groeger <mgroeger at sysgo.de>
  *
  * Copyright (C) 2001  Erik Mouw (J.A.K.Mouw at its.tudelft.nl)
+ * HYP entry (C) 2012  Ian Molton <ian.molton at codethink.co.uk>
+ *                and  Clemens Fischer <clemens.fischer at h-da.de>
+ *           (C) 2013 Alexander Tarasikov <alexander.tarasikov at gmail.com>
  *
  * SPDX-License-Identifier:	GPL-2.0+
  */
@@ -26,6 +29,89 @@ DECLARE_GLOBAL_DATA_PTR;
 
 static struct tag *params;
 
+/*
+ * function called by cpu 1 after wakeup
+ */
+extern void __hyp_init_sec(void);
+
+asm (
+	".pushsection .text\n"
+	".global __hyp_init_sec\n"
+	"__hyp_init_sec:\n"
+		"ldr r12, =0x102\n"
+		"adr r0, hyp_init_cont\n"
+		"dsb\n"
+		"isb\n"
+		"dmb\n"
+		"smc #0\n"
+		"hyp_init_cont: ldr r1, =0x48281800\n" // AUX_CORE_BOOT_0
+		"mov r2, #0\n"
+		"str r2, [r1, #4]\n"
+		"dsb\n"
+		"isb\n"
+		"dmb\n"
+		"wait: wfe\n"
+		"ldr r2, [r1, #4]\n"
+		"cmp r2, #0\n"
+		"movne pc, r2\n"
+		"b wait\n"
+	".popsection\n"
+);
+
+/*
+ * Enable HYP mode on the OMAP5 CPU
+ *
+ * FIXME: this needs to test to make sure its running on an OMAP5
+ *
+ * We wake up CPU1 at __hyp_init_sec which allows us to put it into HYP
+ * mode.
+ *
+ * CPU1 then clears AUX_CORE_BOOT_0 and enters WFE, until the kernel wakes it.
+ *
+ * In order to avoid CPU1 continuing execution on just about any event, we
+ * wait for AUX_CORE_BOOT_0 to contain a non-zero address, at which point
+ * we continue execution at that address.
+ *
+ */
+
+static void hyp_enable(void) {
+	 /*Wake up CPU1 and enable HYP on CPU0. */
+	asm(
+		"ldr r1, =0x48281800\n"     // AUX_CORE_BOOT_1
+		"ldr r2, =__hyp_init_sec\n"
+		"str r2, [r1, #4]\n"
+		"mov r2, #0x20\n"
+		"str r2, [r1]\n"            // AUX_CORE_BOOT_0
+		"isb\n"
+		"dmb\n"
+		"dsb\n"
+		"sev\n"                     // Wake CPU1
+		"adr r1, hyp_stack\n"
+		"stm r1, {r4-r14}\n" //save registers on the temporary stack
+		"ldr r12, =0x102\n" //start hypervisor SMC code
+		"adr r0, hyp_ena_cont\n" //address after SMC instruction
+		"dsb\n"
+		"isb\n"
+		"dmb\n"
+		"smc #0\n"                 // CPU0 -> HYP mode
+		"hyp_stack:\n"
+		".word 0x0\n"
+		".word 0x0\n"
+		".word 0x0\n"
+		".word 0x0\n"
+		".word 0x0\n"
+		".word 0x0\n"
+		".word 0x0\n"
+		".word 0x0\n"
+		".word 0x0\n"
+		".word 0x0\n"
+		".word 0x0\n"
+		"hyp_ena_cont: adr r1,hyp_stack\n"
+		"ldm   r1, {r4-r14}\n"
+		:::"r0", "r1", "r2", "r3", "cc", "memory"
+	);
+};
+
 static ulong get_sp(void)
 {
 	ulong ret;
@@ -144,7 +230,6 @@ static void setup_initrd_tag(bd_t *bd, ulong initrd_start, ulong initrd_end)
 
 	params->u.initrd.start = initrd_start;
 	params->u.initrd.size = initrd_end - initrd_start;
-
 	params = tag_next (params);
 }
 
@@ -185,7 +270,6 @@ __weak void setup_board_tags(struct tag **in_params) {}
 static void boot_prep_linux(bootm_headers_t *images)
 {
 	char *commandline = getenv("bootargs");
-
 	if (IMAGE_ENABLE_OF_LIBFDT && images->ft_len) {
 #ifdef CONFIG_OF_LIBFDT
 		debug("using: FDT\n");
@@ -246,6 +330,7 @@ static void boot_jump_linux(bootm_headers_t *images, int flag)
 	else
 		r2 = gd->bd->bi_boot_params;
 
+	hyp_enable();
 	if (!fake)
 		kernel_entry(0, machid, r2);
 }
-- 
1.8.4