Merge branch 'master' of git://git.denx.de/u-boot-arm

6 files changed, 455 insertions, 1 deletions

diff --git a/arch/arm/cpu/armv7/Makefile b/arch/arm/cpu/armv7/Makefile
index b723e22a5c..ee4b02183a 100644
--- a/arch/arm/cpu/armv7/Makefile
+++ b/arch/arm/cpu/armv7/Makefile
@@ -20,6 +20,11 @@ ifneq ($(CONFIG_AM43XX)$(CONFIG_AM33XX)$(CONFIG_OMAP44XX)$(CONFIG_OMAP54XX)$(CON
 SOBJS	+= lowlevel_init.o
 endif
 
+ifneq ($(CONFIG_ARMV7_NONSEC)$(CONFIG_ARMV7_VIRT),)
+SOBJS	+= nonsec_virt.o
+COBJS	+= virt-v7.o
+endif
+
 SRCS	:= $(START:.o=.S) $(COBJS:.o=.c)
 OBJS	:= $(addprefix $(obj),$(COBJS) $(SOBJS))
 START	:= $(addprefix $(obj),$(START))
diff --git a/arch/arm/cpu/armv7/nonsec_virt.S b/arch/arm/cpu/armv7/nonsec_virt.S
new file mode 100644
index 0000000000..358348ffa5
--- /dev/null
+++ b/arch/arm/cpu/armv7/nonsec_virt.S
@@ -0,0 +1,208 @@
+/*
+ * code for switching cores into non-secure state and into HYP mode
+ *
+ * Copyright (c) 2013	Andre Przywara <andre.przywara@linaro.org>
+ *
+ * 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 <config.h>
+#include <linux/linkage.h>
+#include <asm/gic.h>
+#include <asm/armv7.h>
+
+.arch_extension sec
+.arch_extension virt
+
+/* the vector table for secure state and HYP mode */
+_monitor_vectors:
+	.word 0	/* reset */
+	.word 0 /* undef */
+	adr pc, _secure_monitor
+	.word 0
+	.word 0
+	adr pc, _hyp_trap
+	.word 0
+	.word 0
+
+/*
+ * secure monitor handler
+ * U-boot calls this "software interrupt" in start.S
+ * This is executed on a "smc" instruction, we use a "smc #0" to switch
+ * to non-secure state.
+ * We use only r0 and r1 here, due to constraints in the caller.
+ */
+	.align	5
+_secure_monitor:
+	mrc	p15, 0, r1, c1, c1, 0		@ read SCR
+	bic	r1, r1, #0x4e			@ clear IRQ, FIQ, EA, nET bits
+	orr	r1, r1, #0x31			@ enable NS, AW, FW bits
+
+#ifdef CONFIG_ARMV7_VIRT
+	mrc	p15, 0, r0, c0, c1, 1		@ read ID_PFR1
+	and	r0, r0, #CPUID_ARM_VIRT_MASK	@ mask virtualization bits
+	cmp	r0, #(1 << CPUID_ARM_VIRT_SHIFT)
+	orreq	r1, r1, #0x100			@ allow HVC instruction
+#endif
+
+	mcr	p15, 0, r1, c1, c1, 0		@ write SCR (with NS bit set)
+
+#ifdef CONFIG_ARMV7_VIRT
+	mrceq	p15, 0, r0, c12, c0, 1		@ get MVBAR value
+	mcreq	p15, 4, r0, c12, c0, 0		@ write HVBAR
+#endif
+
+	movs	pc, lr				@ return to non-secure SVC
+
+_hyp_trap:
+	mrs	lr, elr_hyp	@ for older asm: .byte 0x00, 0xe3, 0x0e, 0xe1
+	mov pc, lr				@ do no switch modes, but
+						@ return to caller
+
+/*
+ * 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
+	mov	r12, r0				@ save GICC address
+#ifdef CONFIG_ARMV7_VIRT
+	bl	_switch_to_hyp
+#endif
+
+	ldr	r1, [r12, #GICC_IAR]		@ acknowledge IPI
+	str	r1, [r12, #GICC_EOIR]		@ signal end of interrupt
+
+	adr	r0, _smp_pen			@ do not use this address again
+	b	smp_waitloop			@ wait for IPIs, board specific
+ENDPROC(_smp_pen)
+
+/*
+ * Switch a core to non-secure state.
+ *
+ *  1. initialize the GIC per-core interface
+ *  2. allow coprocessor access in non-secure modes
+ *  3. switch the cpu mode (by calling "smc #0")
+ *
+ * Called from smp_pen by secondary cores and directly by the BSP.
+ * Do not assume that the stack is available and only use registers
+ * r0-r3 and r12.
+ *
+ * PERIPHBASE is used to get the GIC address. This could be 40 bits long,
+ * though, but we check this in C before calling this function.
+ */
+ENTRY(_nonsec_init)
+#ifdef CONFIG_ARM_GIC_BASE_ADDRESS
+	ldr	r2, =CONFIG_ARM_GIC_BASE_ADDRESS
+#else
+	mrc	p15, 4, r2, c15, c0, 0		@ read CBAR
+	bfc	r2, #0, #15			@ clear reserved bits
+#endif
+	add	r3, r2, #GIC_DIST_OFFSET	@ GIC dist i/f offset
+	mvn	r1, #0				@ all bits to 1
+	str	r1, [r3, #GICD_IGROUPRn]	@ allow private interrupts
+
+	mrc	p15, 0, r0, c0, c0, 0		@ read MIDR
+	ldr	r1, =MIDR_PRIMARY_PART_MASK
+	and	r0, r0, r1			@ mask out variant and revision
+
+	ldr	r1, =MIDR_CORTEX_A7_R0P0 & MIDR_PRIMARY_PART_MASK
+	cmp	r0, r1				@ check for Cortex-A7
+
+	ldr	r1, =MIDR_CORTEX_A15_R0P0 & MIDR_PRIMARY_PART_MASK
+	cmpne	r0, r1				@ check for Cortex-A15
+
+	movne	r1, #GIC_CPU_OFFSET_A9		@ GIC CPU offset for A9
+	moveq	r1, #GIC_CPU_OFFSET_A15		@ GIC CPU offset for A15/A7
+	add	r3, r2, r1			@ r3 = GIC CPU i/f addr
+
+	mov	r1, #1				@ set GICC_CTLR[enable]
+	str	r1, [r3, #GICC_CTLR]		@ and clear all other bits
+	mov	r1, #0xff
+	str	r1, [r3, #GICC_PMR]		@ set priority mask register
+
+	movw	r1, #0x3fff
+	movt	r1, #0x0006
+	mcr	p15, 0, r1, c1, c1, 2		@ NSACR = all copros to non-sec
+
+/* The CNTFRQ register of 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.
+ */
+#ifdef CONFIG_SYS_CLK_FREQ
+	mrc	p15, 0, r0, c0, c1, 1		@ read ID_PFR1
+	and	r0, r0, #CPUID_ARM_GENTIMER_MASK	@ mask arch timer bits
+	cmp	r0, #(1 << CPUID_ARM_GENTIMER_SHIFT)
+	ldreq	r1, =CONFIG_SYS_CLK_FREQ
+	mcreq	p15, 0, r1, c14, c0, 0		@ write CNTFRQ
+#endif
+
+	adr	r1, _monitor_vectors
+	mcr	p15, 0, r1, c12, c0, 1		@ set MVBAR to secure vectors
+
+	mrc	p15, 0, ip, c12, c0, 0		@ save secure copy of VBAR
+
+	isb
+	smc	#0				@ call into MONITOR mode
+
+	mcr	p15, 0, ip, c12, c0, 0		@ write non-secure copy of VBAR
+
+	mov	r1, #1
+	str	r1, [r3, #GICC_CTLR]		@ enable non-secure CPU i/f
+	add	r2, r2, #GIC_DIST_OFFSET
+	str	r1, [r2, #GICD_CTLR]		@ allow private interrupts
+
+	mov	r0, r3				@ return GICC address
+
+	bx	lr
+ENDPROC(_nonsec_init)
+
+#ifdef CONFIG_SMP_PEN_ADDR
+/* void __weak smp_waitloop(unsigned previous_address); */
+ENTRY(smp_waitloop)
+	wfi
+	ldr	r1, =CONFIG_SMP_PEN_ADDR	@ load start address
+	ldr	r1, [r1]
+	cmp	r0, r1			@ make sure we dont execute this code
+	beq	smp_waitloop		@ again (due to a spurious wakeup)
+	mov	pc, r1
+ENDPROC(smp_waitloop)
+.weak smp_waitloop
+#endif
+
+ENTRY(_switch_to_hyp)
+	mov	r0, lr
+	mov	r1, sp				@ save SVC copy of LR and SP
+	isb
+	hvc #0			 @ for older asm: .byte 0x70, 0x00, 0x40, 0xe1
+	mov	sp, r1
+	mov	lr, r0				@ restore SVC copy of LR and SP
+
+	bx	lr
+ENDPROC(_switch_to_hyp)
diff --git a/arch/arm/cpu/armv7/virt-v7.c b/arch/arm/cpu/armv7/virt-v7.c
new file mode 100644
index 0000000000..6de7fe7813
--- /dev/null
+++ b/arch/arm/cpu/armv7/virt-v7.c
@@ -0,0 +1,173 @@
+/*
+ * (C) Copyright 2013
+ * Andre Przywara, Linaro
+ *
+ * Routines to transition ARMv7 processors from secure into non-secure state
+ * and from non-secure SVC into HYP mode
+ * needed to enable ARMv7 virtualization for current hypervisors
+ *
+ * See file CREDITS for list of people who contributed to this
+ * 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>
+
+unsigned long gic_dist_addr;
+
+static unsigned int read_cpsr(void)
+{
+	unsigned int reg;
+
+	asm volatile ("mrs %0, cpsr\n" : "=r" (reg));
+	return reg;
+}
+
+static unsigned int read_id_pfr1(void)
+{
+	unsigned int reg;
+
+	asm("mrc p15, 0, %0, c0, c1, 1\n" : "=r"(reg));
+	return reg;
+}
+
+static unsigned long get_gicd_base_address(void)
+{
+#ifdef CONFIG_ARM_GIC_BASE_ADDRESS
+	return CONFIG_ARM_GIC_BASE_ADDRESS + GIC_DIST_OFFSET;
+#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:
+		printf("nonsec: could not determine GIC address.\n");
+		return -1;
+	}
+
+	/* 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) {
+		printf("nonsec: PERIPHBASE is above 4 GB, no access.\n");
+		return -1;
+	}
+
+	return (periphbase & CBAR_MASK) + GIC_DIST_OFFSET;
+#endif
+}
+
+static void kick_secondary_cpus_gic(unsigned long gicdaddr)
+{
+	/* kick all CPUs (except this one) by writing to GICD_SGIR */
+	writel(1U << 24, gicdaddr + GICD_SGIR);
+}
+
+void __weak smp_kick_all_cpus(void)
+{
+	kick_secondary_cpus_gic(gic_dist_addr);
+}
+
+int armv7_switch_hyp(void)
+{
+	unsigned int reg;
+
+	/* check whether we are in HYP mode already */
+	if ((read_cpsr() & 0x1f) == 0x1a) {
+		debug("CPU already in HYP mode\n");
+		return 0;
+	}
+
+	/* check whether the CPU supports the virtualization extensions */
+	reg = read_id_pfr1();
+	if ((reg & CPUID_ARM_VIRT_MASK) != 1 << CPUID_ARM_VIRT_SHIFT) {
+		printf("HYP mode: Virtualization extensions not implemented.\n");
+		return -1;
+	}
+
+	/* call the HYP switching code on this CPU also */
+	_switch_to_hyp();
+
+	if ((read_cpsr() & 0x1F) != 0x1a) {
+		printf("HYP mode: switch not successful.\n");
+		return -1;
+	}
+
+	return 0;
+}
+
+int armv7_switch_nonsec(void)
+{
+	unsigned int reg;
+	unsigned itlinesnr, i;
+
+	/* check whether the CPU supports the security extensions */
+	reg = read_id_pfr1();
+	if ((reg & 0xF0) == 0) {
+		printf("nonsec: Security extensions not implemented.\n");
+		return -1;
+	}
+
+	/* 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.
+	 */
+
+	gic_dist_addr = get_gicd_base_address();
+	if (gic_dist_addr == -1)
+		return -1;
+
+	/* enable the GIC distributor */
+	writel(readl(gic_dist_addr + GICD_CTLR) | 0x03,
+	       gic_dist_addr + GICD_CTLR);
+
+	/* TYPER[4:0] contains an encoded number of available interrupts */
+	itlinesnr = readl(gic_dist_addr + GICD_TYPER) & 0x1f;
+
+	/* set all bits in the GIC group registers to one to allow access
+	 * from non-secure state. The first 32 interrupts are private per
+	 * CPU and will be set later when enabling the GIC for each core
+	 */
+	for (i = 1; i <= itlinesnr; i++)
+		writel((unsigned)-1, gic_dist_addr + GICD_IGROUPRn + 4 * i);
+
+	smp_set_core_boot_addr((unsigned long)_smp_pen, -1);
+	smp_kick_all_cpus();
+
+	/* call the non-sec switching code on this CPU also */
+	_nonsec_init();
+
+	return 0;
+}
diff --git a/arch/arm/include/asm/armv7.h b/arch/arm/include/asm/armv7.h
index 392d6a2db5..395444ee4f 100644
--- a/arch/arm/include/asm/armv7.h
+++ b/arch/arm/include/asm/armv7.h
@@ -7,7 +7,6 @@
  */
 #ifndef ARMV7_H
 #define ARMV7_H
-#include <linux/types.h>
 
 /* Cortex-A9 revisions */
 #define MIDR_CORTEX_A9_R0P1	0x410FC091
@@ -19,6 +18,22 @@
 #define MIDR_CORTEX_A15_R0P0	0x410FC0F0
 #define MIDR_CORTEX_A15_R2P2	0x412FC0F2
 
+/* Cortex-A7 revisions */
+#define MIDR_CORTEX_A7_R0P0	0x410FC070
+
+#define MIDR_PRIMARY_PART_MASK	0xFF0FFFF0
+
+/* ID_PFR1 feature fields */
+#define CPUID_ARM_SEC_SHIFT		4
+#define CPUID_ARM_SEC_MASK		(0xF << CPUID_ARM_SEC_SHIFT)
+#define CPUID_ARM_VIRT_SHIFT		12
+#define CPUID_ARM_VIRT_MASK		(0xF << CPUID_ARM_VIRT_SHIFT)
+#define CPUID_ARM_GENTIMER_SHIFT	16
+#define CPUID_ARM_GENTIMER_MASK		(0xF << CPUID_ARM_GENTIMER_SHIFT)
+
+/* valid bits in CBAR register / PERIPHBASE value */
+#define CBAR_MASK			0xFFFF8000
+
 /* CCSIDR */
 #define CCSIDR_LINE_SIZE_OFFSET		0
 #define CCSIDR_LINE_SIZE_MASK		0x7
@@ -41,6 +56,9 @@
 #define ARMV7_CLIDR_CTYPE_INSTRUCTION_DATA	3
 #define ARMV7_CLIDR_CTYPE_UNIFIED		4
 
+#ifndef __ASSEMBLY__
+#include <linux/types.h>
+
 /*
  * CP15 Barrier instructions
  * Please note that we have separate barrier instructions in ARMv7
@@ -58,4 +76,17 @@ 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);
 
+#if defined(CONFIG_ARMV7_NONSEC) || defined(CONFIG_ARMV7_VIRT)
+
+int armv7_switch_nonsec(void);
+int armv7_switch_hyp(void);
+
+/* defined in assembly file */
+unsigned int _nonsec_init(void);
+void _smp_pen(void);
+void _switch_to_hyp(void);
+#endif /* CONFIG_ARMV7_NONSEC || CONFIG_ARMV7_VIRT */
+
+#endif /* ! __ASSEMBLY__ */
+
 #endif
diff --git a/arch/arm/include/asm/gic.h b/arch/arm/include/asm/gic.h
new file mode 100644
index 0000000000..a0891cc09c
--- /dev/null
+++ b/arch/arm/include/asm/gic.h
@@ -0,0 +1,19 @@
+#ifndef __GIC_V2_H__
+#define __GIC_V2_H__
+
+/* register offsets for the ARM generic interrupt controller (GIC) */
+
+#define GIC_DIST_OFFSET		0x1000
+#define GICD_CTLR		0x0000
+#define GICD_TYPER		0x0004
+#define GICD_IGROUPRn		0x0080
+#define GICD_SGIR		0x0F00
+
+#define GIC_CPU_OFFSET_A9	0x0100
+#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/arch/arm/lib/bootm.c b/arch/arm/lib/bootm.c
index eefb456efb..f476a89702 100644
--- a/arch/arm/lib/bootm.c
+++ b/arch/arm/lib/bootm.c
@@ -22,6 +22,10 @@
 #include <asm/bootm.h>
 #include <linux/compiler.h>
 
+#if defined(CONFIG_ARMV7_NONSEC) || defined(CONFIG_ARMV7_VIRT)
+#include <asm/armv7.h>
+#endif
+
 DECLARE_GLOBAL_DATA_PTR;
 
 static struct tag *params;
@@ -181,6 +185,19 @@ static void setup_end_tag(bd_t *bd)
 
 __weak void setup_board_tags(struct tag **in_params) {}
 
+static void do_nonsec_virt_switch(void)
+{
+#if defined(CONFIG_ARMV7_NONSEC) || defined(CONFIG_ARMV7_VIRT)
+	if (armv7_switch_nonsec() == 0)
+#ifdef CONFIG_ARMV7_VIRT
+		if (armv7_switch_hyp() == 0)
+			debug("entered HYP mode\n");
+#else
+		debug("entered non-secure state\n");
+#endif
+#endif
+}
+
 /* Subcommand: PREP */
 static void boot_prep_linux(bootm_headers_t *images)
 {
@@ -217,6 +234,7 @@ static void boot_prep_linux(bootm_headers_t *images)
 		printf("FDT and ATAGS support not compiled in - hanging\n");
 		hang();
 	}
+	do_nonsec_virt_switch();
 }
 
 /* Subcommand: GO */