From e43458ea41105b3c7056261771499cd7eb94f8df Mon Sep 17 00:00:00 2001
From: Zbigniew Bodek <zbb@FreeBSD.org>
Date: Tue, 25 Aug 2015 14:49:11 +0000
Subject: [PATCH] Leave hypervisor mode upon startup on ARMv7

If ARMv7 boots in HYP mode, switch to SVC32.

Reviewed by:   ian
Submitted by:  Wojciech Macek <wma@semihalf.com>
               Jakub Palider  <jpa@semihalf.com>
Obtained from: Semihalf
Sponsored by:  Annapurna Labs
Differential Revision: https://reviews.freebsd.org/D1810
---
 sys/arm/arm/locore-v6.S | 39 +++++++++++++++++++++++++++++++++++++++
 1 file changed, 39 insertions(+)

diff --git a/sys/arm/arm/locore-v6.S b/sys/arm/arm/locore-v6.S
index cddcdab787cc..7990f28ab4e7 100644
--- a/sys/arm/arm/locore-v6.S
+++ b/sys/arm/arm/locore-v6.S
@@ -45,12 +45,47 @@ __FBSDID("$FreeBSD$");
 #define	PTE1_SIZE	L1_S_SIZE
 #endif
 
+#if __ARM_ARCH >= 7
+#if defined(__ARM_ARCH_7VE__) || defined(__clang__)
+/*
+ * HYP support is in bintuils >= 2.21 and gcc >= 4.9 defines __ARM_ARCH_7VE__
+ * when enabled. llvm >= 3.6 supports it too.
+ */
+.arch_extension virt
+#define	MSR_ELR_HYP(regnum)	msr	elr_hyp, lr
+#define	ERET	eret
+#else
+#define MSR_ELR_HYP(regnum) .word (0xe12ef300 | regnum)
+#define ERET .word 0xe160006e
+#endif
+#endif /* __ARM_ARCH >= 7 */
+
 /* A small statically-allocated stack used only during initarm() and AP startup. */
 #define	INIT_ARM_STACK_SIZE	2048
 
 	.text
 	.align	2
 
+#if __ARM_ARCH >= 7
+#define LEAVE_HYP							\
+       /* Leave HYP mode */						;\
+       mrs	r0, cpsr		   				;\
+       and	r0, r0, #(PSR_MODE)     /* Mode is in the low 5 bits of CPSR */	;\
+       teq	r0, #(PSR_HYP32_MODE)   /* Hyp Mode? */			;\
+       bne	1f							;\
+       /* Ensure that IRQ, FIQ and Aborts will be disabled after eret */;\
+       mrs	r0, spsr						;\
+       orr	r0, r0, #(PSR_I | PSR_F | PSR_A)			;\
+       msr	spsr, r0						;\
+       /* Exit hypervisor mode */					;\
+       adr	lr, 1f							;\
+       MSR_ELR_HYP(14)							;\
+       ERET								;\
+1:
+#else
+#define LEAVE_HYP
+#endif /* __ARM_ARCH >= 7 */
+
 /*
  * On entry for FreeBSD boot ABI:
  *	r0 - metadata pointer or 0 (boothowto on AT91's boot2)
@@ -76,6 +111,8 @@ ASENTRY_NP(_start)
 	mov	r10, r2		/* Save meta data */
 	mov	r11, r3		/* Future expansion */
 
+	LEAVE_HYP
+
 	/*
 	 * Check whether data cache is enabled.  If it is, then we know
 	 * current tags are valid (not power-on garbage values) and there
@@ -401,6 +438,8 @@ ASENTRY_NP(mpentry)
 	/* Make sure interrupts are disabled. */
 	cpsid	ifa
 
+	LEAVE_HYP
+
 	/* Setup core, disable all caches. */
 	mrc	CP15_SCTLR(r0)
 	bic	r0, #CPU_CONTROL_MMU_ENABLE