Patchwork [U-Boot,3/6] ARM: switch to non-secure state during bootm execution

login
register
mail settings
Submitter Andre Przywara
Date May 6, 2013, 1:17 p.m.
Message ID <1367846270-1827-4-git-send-email-andre.przywara@linaro.org>
Download mbox | patch
Permalink /patch/241649/
State Changes Requested
Delegated to: Albert ARIBAUD
Headers show

Comments

Andre Przywara - May 6, 2013, 1:17 p.m.
To actually trigger the non-secure switch we just implemented, call
the switching routine from within the bootm command implementation.
This way we automatically enable this feature without further user
intervention.

Some part of the work is done in the assembly routine in start.S,
introduced with the previous patch, but for the full glory we need
to setup the GIC distributor interface once for the whole system,
which is done in C here.
The routine is placed in arch/arm/lib to allow easy access from
different boards or CPUs.

First we check for the availability of the security extensions.

The generic timer base frequency register is only accessible from
secure state, so we have to program it now. Actually this should be
done from primary firmware before, but some boards seems to omit
this, so if needed we do this here with a board specific value.

Since we need a safe way to access the GIC, we use the PERIPHBASE
registers on Cortex-A15 and A7 CPUs and do some sanity checks.

Then we actually do the GIC enablement:
a) enable the GIC distributor, both for non-secure and secure state
   (GICD_CTLR[1:0] = 11b)
b) allow all interrupts to be handled from non-secure state
   (GICD_IGROUPRn = 0xFFFFFFFF)
The core specific GIC setup is then done in the assembly routine.

The actual bootm trigger is pretty small: calling the routine and
doing some error reporting. A return value of 1 will be added later.

To enable the whole code we introduce the CONFIG_ARMV7_VIRT variable.

Signed-off-by: Andre Przywara <andre.przywara@linaro.org>
---
 arch/arm/include/asm/armv7.h |   7 +++
 arch/arm/lib/Makefile        |   2 +
 arch/arm/lib/bootm.c         |  20 ++++++++
 arch/arm/lib/virt-v7.c       | 113 +++++++++++++++++++++++++++++++++++++++++++
 4 files changed, 142 insertions(+)
 create mode 100644 arch/arm/lib/virt-v7.c
Christoffer Dall - May 31, 2013, 5:10 a.m.
On Mon, May 06, 2013 at 03:17:47PM +0200, Andre Przywara wrote:
> To actually trigger the non-secure switch we just implemented, call
> the switching routine from within the bootm command implementation.
> This way we automatically enable this feature without further user
> intervention.
> 
> Some part of the work is done in the assembly routine in start.S,
> introduced with the previous patch, but for the full glory we need
> to setup the GIC distributor interface once for the whole system,
> which is done in C here.
> The routine is placed in arch/arm/lib to allow easy access from
> different boards or CPUs.

I'm beginning to loose track of exactly which parts is in assembly and
which parts are in C, and why.  We are fiddling with some gic dist.
settings in assembly, and some of them in C as well.

I think it's just the ordering or naming of the patches that is a little
confusing.

> 
> First we check for the availability of the security extensions.
> 
> The generic timer base frequency register is only accessible from
> secure state, so we have to program it now. Actually this should be
> done from primary firmware before, but some boards seems to omit
> this, so if needed we do this here with a board specific value.
> 
> Since we need a safe way to access the GIC, we use the PERIPHBASE
> registers on Cortex-A15 and A7 CPUs and do some sanity checks.
> 
> Then we actually do the GIC enablement:
> a) enable the GIC distributor, both for non-secure and secure state
>    (GICD_CTLR[1:0] = 11b)
> b) allow all interrupts to be handled from non-secure state
>    (GICD_IGROUPRn = 0xFFFFFFFF)
> The core specific GIC setup is then done in the assembly routine.
> 
> The actual bootm trigger is pretty small: calling the routine and
> doing some error reporting. A return value of 1 will be added later.
> 
> To enable the whole code we introduce the CONFIG_ARMV7_VIRT variable.
> 
> Signed-off-by: Andre Przywara <andre.przywara@linaro.org>
> ---
>  arch/arm/include/asm/armv7.h |   7 +++
>  arch/arm/lib/Makefile        |   2 +
>  arch/arm/lib/bootm.c         |  20 ++++++++
>  arch/arm/lib/virt-v7.c       | 113 +++++++++++++++++++++++++++++++++++++++++++
>  4 files changed, 142 insertions(+)
>  create mode 100644 arch/arm/lib/virt-v7.c
> 
> diff --git a/arch/arm/include/asm/armv7.h b/arch/arm/include/asm/armv7.h
> index a73630b..25afffe 100644
> --- a/arch/arm/include/asm/armv7.h
> +++ b/arch/arm/include/asm/armv7.h
> @@ -74,4 +74,11 @@ 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);
>  
> +#ifdef CONFIG_ARMV7_VIRT
> +int armv7_switch_nonsec(void);
> +
> +/* defined in cpu/armv7/start.S */
> +void _nonsec_gic_switch(void);
> +#endif /* CONFIG_ARMV7_VIRT */
> +
>  #endif
> diff --git a/arch/arm/lib/Makefile b/arch/arm/lib/Makefile
> index 6ae161a..37a0e71 100644
> --- a/arch/arm/lib/Makefile
> +++ b/arch/arm/lib/Makefile
> @@ -58,6 +58,8 @@ COBJS-y	+= reset.o
>  COBJS-y	+= cache.o
>  COBJS-y	+= cache-cp15.o
>  
> +COBJS-$(CONFIG_ARMV7_VIRT) += virt-v7.o
> +
>  SRCS	:= $(GLSOBJS:.o=.S) $(GLCOBJS:.o=.c) \
>  	   $(SOBJS-y:.o=.S) $(COBJS-y:.o=.c)
>  OBJS	:= $(addprefix $(obj),$(SOBJS-y) $(COBJS-y))
> diff --git a/arch/arm/lib/bootm.c b/arch/arm/lib/bootm.c
> index f3b30c5..a3d3aae 100644
> --- a/arch/arm/lib/bootm.c
> +++ b/arch/arm/lib/bootm.c
> @@ -35,6 +35,10 @@
>  #include <asm/bootm.h>
>  #include <linux/compiler.h>
>  
> +#ifdef CONFIG_ARMV7_VIRT
> +#include <asm/armv7.h>
> +#endif
> +
>  DECLARE_GLOBAL_DATA_PTR;
>  
>  #if defined(CONFIG_SETUP_MEMORY_TAGS) || \
> @@ -319,6 +323,22 @@ static void boot_prep_linux(bootm_headers_t *images)
>  		hang();
>  #endif /* all tags */
>  	}
> +#ifdef CONFIG_ARMV7_VIRT
> +	switch (armv7_switch_nonsec()) {
> +	case 0:
> +		debug("entered non-secure state\n");
> +		break;
> +	case 2:
> +		printf("HYP mode: Security extensions not implemented.\n");
> +		break;
> +	case 3:
> +		printf("HYP mode: CPU not supported (must be Cortex-A15 or A7).\n");

I would drop the specifics of what's supported here.

> +		break;
> +	case 4:
> +		printf("HYP mode: PERIPHBASE is above 4 GB, cannot access this.\n");
> +		break;
> +	}

I think these hard-coded numbers are a bit ugly, either define an enum
or some defines somewhere, or just make the armv7_switch_nonsec return a
boolean value and put the prints in there.

That will also make it easier to read the other function and not go
"return 2" hmmm, I wonder what that means ;)

> +#endif
>  }
>  
>  /* Subcommand: GO */
> diff --git a/arch/arm/lib/virt-v7.c b/arch/arm/lib/virt-v7.c
> new file mode 100644
> index 0000000..3a48aee
> --- /dev/null
> +++ b/arch/arm/lib/virt-v7.c
> @@ -0,0 +1,113 @@
> +/*
> + * (C) Copyright 2013
> + * Andre Przywara, Linaro
> + *
> + * routines to push ARMv7 processors from secure into non-secure state
> + * needed to enable ARMv7 virtualization for current hypervisors

Nits: Routines should be capitalized.  Also not completely sure about
the wording about pushing between secure and non-secure state, changing,
transitioning, seems like more commonly used terms, but I dont' feel
strongly about any of this.

Again, I really think this is the wrong way to go about it.
Transitioning from secure to non-secure is really a feature of its own
which is a useful feature in U-boot.  Orthogonal to that is the need to
boot kernels in Hyp-mode, and being booted in secure more and
controlling all of the non-secure worlds is just one scenario for that.

> + *
> + * See file CREDITS for list of people who contributed to this
> + * project.
> + *
> + * This program is free software; you can redistribute it and/or
> + * modify it under the terms of the GNU General Public License as
> + * published by the Free Software Foundation; either version 2 of
> + * the License, or (at your option) any later version.
> + *
> + * This program is distributed in the hope that it will be useful,
> + * but WITHOUT ANY WARRANTY; without even the implied warranty of
> + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
> + * GNU General Public License for more details.
> + *
> + * You should have received a copy of the GNU General Public License
> + * along with this program; if not, write to the Free Software
> + * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
> + * MA 02111-1307 USA
> + */
> +
> +#include <common.h>
> +#include <asm/armv7.h>
> +
> +#define GICD_CTLR	0x000
> +#define GICD_TYPER	0x004
> +#define GICD_IGROUPR0	0x080
> +#define GICD_SGIR	0xf00
> +
> +#define CPU_ARM_CORTEX_A15	0x4100c0f0
> +#define CPU_ARM_CORTEX_A7	0x4100c070
> +
> +static inline unsigned int read_cpsr(void)

inline is typically not used in C-files - at least not in the kernel.
GCC is pretty smart about this on its own.

> +{
> +	unsigned int reg;
> +
> +	asm volatile ("mrs %0, cpsr\n" : "=r" (reg));
> +	return reg;
> +}
> +
> +int armv7_switch_nonsec(void)
> +{
> +	unsigned int reg;
> +	volatile unsigned int *gicdptr;
> +	unsigned itlinesnr, i;
> +
> +	/* check whether the CPU supports the security extensions */
> +	asm("mrc p15, 0, %0, c0, c1, 1\n" : "=r"(reg));

My brain hasn't managed to learn all the coprocessor register by heart
yet, so if you could provide the name for these registers it would be
cool.  Alternatively you could just create nice static little functions
just like you do with the cpsr.

> +	if ((reg & 0xF0) == 0)
> +		return 2;
> +
> +	/* the timer frequency for the generic timer needs to be
> +	 * programmed still in secure state, should be done by firmware.

nit: drop 'still'
nit: the 'should be done by firmware' is not very descriptive, consider
stating clearly that this is a work-around for broken bootloaders.

> +	 * check whether we have the generic timer first
> +	 */
> +#ifdef CONFIG_SYS_CLK_FREQ
> +	asm("mrc p15, 0, %0, c0, c1, 1\n" : "=r"(reg));
> +	if ((reg & 0xF0000) == 0x10000)
> +		asm("mcr p15, 0, %0, c14, c0, 0\n"
> +		: : "r"(CONFIG_SYS_CLK_FREQ));
> +#endif
> +
> +	/* the SCR register will be set directly in the monitor mode handler,
> +	 * according to the spec one should not tinker with it in secure state
> +	 * in SVC mode. Do not try to read it once in non-secure state,
> +	 * any access to it will trap.
> +	 */
> +
> +	/* check whether we are an Cortex-A15 or A7.

nit: s/whether/if/

> +	 * The actual non-secure switch should work with all CPUs supporting
> +	 * the security 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));
> +	if (((reg & 0xFF00FFF0) != 0x4100C0F0) &&
> +	    ((reg & 0xFF00FFF0) != 0x4100C070))

Are there really no defines for this in U-boot already?

It seems to me that a static inline in a header file somewhere that
gives you a processor type back would be useful for other things as
well, but I don't know U-boot enough to properly say...

> +		return 3;
> +
> +	/* get the GIC base address from the A15 PERIPHBASE register */
> +	asm("mrc p15, 4, %0, c15, c0, 0\n" : "=r" (reg));
> +
> +	/* the PERIPHBASE can be mapped above 4 GB (lower 8 bits used to
> +	 * encode this). Bail out here since we cannot access this without
> +	 * enabling paging.
> +	 */

ah, here you check for it...

> +	if ((reg & 0xff) != 0)
> +		return 4;

if you're getting the PERIPHBASE here anyway, why not just pass it as a
parameter to the non-secure gic init routine?

> +
> +	/* GIC distributor registers start at offset 0x1000 */
> +	gicdptr = (unsigned *)(reg + 0x1000);
> +
> +	/* enable the GIC distributor */
> +	gicdptr[GICD_CTLR / 4] |= 0x03;

so this is I/O accesses, so you could consider using readl for
consistency, which would also get rid of the division in the array
element accessors.

> +
> +	/* TYPER[4:0] contains an encoded number of all interrupts */
> +	itlinesnr = gicdptr[GICD_TYPER / 4] & 0x1f;
> +
> +	/* set all bits in the GIC group registers to one to allow access
> +	 * from non-secure state
> +	 */
> +	for (i = 0; i <= itlinesnr; i++)
> +		gicdptr[GICD_IGROUPR0 / 4 + i] = (unsigned)-1;

didn't you also do this in the assembly routine for IGROUP0 only?

oh wait, it's dawning on me, the _nonsec_gic_switch is actually per-cpu
non-secure init, right?

> +
> +	/* call the non-sec switching code on this CPU */
> +	_nonsec_gic_switch();
> +
> +	return 0;
> +}
> -- 
> 1.7.12.1
>
Andre Przywara - May 31, 2013, 9:30 a.m.
On 05/31/2013 07:10 AM, Christoffer Dall wrote:
> On Mon, May 06, 2013 at 03:17:47PM +0200, Andre Przywara wrote:
>> To actually trigger the non-secure switch we just implemented, call
>> the switching routine from within the bootm command implementation.
>> This way we automatically enable this feature without further user
>> intervention.
>>
>> Some part of the work is done in the assembly routine in start.S,
>> introduced with the previous patch, but for the full glory we need
>> to setup the GIC distributor interface once for the whole system,
>> which is done in C here.
>> The routine is placed in arch/arm/lib to allow easy access from
>> different boards or CPUs.
>
> I'm beginning to loose track of exactly which parts is in assembly and
> which parts are in C, and why.  We are fiddling with some gic dist.
> settings in assembly, and some of them in C as well.

I fear I dropped the explanation some time during a patch split earlier.
So the assembly code is the per core part - including GICD_IGROUPR0, 
which is banked per core. The reason this is in assembly is to make it 
easily run right out of the SMP pen.

In C I do anything that needs to be only done once for the whole system.

More comments inline...

> I think it's just the ordering or naming of the patches that is a little
> confusing.
>
>>
>> First we check for the availability of the security extensions.
>>
>> The generic timer base frequency register is only accessible from
>> secure state, so we have to program it now. Actually this should be
>> done from primary firmware before, but some boards seems to omit
>> this, so if needed we do this here with a board specific value.
>>
>> Since we need a safe way to access the GIC, we use the PERIPHBASE
>> registers on Cortex-A15 and A7 CPUs and do some sanity checks.
>>
>> Then we actually do the GIC enablement:
>> a) enable the GIC distributor, both for non-secure and secure state
>>     (GICD_CTLR[1:0] = 11b)
>> b) allow all interrupts to be handled from non-secure state
>>     (GICD_IGROUPRn = 0xFFFFFFFF)
>> The core specific GIC setup is then done in the assembly routine.
>>
>> The actual bootm trigger is pretty small: calling the routine and
>> doing some error reporting. A return value of 1 will be added later.
>>
>> To enable the whole code we introduce the CONFIG_ARMV7_VIRT variable.
>>
>> Signed-off-by: Andre Przywara <andre.przywara@linaro.org>
>> ---
>>   arch/arm/include/asm/armv7.h |   7 +++
>>   arch/arm/lib/Makefile        |   2 +
>>   arch/arm/lib/bootm.c         |  20 ++++++++
>>   arch/arm/lib/virt-v7.c       | 113 +++++++++++++++++++++++++++++++++++++++++++
>>   4 files changed, 142 insertions(+)
>>   create mode 100644 arch/arm/lib/virt-v7.c
>>
>> diff --git a/arch/arm/include/asm/armv7.h b/arch/arm/include/asm/armv7.h
>> index a73630b..25afffe 100644
>> --- a/arch/arm/include/asm/armv7.h
>> +++ b/arch/arm/include/asm/armv7.h
>> @@ -74,4 +74,11 @@ 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);
>>
>> +#ifdef CONFIG_ARMV7_VIRT
>> +int armv7_switch_nonsec(void);
>> +
>> +/* defined in cpu/armv7/start.S */
>> +void _nonsec_gic_switch(void);
>> +#endif /* CONFIG_ARMV7_VIRT */
>> +
>>   #endif
>> diff --git a/arch/arm/lib/Makefile b/arch/arm/lib/Makefile
>> index 6ae161a..37a0e71 100644
>> --- a/arch/arm/lib/Makefile
>> +++ b/arch/arm/lib/Makefile
>> @@ -58,6 +58,8 @@ COBJS-y	+= reset.o
>>   COBJS-y	+= cache.o
>>   COBJS-y	+= cache-cp15.o
>>
>> +COBJS-$(CONFIG_ARMV7_VIRT) += virt-v7.o
>> +
>>   SRCS	:= $(GLSOBJS:.o=.S) $(GLCOBJS:.o=.c) \
>>   	   $(SOBJS-y:.o=.S) $(COBJS-y:.o=.c)
>>   OBJS	:= $(addprefix $(obj),$(SOBJS-y) $(COBJS-y))
>> diff --git a/arch/arm/lib/bootm.c b/arch/arm/lib/bootm.c
>> index f3b30c5..a3d3aae 100644
>> --- a/arch/arm/lib/bootm.c
>> +++ b/arch/arm/lib/bootm.c
>> @@ -35,6 +35,10 @@
>>   #include <asm/bootm.h>
>>   #include <linux/compiler.h>
>>
>> +#ifdef CONFIG_ARMV7_VIRT
>> +#include <asm/armv7.h>
>> +#endif
>> +
>>   DECLARE_GLOBAL_DATA_PTR;
>>
>>   #if defined(CONFIG_SETUP_MEMORY_TAGS) || \
>> @@ -319,6 +323,22 @@ static void boot_prep_linux(bootm_headers_t *images)
>>   		hang();
>>   #endif /* all tags */
>>   	}
>> +#ifdef CONFIG_ARMV7_VIRT
>> +	switch (armv7_switch_nonsec()) {
>> +	case 0:
>> +		debug("entered non-secure state\n");
>> +		break;
>> +	case 2:
>> +		printf("HYP mode: Security extensions not implemented.\n");
>> +		break;
>> +	case 3:
>> +		printf("HYP mode: CPU not supported (must be Cortex-A15 or A7).\n");
>
> I would drop the specifics of what's supported here.
>

This is in particular here since I rely on the PERIPHBASE register, 
which is A15/A7 implementation specific. This is different from case 2 
(which will later be changed to "Virtualization extensions...")

>> +		break;
>> +	case 4:
>> +		printf("HYP mode: PERIPHBASE is above 4 GB, cannot access this.\n");
>> +		break;
>> +	}
>
> I think these hard-coded numbers are a bit ugly, either define an enum
> or some defines somewhere, or just make the armv7_switch_nonsec return a
> boolean value and put the prints in there.
>
> That will also make it easier to read the other function and not go
> "return 2" hmmm, I wonder what that means ;)

Right, will fix this.

>> +#endif
>>   }
>>
>>   /* Subcommand: GO */
>> diff --git a/arch/arm/lib/virt-v7.c b/arch/arm/lib/virt-v7.c
>> new file mode 100644
>> index 0000000..3a48aee
>> --- /dev/null
>> +++ b/arch/arm/lib/virt-v7.c
>> @@ -0,0 +1,113 @@
>> +/*
>> + * (C) Copyright 2013
>> + * Andre Przywara, Linaro
>> + *
>> + * routines to push ARMv7 processors from secure into non-secure state
>> + * needed to enable ARMv7 virtualization for current hypervisors
>
> Nits: Routines should be capitalized.  Also not completely sure about
> the wording about pushing between secure and non-secure state, changing,
> transitioning, seems like more commonly used terms, but I dont' feel
> strongly about any of this.
>
> Again, I really think this is the wrong way to go about it.
> Transitioning from secure to non-secure is really a feature of its own
> which is a useful feature in U-boot.  Orthogonal to that is the need to
> boot kernels in Hyp-mode, and being booted in secure more and
> controlling all of the non-secure worlds is just one scenario for that.

OK. Originally I had an implementation which separated the non-sec 
switch and the HYP switch. Later I got the impression that there is no 
real use-case for a non-sec switch only, so I dropped the specific 
command and just kept the logical separation in the patch split. That 
simplified the patches quite a bit.

So do you want to have a new u-boot command switching to non-secure 
state? I think that would make the patches more complicated, but if you 
recommend to have such a thing, I am happy to provide it.
Core problem here is that there is no easy way to check whether you are 
non-secure. Accessing the SCR register will trap if the NS bit is set. I 
could just catch this and return back with an error condition. Just not 
sure this is really worth the effort.

Also leaves one question: How to handle if the users switched 
deliberately to non-secure before and now the bootm routine wants to go 
to HYP mode? Do we want to stay in SVC in this case?

>> + *
>> + * See file CREDITS for list of people who contributed to this
>> + * project.
>> + *
>> + * This program is free software; you can redistribute it and/or
>> + * modify it under the terms of the GNU General Public License as
>> + * published by the Free Software Foundation; either version 2 of
>> + * the License, or (at your option) any later version.
>> + *
>> + * This program is distributed in the hope that it will be useful,
>> + * but WITHOUT ANY WARRANTY; without even the implied warranty of
>> + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
>> + * GNU General Public License for more details.
>> + *
>> + * You should have received a copy of the GNU General Public License
>> + * along with this program; if not, write to the Free Software
>> + * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
>> + * MA 02111-1307 USA
>> + */
>> +
>> +#include <common.h>
>> +#include <asm/armv7.h>
>> +
>> +#define GICD_CTLR	0x000
>> +#define GICD_TYPER	0x004
>> +#define GICD_IGROUPR0	0x080
>> +#define GICD_SGIR	0xf00
>> +
>> +#define CPU_ARM_CORTEX_A15	0x4100c0f0
>> +#define CPU_ARM_CORTEX_A7	0x4100c070
>> +
>> +static inline unsigned int read_cpsr(void)
>
> inline is typically not used in C-files - at least not in the kernel.
> GCC is pretty smart about this on its own.

Right. I think newer GCCs inline short static functions automatically.

>> +{
>> +	unsigned int reg;
>> +
>> +	asm volatile ("mrs %0, cpsr\n" : "=r" (reg));
>> +	return reg;
>> +}
>> +
>> +int armv7_switch_nonsec(void)
>> +{
>> +	unsigned int reg;
>> +	volatile unsigned int *gicdptr;
>> +	unsigned itlinesnr, i;
>> +
>> +	/* check whether the CPU supports the security extensions */
>> +	asm("mrc p15, 0, %0, c0, c1, 1\n" : "=r"(reg));
>
> My brain hasn't managed to learn all the coprocessor register by heart
> yet, so if you could provide the name for these registers it would be
> cool.  Alternatively you could just create nice static little functions
> just like you do with the cpsr.

OK. Actually I found this quite some overhead if its only called once 
(cpsr is called twice). Would just a comment suffice?

>> +	if ((reg & 0xF0) == 0)
>> +		return 2;
>> +
>> +	/* the timer frequency for the generic timer needs to be
>> +	 * programmed still in secure state, should be done by firmware.
>
> nit: drop 'still'
> nit: the 'should be done by firmware' is not very descriptive, consider
> stating clearly that this is a work-around for broken bootloaders.

ACK.

>> +	 * check whether we have the generic timer first
>> +	 */
>> +#ifdef CONFIG_SYS_CLK_FREQ
>> +	asm("mrc p15, 0, %0, c0, c1, 1\n" : "=r"(reg));
>> +	if ((reg & 0xF0000) == 0x10000)
>> +		asm("mcr p15, 0, %0, c14, c0, 0\n"
>> +		: : "r"(CONFIG_SYS_CLK_FREQ));
>> +#endif
>> +
>> +	/* the SCR register will be set directly in the monitor mode handler,
>> +	 * according to the spec one should not tinker with it in secure state
>> +	 * in SVC mode. Do not try to read it once in non-secure state,
>> +	 * any access to it will trap.
>> +	 */
>> +
>> +	/* check whether we are an Cortex-A15 or A7.
>
> nit: s/whether/if/
>
>> +	 * The actual non-secure switch should work with all CPUs supporting
>> +	 * the security 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));
>> +	if (((reg & 0xFF00FFF0) != 0x4100C0F0) &&
>> +	    ((reg & 0xFF00FFF0) != 0x4100C070))
>
> Are there really no defines for this in U-boot already?
>
> It seems to me that a static inline in a header file somewhere that
> gives you a processor type back would be useful for other things as
> well, but I don't know U-boot enough to properly say...

You are right, this is barely readable. Will fix this.

>> +		return 3;
>> +
>> +	/* get the GIC base address from the A15 PERIPHBASE register */
>> +	asm("mrc p15, 4, %0, c15, c0, 0\n" : "=r" (reg));
>> +
>> +	/* the PERIPHBASE can be mapped above 4 GB (lower 8 bits used to
>> +	 * encode this). Bail out here since we cannot access this without
>> +	 * enabling paging.
>> +	 */
>
> ah, here you check for it...
>
>> +	if ((reg & 0xff) != 0)
>> +		return 4;
>
> if you're getting the PERIPHBASE here anyway, why not just pass it as a
> parameter to the non-secure gic init routine?

Because this routine is also called directly out of the SMP pen, so I 
cannot pass any parameters there.

>> +
>> +	/* GIC distributor registers start at offset 0x1000 */
>> +	gicdptr = (unsigned *)(reg + 0x1000);
>> +
>> +	/* enable the GIC distributor */
>> +	gicdptr[GICD_CTLR / 4] |= 0x03;
>
> so this is I/O accesses, so you could consider using readl for
> consistency, which would also get rid of the division in the array
> element accessors.

Good hint, thanks.

>> +
>> +	/* TYPER[4:0] contains an encoded number of all interrupts */
>> +	itlinesnr = gicdptr[GICD_TYPER / 4] & 0x1f;
>> +
>> +	/* set all bits in the GIC group registers to one to allow access
>> +	 * from non-secure state
>> +	 */
>> +	for (i = 0; i <= itlinesnr; i++)
>> +		gicdptr[GICD_IGROUPR0 / 4 + i] = (unsigned)-1;
>
> didn't you also do this in the assembly routine for IGROUP0 only?
>
> oh wait, it's dawning on me, the _nonsec_gic_switch is actually per-cpu
> non-secure init, right?

Right. I take this as an advice to explain this earlier...

Thanks,
Andre.

>> +
>> +	/* call the non-sec switching code on this CPU */
>> +	_nonsec_gic_switch();
>> +
>> +	return 0;
>> +}
>> --
>> 1.7.12.1
>>
Christoffer Dall - May 31, 2013, 11:50 p.m.
On Fri, May 31, 2013 at 11:30:32AM +0200, Andre Przywara wrote:
> On 05/31/2013 07:10 AM, Christoffer Dall wrote:
> >On Mon, May 06, 2013 at 03:17:47PM +0200, Andre Przywara wrote:
> >>To actually trigger the non-secure switch we just implemented, call
> >>the switching routine from within the bootm command implementation.
> >>This way we automatically enable this feature without further user
> >>intervention.
> >>
> >>Some part of the work is done in the assembly routine in start.S,
> >>introduced with the previous patch, but for the full glory we need
> >>to setup the GIC distributor interface once for the whole system,
> >>which is done in C here.
> >>The routine is placed in arch/arm/lib to allow easy access from
> >>different boards or CPUs.
> >
> >I'm beginning to loose track of exactly which parts is in assembly and
> >which parts are in C, and why.  We are fiddling with some gic dist.
> >settings in assembly, and some of them in C as well.
> 
> I fear I dropped the explanation some time during a patch split earlier.
> So the assembly code is the per core part - including GICD_IGROUPR0,
> which is banked per core. The reason this is in assembly is to make
> it easily run right out of the SMP pen.
> 
> In C I do anything that needs to be only done once for the whole system.
> 
> More comments inline...
> 

I think renaming the assembly routine will go a along way.  Ordering
this patch before the assembly patch with just a stub function
implementation may also have been easier to read, but that's easy for me
to say at this point.

> >I think it's just the ordering or naming of the patches that is a little
> >confusing.
> >
> >>
> >>First we check for the availability of the security extensions.
> >>
> >>The generic timer base frequency register is only accessible from
> >>secure state, so we have to program it now. Actually this should be
> >>done from primary firmware before, but some boards seems to omit
> >>this, so if needed we do this here with a board specific value.
> >>
> >>Since we need a safe way to access the GIC, we use the PERIPHBASE
> >>registers on Cortex-A15 and A7 CPUs and do some sanity checks.
> >>
> >>Then we actually do the GIC enablement:
> >>a) enable the GIC distributor, both for non-secure and secure state
> >>    (GICD_CTLR[1:0] = 11b)
> >>b) allow all interrupts to be handled from non-secure state
> >>    (GICD_IGROUPRn = 0xFFFFFFFF)
> >>The core specific GIC setup is then done in the assembly routine.
> >>
> >>The actual bootm trigger is pretty small: calling the routine and
> >>doing some error reporting. A return value of 1 will be added later.
> >>
> >>To enable the whole code we introduce the CONFIG_ARMV7_VIRT variable.
> >>
> >>Signed-off-by: Andre Przywara <andre.przywara@linaro.org>
> >>---
> >>  arch/arm/include/asm/armv7.h |   7 +++
> >>  arch/arm/lib/Makefile        |   2 +
> >>  arch/arm/lib/bootm.c         |  20 ++++++++
> >>  arch/arm/lib/virt-v7.c       | 113 +++++++++++++++++++++++++++++++++++++++++++
> >>  4 files changed, 142 insertions(+)
> >>  create mode 100644 arch/arm/lib/virt-v7.c
> >>
> >>diff --git a/arch/arm/include/asm/armv7.h b/arch/arm/include/asm/armv7.h
> >>index a73630b..25afffe 100644
> >>--- a/arch/arm/include/asm/armv7.h
> >>+++ b/arch/arm/include/asm/armv7.h
> >>@@ -74,4 +74,11 @@ 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);
> >>
> >>+#ifdef CONFIG_ARMV7_VIRT
> >>+int armv7_switch_nonsec(void);
> >>+
> >>+/* defined in cpu/armv7/start.S */
> >>+void _nonsec_gic_switch(void);
> >>+#endif /* CONFIG_ARMV7_VIRT */
> >>+
> >>  #endif
> >>diff --git a/arch/arm/lib/Makefile b/arch/arm/lib/Makefile
> >>index 6ae161a..37a0e71 100644
> >>--- a/arch/arm/lib/Makefile
> >>+++ b/arch/arm/lib/Makefile
> >>@@ -58,6 +58,8 @@ COBJS-y	+= reset.o
> >>  COBJS-y	+= cache.o
> >>  COBJS-y	+= cache-cp15.o
> >>
> >>+COBJS-$(CONFIG_ARMV7_VIRT) += virt-v7.o
> >>+
> >>  SRCS	:= $(GLSOBJS:.o=.S) $(GLCOBJS:.o=.c) \
> >>  	   $(SOBJS-y:.o=.S) $(COBJS-y:.o=.c)
> >>  OBJS	:= $(addprefix $(obj),$(SOBJS-y) $(COBJS-y))
> >>diff --git a/arch/arm/lib/bootm.c b/arch/arm/lib/bootm.c
> >>index f3b30c5..a3d3aae 100644
> >>--- a/arch/arm/lib/bootm.c
> >>+++ b/arch/arm/lib/bootm.c
> >>@@ -35,6 +35,10 @@
> >>  #include <asm/bootm.h>
> >>  #include <linux/compiler.h>
> >>
> >>+#ifdef CONFIG_ARMV7_VIRT
> >>+#include <asm/armv7.h>
> >>+#endif
> >>+
> >>  DECLARE_GLOBAL_DATA_PTR;
> >>
> >>  #if defined(CONFIG_SETUP_MEMORY_TAGS) || \
> >>@@ -319,6 +323,22 @@ static void boot_prep_linux(bootm_headers_t *images)
> >>  		hang();
> >>  #endif /* all tags */
> >>  	}
> >>+#ifdef CONFIG_ARMV7_VIRT
> >>+	switch (armv7_switch_nonsec()) {
> >>+	case 0:
> >>+		debug("entered non-secure state\n");
> >>+		break;
> >>+	case 2:
> >>+		printf("HYP mode: Security extensions not implemented.\n");
> >>+		break;
> >>+	case 3:
> >>+		printf("HYP mode: CPU not supported (must be Cortex-A15 or A7).\n");
> >
> >I would drop the specifics of what's supported here.
> >
> 
> This is in particular here since I rely on the PERIPHBASE register,
> which is A15/A7 implementation specific. This is different from case
> 2 (which will later be changed to "Virtualization extensions...")
> 

I'm just thinking this will be weird when there's added Krait (I know, I
know )support or something else, and this comment is not updated, and
everyone is confused.  But it's also helpful, I guess, your call.

> >>+		break;
> >>+	case 4:
> >>+		printf("HYP mode: PERIPHBASE is above 4 GB, cannot access this.\n");
> >>+		break;
> >>+	}
> >
> >I think these hard-coded numbers are a bit ugly, either define an enum
> >or some defines somewhere, or just make the armv7_switch_nonsec return a
> >boolean value and put the prints in there.
> >
> >That will also make it easier to read the other function and not go
> >"return 2" hmmm, I wonder what that means ;)
> 
> Right, will fix this.
> 
> >>+#endif
> >>  }
> >>
> >>  /* Subcommand: GO */
> >>diff --git a/arch/arm/lib/virt-v7.c b/arch/arm/lib/virt-v7.c
> >>new file mode 100644
> >>index 0000000..3a48aee
> >>--- /dev/null
> >>+++ b/arch/arm/lib/virt-v7.c
> >>@@ -0,0 +1,113 @@
> >>+/*
> >>+ * (C) Copyright 2013
> >>+ * Andre Przywara, Linaro
> >>+ *
> >>+ * routines to push ARMv7 processors from secure into non-secure state
> >>+ * needed to enable ARMv7 virtualization for current hypervisors
> >
> >Nits: Routines should be capitalized.  Also not completely sure about
> >the wording about pushing between secure and non-secure state, changing,
> >transitioning, seems like more commonly used terms, but I dont' feel
> >strongly about any of this.
> >
> >Again, I really think this is the wrong way to go about it.
> >Transitioning from secure to non-secure is really a feature of its own
> >which is a useful feature in U-boot.  Orthogonal to that is the need to
> >boot kernels in Hyp-mode, and being booted in secure more and
> >controlling all of the non-secure worlds is just one scenario for that.
> 
> OK. Originally I had an implementation which separated the non-sec
> switch and the HYP switch. Later I got the impression that there is
> no real use-case for a non-sec switch only, so I dropped the
> specific command and just kept the logical separation in the patch
> split. That simplified the patches quite a bit.

Really, did it simplify it a lot? I would see almost the identical lines
of code, just placed elsewhere and instead of one assembly instruction
falling through into the other one, there would be an explicit branch.
I'm sure that code will be easier to read, because right now that
assembly routine does *a lot* of things.

> 
> So do you want to have a new u-boot command switching to non-secure
> state? I think that would make the patches more complicated, but if
> you recommend to have such a thing, I am happy to provide it.

No, I just want the cpu_switch_to_non_secure function to do just that,
and be a separately contained function (preferebly in its own file as
well; there's no need for anyone else trying to understand U-boot
unrelated of Hyp to read through all that).

So instead of what you have now, you would have something like this:

@ in secure_smp.S

secondary_non_secure_init:
	mrc     p15, 4, r1, c15, c0, 0		@ r1 = PREIPHBASE
	mov	r0, #0
	bl	cpu_non_secure_init
	b	smp_pen

smp_pen:
	b	smp_pen		@ This is a poor smp_pen


@ in secure_boot.S
/**
 * void cpu_non_secure_init(bool boot_cpu, unsigned periphbase)
 *
 * Blah blah blah...
 */
cpu_non_secure_init:
	...
	mov	pc, lr


/* in secure_boot.c */

int armv7_switch_nonsec(void)
{

	...

	writel(vexpress_sysflags + 4, 0xffffffff);
	writel(vexpress_sysflags, secondary_non_secure_init);

	cpu_non_secure_init(true, periphbase);
};

> Core problem here is that there is no easy way to check whether you
> are non-secure. Accessing the SCR register will trap if the NS bit
> is set. I could just catch this and return back with an error
> condition.

But that's an orthogonal issue though right?  There is no good way to
determine if you're in secure on non-secure mode, afaik, so there we
simply have to rely on proper configuration of the boot loader per
board, which is exactly why this is not done in the kernel.

> Just not sure this is really worth the effort.

Meta comment: I think making critical code like this as readable as
possible is almost always worth the effort, for the educational purpose
for other readers, to save time for everyone, and for our own sanity
when we have to go back and debug/extend the code in a couple of years.



> 
> Also leaves one question: How to handle if the users switched
> deliberately to non-secure before and now the bootm routine wants to
> go to HYP mode? Do we want to stay in SVC in this case?
> 

No no, I never wanted users to have a separate command, just structure
the code differently.

> >>+ *
> >>+ * See file CREDITS for list of people who contributed to this
> >>+ * project.
> >>+ *
> >>+ * This program is free software; you can redistribute it and/or
> >>+ * modify it under the terms of the GNU General Public License as
> >>+ * published by the Free Software Foundation; either version 2 of
> >>+ * the License, or (at your option) any later version.
> >>+ *
> >>+ * This program is distributed in the hope that it will be useful,
> >>+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
> >>+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
> >>+ * GNU General Public License for more details.
> >>+ *
> >>+ * You should have received a copy of the GNU General Public License
> >>+ * along with this program; if not, write to the Free Software
> >>+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
> >>+ * MA 02111-1307 USA
> >>+ */
> >>+
> >>+#include <common.h>
> >>+#include <asm/armv7.h>
> >>+
> >>+#define GICD_CTLR	0x000
> >>+#define GICD_TYPER	0x004
> >>+#define GICD_IGROUPR0	0x080
> >>+#define GICD_SGIR	0xf00
> >>+
> >>+#define CPU_ARM_CORTEX_A15	0x4100c0f0
> >>+#define CPU_ARM_CORTEX_A7	0x4100c070
> >>+
> >>+static inline unsigned int read_cpsr(void)
> >
> >inline is typically not used in C-files - at least not in the kernel.
> >GCC is pretty smart about this on its own.
> 
> Right. I think newer GCCs inline short static functions automatically.
> 

Depends a bit on the optimization flag, but yes, I was once told that
using the inline keyword on C files on any compiler rolled after the
90's was considered bad style :)

> >>+{
> >>+	unsigned int reg;
> >>+
> >>+	asm volatile ("mrs %0, cpsr\n" : "=r" (reg));
> >>+	return reg;
> >>+}
> >>+
> >>+int armv7_switch_nonsec(void)
> >>+{
> >>+	unsigned int reg;
> >>+	volatile unsigned int *gicdptr;
> >>+	unsigned itlinesnr, i;
> >>+
> >>+	/* check whether the CPU supports the security extensions */
> >>+	asm("mrc p15, 0, %0, c0, c1, 1\n" : "=r"(reg));
> >
> >My brain hasn't managed to learn all the coprocessor register by heart
> >yet, so if you could provide the name for these registers it would be
> >cool.  Alternatively you could just create nice static little functions
> >just like you do with the cpsr.
> 
> OK. Actually I found this quite some overhead if its only called
> once (cpsr is called twice). Would just a comment suffice?
> 

sure, see whatever is prettiest. I had a slight feeling that this
function would read more nicely with some accessor functions, but it's
your call.

> >>+	if ((reg & 0xF0) == 0)
> >>+		return 2;
> >>+
> >>+	/* the timer frequency for the generic timer needs to be
> >>+	 * programmed still in secure state, should be done by firmware.
> >
> >nit: drop 'still'
> >nit: the 'should be done by firmware' is not very descriptive, consider
> >stating clearly that this is a work-around for broken bootloaders.
> 
> ACK.
> 
> >>+	 * check whether we have the generic timer first
> >>+	 */
> >>+#ifdef CONFIG_SYS_CLK_FREQ
> >>+	asm("mrc p15, 0, %0, c0, c1, 1\n" : "=r"(reg));
> >>+	if ((reg & 0xF0000) == 0x10000)
> >>+		asm("mcr p15, 0, %0, c14, c0, 0\n"
> >>+		: : "r"(CONFIG_SYS_CLK_FREQ));
> >>+#endif
> >>+
> >>+	/* the SCR register will be set directly in the monitor mode handler,
> >>+	 * according to the spec one should not tinker with it in secure state
> >>+	 * in SVC mode. Do not try to read it once in non-secure state,
> >>+	 * any access to it will trap.
> >>+	 */
> >>+
> >>+	/* check whether we are an Cortex-A15 or A7.
> >
> >nit: s/whether/if/
> >
> >>+	 * The actual non-secure switch should work with all CPUs supporting
> >>+	 * the security 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));
> >>+	if (((reg & 0xFF00FFF0) != 0x4100C0F0) &&
> >>+	    ((reg & 0xFF00FFF0) != 0x4100C070))
> >
> >Are there really no defines for this in U-boot already?
> >
> >It seems to me that a static inline in a header file somewhere that
> >gives you a processor type back would be useful for other things as
> >well, but I don't know U-boot enough to properly say...
> 
> You are right, this is barely readable. Will fix this.
> 
> >>+		return 3;
> >>+
> >>+	/* get the GIC base address from the A15 PERIPHBASE register */
> >>+	asm("mrc p15, 4, %0, c15, c0, 0\n" : "=r" (reg));
> >>+
> >>+	/* the PERIPHBASE can be mapped above 4 GB (lower 8 bits used to
> >>+	 * encode this). Bail out here since we cannot access this without
> >>+	 * enabling paging.
> >>+	 */
> >
> >ah, here you check for it...
> >
> >>+	if ((reg & 0xff) != 0)
> >>+		return 4;
> >
> >if you're getting the PERIPHBASE here anyway, why not just pass it as a
> >parameter to the non-secure gic init routine?
> 
> Because this routine is also called directly out of the SMP pen, so
> I cannot pass any parameters there.
> 

you can just read that value once before you start looping in the smp
pen and pass it as an argument, see my example above.

> >>+
> >>+	/* GIC distributor registers start at offset 0x1000 */
> >>+	gicdptr = (unsigned *)(reg + 0x1000);
> >>+
> >>+	/* enable the GIC distributor */
> >>+	gicdptr[GICD_CTLR / 4] |= 0x03;
> >
> >so this is I/O accesses, so you could consider using readl for
> >consistency, which would also get rid of the division in the array
> >element accessors.
> 
> Good hint, thanks.
> 
> >>+
> >>+	/* TYPER[4:0] contains an encoded number of all interrupts */
> >>+	itlinesnr = gicdptr[GICD_TYPER / 4] & 0x1f;
> >>+
> >>+	/* set all bits in the GIC group registers to one to allow access
> >>+	 * from non-secure state
> >>+	 */
> >>+	for (i = 0; i <= itlinesnr; i++)
> >>+		gicdptr[GICD_IGROUPR0 / 4 + i] = (unsigned)-1;
> >
> >didn't you also do this in the assembly routine for IGROUP0 only?
> >
> >oh wait, it's dawning on me, the _nonsec_gic_switch is actually per-cpu
> >non-secure init, right?
> 
> Right. I take this as an advice to explain this earlier...
> 
More an advice to rename the assembly routine.

Thanks,
-Christoffer

Patch

diff --git a/arch/arm/include/asm/armv7.h b/arch/arm/include/asm/armv7.h
index a73630b..25afffe 100644
--- a/arch/arm/include/asm/armv7.h
+++ b/arch/arm/include/asm/armv7.h
@@ -74,4 +74,11 @@  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);
 
+#ifdef CONFIG_ARMV7_VIRT
+int armv7_switch_nonsec(void);
+
+/* defined in cpu/armv7/start.S */
+void _nonsec_gic_switch(void);
+#endif /* CONFIG_ARMV7_VIRT */
+
 #endif
diff --git a/arch/arm/lib/Makefile b/arch/arm/lib/Makefile
index 6ae161a..37a0e71 100644
--- a/arch/arm/lib/Makefile
+++ b/arch/arm/lib/Makefile
@@ -58,6 +58,8 @@  COBJS-y	+= reset.o
 COBJS-y	+= cache.o
 COBJS-y	+= cache-cp15.o
 
+COBJS-$(CONFIG_ARMV7_VIRT) += virt-v7.o
+
 SRCS	:= $(GLSOBJS:.o=.S) $(GLCOBJS:.o=.c) \
 	   $(SOBJS-y:.o=.S) $(COBJS-y:.o=.c)
 OBJS	:= $(addprefix $(obj),$(SOBJS-y) $(COBJS-y))
diff --git a/arch/arm/lib/bootm.c b/arch/arm/lib/bootm.c
index f3b30c5..a3d3aae 100644
--- a/arch/arm/lib/bootm.c
+++ b/arch/arm/lib/bootm.c
@@ -35,6 +35,10 @@ 
 #include <asm/bootm.h>
 #include <linux/compiler.h>
 
+#ifdef CONFIG_ARMV7_VIRT
+#include <asm/armv7.h>
+#endif
+
 DECLARE_GLOBAL_DATA_PTR;
 
 #if defined(CONFIG_SETUP_MEMORY_TAGS) || \
@@ -319,6 +323,22 @@  static void boot_prep_linux(bootm_headers_t *images)
 		hang();
 #endif /* all tags */
 	}
+#ifdef CONFIG_ARMV7_VIRT
+	switch (armv7_switch_nonsec()) {
+	case 0:
+		debug("entered non-secure state\n");
+		break;
+	case 2:
+		printf("HYP mode: Security extensions not implemented.\n");
+		break;
+	case 3:
+		printf("HYP mode: CPU not supported (must be Cortex-A15 or A7).\n");
+		break;
+	case 4:
+		printf("HYP mode: PERIPHBASE is above 4 GB, cannot access this.\n");
+		break;
+	}
+#endif
 }
 
 /* Subcommand: GO */
diff --git a/arch/arm/lib/virt-v7.c b/arch/arm/lib/virt-v7.c
new file mode 100644
index 0000000..3a48aee
--- /dev/null
+++ b/arch/arm/lib/virt-v7.c
@@ -0,0 +1,113 @@ 
+/*
+ * (C) Copyright 2013
+ * Andre Przywara, Linaro
+ *
+ * routines to push ARMv7 processors from secure into non-secure state
+ * needed to enable ARMv7 virtualization for current hypervisors
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#include <common.h>
+#include <asm/armv7.h>
+
+#define GICD_CTLR	0x000
+#define GICD_TYPER	0x004
+#define GICD_IGROUPR0	0x080
+#define GICD_SGIR	0xf00
+
+#define CPU_ARM_CORTEX_A15	0x4100c0f0
+#define CPU_ARM_CORTEX_A7	0x4100c070
+
+static inline unsigned int read_cpsr(void)
+{
+	unsigned int reg;
+
+	asm volatile ("mrs %0, cpsr\n" : "=r" (reg));
+	return reg;
+}
+
+int armv7_switch_nonsec(void)
+{
+	unsigned int reg;
+	volatile unsigned int *gicdptr;
+	unsigned itlinesnr, i;
+
+	/* check whether the CPU supports the security extensions */
+	asm("mrc p15, 0, %0, c0, c1, 1\n" : "=r"(reg));
+	if ((reg & 0xF0) == 0)
+		return 2;
+
+	/* the timer frequency for the generic timer needs to be
+	 * programmed still in secure state, should be done by firmware.
+	 * check whether we have the generic timer first
+	 */
+#ifdef CONFIG_SYS_CLK_FREQ
+	asm("mrc p15, 0, %0, c0, c1, 1\n" : "=r"(reg));
+	if ((reg & 0xF0000) == 0x10000)
+		asm("mcr p15, 0, %0, c14, c0, 0\n"
+		: : "r"(CONFIG_SYS_CLK_FREQ));
+#endif
+
+	/* the SCR register will be set directly in the monitor mode handler,
+	 * according to the spec one should not tinker with it in secure state
+	 * in SVC mode. Do not try to read it once in non-secure state,
+	 * any access to it will trap.
+	 */
+
+	/* 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,
+	 * which we know only for sure for those two CPUs.
+	 */
+	asm("mrc p15, 0, %0, c0, c0, 0\n" : "=r"(reg));
+	if (((reg & 0xFF00FFF0) != 0x4100C0F0) &&
+	    ((reg & 0xFF00FFF0) != 0x4100C070))
+		return 3;
+
+	/* get the GIC base address from the A15 PERIPHBASE register */
+	asm("mrc p15, 4, %0, c15, c0, 0\n" : "=r" (reg));
+
+	/* the PERIPHBASE can be mapped above 4 GB (lower 8 bits used to
+	 * encode this). Bail out here since we cannot access this without
+	 * enabling paging.
+	 */
+	if ((reg & 0xff) != 0)
+		return 4;
+
+	/* GIC distributor registers start at offset 0x1000 */
+	gicdptr = (unsigned *)(reg + 0x1000);
+
+	/* enable the GIC distributor */
+	gicdptr[GICD_CTLR / 4] |= 0x03;
+
+	/* TYPER[4:0] contains an encoded number of all interrupts */
+	itlinesnr = gicdptr[GICD_TYPER / 4] & 0x1f;
+
+	/* set all bits in the GIC group registers to one to allow access
+	 * from non-secure state
+	 */
+	for (i = 0; i <= itlinesnr; i++)
+		gicdptr[GICD_IGROUPR0 / 4 + i] = (unsigned)-1;
+
+	/* call the non-sec switching code on this CPU */
+	_nonsec_gic_switch();
+
+	return 0;
+}