Index: sys/arm/arm/locore-v6.S
===================================================================
--- sys/arm/arm/locore-v6.S
+++ sys/arm/arm/locore-v6.S
@@ -45,6 +45,13 @@
 #define	PTE1_SIZE	L1_S_SIZE
 #endif
 
+#if __ARM_ARCH >= 7
+#if ! (defined(GCC_VERSION) && GCC_VERSION < 4009)
+	/* 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
 
@@ -76,6 +83,24 @@
 	mov	r10, r2		/* Save	meta data */
 	mov	r11, r3		/* Future expansion */
 
+#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	no_hyp
+	adr	lr, no_hyp
+#if (defined(GCC_VERSION) && GCC_VERSION < 4009)
+	MSR_ELR_HYP(14)
+	ERET
+#else
+	msr	elr_hyp, lr
+	eret
+#endif
+
+no_hyp:
+#endif
+
 	/* 
 	 * Check whether data cache is enabled.  If it is, then we know
 	 * current tags are valid (not power-on garbage values) and there
@@ -398,6 +423,25 @@
 #if defined(SMP)
 
 ASENTRY_NP(mpentry)
+
+#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	no_hyp_mp
+	adr	lr, no_hyp_mp
+#if (defined(GCC_VERSION) && GCC_VERSION < 4009)
+	MSR_ELR_HYP(14)
+	ERET
+#else
+	msr	elr_hyp, lr
+	eret
+#endif
+
+no_hyp_mp:
+#endif
+
 	/* Make	sure interrupts	are disabled. */
 	cpsid	ifa
 
Index: sys/arm/include/asm.h
===================================================================
--- sys/arm/include/asm.h
+++ sys/arm/include/asm.h
@@ -228,6 +228,17 @@
 #define DSB	dsb
 #define DMB	dmb
 #define WFI	wfi
+#if (defined(GCC_VERSION) && GCC_VERSION < 4009)
+/* WORKAROUND:
+ * 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
+/* WORKAROUND END */
+#endif /* GCC */
 #elif __ARM_ARCH == 6
 #define ISB	mcr CP15_CP15ISB
 #define DSB	mcr CP15_CP15DSB