[PATCH 1/3] Put both OMAP5 cores into HYP mode

[Ian Molton]

---
 arch/arm/lib/bootm.c |   75 ++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 75 insertions(+)

diff --git a/arch/arm/lib/bootm.c b/arch/arm/lib/bootm.c
index 802e833..bddbff3 100644
--- a/arch/arm/lib/bootm.c
+++ b/arch/arm/lib/bootm.c
@@ -4,6 +4,8 @@
  * 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>
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -30,6 +32,36 @@
 #include <libfdt.h>
 #include <fdt_support.h>
 
+/* A small stack to allow us to safely call the OMAP5 monitor API, in order to
+ * enable HYP mode. This must be accessible in HYP mode.
+ */
+unsigned int hyp_primary_stack[11];
+
+/*
+ * 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"
+		"mov r0, pc\n"
+		"smc 0x1\n"
+		"ldr r1, =0x48281804\n" // AUX_CORE_BOOT_1
+		"mov r2, #0\n"
+		"str r2, [r1]\n"
+		"isb\n"
+		"dsb\n"
+		"1: wfe\n"
+		"ldr r2, [r1]\n"
+		"cmp r2, #0\n"
+		"movne pc, r2\n"
+		"b 1b\n"
+	".popsection\n"
+);
+
 DECLARE_GLOBAL_DATA_PTR;
 
 #if defined (CONFIG_SETUP_MEMORY_TAGS) || \
@@ -93,6 +125,45 @@ static void announce_and_cleanup(void)
 	cleanup_before_linux();
 }
 
+/*
+ * 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.
+ *
+ */
+
+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, #0x200\n"
+		"str r2, [r1]\n"            // AUX_CORE_BOOT_0
+		"isb\n"
+		"dmb\n"
+		"dsb\n"
+		"sev\n"                     // Wake CPU1
+		"ldr r1,=hyp_primary_stack\n"
+		"ldr r12, =0x102\n"
+		"mov r0, pc\n"
+		"stm   r1, {r4-r14}\n"
+		"smc 0x1\n"                 // CPU0 -> HYP mode
+		"ldr r1,=hyp_primary_stack\n"
+		"ldm   r1, {r4-r14}\n"
+		:::"r0", "r1", "r2", "r3", "cc", "memory"
+	);
+};
+
 int do_bootm_linux(int flag, int argc, char *argv[], bootm_headers_t *images)
 {
 	bd_t	*bd = gd->bd;
@@ -152,6 +223,8 @@ int do_bootm_linux(int flag, int argc, char *argv[], bootm_headers_t *images)
 
 	announce_and_cleanup();
 
+	hyp_enable();
+
 	kernel_entry(0, machid, bd->bi_boot_params);
 	/* does not return */
 
@@ -210,6 +283,8 @@ static int bootm_linux_fdt(int machid, bootm_headers_t *images)
 
 	announce_and_cleanup();
 
+	hyp_enable();
+
 	kernel_entry(0, machid, *of_flat_tree);
 	/* does not return */
 
-- 
1.7.10.4


--------------060005000501070600070604--