1 files changed, 29 insertions, 25 deletions

diff --git a/arch/arm/cpu/armv7/tegra2/ap20.c b/arch/arm/cpu/armv7/tegra2/ap20.c
index 5cb4b1b6a0..4c44bb3637 100644
--- a/arch/arm/cpu/armv7/tegra2/ap20.c
+++ b/arch/arm/cpu/armv7/tegra2/ap20.c
@@ -31,7 +31,12 @@
 #include <asm/arch/scu.h>
 #include <common.h>
 
-u32 s_first_boot = 1;
+/* Returns 1 if the current CPU executing is a Cortex-A9, else 0 */
+static int ap20_cpu_is_cortexa9(void)
+{
+	u32 id = readb(NV_PA_PG_UP_BASE + PG_UP_TAG_0);
+	return id == (PG_UP_TAG_0_PID_CPU & 0xff);
+}
 
 void init_pllx(void)
 {
@@ -283,38 +288,37 @@ void init_pmc_scratch(void)
 	writel(CONFIG_SYS_BOARD_ODMDATA, &pmc->pmc_scratch20);
 }
 
-void cpu_start(void)
+void tegra2_start(void)
 {
 	struct pmux_tri_ctlr *pmt = (struct pmux_tri_ctlr *)NV_PA_APB_MISC_BASE;
 
-	/* enable JTAG */
-	writel(0xC0, &pmt->pmt_cfg_ctl);
+	/* If we are the AVP, start up the first Cortex-A9 */
+	if (!ap20_cpu_is_cortexa9()) {
+		/* enable JTAG */
+		writel(0xC0, &pmt->pmt_cfg_ctl);
 
-	if (s_first_boot) {
 		/*
-		 * Need to set this before cold-booting,
-		 *  otherwise we'll end up in an infinite loop.
-		 */
-		s_first_boot = 0;
-		cold_boot();
+		* If we are ARM7 - give it a different stack. We are about to
+		* start up the A9 which will want to use this one.
+		*/
+		asm volatile("ldr	sp, =%c0\n"
+			: : "i"(AVP_EARLY_BOOT_STACK_LIMIT));
+
+		start_cpu((u32)_start);
+		halt_avp();
+		/* not reached */
 	}
-}
 
-void tegra2_start()
-{
-	if (s_first_boot) {
-		/* Init Debug UART Port (115200 8n1) */
-		uart_init();
+	/* Init PMC scratch memory */
+	init_pmc_scratch();
 
-		/* Init PMC scratch memory */
-		init_pmc_scratch();
-	}
+	enable_scu();
 
-#ifdef CONFIG_ENABLE_CORTEXA9
-	/* take the mpcore out of reset */
-	cpu_start();
+	/* enable SMP mode and FW for CPU0, by writing to Auxiliary Ctl reg */
+	asm volatile(
+		"mrc	p15, 0, r0, c1, c0, 1\n"
+		"orr	r0, r0, #0x41\n"
+		"mcr	p15, 0, r0, c1, c0, 1\n");
 
-	/* configure cache */
-	cache_configure();
-#endif
+	/* FIXME: should have ap20's L2 disabled too? */
 }