arndale5250: Boot in Hyp mode and enable architected timers

First, to boot in Hyp mode we need to change to non-secure mode, which
involves configuring the frequency of the arch. timers and setting all
interrupts on the gic to group 1 (this code was inspired by the boot
wrapper code).

Second, signal the secondary CPU while still in secure mode and have the
secondary CPU run the SPL.  The SPL checks the hardware cpu id, and if
it's a secondary CPU, it will initialize non-secure mode inside the SPL,
enter Hyp mode, and finally enter a new SMP pen with the same poking stick
interface as the regular kernel uses.

Third, on CPU0 we wait until u-boot is fully up to actually enter the
non-secure mode on CPU0, and stay in non-secure svc mode right up until
we actually load the kernel, where the last thing we do is enter Hyp
mode.

Let it roll...

Signed-off-by: Jeremy C. Andrus <jeremya@cs.columbia.edu>
Signed-off-by: Christoffer Dall <cdall@cs.columbia.edu>

8 files changed, 369 insertions, 0 deletions

diff --git a/board/samsung/smdk5250/Makefile b/board/samsung/smdk5250/Makefile
index 1474fa8a1..64f8c4236 100644
--- a/board/samsung/smdk5250/Makefile
+++ b/board/samsung/smdk5250/Makefile
@@ -25,8 +25,10 @@ include $(TOPDIR)/config.mk
 LIB	= $(obj)lib$(BOARD).o
 
 SOBJS	:= lowlevel_init.o
+SOBJS	+= monitor.o
 
 COBJS	:= clock_init.o
+COBJS	+= timer_init.o non_secure.o
 COBJS	+= dmc_common.o dmc_init_ddr3.o
 COBJS	+= tzpc_init.o
 COBJS	+= smdk5250_spl.o
@@ -36,6 +38,7 @@ COBJS	+= smdk5250.o
 endif
 
 ifdef CONFIG_SPL_BUILD
+SOBJS	+= smp.o
 COBJS	+= mmc_boot.o
 endif
 
diff --git a/board/samsung/smdk5250/lowlevel_init.S b/board/samsung/smdk5250/lowlevel_init.S
index 07925c3bf..2ef520f67 100644
--- a/board/samsung/smdk5250/lowlevel_init.S
+++ b/board/samsung/smdk5250/lowlevel_init.S
@@ -32,6 +32,29 @@ _TEXT_BASE:
 	.globl lowlevel_init
 lowlevel_init:
 
+#ifdef CONFIG_SPL_BUILD
+	/* check if we're the first cpu or not */
+	mrc	p15, 0, r0, c0, c0, 5	/* MPIDR */
+	and	r0, r0, #15
+	cmp	r0, #0
+	beq	first_cpu
+
+	/* Secondary CPU */
+	bl	arch_timer_init
+	bl	non_secure_init
+	bl	monitor_init
+	bl	enter_ns
+	bl	enter_hyp
+	b	enter_smp_pen
+
+	/*
+	 * We entered the SMP pen above, and we expect kernels to write an
+	 * address into the ALIVE SFR SYSFLAGS register thingy at 0x02020000
+	 * which should not be here, but some kernel secondary entry point.
+	 */
+#endif
+
+first_cpu:
 	/* use iRAM stack in bl2 */
 	ldr	sp, =CONFIG_IRAM_STACK
 	stmdb	r13!, {ip,lr}
@@ -55,6 +78,12 @@ lowlevel_init:
 	cmp	r1, r2
 	beq	wakeup_reset
 
+	/* Init architected timers */
+	bl	arch_timer_init
+
+	/* Non-secure-init */
+	bl	non_secure_init
+
 	/*PS-Hold High*/
 	ldr r0, =0x1004330c
 	ldr r1, [r0]
@@ -81,6 +110,11 @@ lowlevel_init:
 
 1:
 	bl	tzpc_init
+#ifdef CONFIG_SPL_BUILD
+	bl	smp_kick_secondary	/* Bring other CPU1 into smp pen */
+#else
+	bl	monitor_init		/* Setup monitor mode */
+#endif
 	ldmia	r13!, {ip,pc}
 
 wakeup_reset:
diff --git a/board/samsung/smdk5250/monitor.S b/board/samsung/smdk5250/monitor.S
new file mode 100644
index 000000000..8bc21d8f5
--- /dev/null
+++ b/board/samsung/smdk5250/monitor.S
@@ -0,0 +1,97 @@
+/*
+ *
+ * Copyright (c) 2012	Christoffer Dall <c.dall@virtualopensystems.com>
+ * 					 <cdall@cs.columbia.edu>
+ *
+ * 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>
+
+.syntax	unified
+.arch_extension sec
+.arch_extension virt
+.text
+
+.align 5
+
+/* We use the same vector table for Hyp and Monitor mode, since
+ * we will only use each once and they don't overlap.
+ */
+mon_vectors:
+	.word 0	/* reset */
+	.word 0	/* undef */
+	b	2f /* smc */
+	.word 0 /* pabt */
+	.word 0 /* dabt */
+	b	1f
+	.word 0 /* irq */
+	.word 0 /* fiq */
+
+/* Return directly back to the caller without leaving Hyp mode: */
+1:	mrs	lr, elr_hyp
+	mov	pc, lr
+
+/* In monitor mode, set up HVBAR and SCR then return to caller in NS-SVC. */
+2:
+	mrc	p15, 0, r1, c1, c1, 0		@ SCR
+	/*
+	 * Set SCR.NS=1 (needed for setting HVBAR and also returning to NS state)
+	 *        .IRQ,FIQ,EA=0 (don't take aborts/exceptions to Monitor mode)
+	 *        .FW,AW=1 (CPSR.A,F modifiable in NS state)
+	 *        .nET=0 (early termination OK)
+	 *        .SCD=0 (SMC in NS mode OK, so we can call secure firmware)
+	 *        .HCE=1 (HVC does Hyp call)
+	 */
+	bic	r1, r1, #0x07f
+	ldr	r2, =0x131
+	orr	r1, r1, r2
+	mcr	p15, 0, r2, c1, c1, 0		@ SCR
+	isb
+	ldr	r2, =mon_vectors
+	mcr	p15, 4, r2, c12, c0, 0		@ set HVBAR
+	/* ...and return to calling code in NS state */
+	movs	pc, lr
+
+/******************************************************************************
+ * This code is called from u-boot into the above handler
+ */
+
+	.globl monitor_init
+monitor_init:
+	ldr	ip, =mon_vectors
+	mcr	p15, 0, ip, c12, c0, 1		@ Monitor vector base address
+	mov	pc, lr
+
+	/* Try to go into NS-SVC: void enter_ns(void); */
+	.globl enter_ns
+enter_ns:
+	smc	#0
+	mov	pc, lr
+
+	/* void enter_hyp(void); */
+	.globl enter_hyp
+enter_hyp:
+	/* Now we're in NS-SVC, make a Hyp call to get into Hyp mode */
+	mov	r0, lr
+	mov	r1, sp
+	hvc	#0
+	/* We will end up here in NS-Hyp. */
+	mov	sp, r1
+	mov	pc, r0
diff --git a/board/samsung/smdk5250/non_secure.c b/board/samsung/smdk5250/non_secure.c
new file mode 100644
index 000000000..e46b6f81a
--- /dev/null
+++ b/board/samsung/smdk5250/non_secure.c
@@ -0,0 +1,96 @@
+
+/*
+ * Architected Timer setup for SMDK5250 board based on EXYNOS5
+ *
+ * Copyright (C) 2012 Christoffer Dall <cdall@cs.columbia.edu>
+ *
+ * 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 <config.h>
+#include <asm/io.h>
+#include <asm/arch/spl.h>
+
+#include "setup.h"
+
+#define ARM_GICV2_DIST_CTRL		0x00
+#define ARM_GICV2_DIST_TYPE		0x04
+#define ARM_GICV2_DIST_SEC_REG		0x80
+
+#define ARM_GICV2_CPU_CTRL		0x00
+
+#define ARM_GICV2_ICCPMR		0x04
+
+static unsigned long read_mpidr(void)
+{
+	unsigned long val;
+	asm volatile("mrc p15, 0, %0, c0, c0, 5": "=r" (val));
+	return val;
+}
+
+static unsigned long read_nsacr(void)
+{
+	unsigned long val;
+	asm volatile("mrc p15, 0, %0, c1, c1, 2": "=r" (val));
+	return val;
+}
+
+static void write_nsacr(unsigned long val)
+{
+	asm volatile("mcr p15, 0, %0, c1, c1, 2": : "r" (val));
+}
+
+void non_secure_init(void)
+{
+	unsigned long addr, type, num_regs;
+	unsigned long nsacr, ctrl;
+	int i;
+
+	addr = EXYNOS5_GIC_DIST_BASE;
+	type = readl(addr + ARM_GICV2_DIST_TYPE);
+	num_regs = (type & 0x1f) + 1;
+
+	/* Set all interrupts to be non-secure */
+	addr = EXYNOS5_GIC_DIST_BASE + ARM_GICV2_DIST_SEC_REG;
+	for (i = 0; i < num_regs; i++) {
+		writel(0xffffffff, addr);
+		addr += 4;
+	}
+
+	/* Set GIC priority mask bit [7] = 1 */
+	addr = EXYNOS5_GIC_CPU_BASE;
+	writel(0x80, addr + ARM_GICV2_ICCPMR);
+
+	/* Set NSACR to allow coprocessor access from non-secure */
+	nsacr = read_nsacr();
+	nsacr |= 0x43fff;
+	write_nsacr(nsacr);
+
+	/* Enable group 1 interrupts on CPU interface */
+	addr = EXYNOS5_GIC_CPU_BASE + ARM_GICV2_CPU_CTRL;
+	ctrl = readl(addr);
+	writel(ctrl | 0x1, addr);
+
+	/* Disable group 0 interrupts and enable group 1 interrupts on Dist */
+	addr = EXYNOS5_GIC_DIST_BASE + ARM_GICV2_DIST_CTRL;
+	ctrl = readl(addr);
+	ctrl = (ctrl & ~0x1) | 0x2;
+	writel(ctrl | 0x1, addr);
+}
diff --git a/board/samsung/smdk5250/setup.h b/board/samsung/smdk5250/setup.h
index a15960121..bb8e9715b 100644
--- a/board/samsung/smdk5250/setup.h
+++ b/board/samsung/smdk5250/setup.h
@@ -591,4 +591,6 @@ void sdelay(unsigned long);
 void mem_ctrl_init(void);
 void system_clock_init(void);
 void tzpc_init(void);
+void enter_ns(void);
+void enter_hyp(void);
 #endif
diff --git a/board/samsung/smdk5250/smdk5250.c b/board/samsung/smdk5250/smdk5250.c
index b9372d1a5..76687e505 100644
--- a/board/samsung/smdk5250/smdk5250.c
+++ b/board/samsung/smdk5250/smdk5250.c
@@ -30,6 +30,8 @@
 #include <asm/arch/pinmux.h>
 #include <asm/arch/sromc.h>
 
+#include "setup.h"
+
 DECLARE_GLOBAL_DATA_PTR;
 
 #ifdef CONFIG_SMC911X
@@ -215,3 +217,24 @@ int board_early_init_f(void)
 	return err;
 }
 #endif
+
+void arch_preboot_os(void)
+{
+	/*
+	 * We should now long be done with accessing any peripherals or
+	 * setting up any other hardware state that needs to be set up from
+	 * the secure mode (TrustZone), so we can switch to non-secure mode
+	 * and we rely on board-specific logic to put a board-specific monitor
+	 * in place for stuff like L2 cache maintenance and power management.
+	 */
+	enter_ns();
+
+	/*
+	 * Enter Hyp mode immediately before booting the kernel to allow
+	 * the first Linux kernel access to and control of Hyp mode so that
+	 * modules like KVM can run VMs.
+	 *
+	 * Without further ado...
+	 */
+	enter_hyp();
+}
diff --git a/board/samsung/smdk5250/smp.S b/board/samsung/smdk5250/smp.S
new file mode 100644
index 000000000..998d2ab3b
--- /dev/null
+++ b/board/samsung/smdk5250/smp.S
@@ -0,0 +1,69 @@
+/*
+ *
+ * Copyright (c) 2012	Christoffer Dall <c.dall@virtualopensystems.com>
+ * 					 <cdall@cs.columbia.edu>
+ *
+ * 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>
+
+__smp_pen:
+	wfe
+	ldr	r1, [r0]
+	mov	pc, r1
+
+	.globl enter_smp_pen
+enter_smp_pen:
+	adr	r1, __smp_pen
+	ldmia	r1, {r4, r5, r6}
+	ldr	r2, =CONFIG_SPL_SMP_PEN
+	stmia	r2, {r4, r5, r6}
+
+	/*
+	 * Store SMP pen into SYSFLAGS so the processor stays in the loop if
+	 * it gets a spurious interrupt
+	 */
+	ldr	r0, =CONFIG_SYSFLAGS_ADDR
+	str	r2, [r0]
+
+	/*
+	 * Make instruction copy coherent
+	 */
+	mcr	p15, 0, r2, c7, c11, 1 /* Clean the data cache by MVA*/
+	mov	r10, #0
+	mcr	p15, 0, r10, c7, c5, 0 /* Invalidate the I-cache */
+	isb	/* Make sure the invalidate ops are complete */
+	dsb
+
+	mov	pc, r2
+
+	.globl smp_kick_secondary
+smp_kick_secondary:
+	/* Bring up the secondary CPU */
+	ldr	r0, =CONFIG_SYSFLAGS_ADDR
+	ldr	r1, =CONFIG_SPL_TEXT_BASE
+	str	r1, [r0]
+	dsb
+	ldr	r0, =(EXYNOS5_GIC_DIST_BASE + 0xf00) /* GICD_SGIR */
+	ldr	r1, =(2 << 16)		/* Bring up CPU 1*/
+	str	r1, [r0]
+	dsb
+
+	mov	pc, lr
diff --git a/board/samsung/smdk5250/timer_init.c b/board/samsung/smdk5250/timer_init.c
new file mode 100644
index 000000000..cf9017720
--- /dev/null
+++ b/board/samsung/smdk5250/timer_init.c
@@ -0,0 +1,45 @@
+/*
+ * Architected Timer setup for SMDK5250 board based on EXYNOS5
+ *
+ * Copyright (C) 2012 Christoffer Dall <cdall@cs.columbia.edu>
+ *
+ * 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 <config.h>
+#include <asm/io.h>
+#include <asm/arch/clk.h>
+#include <asm/arch/clock.h>
+#include <asm/arch/spl.h>
+
+#include "setup.h"
+
+void arch_timer_init(void)
+{
+	unsigned long cpuid, freq;
+	asm volatile("mrc	p15, 0, %[cpuid], c0, c1, 1":
+		     [cpuid] "=r" (cpuid));
+
+	if ((cpuid >> 16) & 1) {
+		freq = 24000000;
+		asm volatile("mcr p15, 0, %[freq], c14, c0, 0" : :
+			     [freq] "r" (freq));
+	}
+}