Index: sys/arm/arm/hypervisor-stub.S
===================================================================
--- sys/arm/arm/hypervisor-stub.S
+++ sys/arm/arm/hypervisor-stub.S
@@ -0,0 +1,52 @@
+#include "assym.s"
+#include <sys/syscall.h>
+#include <machine/asm.h>
+#include <machine/asmacros.h>
+#include <machine/armreg.h>
+#include <machine/sysreg.h>
+#include <machine/cpuconf.h>
+
+ASENTRY_NP(hypervisor_stub_vect_install)
+
+       /* Install hypervisor stub vectors. */
+       adr     r0, hypervisor_stub_vect
+       mcr     p15, 4, r0, c12, c0, 0  @ set HVBAR
+
+       /* Disable all the traps in the hypervisor. */
+       mov     r0, #0
+       mcr     p15, 4, r0, c1, c1, 0   @ HCR
+       mcr     p15, 4, r0, c1, c1, 2   @ HCPTR
+       mcr     p15, 4, r0, c1, c1, 3   @ HSTR
+       mcr     p15, 4, r0, c1, c0, 0   @ HSCTLR
+
+       /* Don't disable access to perf-mon from PL0,1 */
+       mrc     p15, 4, r0, c1, c1, 1   @ HDCR
+       and     r0, #0x1f               @ Preserve HPMN
+       mcr     p15, 4, r0, c1, c1, 1   @ HDCR
+
+       mov     pc, lr
+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   p15, 4, r0, c12, c0, 0  @ get HVBAR
+       mcrne   p15, 4, r0, c12, c0, 0  @ set HVBAR
+       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 */
Index: sys/arm/arm/locore-v6.S
===================================================================
--- sys/arm/arm/locore-v6.S
+++ sys/arm/arm/locore-v6.S
@@ -61,12 +61,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,14 +79,18 @@
        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 */

 /*
  * On entry for FreeBSD boot ABI:
  *     r0 - metadata pointer or 0 (boothowto on AT91's boot2)
@@ -107,8 +116,9 @@
        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
         * current tags are valid (not power-on garbage values) and there
@@ -411,20 +421,23 @@

 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 */
        .word   _ebss                   /* loaded with a single ldmia and */
        .word   svcstk                  /* must remain in order together. */

 .Lmainreturned:
        .asciz  "main() returned"
        .align  2

        .bss
 svcstk:
        .space  INIT_ARM_STACK_SIZE * MAXCPU

 /*
  * Memory for the initial pagetable. We are unable to place this in
  * the bss as this will be cleared after the table is loaded.
@@ -444,8 +457,8 @@
        /* Make sure interrupts are disabled. */
        cpsid   ifa

-       LEAVE_HYP
+       HANDLE_HYP

        /* Setup core, disable all caches. */
        mrc     CP15_SCTLR(r0)
        bic     r0, #CPU_CONTROL_MMU_ENABLE
Index: sys/conf/files.arm
===================================================================
--- sys/conf/files.arm
+++ 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      standard
 arm/arm/machdep.c              standard
 arm/arm/machdep_boot.c         standard
 arm/arm/machdep_kdb.c          standard