diff --git a/arch/arm/cpu/armv7/start.S b/arch/arm/cpu/armv7/start.S
index 2b47881..00890a3 100644
--- a/arch/arm/cpu/armv7/start.S
+++ b/arch/arm/cpu/armv7/start.S
@@ -41,7 +41,7 @@ _start: b	reset
 	ldr	pc, _software_interrupt
 	ldr	pc, _prefetch_abort
 	ldr	pc, _data_abort
-	ldr	pc, _not_used
+	ldr	pc, _hyp_trap
 	ldr	pc, _irq
 	ldr	pc, _fiq
 #ifdef CONFIG_SPL_BUILD
@@ -49,7 +49,7 @@ _undefined_instruction: .word _undefined_instruction
 _software_interrupt:	.word _software_interrupt
 _prefetch_abort:	.word _prefetch_abort
 _data_abort:		.word _data_abort
-_not_used:		.word _not_used
+_hyp_trap:		.word _hyp_trap
 _irq:			.word _irq
 _fiq:			.word _fiq
 _pad:			.word 0x12345678 /* now 16*4=64 */
@@ -58,7 +58,7 @@ _undefined_instruction: .word undefined_instruction
 _software_interrupt:	.word software_interrupt
 _prefetch_abort:	.word prefetch_abort
 _data_abort:		.word data_abort
-_not_used:		.word not_used
+_hyp_trap:		.word hyp_trap
 _irq:			.word irq
 _fiq:			.word fiq
 _pad:			.word 0x12345678 /* now 16*4=64 */
@@ -502,12 +502,18 @@ software_interrupt:
 	mrc	p15, 0, r1, c1, c1, 0		@ read SCR
 	bic	r1, r1, #0x07f
 	orr	r1, r1, #0x31			@ enable NS, AW, FW
+	mrc	p15, 0, r0, c0, c1, 1		@ check for Virt ext
+	and	r0, r0, #0xf000
+	cmp	r0, #0x1000
+	orreq	r1, r1, #0x100			@ allow HVC instruction
 
 	mrc	p15, 0, r0, c12, c0, 0		@ save secure copy of VBAR
 	mcr	p15, 0, r1, c1, c1, 0		@ write SCR, switch to non-sec
 	isb
 	mcr	p15, 0, r0, c12, c0, 0		@ write non-secure copy of VBAR
 
+	mcreq	p15, 4, r0, c12, c0, 0		@ write HYP mode HVBAR
+
 	movs	pc, lr
 
 	.align	5
@@ -523,10 +529,9 @@ data_abort:
 	bl	do_data_abort
 
 	.align	5
-not_used:
-	get_bad_stack
-	bad_save_user_regs
-	bl	do_not_used
+hyp_trap:
+	.byte 0x00, 0xe3, 0x0e, 0xe1		@ mrs lr, elr_hyp
+	mov pc, lr
 
 #ifdef CONFIG_USE_IRQ
 
@@ -562,21 +567,21 @@ fiq:
 #endif /* CONFIG_USE_IRQ */
 #endif /* CONFIG_SPL_BUILD */
 
-/* Routine to initialize GIC CPU interface and switch to nonsecure state.
- * Will be executed directly by secondary CPUs after coming out of
+/* Routine to initialize GIC CPU interface, switch to nonsecure and to HYP
+ * mode. Will be executed directly by secondary CPUs after coming out of
  * WFI, or can be called directly by C code for CPU 0.
  * Those two paths mandate to not use any stack and to only use registers
  * r0-r3 to comply with both the C ABI and the requirement of SMP startup
  * code.
  */
-.globl _nonsec_gic_switch
+.globl _hyp_gic_switch
 .globl _smp_pen
 _smp_pen:
 	mrs	r0, cpsr
 	orr	r0, r0, #0xc0
 	msr	cpsr, r0			@ disable interrupts
 	mov	lr, #0				@ clear LR to mark secondary
-_nonsec_gic_switch:
+_hyp_gic_switch:
 	mrc	p15, 4, r2, c15, c0, 0		@ r2 = PERIPHBASE
 	add	r3, r2, #0x1000			@ GIC dist i/f offset
 	mvn	r1, #0
@@ -616,6 +621,13 @@ _nonsec_gic_switch:
 	add	r2, r2, #0x1000			@ GIC dist i/f offset
 	str	r1, [r2]			@ allow private interrupts
 
+	mov	r2, lr
+	mov	r1, sp
+	.byte 0x70, 0x00, 0x40, 0xe1		@ hvc #0
+	isb
+	mov	sp, r1
+	mov	lr, r2
+
 	cmp	lr, #0
 	movne	pc, lr				@ CPU 0 to return
 						@ all others: go to sleep
diff --git a/arch/arm/include/asm/armv7.h b/arch/arm/include/asm/armv7.h
index 3567692..6c6955b 100644
--- a/arch/arm/include/asm/armv7.h
+++ b/arch/arm/include/asm/armv7.h
@@ -74,6 +74,6 @@ void v7_outer_cache_inval_all(void);
 void v7_outer_cache_flush_range(u32 start, u32 end);
 void v7_outer_cache_inval_range(u32 start, u32 end);
 
-int armv7_switch_nonsec(void);
+int armv7_switch_hyp(void);
 
 #endif
diff --git a/arch/arm/lib/virt-v7.c b/arch/arm/lib/virt-v7.c
index 5ca093a..72481c6 100644
--- a/arch/arm/lib/virt-v7.c
+++ b/arch/arm/lib/virt-v7.c
@@ -3,6 +3,7 @@
  * Andre Przywara, Linaro
  *
  * routines to push ARMv7 processors from secure into non-secure state
+ * and from non-secure SVC into HYP mode
  * needed to enable ARMv7 virtualization for current hypervisors
  *
  * See file CREDITS for list of people who contributed to this
@@ -28,7 +29,7 @@
 #include <asm/armv7.h>
 
 /* the assembly routine doing the actual work in start.S */
-void _nonsec_gic_switch(void);
+void _hyp_gic_switch(void);
 void _smp_pen(void);
 
 #define GICD_CTLR	0x000
@@ -47,16 +48,20 @@ static inline unsigned int read_cpsr(void)
 	return reg;
 }
 
-int armv7_switch_nonsec(void)
+int armv7_switch_hyp(void)
 {
 	unsigned int reg;
 	volatile unsigned int *gicdptr;
 	unsigned itlinesnr, i;
 	unsigned int *sysflags;
 
-	/* check whether the CPU supports the security extensions */
+	/* check whether we are in HYP mode already */
+	if ((read_cpsr() & 0x1F) == 0x1a)
+		return 1;
+
+	/* check whether the CPU supports the virtualization extensions */
 	asm("mrc p15, 0, %0, c0, c1, 1\n" : "=r"(reg));
-	if ((reg & 0xF0) == 0)
+	if ((reg & 0xF000) != 0x1000)
 		return 2;
 
 	/* the timer frequency for the generic timer needs to be
@@ -77,8 +82,8 @@ int armv7_switch_nonsec(void)
 	 */
 
 	/* check whether we are an Cortex-A15 or A7.
-	 * The actual non-secure switch should work with all CPUs supporting
-	 * the security extension, but we need the GIC address,
+	 * The actual HYP switch should work with all CPUs supporting
+	 * the virtualization extension, but we need the GIC address,
 	 * which we know only for sure for those two CPUs.
 	 */
 	asm("mrc p15, 0, %0, c0, c0, 0\n" : "=r"(reg));
@@ -117,8 +122,11 @@ int armv7_switch_nonsec(void)
 	sysflags[0] = (uintptr_t)_smp_pen;
 	gicdptr[GICD_SGIR / 4] = 1U << 24;
 
-	/* call the non-sec switching code on this CPU also */
-	_nonsec_gic_switch();
+	/* call the HYP switching code on this CPU also */
+	_hyp_gic_switch();
+
+	if ((read_cpsr() & 0x1F) != 0x1a)
+		return 5;
 
 	return 0;
 }
diff --git a/common/cmd_virt.c b/common/cmd_virt.c
index 132b6b1..78b1aa1 100644
--- a/common/cmd_virt.c
+++ b/common/cmd_virt.c
@@ -2,8 +2,8 @@
  * (C) Copyright 2013
  * Andre Przywara, Linaro
  *
- * command to switch an ARMv7 CPU with security extensions into
- * non-secure state
+ * command to switch an ARMv7 CPU with virtualization extensions into
+ * HYP mode to allow hypervisors to install themselves
  *
  * See file CREDITS for list of people who contributed to this
  * project.
@@ -29,17 +29,20 @@
 
 #include <asm/armv7.h>
 
-static int do_nonsec(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
+static int do_hyp(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
 {
 	unsigned int ret;
 
-	ret = armv7_switch_nonsec();
+	ret = armv7_switch_hyp();
 
 	switch (ret) {
 	case 0:
 		break;
+	case 1:
+		printf("Already in HYP mode\n");
+		break;
 	case 2:
-		printf("Security extensions not implemented.\n");
+		printf("Virtualization extensions not implemented.\n");
 		break;
 	case 3:
 		printf("CPU not supported, must be either Cortex-A15 or A7.\n");
@@ -47,13 +50,16 @@ static int do_nonsec(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
 	case 4:
 		printf("PERIPHBASE is above 4 GB, cannot access this.\n");
 		break;
+	case 5:
+		printf("HYP mode switch not successful.\n");
+		break;
 	}
 
 	return ret;
 }
 
 U_BOOT_CMD(
-	hypmode, 1, 0, do_nonsec,
-	"switch ARM CPUs into non-secure state",
+	hypmode, 1, 0, do_hyp,
+	"switch ARM CPUs into HYP mode",
 	""
 );
