ARM: tegra: Redesign Tegra CPU reset handling

- Add a single unified handler for all CPU resets that is copied to
  IRAM.
- Add state information to direct the flow of execution through the
  reset handler based on the reason a CPU was reset.
- Write the EVP CPU reset vector only once per cold/warm boot session.
- Prevent modification of the EVP CPU reset vector in Tegra3.

Bug 786290
Bug 790458

Change-Id: Ica6707f3514986ee914e73a2d9766a4e06ce2d29
Signed-off-by: Scott Williams <scwilliams@nvidia.com>
DW: Split into logical changes
Signed-off-by: Dan Willemsen <dwillemsen@nvidia.com>

Rebase-Id: R7b9859a83717e76c3c083bdde724bd5fef9ce089

12 files changed, 518 insertions, 188 deletions

diff --git a/arch/arm/mach-tegra/Makefile b/arch/arm/mach-tegra/Makefile
index 4cb2984e5753..9e909b6dbd8e 100644
--- a/arch/arm/mach-tegra/Makefile
+++ b/arch/arm/mach-tegra/Makefile
@@ -53,6 +53,7 @@ obj-$(CONFIG_LOCAL_TIMERS)              += localtimer.o
 obj-$(CONFIG_SMP)                       += platsmp.o
 obj-$(CONFIG_HOTPLUG_CPU)               += hotplug.o
 obj-y                                   += headsmp.o
+obj-y                                   += reset.o
 obj-$(CONFIG_TEGRA_SYSTEM_DMA)          += dma.o
 obj-$(CONFIG_CPU_FREQ)                  += cpu-tegra.o
 ifeq ($(CONFIG_TEGRA_AUTO_HOTPLUG),y)
diff --git a/arch/arm/mach-tegra/cpuidle-t2.c b/arch/arm/mach-tegra/cpuidle-t2.c
index d2baba095ca2..326d4e8ec215 100644
--- a/arch/arm/mach-tegra/cpuidle-t2.c
+++ b/arch/arm/mach-tegra/cpuidle-t2.c
@@ -70,7 +70,6 @@ static inline unsigned int time_to_bin(unsigned int time)
 #ifdef CONFIG_SMP
 
 static void __iomem *clk_rst = IO_ADDRESS(TEGRA_CLK_RESET_BASE);
-static void __iomem *evp_reset = IO_ADDRESS(TEGRA_EXCEPTION_VECTORS_BASE) + 0x100;
 static void __iomem *pmc = IO_ADDRESS(TEGRA_PMC_BASE);
 static s64 tegra_cpu1_idle_time = LLONG_MAX;
 
@@ -95,8 +94,6 @@ static void tegra2_wake_reset_cpu(int cpu)
 {
 	u32 reg;
 
-	writel(virt_to_phys(tegra_secondary_resume), evp_reset);
-
 	/* enable cpu clock on cpu */
 	reg = readl(clk_rst + 0x4c);
 	writel(reg & ~(1 << (8 + cpu)), clk_rst + 0x4c);
diff --git a/arch/arm/mach-tegra/headsmp.S b/arch/arm/mach-tegra/headsmp.S
index c30e57cbcdef..5931b7fdc3a8 100644
--- a/arch/arm/mach-tegra/headsmp.S
+++ b/arch/arm/mach-tegra/headsmp.S
@@ -1,7 +1,7 @@
 /*
  * arch/arm/mach-tegra/headsmp.S
  *
- * SMP initialization routines for Tegra SoCs
+ * CPU initialization routines for Tegra SoCs
  *
  * Copyright (c) 2009-2011, NVIDIA Corporation.
  * Copyright (c) 2011 Google, Inc.
@@ -10,8 +10,7 @@
  *
  * 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.
+ * the Free Software Foundation; either version 2 of the License.
  *
  * This program is distributed in the hope that it will be useful, but WITHOUT
  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
@@ -24,17 +23,29 @@
 
 #include <asm/assembler.h>
 #include <asm/cache.h>
+#include <asm/page.h>
 
 #include <mach/iomap.h>
+#include <mach/io.h>
 
 #include "asm_macros.h"
+#include "reset.h"
+#include "sleep.h"
+
+#define DEBUG_CPU_RESET_HANDLER	0	/* Non-zero enables debug code */
+
+#define PMC_SCRATCH41		0x140
+
+#define RESET_DATA(x)		((TEGRA_RESET_##x)*4)
+
 
 #ifdef CONFIG_SMP
 /*
  *	tegra_secondary_startup
  *
  *	 Initial secondary processor boot vector; jumps to kernel's
- *	 secondary_startup routine
+ *	 secondary_startup routine. Used for initial boot and hotplug
+ *	 of secondary CPUs.
  */
 ENTRY(tegra_secondary_startup)
 	bl	tegra_invalidate_l1
@@ -44,29 +55,20 @@ ENDPROC(tegra_secondary_startup)
 #endif
 
 #ifdef CONFIG_PM_SLEEP
-#ifdef CONFIG_SMP
-/*
- *	tegra_secondary_resume
- *
- *	  Secondary CPU boot vector when restarting a CPU following lp2 idle.
- */
-ENTRY(tegra_secondary_resume)
-	bl	tegra_invalidate_l1
-	bl	tegra_enable_coresite
-	b	cpu_resume
-ENDPROC(tegra_secondary_resume)
-#endif
-
 /*
  *	tegra_resume
  *
- *	  CPU boot vector when restarting the master CPU following
+ *	  CPU boot vector when restarting the a CPU following
  *	  an LP2 transition. Also branched to by LP0 and LP1 resume after
  *	  re-enabling sdram.
  */
 ENTRY(tegra_resume)
-	bl	tegra_invalidate_l1
 	bl	tegra_enable_coresite
+	bl	tegra_invalidate_l1
+
+	cpu_id	r0
+	cmp	r0, #0				@ CPU0?
+	bne	cpu_resume      		@ no
 
 #ifndef CONFIG_ARCH_TEGRA_2x_SOC
 	@ Clear the flow controller flags for this CPU.
@@ -132,3 +134,155 @@ tegra_enable_coresite:
 	mov32	r0, 0xC5ACCE55
 	mcr	p14, 0, r0, c7, c12, 6
 	mov	pc, lr
+
+/*
+ * __tegra_cpu_reset_handler_halt_failed:
+ *
+ * Alternate entry point for reset handler for cases where the
+ * WFI halt failed to take effect.
+ *
+ */
+	.align L1_CACHE_SHIFT
+ENTRY(__tegra_cpu_reset_handler_start)
+
+/*
+ * __tegra_cpu_reset_handler:
+ *
+ * Common handler for all CPU reset events.
+ *
+ * Register usage within the reset handler:
+ *
+ *	R7  = CPU present (to the OS) mask
+ *	R8  = CPU in LP1 state mask
+ *	R9  = CPU in LP2 state mask
+ *	R10 = CPU number
+ *	R11 = CPU mask
+ *	R12 = pointer to reset handler data
+ *
+ * NOTE: This code is copied to IRAM. All code and data accesses
+ *	 must be position-independent.
+ */
+
+	.align L1_CACHE_SHIFT
+ENTRY(__tegra_cpu_reset_handler)
+
+#if DEBUG_CPU_RESET_HANDLER
+	mov32	r0, 0xC5ACCE55
+	mcr	p14, 0, r0, c7, c12, 6		@ Enable CoreSight access
+	b	.
+#endif
+	mrc	p15, 0, r10, c0, c0, 5		@ MPIDR
+	and	r10, r10, #0x3			@ R10 = CPU number
+	mov	r11, #1
+	mov	r11, r11, lsl r10  		@ R11 = CPU mask
+	adr	r12, __tegra_cpu_reset_handler_data
+
+#ifdef CONFIG_SMP
+	/* Does the OS know about this CPU? */
+	ldr	r7, [r12, #RESET_DATA(MASK_PRESENT)]
+	tst	r7, r11 			@ if !present
+	bleq	__die				@ CPU not present (to OS)
+#endif
+
+#ifdef CONFIG_PM_SLEEP
+	/* Waking up from LP1? */
+	ldr	r8, [r12, #RESET_DATA(MASK_LP1)]
+	tst	r8, r11				@ if in_lp1
+	beq	__is_not_lp1
+	cmp	r10, #0
+	bne	__die				@ only CPU0 can be here
+	ldr	lr, [r12, #RESET_DATA(STARTUP_LP1)]
+	cmp	lr, #0
+	bleq	__die				@ no LP1 startup handler
+	bx	lr
+__is_not_lp1:
+#endif
+
+	/* Waking up from LP2? */
+	ldr	r9, [r12, #RESET_DATA(MASK_LP2)]
+	tst	r9, r11				@ if in_lp2
+	beq	__is_not_lp2
+#if defined(CONFIG_SMP) && defined(CONFIG_ARCH_TEGRA_2x_SOC)
+	/* Tegra2 CPU1 LP2 wakeup uses the secondary startup handler */
+	cmp	r10, #1
+	bne	__is_not_lp2
+#endif
+	ldr	lr, [r12, #RESET_DATA(STARTUP_LP2)]
+	cmp	lr, #0
+	bleq	__die				@ no LP2 startup handler
+	bx	lr
+
+__is_not_lp2:
+
+#ifdef CONFIG_SMP
+	/* Can only be secondary boot (initial or hotplug) but CPU 0
+	   cannot be here. */
+	cmp	r10, #0
+	bleq	__die				@ CPU0 cannot be here
+	ldr	lr, [r12, #RESET_DATA(STARTUP_SECONDARY)]
+	cmp	lr, #0
+	bleq	__die				@ no secondary startup handler
+	bx	lr
+#endif
+
+/*
+ * We don't know why the CPU reset. Just kill it.
+ * The LR register will contain the address we died at + 4.
+ */
+
+__die:
+	sub	lr, lr, #4
+	mov32	r7, TEGRA_PMC_BASE
+	str	lr, [r7, #PMC_SCRATCH41]
+
+	mov32	r7, TEGRA_CLK_RESET_BASE
+
+#ifdef CONFIG_ARCH_TEGRA_2x_SOC
+	mov32	r0, 0x1111
+	mov	r1, r0, lsl r10
+	str	r1, [r7, #0x340]		@ CLK_RST_CPU_CMPLX_SET
+#else
+	mov32	r6, TEGRA_FLOW_CTRL_BASE
+
+	cmp	r10, #0
+	moveq	r1, #FLOW_CTRL_HALT_CPU0_EVENTS
+	moveq	r2, #FLOW_CTRL_CPU0_CSR
+	movne	r1, r10, lsl #3
+	addne	r2, r1, #(FLOW_CTRL_CPU1_CSR-8)
+	addne	r1, r1, #(FLOW_CTRL_HALT_CPU1_EVENTS-8)
+
+	/* Clear CPU "event" and "interrupt" flags and power gate
+	   it when halting but not before it is in the "WFI" state. */
+	ldr	r0, [r6, +r2]
+	orr	r0, r0, #FLOW_CTRL_CSR_INTR_FLAG | FLOW_CTRL_CSR_EVENT_FLAG
+	orr	r0, r0, #FLOW_CTRL_CSR_ENABLE
+	str	r0, [r6, +r2]
+
+	/* Unconditionally halt this CPU */
+	mov	r0, #FLOW_CTRL_WAITEVENT
+	str	r0, [r6, +r1]
+	ldr	r0, [r6, +r1]			@ memory barrier
+
+	dsb
+	isb
+	wfi					@ CPU should be power gated here
+
+	/* If the CPU didn't power gate above just kill it's clock. */
+
+	mov	r0, r11, lsl #8
+	str	r0, [r7, #348]			@ CLK_CPU_CMPLX_SET
+#endif
+	/* If the CPU still isn't dead, just spin here. */
+	b	.
+
+ENDPROC(__tegra_cpu_reset_handler)
+	.align L1_CACHE_SHIFT
+	.type	__tegra_cpu_reset_handler_data, %object
+	.globl	__tegra_cpu_reset_handler_data
+__tegra_cpu_reset_handler_data:
+	.rept	TEGRA_RESET_DATA_SIZE
+	.long	0
+	.endr
+	.size	__tegra_cpu_reset_handler_data, .-tegra_cpu_reset_handler_data
+	.align L1_CACHE_SHIFT
+ENTRY(__tegra_cpu_reset_handler_end)
diff --git a/arch/arm/mach-tegra/include/mach/iomap.h b/arch/arm/mach-tegra/include/mach/iomap.h
index 5b479f2e77a6..5fbe2a9b73ef 100644
--- a/arch/arm/mach-tegra/include/mach/iomap.h
+++ b/arch/arm/mach-tegra/include/mach/iomap.h
@@ -198,6 +198,9 @@
 #define TEGRA_AHB_GIZMO_BASE		0x6000C004
 #define TEGRA_AHB_GIZMO_SIZE		0x10C
 
+#define TEGRA_SB_BASE			0x6000C200
+#define TEGRA_SB_SIZE			256
+
 #define TEGRA_STATMON_BASE		0x6000C400
 #define TEGRA_STATMON_SIZE		SZ_1K
 
diff --git a/arch/arm/mach-tegra/platsmp.c b/arch/arm/mach-tegra/platsmp.c
index 97bfb4db28a5..a7b81919ba90 100644
--- a/arch/arm/mach-tegra/platsmp.c
+++ b/arch/arm/mach-tegra/platsmp.c
@@ -20,6 +20,7 @@
 #include <linux/smp.h>
 #include <linux/delay.h>
 #include <linux/clk.h>
+#include <linux/cpumask.h>
 
 #include <asm/hardware/gic.h>
 #include <asm/smp_scu.h>
@@ -29,10 +30,15 @@
 
 #include "pm.h"
 #include "clock.h"
+#include "reset.h"
 #include "sleep.h"
 
-#define EVP_CPU_RESET_VECTOR \
-	(IO_ADDRESS(TEGRA_EXCEPTION_VECTORS_BASE) + 0x100)
+bool tegra_all_cpus_booted;
+
+static DECLARE_BITMAP(tegra_cpu_init_bits, CONFIG_NR_CPUS) __read_mostly;
+const struct cpumask *const tegra_cpu_init_mask = to_cpumask(tegra_cpu_init_bits);
+#define tegra_cpu_init_map	(*(cpumask_t *)tegra_cpu_init_mask)
+
 #define CLK_RST_CONTROLLER_CLK_CPU_CMPLX \
 	(IO_ADDRESS(TEGRA_CLK_RESET_BASE) + 0x4c)
 #define CLK_RST_CONTROLLER_RST_CPU_CMPLX_SET \
@@ -43,38 +49,144 @@
 #define CPU_CLOCK(cpu)	(0x1<<(8+cpu))
 #define CPU_RESET(cpu)	(0x1111ul<<(cpu))
 
-static unsigned int available_cpus(void);
-#if defined(CONFIG_ARCH_TEGRA_2x_SOC)
-static inline int is_g_cluster_available(unsigned int cpu)
-{ return -EPERM; }
-static inline bool is_cpu_powered(unsigned int cpu)
-{ return true; }
-static inline int power_up_cpu(unsigned int cpu)
-{ return 0; }
-
-/* For Tegra2 use the software-written value of the reset regsiter for status.*/
+#ifdef CONFIG_ARCH_TEGRA_2x_SOC
+/* For Tegra2 use the software-written value of the reset register for status.*/
 #define CLK_RST_CONTROLLER_CPU_CMPLX_STATUS CLK_RST_CONTROLLER_RST_CPU_CMPLX_SET
-
 #else
-static int is_g_cluster_available(unsigned int cpu);
-static bool is_cpu_powered(unsigned int cpu);
-static int power_up_cpu(unsigned int cpu);
-
+#define CLK_RST_CONTROLLER_CLK_CPU_CMPLX_CLR \
+	(IO_ADDRESS(TEGRA_CLK_RESET_BASE) + 0x34c)
 #define CAR_BOND_OUT_V \
 	(IO_ADDRESS(TEGRA_CLK_RESET_BASE) + 0x390)
 #define CAR_BOND_OUT_V_CPU_G	(1<<0)
 #define CLK_RST_CONTROLLER_CPU_CMPLX_STATUS \
 	(IO_ADDRESS(TEGRA_CLK_RESET_BASE) + 0x470)
+#endif
 
+static void __iomem *scu_base = IO_ADDRESS(TEGRA_ARM_PERIF_BASE);
+
+static unsigned int available_cpus(void)
+{
+	static unsigned int ncores;
+
+	if (ncores == 0) {
+		ncores = scu_get_core_count(scu_base);
+#ifndef CONFIG_ARCH_TEGRA_2x_SOC
+		if (ncores > 1) {
+			u32 fuse_sku = readl(FUSE_SKU_DIRECT_CONFIG);
+			ncores -= FUSE_SKU_NUM_DISABLED_CPUS(fuse_sku);
+			BUG_ON((int)ncores <= 0);
+		}
 #endif
+	}
+	return ncores;
+}
+
+static int is_g_cluster_available(unsigned int cpu)
+{
+#ifdef CONFIG_ARCH_TEGRA_2x_SOC
+	return -EPERM;
+#else
+	u32 fuse_sku = readl(FUSE_SKU_DIRECT_CONFIG);
+	u32 bond_out = readl(CAR_BOND_OUT_V);
 
-extern void tegra_secondary_startup(void);
+	/* Does the G CPU complex exist at all? */
+	if ((fuse_sku & FUSE_SKU_DISABLE_ALL_CPUS) ||
+	    (bond_out & CAR_BOND_OUT_V_CPU_G))
+		return -EPERM;
 
-static void __iomem *scu_base = IO_ADDRESS(TEGRA_ARM_PERIF_BASE);
+	if (cpu >= available_cpus())
+		return -EPERM;
+
+	/* FIXME: The G CPU can be unavailable for a number of reasons
+	 *	  (e.g., low battery, over temperature, etc.). Add checks for
+	 *	  these conditions. */
+	return 0;
+#endif
+}
+
+#ifndef CONFIG_ARCH_TEGRA_2x_SOC
+static bool is_cpu_powered(unsigned int cpu)
+{
+	if (is_lp_cluster())
+		return true;
+	else
+		return tegra_powergate_is_powered(TEGRA_CPU_POWERGATE_ID(cpu));
+}
+#endif
+
+static int power_up_cpu(unsigned int cpu)
+{
+	u32 reg;
+	int ret = 0;
+#ifndef CONFIG_ARCH_TEGRA_2x_SOC
+	unsigned long timeout;
+
+	BUG_ON(cpu == smp_processor_id());
+	BUG_ON(is_lp_cluster());
+
+	/* If this cpu has booted this function is entered after
+	 * CPU has been already un-gated by flow controller. Wait
+	 * for confirmation that cpu is powered and remove clamps.
+	 * On first boot entry do not wait - go to direct ungate.
+	 */
+	if (cpu_isset(cpu, tegra_cpu_init_map)) {
+		timeout = jiffies + HZ;
+		do {
+			if (is_cpu_powered(cpu))
+				goto remove_clamps;
+			udelay(10);
+		} while (time_before(jiffies, timeout));
+	}
+
+	/* First boot or Flow controller did not work as expected. Try to
+	   directly toggle power gates. Error if direct power on also fails. */
+	if (!is_cpu_powered(cpu)) {
+		ret = tegra_powergate_power_on(TEGRA_CPU_POWERGATE_ID(cpu));
+		if (ret)
+			goto fail;
+
+		/* Wait for the power to come up. */
+		timeout = jiffies + 10*HZ;
+
+		do {
+			if (is_cpu_powered(cpu))
+				goto remove_clamps;
+			udelay(10);
+		} while (time_before(jiffies, timeout));
+		ret = -ETIMEDOUT;
+		goto fail;
+	}
+
+remove_clamps:
+	/* CPU partition is powered. Enable the CPU clock. */
+	writel(CPU_CLOCK(cpu), CLK_RST_CONTROLLER_CLK_CPU_CMPLX_CLR);
+	reg = readl(CLK_RST_CONTROLLER_CLK_CPU_CMPLX_CLR);
+	udelay(10);
+
+	/* Remove I/O clamps. */
+	ret = tegra_powergate_remove_clamping(TEGRA_CPU_POWERGATE_ID(cpu));
+	udelay(10);
+fail:
+#else
+	/* Enable the CPU clock. */
+	reg = readl(CLK_RST_CONTROLLER_CLK_CPU_CMPLX);
+	writel(reg & ~CPU_CLOCK(cpu), CLK_RST_CONTROLLER_CLK_CPU_CMPLX);
+	barrier();
+	reg = readl(CLK_RST_CONTROLLER_CLK_CPU_CMPLX);
+#endif
+	/* Clear flow controller CSR. */
+	flowctrl_writel(0, FLOW_CTRL_CPU_CSR(cpu));
+	return ret;
+}
 
 void __cpuinit platform_secondary_init(unsigned int cpu)
 {
 	gic_secondary_init(0);
+
+	cpumask_set_cpu(cpu, to_cpumask(tegra_cpu_init_bits));
+	if (!tegra_all_cpus_booted)
+		if (cpumask_equal(tegra_cpu_init_mask, cpu_present_mask))
+			tegra_all_cpus_booted = true;
 }
 
 int boot_secondary(unsigned int cpu, struct task_struct *idle)
@@ -106,9 +218,6 @@ int boot_secondary(unsigned int cpu, struct task_struct *idle)
 
 	smp_wmb();
 
-	/* set the reset vector to point to the secondary_startup routine */
-	writel(virt_to_phys(tegra_secondary_startup), EVP_CPU_RESET_VECTOR);
-
 	/* Force the CPU into reset. The CPU must remain in reset when the
 	   flow controller state is cleared (which will cause the flow
 	   controller to stop driving reset if the CPU has been power-gated
@@ -123,22 +232,13 @@ int boot_secondary(unsigned int cpu, struct task_struct *idle)
 	   is now driving reset. */
 	flowctrl_writel(0, FLOW_CTRL_HALT_CPU(cpu));
 
-#if defined(CONFIG_ARCH_TEGRA_2x_SOC)
-	{
-		/* enable cpu clock on cpu */
-		u32 reg = readl(CLK_RST_CONTROLLER_CLK_CPU_CMPLX);
-		writel(reg & ~CPU_CLOCK(cpu), CLK_RST_CONTROLLER_CLK_CPU_CMPLX);
-		dmb();
-	}
-#endif
 	status = power_up_cpu(cpu);
 	if (status)
 		goto done;
 
-	dmb();
-	udelay(10);	/* power up delay */
+	/* Take the CPU out of reset. */
 	writel(CPU_RESET(cpu), CLK_RST_CONTROLLER_RST_CPU_CMPLX_CLR);
-
+	wmb();
 done:
 	return status;
 }
@@ -167,111 +267,12 @@ void __init smp_init_cpus(void)
 void __init platform_smp_prepare_cpus(unsigned int max_cpus)
 {
 
-	scu_enable(scu_base);
-}
-
-#if !defined(CONFIG_ARCH_TEGRA_2x_SOC)
-
-static bool is_cpu_powered(unsigned int cpu)
-{
-	if (is_lp_cluster())
-		return true;
-	else
-		return tegra_powergate_is_powered(TEGRA_CPU_POWERGATE_ID(cpu));
-}
-
-static int power_up_cpu(unsigned int cpu)
-{
-	int ret;
-	u32 reg;
-	unsigned long timeout;
-
-	BUG_ON(cpu == smp_processor_id());
-	BUG_ON(is_lp_cluster());
-
-	/* If this cpu has booted this function is entered after
-	 * CPU has been already un-gated by flow controller. Wait
-	 * for confirmation that cpu is powered and remove clamps.
-	 * On first boot entry do not wait - go to direct ungate.
-	 */
-#if 0 /* FIXME! */
-	if (cpu_isset(cpu,*(cpumask_t*)&tegra_cpu_init_map))
-	{
-		timeout = jiffies + HZ;
-		do {
-			if (is_cpu_powered(cpu))
-				goto remove_clamps;
-			udelay(10);
-		} while (time_before(jiffies, timeout));
-	}
-#endif
-	/* 1'st boot or Flow controller did not work as expected - try directly toggle
-	   power gates. Bail out if direct power on also failed */
-	if (!is_cpu_powered(cpu))
-	{
-		ret = tegra_powergate_power_on(TEGRA_CPU_POWERGATE_ID(cpu));
-		if (ret)
-			goto fail;
+	/* Always mark the boot CPU as initialized. */
+	cpumask_set_cpu(0, to_cpumask(tegra_cpu_init_bits));
 
-		/* Wait for the power to come up. */
-		timeout = jiffies + 10*HZ;
+	if (max_cpus == 1)
+		tegra_all_cpus_booted = true;
 
-		do {
-			if (is_cpu_powered(cpu))
-				goto remove_clamps;
-			udelay(10);
-		} while (time_before(jiffies, timeout));
-		ret = -ETIMEDOUT;
-		goto fail;
-	}
-
-remove_clamps:
-	/* now CPU is up: enable clock, propagate reset, and remove clamps */
-	reg = readl(CLK_RST_CONTROLLER_CLK_CPU_CMPLX);
-	writel(reg & ~CPU_CLOCK(cpu), CLK_RST_CONTROLLER_CLK_CPU_CMPLX);
-	barrier();
-	reg = readl(CLK_RST_CONTROLLER_CLK_CPU_CMPLX);
-
-	udelay(10);
-	ret = tegra_powergate_remove_clamping(TEGRA_CPU_POWERGATE_ID(cpu));
-fail:
-	return ret;
-}
-
-static int is_g_cluster_available(unsigned int cpu)
-{
-	u32 fuse_sku = readl(FUSE_SKU_DIRECT_CONFIG);
-	u32 bond_out = readl(CAR_BOND_OUT_V);
-
-	/* Does the G CPU complex exist at all? */
-	if ((fuse_sku & FUSE_SKU_DISABLE_ALL_CPUS) ||
-	    (bond_out & CAR_BOND_OUT_V_CPU_G))
-		return -EPERM;
-
-	if (cpu >= available_cpus())
-		return -EPERM;
-
-	/* FIXME: The G CPU can be unavailable for a number of reasons
-	 *	  (e.g., low battery, over temperature, etc.). Add checks for
-	 *	  these conditions. */
-
-	return 0;
-}
-#endif
-
-static unsigned int available_cpus(void)
-{
-	static unsigned int ncores = 0;
-
-	if (ncores == 0) {
-		ncores = scu_get_core_count(scu_base);
-#ifndef CONFIG_ARCH_TEGRA_2x_SOC
-		if (ncores > 1) {
-			u32 fuse_sku = readl(FUSE_SKU_DIRECT_CONFIG);
-			ncores -= FUSE_SKU_NUM_DISABLED_CPUS(fuse_sku);
-			BUG_ON((int)ncores <= 0);
-		}
-#endif
-	}
-	return ncores;
+	tegra_cpu_reset_handler_init();
+	scu_enable(scu_base);
 }
diff --git a/arch/arm/mach-tegra/pm-t3.c b/arch/arm/mach-tegra/pm-t3.c
index 50ce1063e490..437a2774a3a5 100644
--- a/arch/arm/mach-tegra/pm-t3.c
+++ b/arch/arm/mach-tegra/pm-t3.c
@@ -31,6 +31,7 @@
 #include "clock.h"
 #include "gpio-names.h"
 #include "pm.h"
+#include "sleep.h"
 #include "tegra3_emc.h"
 
 #define CAR_CCLK_BURST_POLICY \
diff --git a/arch/arm/mach-tegra/pm.c b/arch/arm/mach-tegra/pm.c
index 25c4ed453574..5c15dc75b586 100644
--- a/arch/arm/mach-tegra/pm.c
+++ b/arch/arm/mach-tegra/pm.c
@@ -57,6 +57,7 @@
 #include "cpuidle.h"
 #include "pm.h"
 #include "pm-irq.h"
+#include "reset.h"
 #include "sleep.h"
 #include "fuse.h"
 
@@ -88,8 +89,6 @@ static u8 *iram_save;
 static unsigned long iram_save_size;
 static void __iomem *iram_code = IO_ADDRESS(TEGRA_IRAM_CODE_AREA);
 static void __iomem *clk_rst = IO_ADDRESS(TEGRA_CLK_RESET_BASE);
-static void __iomem *evp_reset =
-	IO_ADDRESS(TEGRA_EXCEPTION_VECTORS_BASE) + 0x100;
 static void __iomem *pmc = IO_ADDRESS(TEGRA_PMC_BASE);
 #endif
 
@@ -366,6 +365,7 @@ static void suspend_cpu_complex(void)
 	reg = readl(FLOW_CTRL_CPU_CSR(cpu));
 	reg &= ~FLOW_CTRL_CSR_WFE_BITMAP;	/* clear wfe bitmap */
 	reg &= ~FLOW_CTRL_CSR_WFI_BITMAP;	/* clear wfi bitmap */
+	reg |= FLOW_CTRL_CSR_INTR_FLAG;		/* clear intr flag */
 	reg |= FLOW_CTRL_CSR_EVENT_FLAG;	/* clear event flag */
 #ifdef CONFIG_ARCH_TEGRA_2x_SOC
 	reg |= FLOW_CTRL_CSR_WFE_CPU0 << cpu;	/* enable power gating on wfe */
@@ -390,6 +390,13 @@ void tegra_clear_cpu_in_lp2(int cpu)
 {
 	spin_lock(&tegra_lp2_lock);
 	cpumask_clear_cpu(cpu, &tegra_in_lp2);
+
+	/* Update the IRAM copy used by the reset handler. The IRAM copy
+	   can't use used directly by cpumask_clear_cpu() because it uses
+	   LDREX/STREX which requires the addressed location to be inner
+	   cacheable and sharable which IRAM isn't. */
+	*tegra_cpu_lp2_mask = tegra_in_lp2;
+
 	spin_unlock(&tegra_lp2_lock);
 }
 
@@ -398,8 +405,14 @@ bool tegra_set_cpu_in_lp2(int cpu)
 	bool last_cpu = false;
 
 	spin_lock(&tegra_lp2_lock);
-
 	cpumask_set_cpu(cpu, &tegra_in_lp2);
+
+	/* Update the IRAM copy used by the reset handler. The IRAM copy
+	   can't use used directly by cpumask_set_cpu() because it uses
+	   LDREX/STREX which requires the addressed location to be inner
+	   cacheable and sharable which IRAM isn't. */
+	*tegra_cpu_lp2_mask = tegra_in_lp2;
+
 	if (cpumask_equal(&tegra_in_lp2, cpu_online_mask))
 		last_cpu = true;
 #ifdef CONFIG_ARCH_TEGRA_2x_SOC
@@ -426,8 +439,6 @@ unsigned int tegra_idle_lp2_last(unsigned int sleep_time, unsigned int flags)
 
 	tegra_cluster_switch_time(flags, tegra_cluster_switch_time_id_start);
 
-	writel(virt_to_phys(tegra_resume), evp_reset);
-
 	/*
 	 * We can use clk_get_rate_all_locked() here, because all other cpus
 	 * are in LP2 state and irqs are disabled
@@ -565,21 +576,8 @@ static void tegra_pm_set(enum tegra_suspend_mode mode)
 		writel(0x1, pmc + PMC_DPD_SAMPLE);
 		break;
 	case TEGRA_SUSPEND_LP1:
-		/*
-		 * LP1 boots through the normal cpu reset vector pointing to
-		 * tegra_lp1_reset in IRAM, which resumes the CPU to
-		 * the address in scratch 41 to tegra_resume
-		 */
-		writel(tegra_lp1_reset() - tegra_iram_start() +
-			TEGRA_IRAM_CODE_AREA, evp_reset);
-		__raw_writel(virt_to_phys(tegra_resume), pmc + PMC_SCRATCH41);
 		break;
 	case TEGRA_SUSPEND_LP2:
-		/*
-		 * LP2 boots through the normal cpu reset vector directly to
-		 * tegra_resume
-		 */
-		writel(virt_to_phys(tegra_resume), evp_reset);
 		rate = clk_get_rate(tegra_pclk);
 		break;
 	default:
diff --git a/arch/arm/mach-tegra/pm.h b/arch/arm/mach-tegra/pm.h
index b64de7e41aea..ff8480bc76ca 100644
--- a/arch/arm/mach-tegra/pm.h
+++ b/arch/arm/mach-tegra/pm.h
@@ -194,4 +194,10 @@ static inline void tegra_cluster_switch_set_parameters(
 { }
 #endif
 
+#ifdef CONFIG_SMP
+extern bool tegra_all_cpus_booted __read_mostly;
+#else
+#define tegra_all_cpus_booted (true)
+#endif
+
 #endif /* _MACH_TEGRA_PM_H_ */
diff --git a/arch/arm/mach-tegra/reset.c b/arch/arm/mach-tegra/reset.c
new file mode 100644
index 000000000000..1ae5a1407a37
--- /dev/null
+++ b/arch/arm/mach-tegra/reset.c
@@ -0,0 +1,117 @@
+/*
+ * arch/arm/mach-tegra/reset.c
+ *
+ * Copyright (C) 2011 NVIDIA Corporation.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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.
+ *
+ */
+
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/cpumask.h>
+#include <linux/bitops.h>
+
+#include <asm/cacheflush.h>
+#include <asm/hardware/cache-l2x0.h>
+
+#include <mach/iomap.h>
+
+#include "reset.h"
+#include "sleep.h"
+
+static bool is_enabled;
+
+void tegra_cpu_reset_handler_enable(void)
+{
+	void __tegra_cpu_reset_handler(void);
+	void __tegra_cpu_reset_handler_start(void);
+	void __tegra_cpu_reset_handler_end(void);
+	void __iomem *evp_cpu_reset =
+		IO_ADDRESS(TEGRA_EXCEPTION_VECTORS_BASE + 0x100);
+	void __iomem *iram_base = IO_ADDRESS(TEGRA_IRAM_BASE);
+	void __iomem *sb_ctrl = IO_ADDRESS(TEGRA_SB_BASE);
+	unsigned long cpu_reset_handler_size =
+		__tegra_cpu_reset_handler_end - __tegra_cpu_reset_handler_start;
+	unsigned long cpu_reset_handler_offset =
+		__tegra_cpu_reset_handler - __tegra_cpu_reset_handler_start;
+	unsigned long reg;
+
+	BUG_ON(is_enabled);
+	BUG_ON(cpu_reset_handler_size > TEGRA_RESET_HANDLER_SIZE);
+
+	memcpy(iram_base, (void *)__tegra_cpu_reset_handler_start,
+	       cpu_reset_handler_size);
+
+	/* NOTE: This must be the one and only write to the EVP CPU reset
+		 vector in the entire system. */
+	writel(TEGRA_RESET_HANDLER_BASE + cpu_reset_handler_offset,
+		evp_cpu_reset);
+	wmb();
+	reg = readl(evp_cpu_reset);
+
+	/* Prevent further modifications to the physical reset vector.
+	   NOTE: Has no effect on chips prior to Tegra3. */
+	reg = readl(sb_ctrl);
+	reg |= 2;
+	writel(reg, sb_ctrl);
+	is_enabled = true;
+	wmb();
+}
+
+#ifdef CONFIG_PM_SLEEP
+static unsigned long cpu_reset_handler_save[TEGRA_RESET_DATA_SIZE];
+
+void tegra_cpu_reset_handler_save(void)
+{
+	unsigned int i;
+	BUG_ON(!is_enabled);
+	for (i = 0; i < TEGRA_RESET_DATA_SIZE; i++)
+		cpu_reset_handler_save[i] =
+					tegra_cpu_reset_handler_ptr[i];
+	is_enabled = false;
+}
+
+void tegra_cpu_reset_handler_restore(void)
+{
+	unsigned int i;
+	BUG_ON(is_enabled);
+	for (i = 0; i < TEGRA_RESET_DATA_SIZE; i++)
+		tegra_cpu_reset_handler_ptr[i] =
+					cpu_reset_handler_save[i];
+	is_enabled = true;
+}
+#endif
+
+void __init tegra_cpu_reset_handler_init(void)
+{
+#ifdef CONFIG_SMP
+	__tegra_cpu_reset_handler_data[TEGRA_RESET_MASK_PRESENT] =
+		*((u32 *)cpu_present_mask);
+	__tegra_cpu_reset_handler_data[TEGRA_RESET_STARTUP_SECONDARY] =
+		virt_to_phys((void *)tegra_secondary_startup);
+#endif
+#ifdef CONFIG_PM_SLEEP
+	__tegra_cpu_reset_handler_data[TEGRA_RESET_STARTUP_LP1] =
+		TEGRA_IRAM_CODE_AREA;
+	__tegra_cpu_reset_handler_data[TEGRA_RESET_STARTUP_LP2] =
+		virt_to_phys((void *)tegra_resume);
+#endif
+
+	/* Push all of reset handler data out to the L3 memory system. */
+	__cpuc_coherent_kern_range(
+		(unsigned long)&__tegra_cpu_reset_handler_data[0],
+		(unsigned long)&__tegra_cpu_reset_handler_data[TEGRA_RESET_DATA_SIZE]);
+
+	outer_clean_range(__pa(&__tegra_cpu_reset_handler_data[0]),
+			  __pa(&__tegra_cpu_reset_handler_data[TEGRA_RESET_DATA_SIZE]));
+
+	tegra_cpu_reset_handler_enable();
+}
diff --git a/arch/arm/mach-tegra/reset.h b/arch/arm/mach-tegra/reset.h
new file mode 100644
index 000000000000..4674194547ce
--- /dev/null
+++ b/arch/arm/mach-tegra/reset.h
@@ -0,0 +1,61 @@
+/*
+ * arch/arm/mach-tegra/reset.h
+ *
+ * CPU reset dispatcher.
+ *
+ * Copyright (c) 2011, NVIDIA Corporation.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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.
+ *
+ */
+
+#ifndef __MACH_TEGRA_RESET_H
+#define __MACH_TEGRA_RESET_H
+
+#define TEGRA_RESET_MASK_PRESENT	0
+#define TEGRA_RESET_MASK_LP1		1
+#define TEGRA_RESET_MASK_LP2		2
+#define TEGRA_RESET_STARTUP_SECONDARY	3
+#define TEGRA_RESET_STARTUP_LP2		4
+#define TEGRA_RESET_STARTUP_LP1		5
+#define TEGRA_RESET_DATA_SIZE		6
+
+#ifndef __ASSEMBLY__
+
+#include <linux/cpumask.h>
+
+extern unsigned long __tegra_cpu_reset_handler_data[TEGRA_RESET_DATA_SIZE];
+
+void __tegra_cpu_reset_handler_start(void);
+void tegra_secondary_startup(void);
+
+#ifdef CONFIG_PM_SLEEP
+#define tegra_cpu_lp1_map (*(unsigned long *)(IO_ADDRESS(TEGRA_RESET_HANDLER_BASE + \
+		((u32)&__tegra_cpu_reset_handler_data[TEGRA_RESET_MASK_LP1] - \
+		 (u32)__tegra_cpu_reset_handler_start))))
+
+#define tegra_cpu_reset_handler_ptr ((u32 *)(IO_ADDRESS(TEGRA_RESET_HANDLER_BASE + \
+		((u32)__tegra_cpu_reset_handler_data - \
+		 (u32)__tegra_cpu_reset_handler_start))))
+
+#define tegra_cpu_lp2_mask ((cpumask_t *)(IO_ADDRESS(TEGRA_RESET_HANDLER_BASE + \
+		((u32)&__tegra_cpu_reset_handler_data[TEGRA_RESET_MASK_LP2] - \
+		 (u32)__tegra_cpu_reset_handler_start))))
+#endif
+
+void tegra_cpu_reset_handler_enable(void);
+void __init tegra_cpu_reset_handler_init(void);
+
+#ifdef CONFIG_PM_SLEEP
+void tegra_cpu_reset_handler_save(void);
+void tegra_cpu_reset_handler_restore(void);
+#endif
+#endif
+#endif
diff --git a/arch/arm/mach-tegra/sleep-t2.S b/arch/arm/mach-tegra/sleep-t2.S
index 271b188448a6..aacf630db2b2 100644
--- a/arch/arm/mach-tegra/sleep-t2.S
+++ b/arch/arm/mach-tegra/sleep-t2.S
@@ -211,7 +211,7 @@ ENTRY(tegra2_sleep_wfi)
 
 	/*
 	 * cpu may be reset while in wfi, which will return through
-	 * tegra_secondary_resume to cpu_resume to tegra_cpu_resume
+	 * tegra_resume to cpu_resume to tegra_cpu_resume
 	 * or interrupt may wake wfi, which will return here
 	 * cpu state is unchanged - MMU is on, cache is on, coherency is off
 	 *
diff --git a/arch/arm/mach-tegra/sleep.h b/arch/arm/mach-tegra/sleep.h
index c6700af4b45e..61ea3dfaffec 100644
--- a/arch/arm/mach-tegra/sleep.h
+++ b/arch/arm/mach-tegra/sleep.h
@@ -90,7 +90,6 @@ void tegra_cpu_wfi(void);
 #ifdef CONFIG_ARCH_TEGRA_2x_SOC
 extern void tegra2_iram_start;
 extern void tegra2_iram_end;
-extern void tegra2_lp1_reset;
 int  tegra2_cpu_is_resettable_soon(void);
 void tegra2_cpu_reset(int cpu);
 void tegra2_cpu_set_resettable_soon(void);
@@ -113,13 +112,6 @@ static inline void *tegra_iram_end(void)
 #endif
 }
 
-static inline void *tegra_lp1_reset(void)
-{
-#ifdef CONFIG_ARCH_TEGRA_2x_SOC
-	return &tegra2_lp1_reset;
-#endif
-}
-
 static inline void tegra_sleep_core(unsigned long v2p)
 {
 #ifdef CONFIG_ARCH_TEGRA_2x_SOC
@@ -129,7 +121,6 @@ static inline void tegra_sleep_core(unsigned long v2p)
 
 void tegra_sleep_cpu(unsigned long v2p);
 void tegra_resume(void);
-void tegra_secondary_resume(void);
 
 #endif