diff mbox

[U-Boot,v2,4/7] ARM: switch to non-secure state during bootm execution

Message ID 1371121273-18763-5-git-send-email-andre.przywara@linaro.org
State Superseded
Delegated to: Albert ARIBAUD
Headers show

Commit Message

Andre Przywara June 13, 2013, 11:01 a.m. UTC 
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.

The core specific part of the work is done in the assembly routine
in nonsec_virt.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.

We check the availability of the security extensions first.

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.
The Versatile Express board does not need this, so we remove the
frequency from the configuration file here.

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.
Board not implementing the CBAR can override this value via a
configuration file variable.

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.

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              | 137 ++++++++++++++++++++++++++++++++++++
 include/configs/vexpress_ca15_tc2.h |   2 -
 5 files changed, 166 insertions(+), 2 deletions(-)
 create mode 100644 arch/arm/lib/virt-v7.c

Comments

Christoffer Dall June 19, 2013, 11:13 p.m. UTC | #1 
On Thu, Jun 13, 2013 at 01:01:10PM +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.
> 
> The core specific part of the work is done in the assembly routine
> in nonsec_virt.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.
> 
> We check the availability of the security extensions first.
> 
> 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.
> The Versatile Express board does not need this, so we remove the
> frequency from the configuration file here.
> 
> 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.
> Board not implementing the CBAR can override this value via a
> configuration file variable.
> 
> 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.
> 
> 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              | 137 ++++++++++++++++++++++++++++++++++++
>  include/configs/vexpress_ca15_tc2.h |   2 -
>  5 files changed, 166 insertions(+), 2 deletions(-)
>  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 989bb72..56d0dd0 100644
> --- a/arch/arm/include/asm/armv7.h
> +++ b/arch/arm/include/asm/armv7.h
> @@ -88,6 +88,13 @@ void v7_outer_cache_flush_range(u32 start, u32 end);
>  void v7_outer_cache_inval_range(u32 start, u32 end);
>  
>  #ifdef CONFIG_ARMV7_VIRT
> +
> +#define HYP_ERR_NO_SEC_EXT		2
> +#define HYP_ERR_NO_GIC_ADDRESS		3
> +#define HYP_ERR_GIC_ADDRESS_ABOVE_4GB	4

enum?

> +
> +int armv7_switch_nonsec(void);
> +
>  /* defined in cpu/armv7/nonsec_virt.S */
>  void _nonsec_init(void);
>  #endif /* CONFIG_ARMV7_VIRT */
> diff --git a/arch/arm/lib/Makefile b/arch/arm/lib/Makefile
> index 8ad9f66..1570ad5 100644
> --- a/arch/arm/lib/Makefile
> +++ b/arch/arm/lib/Makefile
> @@ -60,6 +60,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 1b6e0ac..8251a89 100644
> --- a/arch/arm/lib/bootm.c
> +++ b/arch/arm/lib/bootm.c
> @@ -34,6 +34,10 @@
>  #include <asm/bootm.h>
>  #include <linux/compiler.h>
>  
> +#ifdef CONFIG_ARMV7_VIRT
> +#include <asm/armv7.h>
> +#endif
> +
>  DECLARE_GLOBAL_DATA_PTR;
>  
>  static struct tag *params;
> @@ -222,6 +226,22 @@ static void boot_prep_linux(bootm_headers_t *images)
>  		printf("FDT and ATAGS support not compiled in - hanging\n");
>  		hang();
>  	}
> +#ifdef CONFIG_ARMV7_VIRT
> +	switch (armv7_switch_nonsec()) {
> +	case 0:
> +		debug("entered non-secure state\n");
> +		break;

this is weird, why not have a define for the success case?

I still think the debug printing should be done inside
armv7_switch_nonsec instead, and you can just have it be a void();

> +	case HYP_ERR_NO_SEC_EXT:
> +		printf("HYP mode: Security extensions not implemented.\n");
> +		break;
> +	case HYP_ERR_NO_GIC_ADDRESS:
> +		printf("HYP mode: could not determine GIC address.\n");
> +		break;
> +	case HYP_ERR_GIC_ADDRESS_ABOVE_4GB:
> +		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..7876a77
> --- /dev/null
> +++ b/arch/arm/lib/virt-v7.c
> @@ -0,0 +1,137 @@
> +/*
> + * (C) Copyright 2013
> + * Andre Przywara, Linaro
> + *
> + * Routines to transition 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>
> +#include <asm/gic.h>
> +#include <asm/io.h>
> +
> +static unsigned int read_id_pfr1(void)
> +{
> +	unsigned int reg;
> +
> +	asm("mrc p15, 0, %0, c0, c1, 1\n" : "=r"(reg));
> +	return reg;
> +}
> +
> +/* The timer frequency for the generic timer needs to be
> + * programmed in secure state. Some primary bootloaders / firmware
> + * omit this, so if the frequency is provided in the configuration,
> + * we do this here instead.
> + * But first check if we have the generic timer.
> + */
> +static void set_generic_timer_frequency(void)
> +{
> +#ifdef CONFIG_SYS_CLK_FREQ
> +	unsigned int reg;
> +
> +	reg = read_id_pfr1();
> +	if ((reg & CPUID_ARM_TIMER_MASK) == 1U << CPUID_ARM_TIMER_SHIFT)
> +		asm("mcr p15, 0, %0, c14, c0, 0\n"
> +		: : "r"(CONFIG_SYS_CLK_FREQ));
> +#endif
> +}
> +
> +static int get_gic_base_address(char **gicdptr)

you could simplify this function and make it an unsigned function and
return (unsigned)-1 on error (and do the debug error print right away in
there).

> +{
> +#ifdef CONFIG_ARM_GIC_BASE_ADDRESS
> +	*gicdptr = (void *)(CONFIG_ARM_GIC_BASE_ADDRESS + GIC_DIST_OFFSET);
> +	return 0;
> +#else
> +	unsigned midr;
> +	unsigned periphbase;
> +
> +	/* check whether we are an Cortex-A15 or A7.
> +	 * 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"(midr));
> +	switch (midr & MIDR_PRIMARY_PART_MASK) {
> +	case MIDR_CORTEX_A9_R0P1:
> +	case MIDR_CORTEX_A15_R0P0:
> +	case MIDR_CORTEX_A7_R0P0:
> +		break;
> +	default:
> +		return HYP_ERR_NO_GIC_ADDRESS;
> +	}
> +
> +	/* get the GIC base address from the CBAR register */
> +	asm("mrc p15, 4, %0, c15, c0, 0\n" : "=r" (periphbase));
> +
> +	/* 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 ((periphbase & 0xff) != 0)
> +		return HYP_ERR_GIC_ADDRESS_ABOVE_4GB;
> +
> +	*gicdptr = (void *)(periphbase + GIC_DIST_OFFSET);

this is weird, the function is called get_gic_base_address, but you're
returning the distributor base address, and the GIC_DIST_OFFSET is
actually an A15/A7 specific thing.

> +
> +	return 0;
> +#endif
> +}
> +
> +int armv7_switch_nonsec(void)
> +{
> +	unsigned int reg, ret;
> +	char *gicdptr;

there's really no need having this be a pointer when you use writel /
readl to access it.

> +	unsigned itlinesnr, i;
> +
> +	/* check whether the CPU supports the security extensions */
> +	reg = read_id_pfr1();
> +	if ((reg & 0xF0) == 0)
> +		return HYP_ERR_NO_SEC_EXT;
> +
> +	set_generic_timer_frequency();
> +
> +	/* 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.
> +	 */
> +
> +	ret = get_gic_base_address(&gicdptr);
> +	if (ret != 0)
> +		return ret;
> +
> +	/* enable the GIC distributor */
> +	writel(readl(&gicdptr[GICD_CTLR]) | 0x03, &gicdptr[GICD_CTLR]);

I would do the readl first, and then the writel, but it's just a matter
of style.

"gicdptr + GICD_CTLR" seems cleaner

> +
> +	/* TYPER[4:0] contains an encoded number of all interrupts */

nit: s/all/avail./

> +	itlinesnr = readl(&gicdptr[GICD_TYPER]) & 0x1f;
> +
> +	/* set all bits in the GIC group registers to one to allow access
> +	 * from non-secure state
> +	 */
> +	for (i = 0; i <= itlinesnr; i++)
> +		writel((unsigned)-1, &gicdptr[GICD_IGROUPRn + 4 * i]);
> +
> +	/* call the non-sec switching code on this CPU */
> +	_nonsec_init();
> +
> +	return 0;
> +}
> diff --git a/include/configs/vexpress_ca15_tc2.h b/include/configs/vexpress_ca15_tc2.h
> index 9e230ad..4f425ac 100644
> --- a/include/configs/vexpress_ca15_tc2.h
> +++ b/include/configs/vexpress_ca15_tc2.h
> @@ -31,6 +31,4 @@
>  #include "vexpress_common.h"
>  #define CONFIG_BOOTP_VCI_STRING     "U-boot.armv7.vexpress_ca15x2_tc2"
>  
> -#define CONFIG_SYS_CLK_FREQ 24000000
> -
>  #endif
> -- 
> 1.7.12.1
> 

Besides my crazy nit-picking, this looks good to me.

-Christoffer
Masahiro Yamada June 28, 2013, 3:18 a.m. UTC | #2 
Hi Andre.


> --- a/arch/arm/lib/Makefile
> +++ b/arch/arm/lib/Makefile
> @@ -60,6 +60,8 @@ COBJS-y	+= reset.o
>  COBJS-y	+= cache.o
>  COBJS-y	+= cache-cp15.o
>  
> +COBJS-$(CONFIG_ARMV7_VIRT) += virt-v7.o
> +

Judging from the file name virt-v7.c,
you are thinkig this file is specific to ARMv7, aren't you?

If so, why don't you move this file
to arch/arm/cpu/armv7/ ?


> +static void set_generic_timer_frequency(void)
> +{
> +#ifdef CONFIG_SYS_CLK_FREQ
> +	unsigned int reg;
> +
> +	reg = read_id_pfr1();
> +	if ((reg & CPUID_ARM_TIMER_MASK) == 1U << CPUID_ARM_TIMER_SHIFT)
> +		asm("mcr p15, 0, %0, c14, c0, 0\n"
> +		: : "r"(CONFIG_SYS_CLK_FREQ));
> +#endif

CPUID_ARM_TIMER_MASK
CPUID_ARM_TIMER_SHIFT

I think these macro names are vague.
There are Generic Timer, Global Timer, Private Timer etc.


Unlike other parts, the care for Cortex-A9 is missing here.
To be more generic, I'd like to suggest to allow Non-secure access
to Global/Private timers before switching to non-secure state.


How about like this for armv7_switch_nonsec function?


    /* check whether the CPU supports the security extensions */
    reg = read_id_pfr1();
    if ((reg & 0xF0) == 0)
            return HYP_ERR_NO_SEC_EXT;

    if ((reg & CPUID_ARM_TIMER_MASK) == 1U << CPUID_ARM_TIMER_SHIFT)
            set_generic_timer_frequency();
    else
            /* Allow Non-secure access to Global/Private timers */
            writel(0xfff, periph_base + SCU_SNSAC);



For more info about SCU Non-secure Access Control (SNSAC) Register,
please refer Cortex A9 mpcore TRM.  page 2-11
http://infocenter.arm.com/help/topic/com.arm.doc.ddi0407g/DDI0407G_cortex_a9_mpcore_r3p0_trm.pdf



> +	/* enable the GIC distributor */
> +	writel(readl(&gicdptr[GICD_CTLR]) | 0x03, &gicdptr[GICD_CTLR]);

I am not sure this code is really necessary.

Because your are setting all available interrupts to Group1 just below,
I think we don't need to do this in secure state.


Best Regards
Masahiro Yamada
Andre Przywara July 4, 2013, 7:42 a.m. UTC | #3 
On 06/28/2013 05:18 AM, Masahiro Yamada wrote:
> Hi Andre.
>
>
>> --- a/arch/arm/lib/Makefile
>> +++ b/arch/arm/lib/Makefile
>> @@ -60,6 +60,8 @@ COBJS-y	+= reset.o
>>   COBJS-y	+= cache.o
>>   COBJS-y	+= cache-cp15.o
>>
>> +COBJS-$(CONFIG_ARMV7_VIRT) += virt-v7.o
>> +
>
> Judging from the file name virt-v7.c,
> you are thinkig this file is specific to ARMv7, aren't you?
>
> If so, why don't you move this file
> to arch/arm/cpu/armv7/ ?
>
>
>> +static void set_generic_timer_frequency(void)
>> +{
>> +#ifdef CONFIG_SYS_CLK_FREQ
>> +	unsigned int reg;
>> +
>> +	reg = read_id_pfr1();
>> +	if ((reg & CPUID_ARM_TIMER_MASK) == 1U << CPUID_ARM_TIMER_SHIFT)
>> +		asm("mcr p15, 0, %0, c14, c0, 0\n"
>> +		: : "r"(CONFIG_SYS_CLK_FREQ));
>> +#endif
>
> CPUID_ARM_TIMER_MASK
> CPUID_ARM_TIMER_SHIFT
>
> I think these macro names are vague.
> There are Generic Timer, Global Timer, Private Timer etc.

Good point.

> Unlike other parts, the care for Cortex-A9 is missing here.
> To be more generic, I'd like to suggest to allow Non-secure access
> to Global/Private timers before switching to non-secure state.
>
>
> How about like this for armv7_switch_nonsec function?
>
>
>      /* check whether the CPU supports the security extensions */
>      reg = read_id_pfr1();
>      if ((reg & 0xF0) == 0)
>              return HYP_ERR_NO_SEC_EXT;
>
>      if ((reg & CPUID_ARM_TIMER_MASK) == 1U << CPUID_ARM_TIMER_SHIFT)
>              set_generic_timer_frequency();
>      else
>              /* Allow Non-secure access to Global/Private timers */
>              writel(0xfff, periph_base + SCU_SNSAC);

Interesting, however I don't have access to an A9 board currently to 
properly test this. So I will do the renaming and let the access to the 
other timers up to a follow-up patch.

Thanks,
Andre.

>
> For more info about SCU Non-secure Access Control (SNSAC) Register,
> please refer Cortex A9 mpcore TRM.  page 2-11
> http://infocenter.arm.com/help/topic/com.arm.doc.ddi0407g/DDI0407G_cortex_a9_mpcore_r3p0_trm.pdf
>
>
>
>> +	/* enable the GIC distributor */
>> +	writel(readl(&gicdptr[GICD_CTLR]) | 0x03, &gicdptr[GICD_CTLR]);
>
> I am not sure this code is really necessary.
>
> Because your are setting all available interrupts to Group1 just below,
> I think we don't need to do this in secure state.
>
>
> Best Regards
> Masahiro Yamada
>
diff mbox

Patch

diff --git a/arch/arm/include/asm/armv7.h b/arch/arm/include/asm/armv7.h
index 989bb72..56d0dd0 100644
--- a/arch/arm/include/asm/armv7.h
+++ b/arch/arm/include/asm/armv7.h
@@ -88,6 +88,13 @@  void v7_outer_cache_flush_range(u32 start, u32 end);
 void v7_outer_cache_inval_range(u32 start, u32 end);
 
 #ifdef CONFIG_ARMV7_VIRT
+
+#define HYP_ERR_NO_SEC_EXT		2
+#define HYP_ERR_NO_GIC_ADDRESS		3
+#define HYP_ERR_GIC_ADDRESS_ABOVE_4GB	4
+
+int armv7_switch_nonsec(void);
+
 /* defined in cpu/armv7/nonsec_virt.S */
 void _nonsec_init(void);
 #endif /* CONFIG_ARMV7_VIRT */
diff --git a/arch/arm/lib/Makefile b/arch/arm/lib/Makefile
index 8ad9f66..1570ad5 100644
--- a/arch/arm/lib/Makefile
+++ b/arch/arm/lib/Makefile
@@ -60,6 +60,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 1b6e0ac..8251a89 100644
--- a/arch/arm/lib/bootm.c
+++ b/arch/arm/lib/bootm.c
@@ -34,6 +34,10 @@ 
 #include <asm/bootm.h>
 #include <linux/compiler.h>
 
+#ifdef CONFIG_ARMV7_VIRT
+#include <asm/armv7.h>
+#endif
+
 DECLARE_GLOBAL_DATA_PTR;
 
 static struct tag *params;
@@ -222,6 +226,22 @@  static void boot_prep_linux(bootm_headers_t *images)
 		printf("FDT and ATAGS support not compiled in - hanging\n");
 		hang();
 	}
+#ifdef CONFIG_ARMV7_VIRT
+	switch (armv7_switch_nonsec()) {
+	case 0:
+		debug("entered non-secure state\n");
+		break;
+	case HYP_ERR_NO_SEC_EXT:
+		printf("HYP mode: Security extensions not implemented.\n");
+		break;
+	case HYP_ERR_NO_GIC_ADDRESS:
+		printf("HYP mode: could not determine GIC address.\n");
+		break;
+	case HYP_ERR_GIC_ADDRESS_ABOVE_4GB:
+		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..7876a77
--- /dev/null
+++ b/arch/arm/lib/virt-v7.c
@@ -0,0 +1,137 @@ 
+/*
+ * (C) Copyright 2013
+ * Andre Przywara, Linaro
+ *
+ * Routines to transition 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>
+#include <asm/gic.h>
+#include <asm/io.h>
+
+static unsigned int read_id_pfr1(void)
+{
+	unsigned int reg;
+
+	asm("mrc p15, 0, %0, c0, c1, 1\n" : "=r"(reg));
+	return reg;
+}
+
+/* The timer frequency for the generic timer needs to be
+ * programmed in secure state. Some primary bootloaders / firmware
+ * omit this, so if the frequency is provided in the configuration,
+ * we do this here instead.
+ * But first check if we have the generic timer.
+ */
+static void set_generic_timer_frequency(void)
+{
+#ifdef CONFIG_SYS_CLK_FREQ
+	unsigned int reg;
+
+	reg = read_id_pfr1();
+	if ((reg & CPUID_ARM_TIMER_MASK) == 1U << CPUID_ARM_TIMER_SHIFT)
+		asm("mcr p15, 0, %0, c14, c0, 0\n"
+		: : "r"(CONFIG_SYS_CLK_FREQ));
+#endif
+}
+
+static int get_gic_base_address(char **gicdptr)
+{
+#ifdef CONFIG_ARM_GIC_BASE_ADDRESS
+	*gicdptr = (void *)(CONFIG_ARM_GIC_BASE_ADDRESS + GIC_DIST_OFFSET);
+	return 0;
+#else
+	unsigned midr;
+	unsigned periphbase;
+
+	/* check whether we are an Cortex-A15 or A7.
+	 * 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"(midr));
+	switch (midr & MIDR_PRIMARY_PART_MASK) {
+	case MIDR_CORTEX_A9_R0P1:
+	case MIDR_CORTEX_A15_R0P0:
+	case MIDR_CORTEX_A7_R0P0:
+		break;
+	default:
+		return HYP_ERR_NO_GIC_ADDRESS;
+	}
+
+	/* get the GIC base address from the CBAR register */
+	asm("mrc p15, 4, %0, c15, c0, 0\n" : "=r" (periphbase));
+
+	/* 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 ((periphbase & 0xff) != 0)
+		return HYP_ERR_GIC_ADDRESS_ABOVE_4GB;
+
+	*gicdptr = (void *)(periphbase + GIC_DIST_OFFSET);
+
+	return 0;
+#endif
+}
+
+int armv7_switch_nonsec(void)
+{
+	unsigned int reg, ret;
+	char *gicdptr;
+	unsigned itlinesnr, i;
+
+	/* check whether the CPU supports the security extensions */
+	reg = read_id_pfr1();
+	if ((reg & 0xF0) == 0)
+		return HYP_ERR_NO_SEC_EXT;
+
+	set_generic_timer_frequency();
+
+	/* 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.
+	 */
+
+	ret = get_gic_base_address(&gicdptr);
+	if (ret != 0)
+		return ret;
+
+	/* enable the GIC distributor */
+	writel(readl(&gicdptr[GICD_CTLR]) | 0x03, &gicdptr[GICD_CTLR]);
+
+	/* TYPER[4:0] contains an encoded number of all interrupts */
+	itlinesnr = readl(&gicdptr[GICD_TYPER]) & 0x1f;
+
+	/* set all bits in the GIC group registers to one to allow access
+	 * from non-secure state
+	 */
+	for (i = 0; i <= itlinesnr; i++)
+		writel((unsigned)-1, &gicdptr[GICD_IGROUPRn + 4 * i]);
+
+	/* call the non-sec switching code on this CPU */
+	_nonsec_init();
+
+	return 0;
+}
diff --git a/include/configs/vexpress_ca15_tc2.h b/include/configs/vexpress_ca15_tc2.h
index 9e230ad..4f425ac 100644
--- a/include/configs/vexpress_ca15_tc2.h
+++ b/include/configs/vexpress_ca15_tc2.h
@@ -31,6 +31,4 @@ 
 #include "vexpress_common.h"
 #define CONFIG_BOOTP_VCI_STRING     "U-boot.armv7.vexpress_ca15x2_tc2"
 
-#define CONFIG_SYS_CLK_FREQ 24000000
-
 #endif