Index: head/sys/arm/arm/hypervisor-stub.S
===================================================================
--- head/sys/arm/arm/hypervisor-stub.S
+++ head/sys/arm/arm/hypervisor-stub.S
@@ -0,0 +1,87 @@
+/*
+ * Copyright (C) 2015 Mihai Carabas <mihai.carabas@gmail.com>
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#include "assym.s"
+#include <sys/syscall.h>
+#include <machine/asm.h>
+#include <machine/asmacros.h>
+#include <machine/armreg.h>
+
+__FBSDID("$FreeBSD$");
+
+#if __ARM_ARCH >= 7
+#if defined(__ARM_ARCH_7VE__) || defined(__clang__)
+.arch_extension virt
+#endif
+
+ASENTRY_NP(hypervisor_stub_vect_install)
+
+	/* Install hypervisor stub vectors. */
+	adr	r0, hypervisor_stub_vect
+	mcr	CP15_HVBAR(r0)
+
+	/* Disable all the traps in the hypervisor. */
+	mov	r0, #0
+	mcr	CP15_HCR(r0)
+	mcr	CP15_HCPTR(r0)
+	mcr	CP15_HSTR(r0)
+	mcr	CP15_HSCTLR(r0)
+
+	/* Don't disable access to perf-mon from PL0,1 and preserve HPMN. */
+	mrc	CP15_HDCR(r0)
+	and	r0, #(ARM_CP15_HDCR_HPMN)
+	/*  Caller implicit instruction barrier in the ERET. */
+	mcr	CP15_HDCR(r0)
+
+	RET
+
+END(hypervisor_stub_vect_install)
+
+ASENTRY_NP(hypervisor_stub_trap)
+	/*
+	 * If the first parameter is -1 than return the
+	 * exception vector (HVBAR), otherwise set it to
+	 * the value of it.
+	 */
+	cmp	r0, #-1
+	mrceq	CP15_HVBAR(r0)
+	mcrne	CP15_HVBAR(r0)
+	ERET
+END(hypervisor_stub_trap)
+
+	.globl hypervisor_stub_vect
+	.align 5
+_C_LABEL(hypervisor_stub_vect):
+	.word 0 /* Reset */
+	.word 0 /* undev */
+	.word 0 /* SMC */
+	.word 0 /* PABT */
+	.word 0 /* DABT */
+	b	hypervisor_stub_trap /* HYP-Mode */
+	.word 0 /* FIQ */
+	.word 0 /* IRQ */
+#endif /* __ARM_ARCH >= 7 */
+
Index: head/sys/arm/arm/locore-v6.S
===================================================================
--- head/sys/arm/arm/locore-v6.S
+++ head/sys/arm/arm/locore-v6.S
@@ -38,7 +38,6 @@
 
 __FBSDID("$FreeBSD$");
 
-
 #if __ARM_ARCH >= 7
 #if defined(__ARM_ARCH_7VE__) || defined(__clang__)
 /*
@@ -46,11 +45,6 @@
  * 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 */
 
@@ -61,12 +55,17 @@
 	.align	2
 
 #if __ARM_ARCH >= 7
-#define	LEAVE_HYP							\
+#define	HANDLE_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							;\
+	/* Install Hypervisor Stub Exception Vector */			;\
+	bl hypervisor_stub_vect_install					;\
+	mov	r0, 0							;\
+	adr	r1, hypmode_enabled					;\
+	str	r0, [r1]						;\
 	/* Ensure that IRQ, FIQ and Aborts will be disabled after eret */ ;\
 	mrs	r0, cpsr						;\
 	bic	r0, r0, #(PSR_MODE)					;\
@@ -74,12 +73,16 @@
 	orr	r0, r0, #(PSR_I | PSR_F | PSR_A)			;\
 	msr	spsr_cxsf, r0						;\
 	/* Exit hypervisor mode */					;\
-	adr	lr, 1f							;\
+	adr	lr, 2f							;\
 	MSR_ELR_HYP(14)							;\
 	ERET								;\
-1:
+1:									;\
+	mov	r0, -1							;\
+	adr	r1, hypmode_enabled					;\
+	str	r0, [r1]						;\
+2:
 #else
-#define	LEAVE_HYP
+#define	HANDLE_HYP
 #endif /* __ARM_ARCH >= 7 */
 
 /*
@@ -107,7 +110,8 @@
 	mov	r10, r2		/* Save meta data */
 	mov	r11, r3		/* Future expansion */
 
-	LEAVE_HYP
+	# If HYP-MODE is active, install an exception vector stub
+	HANDLE_HYP
 
 	/*
 	 * Check whether data cache is enabled.  If it is, then we know
@@ -411,6 +415,9 @@
 
 VA_TO_PA_POINTER(Lpagetable, boot_pt1)
 
+	.global _C_LABEL(hypmode_enabled)
+_C_LABEL(hypmode_enabled):
+	.word 0
 
 .Lstart:
 	.word	_edata			/* Note that these three items are */
@@ -444,7 +451,7 @@
 	/* Make sure interrupts are disabled. */
 	cpsid	ifa
 
-	LEAVE_HYP
+	HANDLE_HYP
 
 	/* Setup core, disable all caches. */
 	mrc	CP15_SCTLR(r0)
Index: head/sys/arm/include/armreg.h
===================================================================
--- head/sys/arm/include/armreg.h
+++ head/sys/arm/include/armreg.h
@@ -477,4 +477,7 @@
 
 #define THUMB_INSN_SIZE		2		/* Some are 4 bytes.  */
 
+/* ARM Hypervisor Related Defines */
+#define	ARM_CP15_HDCR_HPMN	0x0000001f
+
 #endif /* !MACHINE_ARMREG_H */
Index: head/sys/arm/include/asm.h
===================================================================
--- head/sys/arm/include/asm.h
+++ head/sys/arm/include/asm.h
@@ -235,6 +235,15 @@
 #define DSB	dsb
 #define DMB	dmb
 #define WFI	wfi
+
+#if defined(__ARM_ARCH_7VE__) || defined(__clang__)
+#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
+
 #elif __ARM_ARCH == 6
 #define ISB	mcr CP15_CP15ISB
 #define DSB	mcr CP15_CP15DSB
Index: head/sys/arm/include/sysreg.h
===================================================================
--- head/sys/arm/include/sysreg.h
+++ head/sys/arm/include/sysreg.h
@@ -76,6 +76,12 @@
 #define	CP15_ID_MMFR2(rr)	p15, 0, rr, c0, c1,  6 /* Memory Model Feature Register 2 */
 #define	CP15_ID_MMFR3(rr)	p15, 0, rr, c0, c1,  7 /* Memory Model Feature Register 3 */
 
+#define	CP15_HCR(rr)		p15, 4, rr, c1, c1, 0 /* Hyp Configuration Register */
+#define	CP15_HCPTR(rr)		p15, 4, rr, c1, c1, 2 /* Hyp Coprocessor Trap Register */
+#define	CP15_HSTR(rr)		p15, 4, rr, c1, c1, 3 /* Hyp System Trap Register */
+#define	CP15_HSCTLR(rr)		p15, 4, rr, c1, c0, 0 /* Hyp System Control Register */
+#define	CP15_HDCR(rr)		p15, 4, rr, c1, c1, 1 /* Hyp Debug Configuration Register */
+
 #define	CP15_ID_ISAR0(rr)	p15, 0, rr, c0, c2,  0 /* Instruction Set Attribute Register 0 */
 #define	CP15_ID_ISAR1(rr)	p15, 0, rr, c0, c2,  1 /* Instruction Set Attribute Register 1 */
 #define	CP15_ID_ISAR2(rr)	p15, 0, rr, c0, c2,  2 /* Instruction Set Attribute Register 2 */
@@ -265,6 +271,7 @@
 #define	CP15_MVBAR(rr)		p15, 0, rr, c12, c0, 1 /* Monitor Vector Base Address Register */
 
 #define	CP15_ISR(rr)		p15, 0, rr, c12, c1, 0 /* Interrupt Status Register */
+#define	CP15_HVBAR(rr)		p15, 4, rr, c12, c0, 0 /* Hyp Vector Base Address Register*/
 
 /*
  * CP15 C13 registers
Index: head/sys/conf/files.arm
===================================================================
--- head/sys/conf/files.arm
+++ head/sys/conf/files.arm
@@ -67,6 +67,7 @@
 arm/arm/intr.c			optional	!intrng
 kern/subr_intr.c		optional	intrng
 arm/arm/locore.S		standard	no-obj
+arm/arm/hypervisor-stub.S	optional	armv6
 arm/arm/machdep.c		standard
 arm/arm/machdep_boot.c		standard
 arm/arm/machdep_kdb.c		standard