Index: sys/arm/arm/locore-v6.S
===================================================================
--- sys/arm/arm/locore-v6.S
+++ sys/arm/arm/locore-v6.S
@@ -45,12 +45,44 @@
 #define	PTE1_SIZE	L1_S_SIZE
 #endif
 
+#if __ARM_ARCH >= 7
+#if defined(__clang__)
+       /* For HYP support, either gcc > 4.9 or llvm > 3.6 */
+       .arch_extension virt
+#endif
+#endif
+
 /* A small statically-allocated stack used only during initarm() and AP startup. */
 #define	INIT_ARM_STACK_SIZE	2048
 
 	.text
 	.align	2
 
+.macro leave_hyp
+#if __ARM_ARCH >= 7
+       /* 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, 1
+#if defined(__clang__)
+       msr     elr_hyp, lr
+       eret
+#else
+       MSR_ELR_HYP(14)
+       ERET
+#endif
+
+1:
+#endif
+.endm
+
 /*
  * On entry for FreeBSD boot ABI:
  *	r0 - metadata pointer or 0 (boothowto on AT91's boot2)
@@ -76,6 +108,8 @@
 	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
@@ -398,8 +432,10 @@
 #if defined(SMP)
 
 ASENTRY_NP(mpentry)
-	/* Make sure interrupts are disabled. */
-	cpsid	ifa
+        /* Make sure interrupts are disabled. */
+        cpsid   ifa
+
+	leave_hyp
 
 	/* Setup core, disable all caches. */
 	mrc	CP15_SCTLR(r0)
Index: sys/arm/include/asm.h
===================================================================
--- sys/arm/include/asm.h
+++ sys/arm/include/asm.h
@@ -234,6 +234,19 @@
 #define DSB	dsb
 #define DMB	dmb
 #define WFI	wfi
+
+
+#if !defined(__clang__)
+/*
+ * XXX: This is temporary workaround. Since we still support gcc compiler
+ * we must create a substitution for current version. When gcc support is
+ * dropped or gcc is upgraded to version 4.9 or higher (armv7ve) this
+ * shall be removed.
+ */
+#define MSR_ELR_HYP(regnum) .word (0xe12ef300 | regnum)
+#define ERET .word 0xe160006e
+#endif /* !defined(__clang__) */
+
 #elif __ARM_ARCH == 6
 #define ISB	mcr CP15_CP15ISB
 #define DSB	mcr CP15_CP15DSB