Index: sys/arm/arm/locore-v6.S
===================================================================
--- sys/arm/arm/locore-v6.S
+++ sys/arm/arm/locore-v6.S
@@ -76,6 +76,19 @@
 	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
+
+	MSR_ELR_HYP(14)	/* msr  elr_hyp, lr */
+	ERET			/* eret */
+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 +411,20 @@
 #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
+
+	MSR_ELR_HYP(14)	/* msr  elr_hyp, lr */
+	ERET			/* eret */
+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,14 @@
 #define DSB	dsb
 #define DMB	dmb
 #define WFI	wfi
+/* WORKAROUND:
+ * This is temporary workaround. Current clang version does
+ * not support these instructions. Use hardcoded values
+ * until llvm-3.6 is integrated into FreeBSD.
+ */
+#define MSR_ELR_HYP(regnum)	.word	(0xe12ef300 | regnum)
+#define ERET	.word	0xe160006e
+/* WORKAROUND END */
 #elif __ARM_ARCH == 6
 #define ISB	mcr CP15_CP15ISB
 #define DSB	mcr CP15_CP15DSB