Index: sys/arm/arm/locore-v6.S
===================================================================
--- sys/arm/arm/locore-v6.S
+++ sys/arm/arm/locore-v6.S
@@ -34,11 +34,12 @@
 #include <machine/asmacros.h>
 #include <machine/armreg.h>
 #include <machine/sysreg.h>
+#include <machine/cpuconf.h>
 #include <machine/pte-v6.h>

 __FBSDID("$FreeBSD$");


 #if __ARM_ARCH >= 7
 #if defined(__ARM_ARCH_7VE__) || defined(__clang__)
 /*
@@ -61,12 +62,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 +80,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 +117,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 +422,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 +458,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