Index: sys/arm/arm/locore-v6.S
===================================================================
--- sys/arm/arm/locore-v6.S
+++ sys/arm/arm/locore-v6.S
@@ -45,6 +45,11 @@
 #define	PTE1_SIZE	L1_S_SIZE
 #endif
 
+#if __ARM_ARCH >= 7
+	/* For HYP instructions */
+	.arch_extension virt
+#endif
+
 /* A small statically-allocated stack used only during initarm() and AP startup. */
 #define	INIT_ARM_STACK_SIZE	2048
 
@@ -76,6 +81,18 @@
 	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, lr
+	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 +415,19 @@
 #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, lr
+	eret
+no_hyp_mp:
+#endif
+
 	/* Make	sure interrupts	are disabled. */
 	cpsid	ifa