Index: sys/arm/arm/hypervisor-stub.S
===================================================================
--- sys/arm/arm/hypervisor-stub.S
+++ sys/arm/arm/hypervisor-stub.S
@@ -33,11 +33,15 @@
 __FBSDID("$FreeBSD$");
 
 #if __ARM_ARCH >= 7
-#if defined(__ARM_ARCH_7VE__) || defined(__clang__)
 .arch_extension virt
-#endif
 
 ASENTRY_NP(hypervisor_stub_vect_install)
+	/* 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? */
+	movne	r0, #-1
+	RETne
 
 	/* Install hypervisor stub vectors. */
 	adr	r0, hypervisor_stub_vect
@@ -53,10 +57,18 @@
 	/* Don't disable access to perf-mon from PL0,1 and preserve HPMN. */
 	mrc	CP15_HDCR(r0)
 	and	r0, #(ARM_CP15_HDCR_HPMN)
-	/*  Caller implicit instruction barrier in the ERET. */
+	/* Caller implicit instruction barrier in the ERET. */
 	mcr	CP15_HDCR(r0)
 
-	RET
+	mrs	r0, cpsr
+	bic	r0, r0, #(PSR_MODE)
+	orr	r0, r0, #(PSR_SVC32_MODE)
+	orr	r0, r0, #(PSR_I | PSR_F | PSR_A)
+	msr	spsr_cxsf, r0
+	mov	r0, #0
+	/* Exit hypervisor mode */
+	msr	elr_hyp, lr
+	eret
 
 END(hypervisor_stub_vect_install)
 
Index: sys/arm/arm/locore-v6.S
===================================================================
--- sys/arm/arm/locore-v6.S
+++ sys/arm/arm/locore-v6.S
@@ -56,31 +56,10 @@
 
 #if __ARM_ARCH >= 7
 #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)					;\
-	orr	r0, r0, #(PSR_SVC32_MODE)				;\
-	orr	r0, r0, #(PSR_I | PSR_F | PSR_A)			;\
-	msr	spsr_cxsf, r0						;\
-	/* Exit hypervisor mode */					;\
-	adr	lr, 2f							;\
-	MSR_ELR_HYP(14)							;\
-	ERET								;\
-1:									;\
-	mov	r0, -1							;\
-	adr	r1, hypmode_enabled					;\
-	str	r0, [r1]						;\
-2:
+	str	r0, [r1]
 #else
 #define	HANDLE_HYP
 #endif /* __ARM_ARCH >= 7 */
Index: sys/arm/include/asm.h
===================================================================
--- sys/arm/include/asm.h
+++ sys/arm/include/asm.h
@@ -236,14 +236,6 @@
 #define DMB	dmb
 #define WFI	wfi
 
-#if defined(__ARM_ARCH_7VE__) || defined(__clang__)
-#define	MSR_ELR_HYP(regnum)	msr	elr_hyp, lr
-#define	ERET	eret
-#else
-#define MSR_ELR_HYP(regnum) .word (0xe12ef300 | regnum)
-#define ERET .word 0xe160006e
-#endif
-
 #elif __ARM_ARCH == 6
 #define ISB	mcr CP15_CP15ISB
 #define DSB	mcr CP15_CP15DSB