Patchwork [U-Boot,v3,5/7] ARM: add SMP support for non-secure switch

login
register
mail settings
Submitter Andre Przywara
Date July 9, 2013, 11:54 p.m.
Message ID <1373414059-22779-6-git-send-email-andre.przywara@linaro.org>
Download mbox | patch
Permalink /patch/257922/
State Superseded
Delegated to: Albert ARIBAUD
Headers show

Comments

Andre Przywara - July 9, 2013, 11:54 p.m.
Currently the non-secure switch is only done for the boot processor.
To enable full SMP support, we have to switch all secondary cores
into non-secure state also.

So we add an entry point for secondary CPUs coming out of low-power
state and make sure we put them into WFI again after having switched
to non-secure state.
For this we acknowledge and EOI the wake-up IPI, then go into WFI.
Once being kicked out of it later, we sanity check that the start
address has actually been changed (since another attempt to switch
to non-secure would block the core) and jump to the new address.

The actual CPU kick is done by sending an inter-processor interrupt
via the GIC to all CPU interfaces except the requesting processor.
The secondary cores will then setup their respective GIC CPU
interface.

The address secondary cores jump to is board specific, we provide
the value here for the Versatile Express board.

Signed-off-by: Andre Przywara <andre.przywara@linaro.org>
---
 arch/arm/cpu/armv7/nonsec_virt.S    | 27 +++++++++++++++++++++++++++
 arch/arm/cpu/armv7/virt-v7.c        | 19 ++++++++++++++++++-
 arch/arm/include/asm/armv7.h        |  1 +
 arch/arm/include/asm/gic.h          |  2 ++
 include/configs/vexpress_ca15_tc2.h |  3 +++
 5 files changed, 51 insertions(+), 1 deletion(-)
Christoffer Dall - July 29, 2013, 10:02 p.m.
On Wed, Jul 10, 2013 at 01:54:17AM +0200, Andre Przywara wrote:
> Currently the non-secure switch is only done for the boot processor.
> To enable full SMP support, we have to switch all secondary cores
> into non-secure state also.
> 
> So we add an entry point for secondary CPUs coming out of low-power
> state and make sure we put them into WFI again after having switched
> to non-secure state.
> For this we acknowledge and EOI the wake-up IPI, then go into WFI.
> Once being kicked out of it later, we sanity check that the start
> address has actually been changed (since another attempt to switch
> to non-secure would block the core) and jump to the new address.
> 
> The actual CPU kick is done by sending an inter-processor interrupt
> via the GIC to all CPU interfaces except the requesting processor.
> The secondary cores will then setup their respective GIC CPU
> interface.
> 
> The address secondary cores jump to is board specific, we provide
> the value here for the Versatile Express board.
> 
> Signed-off-by: Andre Przywara <andre.przywara@linaro.org>
> ---
>  arch/arm/cpu/armv7/nonsec_virt.S    | 27 +++++++++++++++++++++++++++
>  arch/arm/cpu/armv7/virt-v7.c        | 19 ++++++++++++++++++-
>  arch/arm/include/asm/armv7.h        |  1 +
>  arch/arm/include/asm/gic.h          |  2 ++
>  include/configs/vexpress_ca15_tc2.h |  3 +++
>  5 files changed, 51 insertions(+), 1 deletion(-)
> 
> diff --git a/arch/arm/cpu/armv7/nonsec_virt.S b/arch/arm/cpu/armv7/nonsec_virt.S
> index e9ee831..f9b6b39 100644
> --- a/arch/arm/cpu/armv7/nonsec_virt.S
> +++ b/arch/arm/cpu/armv7/nonsec_virt.S
> @@ -58,6 +58,33 @@ _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	r1, _smp_pen
> +waitloop:
> +	wfi
> +	ldr	r0, =CONFIG_SYSFLAGS_ADDR	@ load start address

You seem to have ignored my comment about using the sysflags name?

As I understand, the sysflags name is a versatile express specific
register name that just happens to be used for the SMP boot address as
well...

Therefore, this should really be CONFIG_SMP_BOOT_ADDR or something like
that, at the very least.

> +	ldr	r0, [r0]
> +	cmp	r0, r1			@ make sure we dont execute this code
> +	beq	waitloop		@ again (due to a spurious wakeup)
> +	mov	pc, r0
> +ENDPROC(_smp_pen)
> +
> +/*
>   * Switch a core to non-secure state.
>   *
>   *  1. initialize the GIC per-core interface
> diff --git a/arch/arm/cpu/armv7/virt-v7.c b/arch/arm/cpu/armv7/virt-v7.c
> index 54f9746..a0d0b34 100644
> --- a/arch/arm/cpu/armv7/virt-v7.c
> +++ b/arch/arm/cpu/armv7/virt-v7.c
> @@ -77,6 +77,21 @@ static int get_gicd_base_address(unsigned int *gicdaddr)
>  #endif
>  }
>  
> +static void kick_secondary_cpus(unsigned int gicdaddr)
> +{
> +	unsigned int *secondary_boot_addr;
> +
> +	secondary_boot_addr = (void *)CONFIG_SYSFLAGS_ADDR;
> +#ifdef CONFIG_SYSFLAGS_NEED_CLEAR_BITS
> +	secondary_boot_addr[1] = (unsigned)-1;
> +#endif

again, if you disagreed with my previous comment, please comment on it
and rationalize your choice.

I still feel you're wrapping board specific logic into generic code, and
that you should call out to a more generic function.  Imagine an SOC
that uses an implementation defined control register for this instead of
a memory address...

perhaps what you need is:

void set_board_smp_boot_addr(unsigned long addr);
unsigned long get_board_smp_boot_addr(void);

and call these instead of your direct use of sysflags addr here...?

> +	*secondary_boot_addr = (uintptr_t)_smp_pen;
> +	dmb();
> +
> +	/* now kick all CPUs (except this one) by writing to GICD_SGIR */
> +	writel(1U << 24, gicdaddr + GICD_SGIR);
> +}
> +
>  enum nonsec_virt_errors armv7_switch_nonsec(void)
>  {
>  	unsigned int reg, ret;
> @@ -110,7 +125,9 @@ enum nonsec_virt_errors armv7_switch_nonsec(void)
>  	for (i = 0; i <= itlinesnr; i++)
>  		writel((unsigned)-1, gicdaddr + GICD_IGROUPRn + 4 * i);
>  
> -	/* call the non-sec switching code on this CPU */
> +	kick_secondary_cpus(gicdaddr);
> +
> +	/* call the non-sec switching code on this CPU also */
>  	_nonsec_init();
>  
>  	return NONSEC_VIRT_SUCCESS;
> diff --git a/arch/arm/include/asm/armv7.h b/arch/arm/include/asm/armv7.h
> index e5c0279..f6582a1 100644
> --- a/arch/arm/include/asm/armv7.h
> +++ b/arch/arm/include/asm/armv7.h
> @@ -102,6 +102,7 @@ enum nonsec_virt_errors 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 c2b1e28..a0891cc 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/configs/vexpress_ca15_tc2.h b/include/configs/vexpress_ca15_tc2.h
> index 4f425ac..ade9e5b 100644
> --- a/include/configs/vexpress_ca15_tc2.h
> +++ b/include/configs/vexpress_ca15_tc2.h
> @@ -31,4 +31,7 @@
>  #include "vexpress_common.h"
>  #define CONFIG_BOOTP_VCI_STRING     "U-boot.armv7.vexpress_ca15x2_tc2"
>  
> +#define CONFIG_SYSFLAGS_ADDR 0x1c010030
> +#define CONFIG_SYSFLAGS_NEED_CLEAR_BITS
> +
>  #endif
> -- 
> 1.7.12.1
>
Andre Przywara - July 30, 2013, 11:51 a.m.
On 07/30/2013 12:02 AM, Christoffer Dall wrote:
> On Wed, Jul 10, 2013 at 01:54:17AM +0200, Andre Przywara wrote:
>> Currently the non-secure switch is only done for the boot processor.
>> To enable full SMP support, we have to switch all secondary cores
>> into non-secure state also.
>>
>> So we add an entry point for secondary CPUs coming out of low-power
>> state and make sure we put them into WFI again after having switched
>> to non-secure state.
>> For this we acknowledge and EOI the wake-up IPI, then go into WFI.
>> Once being kicked out of it later, we sanity check that the start
>> address has actually been changed (since another attempt to switch
>> to non-secure would block the core) and jump to the new address.
>>
>> The actual CPU kick is done by sending an inter-processor interrupt
>> via the GIC to all CPU interfaces except the requesting processor.
>> The secondary cores will then setup their respective GIC CPU
>> interface.
>>
>> The address secondary cores jump to is board specific, we provide
>> the value here for the Versatile Express board.
>>
>> Signed-off-by: Andre Przywara <andre.przywara@linaro.org>
>> ---
>>   arch/arm/cpu/armv7/nonsec_virt.S    | 27 +++++++++++++++++++++++++++
>>   arch/arm/cpu/armv7/virt-v7.c        | 19 ++++++++++++++++++-
>>   arch/arm/include/asm/armv7.h        |  1 +
>>   arch/arm/include/asm/gic.h          |  2 ++
>>   include/configs/vexpress_ca15_tc2.h |  3 +++
>>   5 files changed, 51 insertions(+), 1 deletion(-)
>>
>> diff --git a/arch/arm/cpu/armv7/nonsec_virt.S b/arch/arm/cpu/armv7/nonsec_virt.S
>> index e9ee831..f9b6b39 100644
>> --- a/arch/arm/cpu/armv7/nonsec_virt.S
>> +++ b/arch/arm/cpu/armv7/nonsec_virt.S
>> @@ -58,6 +58,33 @@ _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	r1, _smp_pen
>> +waitloop:
>> +	wfi
>> +	ldr	r0, =CONFIG_SYSFLAGS_ADDR	@ load start address
>
> You seem to have ignored my comment about using the sysflags name?
>
> As I understand, the sysflags name is a versatile express specific
> register name that just happens to be used for the SMP boot address as
> well...
>
> Therefore, this should really be CONFIG_SMP_BOOT_ADDR or something like
> that, at the very least.

>
>> +	ldr	r0, [r0]
>> +	cmp	r0, r1			@ make sure we dont execute this code
>> +	beq	waitloop		@ again (due to a spurious wakeup)
>> +	mov	pc, r0
>> +ENDPROC(_smp_pen)
>> +
>> +/*
>>    * Switch a core to non-secure state.
>>    *
>>    *  1. initialize the GIC per-core interface
>> diff --git a/arch/arm/cpu/armv7/virt-v7.c b/arch/arm/cpu/armv7/virt-v7.c
>> index 54f9746..a0d0b34 100644
>> --- a/arch/arm/cpu/armv7/virt-v7.c
>> +++ b/arch/arm/cpu/armv7/virt-v7.c
>> @@ -77,6 +77,21 @@ static int get_gicd_base_address(unsigned int *gicdaddr)
>>   #endif
>>   }
>>
>> +static void kick_secondary_cpus(unsigned int gicdaddr)
>> +{
>> +	unsigned int *secondary_boot_addr;
>> +
>> +	secondary_boot_addr = (void *)CONFIG_SYSFLAGS_ADDR;
>> +#ifdef CONFIG_SYSFLAGS_NEED_CLEAR_BITS
>> +	secondary_boot_addr[1] = (unsigned)-1;
>> +#endif
>
> again, if you disagreed with my previous comment, please comment on it
> and rationalize your choice.

I am sorry, I thought I reasoned that already in an earlier mail:
I don't want to introduce abstraction prematurely, so I wanted to 
refactor the code as soon as a second board gets supported. Which will 
probably be the Arndale, and also probably done by me ...

> I still feel you're wrapping board specific logic into generic code, and
> that you should call out to a more generic function.  Imagine an SOC
> that uses an implementation defined control register for this instead of
> a memory address...

Well, I can imagine quite some ways to do this, but in fact I'd like to 
focus on things that really exist ;-)
My fear is that any abstraction I introduce now will not suffice for a 
future board and needs to be refactored then anyway.

Regards,
Andre.

>
> perhaps what you need is:
>
> void set_board_smp_boot_addr(unsigned long addr);
> unsigned long get_board_smp_boot_addr(void);
>
> and call these instead of your direct use of sysflags addr here...?
>
>> +	*secondary_boot_addr = (uintptr_t)_smp_pen;
>> +	dmb();
>> +
>> +	/* now kick all CPUs (except this one) by writing to GICD_SGIR */
>> +	writel(1U << 24, gicdaddr + GICD_SGIR);
>> +}
>> +
>>   enum nonsec_virt_errors armv7_switch_nonsec(void)
>>   {
>>   	unsigned int reg, ret;
>> @@ -110,7 +125,9 @@ enum nonsec_virt_errors armv7_switch_nonsec(void)
>>   	for (i = 0; i <= itlinesnr; i++)
>>   		writel((unsigned)-1, gicdaddr + GICD_IGROUPRn + 4 * i);
>>
>> -	/* call the non-sec switching code on this CPU */
>> +	kick_secondary_cpus(gicdaddr);
>> +
>> +	/* call the non-sec switching code on this CPU also */
>>   	_nonsec_init();
>>
>>   	return NONSEC_VIRT_SUCCESS;
>> diff --git a/arch/arm/include/asm/armv7.h b/arch/arm/include/asm/armv7.h
>> index e5c0279..f6582a1 100644
>> --- a/arch/arm/include/asm/armv7.h
>> +++ b/arch/arm/include/asm/armv7.h
>> @@ -102,6 +102,7 @@ enum nonsec_virt_errors 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 c2b1e28..a0891cc 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/configs/vexpress_ca15_tc2.h b/include/configs/vexpress_ca15_tc2.h
>> index 4f425ac..ade9e5b 100644
>> --- a/include/configs/vexpress_ca15_tc2.h
>> +++ b/include/configs/vexpress_ca15_tc2.h
>> @@ -31,4 +31,7 @@
>>   #include "vexpress_common.h"
>>   #define CONFIG_BOOTP_VCI_STRING     "U-boot.armv7.vexpress_ca15x2_tc2"
>>
>> +#define CONFIG_SYSFLAGS_ADDR 0x1c010030
>> +#define CONFIG_SYSFLAGS_NEED_CLEAR_BITS
>> +
>>   #endif
>> --
>> 1.7.12.1
>>
Christoffer Dall - July 30, 2013, 2:28 p.m.
On Tue, Jul 30, 2013 at 01:51:33PM +0200, Andre Przywara wrote:
> On 07/30/2013 12:02 AM, Christoffer Dall wrote:
> >On Wed, Jul 10, 2013 at 01:54:17AM +0200, Andre Przywara wrote:
> >>Currently the non-secure switch is only done for the boot processor.
> >>To enable full SMP support, we have to switch all secondary cores
> >>into non-secure state also.
> >>
> >>So we add an entry point for secondary CPUs coming out of low-power
> >>state and make sure we put them into WFI again after having switched
> >>to non-secure state.
> >>For this we acknowledge and EOI the wake-up IPI, then go into WFI.
> >>Once being kicked out of it later, we sanity check that the start
> >>address has actually been changed (since another attempt to switch
> >>to non-secure would block the core) and jump to the new address.
> >>
> >>The actual CPU kick is done by sending an inter-processor interrupt
> >>via the GIC to all CPU interfaces except the requesting processor.
> >>The secondary cores will then setup their respective GIC CPU
> >>interface.
> >>
> >>The address secondary cores jump to is board specific, we provide
> >>the value here for the Versatile Express board.
> >>
> >>Signed-off-by: Andre Przywara <andre.przywara@linaro.org>
> >>---
> >>  arch/arm/cpu/armv7/nonsec_virt.S    | 27 +++++++++++++++++++++++++++
> >>  arch/arm/cpu/armv7/virt-v7.c        | 19 ++++++++++++++++++-
> >>  arch/arm/include/asm/armv7.h        |  1 +
> >>  arch/arm/include/asm/gic.h          |  2 ++
> >>  include/configs/vexpress_ca15_tc2.h |  3 +++
> >>  5 files changed, 51 insertions(+), 1 deletion(-)
> >>
> >>diff --git a/arch/arm/cpu/armv7/nonsec_virt.S b/arch/arm/cpu/armv7/nonsec_virt.S
> >>index e9ee831..f9b6b39 100644
> >>--- a/arch/arm/cpu/armv7/nonsec_virt.S
> >>+++ b/arch/arm/cpu/armv7/nonsec_virt.S
> >>@@ -58,6 +58,33 @@ _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	r1, _smp_pen
> >>+waitloop:
> >>+	wfi
> >>+	ldr	r0, =CONFIG_SYSFLAGS_ADDR	@ load start address
> >
> >You seem to have ignored my comment about using the sysflags name?
> >
> >As I understand, the sysflags name is a versatile express specific
> >register name that just happens to be used for the SMP boot address as
> >well...
> >
> >Therefore, this should really be CONFIG_SMP_BOOT_ADDR or something like
> >that, at the very least.
> 
> >
> >>+	ldr	r0, [r0]
> >>+	cmp	r0, r1			@ make sure we dont execute this code
> >>+	beq	waitloop		@ again (due to a spurious wakeup)
> >>+	mov	pc, r0
> >>+ENDPROC(_smp_pen)
> >>+
> >>+/*
> >>   * Switch a core to non-secure state.
> >>   *
> >>   *  1. initialize the GIC per-core interface
> >>diff --git a/arch/arm/cpu/armv7/virt-v7.c b/arch/arm/cpu/armv7/virt-v7.c
> >>index 54f9746..a0d0b34 100644
> >>--- a/arch/arm/cpu/armv7/virt-v7.c
> >>+++ b/arch/arm/cpu/armv7/virt-v7.c
> >>@@ -77,6 +77,21 @@ static int get_gicd_base_address(unsigned int *gicdaddr)
> >>  #endif
> >>  }
> >>
> >>+static void kick_secondary_cpus(unsigned int gicdaddr)
> >>+{
> >>+	unsigned int *secondary_boot_addr;
> >>+
> >>+	secondary_boot_addr = (void *)CONFIG_SYSFLAGS_ADDR;
> >>+#ifdef CONFIG_SYSFLAGS_NEED_CLEAR_BITS
> >>+	secondary_boot_addr[1] = (unsigned)-1;
> >>+#endif
> >
> >again, if you disagreed with my previous comment, please comment on it
> >and rationalize your choice.
> 
> I am sorry, I thought I reasoned that already in an earlier mail:
> I don't want to introduce abstraction prematurely, so I wanted to
> refactor the code as soon as a second board gets supported. Which
> will probably be the Arndale, and also probably done by me ...

Hmm, a big motivation for doing this is to make it clear what new boards
need to do to support Hyp mode, and make it as easy as possible.  We
have already made the choice to make this code generic in virt-v7.c as
opposed to vexpress.c (or whatever that file would be called), so I
don't really understand your argument about being premature, to be
completely honest.

> 
> >I still feel you're wrapping board specific logic into generic code, and
> >that you should call out to a more generic function.  Imagine an SOC
> >that uses an implementation defined control register for this instead of
> >a memory address...
> 
> Well, I can imagine quite some ways to do this, but in fact I'd like
> to focus on things that really exist ;-)
> My fear is that any abstraction I introduce now will not suffice for
> a future board and needs to be refactored then anyway.
> 

Sorry, but I don't see this.  You need to do three very simple things:

get_smp_boot_addr
set_smp_boot_addr
kick_other_cpus

We can't hold back abstractions until we have a concrete example as a
general principle, because we'd never make anything generic.  We can
only try to find the right balance between some abstraction and
something concrete enough that it makes sense to users.

-Christoffer

Patch

diff --git a/arch/arm/cpu/armv7/nonsec_virt.S b/arch/arm/cpu/armv7/nonsec_virt.S
index e9ee831..f9b6b39 100644
--- a/arch/arm/cpu/armv7/nonsec_virt.S
+++ b/arch/arm/cpu/armv7/nonsec_virt.S
@@ -58,6 +58,33 @@  _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	r1, _smp_pen
+waitloop:
+	wfi
+	ldr	r0, =CONFIG_SYSFLAGS_ADDR	@ load start address
+	ldr	r0, [r0]
+	cmp	r0, r1			@ make sure we dont execute this code
+	beq	waitloop		@ again (due to a spurious wakeup)
+	mov	pc, r0
+ENDPROC(_smp_pen)
+
+/*
  * Switch a core to non-secure state.
  *
  *  1. initialize the GIC per-core interface
diff --git a/arch/arm/cpu/armv7/virt-v7.c b/arch/arm/cpu/armv7/virt-v7.c
index 54f9746..a0d0b34 100644
--- a/arch/arm/cpu/armv7/virt-v7.c
+++ b/arch/arm/cpu/armv7/virt-v7.c
@@ -77,6 +77,21 @@  static int get_gicd_base_address(unsigned int *gicdaddr)
 #endif
 }
 
+static void kick_secondary_cpus(unsigned int gicdaddr)
+{
+	unsigned int *secondary_boot_addr;
+
+	secondary_boot_addr = (void *)CONFIG_SYSFLAGS_ADDR;
+#ifdef CONFIG_SYSFLAGS_NEED_CLEAR_BITS
+	secondary_boot_addr[1] = (unsigned)-1;
+#endif
+	*secondary_boot_addr = (uintptr_t)_smp_pen;
+	dmb();
+
+	/* now kick all CPUs (except this one) by writing to GICD_SGIR */
+	writel(1U << 24, gicdaddr + GICD_SGIR);
+}
+
 enum nonsec_virt_errors armv7_switch_nonsec(void)
 {
 	unsigned int reg, ret;
@@ -110,7 +125,9 @@  enum nonsec_virt_errors armv7_switch_nonsec(void)
 	for (i = 0; i <= itlinesnr; i++)
 		writel((unsigned)-1, gicdaddr + GICD_IGROUPRn + 4 * i);
 
-	/* call the non-sec switching code on this CPU */
+	kick_secondary_cpus(gicdaddr);
+
+	/* call the non-sec switching code on this CPU also */
 	_nonsec_init();
 
 	return NONSEC_VIRT_SUCCESS;
diff --git a/arch/arm/include/asm/armv7.h b/arch/arm/include/asm/armv7.h
index e5c0279..f6582a1 100644
--- a/arch/arm/include/asm/armv7.h
+++ b/arch/arm/include/asm/armv7.h
@@ -102,6 +102,7 @@  enum nonsec_virt_errors 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 c2b1e28..a0891cc 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/configs/vexpress_ca15_tc2.h b/include/configs/vexpress_ca15_tc2.h
index 4f425ac..ade9e5b 100644
--- a/include/configs/vexpress_ca15_tc2.h
+++ b/include/configs/vexpress_ca15_tc2.h
@@ -31,4 +31,7 @@ 
 #include "vexpress_common.h"
 #define CONFIG_BOOTP_VCI_STRING     "U-boot.armv7.vexpress_ca15x2_tc2"
 
+#define CONFIG_SYSFLAGS_ADDR 0x1c010030
+#define CONFIG_SYSFLAGS_NEED_CLEAR_BITS
+
 #endif