diff --git a/arch/arm/cpu/armv7/nonsec_virt.S b/arch/arm/cpu/armv7/nonsec_virt.S
index 3dd60b7137a8c50ba159f8bc91428fc263ba93b4..cbee8f70a28b4a02443f3c3061ed7b9ad7b52589 100644
--- a/arch/arm/cpu/armv7/nonsec_virt.S
+++ b/arch/arm/cpu/armv7/nonsec_virt.S
@@ -57,6 +57,28 @@ _secure_monitor:
 
 	movs	pc, lr				@ return to non-secure SVC
 
+/*
+ * Secondary CPUs start here and call the code for the core specific parts
+ * of the non-secure and HYP mode transition. The GIC distributor specific
+ * code has already been executed by a C function before.
+ * Then they go back to wfi and wait to be woken up by the kernel again.
+ */
+ENTRY(_smp_pen)
+	mrs	r0, cpsr
+	orr	r0, r0, #0xc0
+	msr	cpsr, r0			@ disable interrupts
+	ldr	r1, =_start
+	mcr	p15, 0, r1, c12, c0, 0		@ set VBAR
+
+	bl	_nonsec_init
+
+	ldr	r1, [r0, #GICC_IAR]		@ acknowledge IPI
+	str	r1, [r0, #GICC_EOIR]		@ signal end of interrupt
+
+	adr	r0, _smp_pen			@ do not use this address again
+	b	smp_waitloop			@ wait for IPIs, board specific
+ENDPROC(_smp_pen)
+
 /*
  * Switch a core to non-secure state.
  *
@@ -138,3 +160,16 @@ ENTRY(_nonsec_init)
 
 	bx	lr
 ENDPROC(_nonsec_init)
+
+#ifdef CONFIG_SMP_PEN_ADDR
+/* void __weak smp_waitloop(unsigned previous_address); */
+ENTRY(smp_waitloop)
+	wfi
+	ldr	r1, =CONFIG_SMP_PEN_ADDR	@ load start address
+	ldr	r1, [r1]
+	cmp	r0, r1			@ make sure we dont execute this code
+	beq	smp_waitloop		@ again (due to a spurious wakeup)
+	mov	pc, r1
+ENDPROC(smp_waitloop)
+.weak smp_waitloop
+#endif
diff --git a/arch/arm/cpu/armv7/virt-v7.c b/arch/arm/cpu/armv7/virt-v7.c
index 068ac85225fc6aae1206ac812d9bab4cd16773ff..a0b8742464433c22a6a0016bbb08dc2022b61941 100644
--- a/arch/arm/cpu/armv7/virt-v7.c
+++ b/arch/arm/cpu/armv7/virt-v7.c
@@ -79,6 +79,17 @@ static unsigned long get_gicd_base_address(void)
 #endif
 }
 
+static void kick_secondary_cpus_gic(unsigned long gicdaddr)
+{
+	/* kick all CPUs (except this one) by writing to GICD_SGIR */
+	writel(1U << 24, gicdaddr + GICD_SGIR);
+}
+
+void __weak smp_kick_all_cpus(void)
+{
+	kick_secondary_cpus_gic(gic_dist_addr);
+}
+
 int armv7_switch_nonsec(void)
 {
 	unsigned int reg;
@@ -115,7 +126,10 @@ int armv7_switch_nonsec(void)
 	for (i = 1; i <= itlinesnr; i++)
 		writel((unsigned)-1, gic_dist_addr + GICD_IGROUPRn + 4 * i);
 
-	/* call the non-sec switching code on this CPU */
+	smp_set_core_boot_addr((unsigned long)_smp_pen, -1);
+	smp_kick_all_cpus();
+
+	/* call the non-sec switching code on this CPU also */
 	_nonsec_init();
 
 	return 0;
diff --git a/arch/arm/include/asm/armv7.h b/arch/arm/include/asm/armv7.h
index b352d431b19d76fc2f92009a93f0b43e6d3e4b8a..2efd4bcf424d45c3d8c41386b2fce00bb722caa8 100644
--- a/arch/arm/include/asm/armv7.h
+++ b/arch/arm/include/asm/armv7.h
@@ -82,6 +82,7 @@ int armv7_switch_nonsec(void);
 
 /* defined in assembly file */
 unsigned int _nonsec_init(void);
+void _smp_pen(void);
 #endif /* CONFIG_ARMV7_NONSEC */
 
 #endif /* ! __ASSEMBLY__ */
diff --git a/arch/arm/include/asm/gic.h b/arch/arm/include/asm/gic.h
index c2b1e28624244aa4e825334e89f4190161987ec4..a0891cc09c9b2efde5bda365bc3eb394f7c2ca67 100644
--- a/arch/arm/include/asm/gic.h
+++ b/arch/arm/include/asm/gic.h
@@ -13,5 +13,7 @@
 #define GIC_CPU_OFFSET_A15	0x2000
 #define GICC_CTLR		0x0000
 #define GICC_PMR		0x0004
+#define GICC_IAR		0x000C
+#define GICC_EOIR		0x0010
 
 #endif
diff --git a/include/common.h b/include/common.h
index 8addf4334334b6e09bd9b9233cf425eda7bfb480..4d2a56d0dea327cffd5f805d84132a655a7c760d 100644
--- a/include/common.h
+++ b/include/common.h
@@ -627,6 +627,8 @@ void ft_pci_setup(void *blob, bd_t *bd);
 #endif
 #endif
 
+void smp_set_core_boot_addr(unsigned long addr, int corenr);
+void smp_kick_all_cpus(void);
 
 /* $(CPU)/serial.c */
 int	serial_init   (void);