ARM: tegra3: power: Add LP2 power mode support for CPU 0

Add support for forced Tegra3 LP2 low power mode on the boot processor
(CPU 0) via the cluster control interface when all others are offline.
Switching to the LP CPU mode is also enabled with this change.

LP2 in idle and LP2 mode on the secondary processors is not yet
supported.

Change-Id: Icb898729f093be5e006c413f701532dd45228687
Signed-off-by: Scott Williams <scwilliams@nvidia.com>
Signed-off-by: Dan Willemsen <dwillemsen@nvidia.com>

Rebase-Id: Rd5d8c2b0addfd6853033670b992ae082e4a0d9c8

10 files changed, 552 insertions, 15 deletions

diff --git a/arch/arm/mach-tegra/Makefile b/arch/arm/mach-tegra/Makefile
index 191b7e320f9a..a63aac09c228 100644
--- a/arch/arm/mach-tegra/Makefile
+++ b/arch/arm/mach-tegra/Makefile
@@ -68,6 +68,7 @@ ifeq ($(CONFIG_CPU_IDLE),y)
 obj-y                                   += cpuidle.o
 ifeq ($(CONFIG_PM_SLEEP),y)
 obj-$(CONFIG_ARCH_TEGRA_2x_SOC)         += cpuidle-t2.o
+obj-$(CONFIG_ARCH_TEGRA_3x_SOC)         += cpuidle-t3.o
 endif
 endif
 obj-$(CONFIG_TEGRA_IOVMM)               += iovmm.o
diff --git a/arch/arm/mach-tegra/asm_macros.h b/arch/arm/mach-tegra/asm_macros.h
index 24d1449efbae..61a7028a6cc0 100644
--- a/arch/arm/mach-tegra/asm_macros.h
+++ b/arch/arm/mach-tegra/asm_macros.h
@@ -19,6 +19,16 @@
 
 #ifdef __ASSEMBLY__
 
+/* waits until the microsecond counter (base) is > rn */
+.macro	wait_until, rn, base, tmp
+	add	\rn, \rn, #1
+1002:	ldr	\tmp, [\base]
+	sub	\tmp, \tmp, \rn
+	ands	\tmp, \tmp, #0x80000000
+	dmb
+	bne	1002b
+.endm
+
 /* returns the offset of the flow controller halt register for a cpu */
 .macro cpu_to_halt_reg rd, rcpu
 	cmp	\rcpu, #0
diff --git a/arch/arm/mach-tegra/cpuidle-t3.c b/arch/arm/mach-tegra/cpuidle-t3.c
new file mode 100644
index 000000000000..25f7d26d665c
--- /dev/null
+++ b/arch/arm/mach-tegra/cpuidle-t3.c
@@ -0,0 +1,397 @@
+/*
+ * arch/arm/mach-tegra/cpuidle-t3.c
+ *
+ * CPU idle driver for Tegra3 CPUs
+ *
+ * Copyright (c) 2010-2011, NVIDIA Corporation.
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include <linux/kernel.h>
+#include <linux/cpu.h>
+#include <linux/cpuidle.h>
+#include <linux/debugfs.h>
+#include <linux/delay.h>
+#include <linux/hrtimer.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/io.h>
+#include <linux/ratelimit.h>
+#include <linux/sched.h>
+#include <linux/seq_file.h>
+#include <linux/slab.h>
+#include <linux/smp.h>
+#include <linux/suspend.h>
+#include <linux/tick.h>
+
+#include <asm/cacheflush.h>
+#include <asm/cpu_pm.h>
+#include <asm/hardware/gic.h>
+#include <asm/localtimer.h>
+
+#include <mach/iomap.h>
+#include <mach/irqs.h>
+
+#include <trace/events/power.h>
+
+#include "clock.h"
+#include "cpuidle.h"
+#include "dvfs.h"
+#include "fuse.h"
+#include "gic.h"
+#include "pm.h"
+#include "reset.h"
+#include "sleep.h"
+
+#define CLK_RST_CONTROLLER_CPU_CMPLX_STATUS \
+	(IO_ADDRESS(TEGRA_CLK_RESET_BASE) + 0x470)
+
+#ifdef CONFIG_SMP
+static s64 tegra_cpu_wake_by_time[4] = {
+	LLONG_MAX, LLONG_MAX, LLONG_MAX, LLONG_MAX };
+#endif
+
+static struct clk *cpu_clk_for_dvfs;
+
+static struct {
+	unsigned int cpu_ready_count[5];
+	unsigned int tear_down_count[5];
+	unsigned long long cpu_wants_lp2_time[5];
+	unsigned long long in_lp2_time;
+	unsigned int lp2_count;
+	unsigned int lp2_completed_count;
+	unsigned int lp2_count_bin[32];
+	unsigned int lp2_completed_count_bin[32];
+	unsigned int lp2_int_count[NR_IRQS];
+	unsigned int last_lp2_int_count[NR_IRQS];
+} idle_stats;
+
+static inline unsigned int time_to_bin(unsigned int time)
+{
+	return fls(time);
+}
+
+static inline void tegra_irq_unmask(int irq)
+{
+	struct irq_data *data = irq_get_irq_data(irq);
+	data->chip->irq_unmask(data);
+}
+
+static inline unsigned int cpu_number(unsigned int n)
+{
+	return is_lp_cluster() ? 4 : n;
+}
+
+void tegra3_cpu_idle_stats_lp2_ready(unsigned int cpu)
+{
+	idle_stats.cpu_ready_count[cpu_number(cpu)]++;
+}
+
+void tegra3_cpu_idle_stats_lp2_time(unsigned int cpu, s64 us)
+{
+	idle_stats.cpu_wants_lp2_time[cpu_number(cpu)] += us;
+}
+
+bool tegra3_lp2_is_allowed(struct cpuidle_device *dev,
+	struct cpuidle_state *state)
+{
+	s64 request;
+
+	if (!tegra_all_cpus_booted)
+		return false;
+
+	/* On A01, LP2 on slave CPU's cause ranhdom CPU hangs.
+	 * Refer to Bug 804085.
+	 */
+	if ((tegra_get_revision() == TEGRA_REVISION_A01) &&
+		num_online_cpus() > 1)
+		return false;
+
+	/* FIXME: All CPU's entering LP2 is not working.
+	 * Don't let CPU0 enter LP2 when any secondary CPU is online.
+	 */
+	if ((dev->cpu == 0) && (num_online_cpus() > 1))
+		return false;
+
+	if (dev->cpu == 0) {
+		u32 reg = readl(CLK_RST_CONTROLLER_CPU_CMPLX_STATUS);
+		if ((reg & 0xE) != 0xE)
+			return false;
+
+		if (tegra_dvfs_rail_updating(cpu_clk_for_dvfs))
+			return false;
+	}
+
+	request = ktime_to_us(tick_nohz_get_sleep_length());
+	if (request < state->target_residency) {
+		/* Not enough time left to enter LP2 */
+		return false;
+	}
+
+	return true;
+}
+
+static void tegra3_idle_enter_lp2_cpu_0(struct cpuidle_device *dev,
+			   struct cpuidle_state *state, s64 request)
+{
+	ktime_t enter;
+	ktime_t exit;
+	bool sleep_completed = false;
+	int bin;
+
+	/* LP2 entry time */
+	enter = ktime_get();
+
+	if (request < state->target_residency) {
+		/* Not enough time left to enter LP2 */
+		tegra_cpu_wfi();
+		return;
+	}
+
+#ifdef CONFIG_SMP
+	if (!is_lp_cluster() && (num_online_cpus() > 1)) {
+		s64 wake_time;
+		unsigned int i;
+
+		/* Disable the distributor -- this is the only way to
+		   prevent the other CPUs from responding to interrupts
+		   and potentially fiddling with the distributor
+		   registers while we're fiddling with them. */
+		tegra_gic_dist_disable();
+
+		/* Did an interrupt come in for another CPU before we
+		   could disable the distributor? */
+		if (!tegra3_lp2_is_allowed(dev, state)) {
+			/* Yes, re-enable the distributor and LP3. */
+			tegra_gic_dist_enable();
+			tegra_cpu_wfi();
+			return;
+		}
+
+		/* Save and disable the affinity setting for the other
+		   CPUs and route all interrupts to CPU0. */
+		tegra_gic_disable_affinity();
+
+		/* Re-enable the distributor. */
+		tegra_gic_dist_enable();
+
+		/* LP2 initial targeted wake time */
+		wake_time = ktime_to_us(enter) + request;
+
+		/* CPU0 must wake up before any of the other CPUs. */
+		smp_rmb();
+		for (i = 1; i < CONFIG_NR_CPUS; i++)
+			wake_time = min_t(s64, wake_time,
+				tegra_cpu_wake_by_time[i]);
+
+		/* LP2 actual targeted wake time */
+		request = wake_time - ktime_to_us(enter);
+		BUG_ON(wake_time < 0LL);
+	}
+#endif
+
+	if (request > state->target_residency) {
+		s64 sleep_time = request - tegra_lp2_exit_latency;
+
+		bin = time_to_bin((u32)request / 1000);
+		idle_stats.tear_down_count[cpu_number(dev->cpu)]++;
+		idle_stats.lp2_count++;
+		idle_stats.lp2_count_bin[bin]++;
+
+		trace_power_start(POWER_CSTATE, 2, dev->cpu);
+
+		if (tegra_idle_lp2_last(sleep_time, 0) == 0)
+			sleep_completed = true;
+		else {
+			int irq = tegra_gic_pending_interrupt();
+			idle_stats.lp2_int_count[irq]++;
+		}
+	}
+
+#ifdef CONFIG_SMP
+	if (!is_lp_cluster() && (num_online_cpus() > 1)) {
+
+		/* Disable the distributor. */
+		tegra_gic_dist_disable();
+
+		/* Restore the other CPU's interrupt affinity. */
+		tegra_gic_restore_affinity();
+
+		/* Re-enable the distributor. */
+		tegra_gic_dist_enable();
+	}
+#endif
+
+	exit = ktime_get();
+	if (sleep_completed) {
+		/*
+		 * Stayed in LP2 for the full time until the next tick,
+		 * adjust the exit latency based on measurement
+		 */
+		int offset = ktime_to_us(ktime_sub(exit, enter)) - request;
+		int latency = tegra_lp2_exit_latency + offset / 16;
+		latency = clamp(latency, 0, 10000);
+		tegra_lp2_exit_latency = latency;
+		smp_wmb();
+
+		idle_stats.lp2_completed_count++;
+		idle_stats.lp2_completed_count_bin[bin]++;
+		idle_stats.in_lp2_time += ktime_to_us(ktime_sub(exit, enter));
+
+		pr_debug("%lld %lld %d %d\n", request,
+			ktime_to_us(ktime_sub(exit, enter)),
+			offset, bin);
+	}
+}
+
+static void tegra3_idle_enter_lp2_cpu_n(struct cpuidle_device *dev,
+			   struct cpuidle_state *state, s64 request)
+{
+#ifdef CONFIG_SMP
+	s64 sleep_time = request - tegra_lp2_exit_latency;
+
+	tegra_lp2_set_trigger(sleep_time);
+
+	idle_stats.tear_down_count[cpu_number(dev->cpu)]++;
+
+	trace_power_start(POWER_CSTATE, 2, dev->cpu);
+
+	/* Save time this CPU must be awakened by. */
+	tegra_cpu_wake_by_time[dev->cpu] = ktime_to_us(ktime_get()) + request;
+	smp_wmb();
+
+	flush_cache_all();
+	barrier();
+/* !!!FIXME!!! __cortex_a9_save(0); */
+	/* CPUn is powergated */
+
+	/* CPUn woke up */
+	barrier();
+
+	tegra_cpu_wake_by_time[dev->cpu] = LLONG_MAX;
+
+	if (sleep_time)
+		tegra_lp2_set_trigger(0);
+#endif
+}
+
+void tegra3_idle_lp2(struct cpuidle_device *dev,
+			   struct cpuidle_state *state)
+{
+	s64 request = ktime_to_us(tick_nohz_get_sleep_length());
+	bool last_cpu = tegra_set_cpu_in_lp2(dev->cpu);
+
+	cpu_pm_enter();
+
+	if (last_cpu)
+		tegra3_idle_enter_lp2_cpu_0(dev, state, request);
+	else
+		tegra3_idle_enter_lp2_cpu_n(dev, state, request);
+
+	cpu_pm_exit();
+	tegra_clear_cpu_in_lp2(dev->cpu);
+}
+
+int tegra_cpudile_init_soc(void)
+{
+	cpu_clk_for_dvfs = tegra_get_clock_by_name("cpu_g");
+	return 0;
+}
+
+#ifdef CONFIG_DEBUG_FS
+int tegra3_lp2_debug_show(struct seq_file *s, void *data)
+{
+	int bin;
+	int i;
+	seq_printf(s, "                                    cpu0     cpu1     cpu2     cpu3     cpulp\n");
+	seq_printf(s, "-----------------------------------------------------------------------------\n");
+	seq_printf(s, "cpu ready:                      %8u %8u %8u %8u %8u\n",
+		idle_stats.cpu_ready_count[0],
+		idle_stats.cpu_ready_count[1],
+		idle_stats.cpu_ready_count[2],
+		idle_stats.cpu_ready_count[3],
+		idle_stats.cpu_ready_count[4]);
+	seq_printf(s, "tear down:                      %8u %8u %8u %8u %8u\n",
+		idle_stats.tear_down_count[0],
+		idle_stats.tear_down_count[1],
+		idle_stats.tear_down_count[2],
+		idle_stats.tear_down_count[3],
+		idle_stats.tear_down_count[4]);
+	seq_printf(s, "lp2:            %8u\n", idle_stats.lp2_count);
+	seq_printf(s, "lp2 completed:  %8u %7u%%\n",
+		idle_stats.lp2_completed_count,
+		idle_stats.lp2_completed_count * 100 /
+			(idle_stats.lp2_count ?: 1));
+
+	seq_printf(s, "\n");
+	seq_printf(s, "cpu ready time:                 %8llu %8llu %8llu %8llu %8llu ms\n",
+		div64_u64(idle_stats.cpu_wants_lp2_time[0], 1000),
+		div64_u64(idle_stats.cpu_wants_lp2_time[1], 1000),
+		div64_u64(idle_stats.cpu_wants_lp2_time[2], 1000),
+		div64_u64(idle_stats.cpu_wants_lp2_time[3], 1000),
+		div64_u64(idle_stats.cpu_wants_lp2_time[4], 1000));
+
+	seq_printf(s, "lp2 time:       %8llu ms      %7d%% %7d%% %7d%% %7d%% %7d%%\n",
+		div64_u64(idle_stats.in_lp2_time, 1000),
+		(int)(idle_stats.cpu_wants_lp2_time[0] ?
+			div64_u64(idle_stats.in_lp2_time * 100,
+			idle_stats.cpu_wants_lp2_time[0]) : 0),
+		(int)(idle_stats.cpu_wants_lp2_time[1] ?
+			div64_u64(idle_stats.in_lp2_time * 100,
+			idle_stats.cpu_wants_lp2_time[1]) : 0),
+		(int)(idle_stats.cpu_wants_lp2_time[2] ?
+			div64_u64(idle_stats.in_lp2_time * 100,
+			idle_stats.cpu_wants_lp2_time[2]) : 0),
+		(int)(idle_stats.cpu_wants_lp2_time[3] ?
+			div64_u64(idle_stats.in_lp2_time * 100,
+			idle_stats.cpu_wants_lp2_time[3]) : 0),
+		(int)(idle_stats.cpu_wants_lp2_time[4] ?
+			div64_u64(idle_stats.in_lp2_time * 100,
+			idle_stats.cpu_wants_lp2_time[4]) : 0));
+	seq_printf(s, "\n");
+
+	seq_printf(s, "%19s %8s %8s %8s\n", "", "lp2", "comp", "%");
+	seq_printf(s, "-------------------------------------------------\n");
+	for (bin = 0; bin < 32; bin++) {
+		if (idle_stats.lp2_count_bin[bin] == 0)
+			continue;
+		seq_printf(s, "%6u - %6u ms: %8u %8u %7u%%\n",
+			1 << (bin - 1), 1 << bin,
+			idle_stats.lp2_count_bin[bin],
+			idle_stats.lp2_completed_count_bin[bin],
+			idle_stats.lp2_completed_count_bin[bin] * 100 /
+				idle_stats.lp2_count_bin[bin]);
+	}
+
+	seq_printf(s, "\n");
+	seq_printf(s, "%3s %20s %6s %10s\n",
+		"int", "name", "count", "last count");
+	seq_printf(s, "--------------------------------------------\n");
+	for (i = 0; i < NR_IRQS; i++) {
+		if (idle_stats.lp2_int_count[i] == 0)
+			continue;
+		seq_printf(s, "%3d %20s %6d %10d\n",
+			i, irq_to_desc(i)->action ?
+				irq_to_desc(i)->action->name ?: "???" : "???",
+			idle_stats.lp2_int_count[i],
+			idle_stats.lp2_int_count[i] -
+				idle_stats.last_lp2_int_count[i]);
+		idle_stats.last_lp2_int_count[i] = idle_stats.lp2_int_count[i];
+	};
+	return 0;
+}
+#endif
diff --git a/arch/arm/mach-tegra/cpuidle.h b/arch/arm/mach-tegra/cpuidle.h
index 5d0c3d695342..d5e1e3e0152e 100644
--- a/arch/arm/mach-tegra/cpuidle.h
+++ b/arch/arm/mach-tegra/cpuidle.h
@@ -35,12 +35,25 @@ bool tegra2_lp2_is_allowed(struct cpuidle_device *dev,
 int tegra2_lp2_debug_show(struct seq_file *s, void *data);
 #endif
 #endif
+#ifdef CONFIG_ARCH_TEGRA_3x_SOC
+void tegra3_idle_lp2(struct cpuidle_device *dev, struct cpuidle_state *state);
+void tegra3_cpu_idle_stats_lp2_ready(unsigned int cpu);
+void tegra3_cpu_idle_stats_lp2_time(unsigned int cpu, s64 us);
+bool tegra3_lp2_is_allowed(struct cpuidle_device *dev,
+			   struct cpuidle_state *state);
+#ifdef CONFIG_DEBUG_FS
+int tegra3_lp2_debug_show(struct seq_file *s, void *data);
+#endif
+#endif
 
 static inline void tegra_cpu_idle_stats_lp2_ready(unsigned int cpu)
 {
 #ifdef CONFIG_ARCH_TEGRA_2x_SOC
 	tegra2_cpu_idle_stats_lp2_ready(cpu);
 #endif
+#ifdef CONFIG_ARCH_TEGRA_3x_SOC
+	tegra3_cpu_idle_stats_lp2_ready(cpu);
+#endif
 }
 
 static inline void tegra_cpu_idle_stats_lp2_time(unsigned int cpu, s64 us)
@@ -48,6 +61,9 @@ static inline void tegra_cpu_idle_stats_lp2_time(unsigned int cpu, s64 us)
 #ifdef CONFIG_ARCH_TEGRA_2x_SOC
 	tegra2_cpu_idle_stats_lp2_time(cpu, us);
 #endif
+#ifdef CONFIG_ARCH_TEGRA_3x_SOC
+	tegra3_cpu_idle_stats_lp2_time(cpu, us);
+#endif
 }
 
 static inline void tegra_idle_lp2(struct cpuidle_device *dev,
@@ -56,6 +72,9 @@ static inline void tegra_idle_lp2(struct cpuidle_device *dev,
 #ifdef CONFIG_ARCH_TEGRA_2x_SOC
 	tegra2_idle_lp2(dev, state);
 #endif
+#ifdef CONFIG_ARCH_TEGRA_3x_SOC
+	tegra3_idle_lp2(dev, state);
+#endif
 }
 
 static inline bool tegra_lp2_is_allowed(struct cpuidle_device *dev,
@@ -64,6 +83,9 @@ static inline bool tegra_lp2_is_allowed(struct cpuidle_device *dev,
 #ifdef CONFIG_ARCH_TEGRA_2x_SOC
 	return tegra2_lp2_is_allowed(dev, state);
 #endif
+#ifdef CONFIG_ARCH_TEGRA_3x_SOC
+	return tegra3_lp2_is_allowed(dev, state);
+#endif
 }
 
 #ifdef CONFIG_DEBUG_FS
@@ -72,6 +94,9 @@ static inline int tegra_lp2_debug_show(struct seq_file *s, void *data)
 #ifdef CONFIG_ARCH_TEGRA_2x_SOC
 	return tegra2_lp2_debug_show(s, data);
 #endif
+#ifdef CONFIG_ARCH_TEGRA_3x_SOC
+	return tegra3_lp2_debug_show(s, data);
+#endif
 }
 #endif
 
diff --git a/arch/arm/mach-tegra/platsmp.c b/arch/arm/mach-tegra/platsmp.c
index a7b81919ba90..6cd9c61b68e9 100644
--- a/arch/arm/mach-tegra/platsmp.c
+++ b/arch/arm/mach-tegra/platsmp.c
@@ -49,17 +49,12 @@ const struct cpumask *const tegra_cpu_init_mask = to_cpumask(tegra_cpu_init_bits
 #define CPU_CLOCK(cpu)	(0x1<<(8+cpu))
 #define CPU_RESET(cpu)	(0x1111ul<<(cpu))
 
-#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
+#ifndef CONFIG_ARCH_TEGRA_2x_SOC
 #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);
diff --git a/arch/arm/mach-tegra/pm-t3.c b/arch/arm/mach-tegra/pm-t3.c
index 5f869a124053..423b1e3d8cdb 100644
--- a/arch/arm/mach-tegra/pm-t3.c
+++ b/arch/arm/mach-tegra/pm-t3.c
@@ -31,6 +31,7 @@
 #include <asm/hardware/gic.h>
 
 #include "clock.h"
+#include "cpuidle.h"
 #include "gpio-names.h"
 #include "pm.h"
 #include "sleep.h"
@@ -313,9 +314,11 @@ int tegra_cluster_control(unsigned int us, unsigned int flags)
 		if (us)
 			tegra_lp2_set_trigger(0);
 	} else {
+		tegra_set_cpu_in_lp2(0);
 		cpu_pm_enter();
 		tegra_idle_lp2_last(0, flags);
 		cpu_pm_exit();
+		tegra_clear_cpu_in_lp2(0);
 	}
 	local_irq_enable();
 
diff --git a/arch/arm/mach-tegra/pm.c b/arch/arm/mach-tegra/pm.c
index 3fe3ea1c289c..659db1d5d726 100644
--- a/arch/arm/mach-tegra/pm.c
+++ b/arch/arm/mach-tegra/pm.c
@@ -55,6 +55,7 @@
 #include "board.h"
 #include "clock.h"
 #include "cpuidle.h"
+#include "fuse.h"
 #include "gic.h"
 #include "pm.h"
 #include "pm-irq.h"
@@ -433,16 +434,16 @@ bool tegra_set_cpu_in_lp2(int cpu)
 
 unsigned int tegra_idle_lp2_last(unsigned int sleep_time, unsigned int flags)
 {
-	u32 reg;
+	u32 mode;	/* hardware + software power mode flags */
 	unsigned int remain;
 
 	/* Only the last cpu down does the final suspend steps */
-	reg = readl(pmc + PMC_CTRL);
-	reg |= TEGRA_POWER_CPU_PWRREQ_OE;
-	reg |= TEGRA_POWER_PWRREQ_OE;
-	reg |= flags;
-	reg &= ~TEGRA_POWER_EFFECT_LP0;
-	pmc_32kwritel(reg, PMC_CTRL);
+	mode = readl(pmc + PMC_CTRL);
+	mode |= TEGRA_POWER_CPU_PWRREQ_OE;
+	mode |= TEGRA_POWER_PWRREQ_OE;
+	mode &= ~TEGRA_POWER_EFFECT_LP0;
+	pmc_32kwritel(mode, PMC_CTRL);
+	mode |= flags;
 
 	tegra_cluster_switch_time(flags, tegra_cluster_switch_time_id_start);
 
@@ -454,7 +455,7 @@ unsigned int tegra_idle_lp2_last(unsigned int sleep_time, unsigned int flags)
 		clk_get_rate_all_locked(tegra_pclk));
 
 	if (flags & TEGRA_POWER_CLUSTER_MASK)
-		tegra_cluster_switch_prolog(reg);
+		tegra_cluster_switch_prolog(mode);
 
 	if (sleep_time)
 		tegra_lp2_set_trigger(sleep_time);
@@ -480,7 +481,7 @@ unsigned int tegra_idle_lp2_last(unsigned int sleep_time, unsigned int flags)
 		tegra_lp2_set_trigger(0);
 
 	if (flags & TEGRA_POWER_CLUSTER_MASK)
-		tegra_cluster_switch_epilog(reg);
+		tegra_cluster_switch_epilog(mode);
 
 	tegra_cluster_switch_time(flags, tegra_cluster_switch_time_id_epilog);
 
diff --git a/arch/arm/mach-tegra/sleep-t3.S b/arch/arm/mach-tegra/sleep-t3.S
index 769ef6f1fd32..b234ab3a671f 100644
--- a/arch/arm/mach-tegra/sleep-t3.S
+++ b/arch/arm/mach-tegra/sleep-t3.S
@@ -69,7 +69,9 @@ ENTRY(tegra3_hotplug_shutdown)
 	bl	tegra3_cpu_reset
 	mov	pc, lr				@ should never get here
 ENDPROC(tegra3_hotplug_shutdown)
+#endif
 
+#if defined(CONFIG_HOTPLUG_CPU) || defined(CONFIG_PM_SLEEP)
 /*
  * tegra3_cpu_reset(unsigned long flags)
  *
@@ -127,3 +129,94 @@ wfe_war:
 
 ENDPROC(tegra3_cpu_reset)
 #endif
+
+#ifdef CONFIG_PM_SLEEP
+/*
+ * tegra3_sleep_cpu(unsigned long v2p)
+ *
+ * enters suspend in LP2 by turning off the mmu and jumping to
+ * tegra3_tear_down_cpu
+ */
+ENTRY(tegra3_sleep_cpu)
+	mov	r3, lr			@ set resume address to lr
+	bl	tegra_cpu_save
+
+	mov32	r1, tegra3_tear_down_cpu
+	add	r1, r1, r0
+	b	tegra_turn_off_mmu
+ENDPROC(tegra3_sleep_cpu)
+
+/*
+ * tegra3_tear_down_cpu
+ *
+ * Switches the CPU cluster to PLL-P and enters sleep.
+ */
+ENTRY(tegra3_tear_down_cpu)
+	bl	tegra_cpu_pllp
+	b	tegra3_enter_sleep
+ENDPROC(tegra3_tear_down_cpu)
+
+/* START OF ROUTINES COPIED TO IRAM */
+	.align L1_CACHE_SHIFT
+	.globl tegra3_iram_start
+tegra3_iram_start:
+
+/*
+ * tegra3_lp1_reset
+ *
+ * reset vector for LP1 restore; copied into IRAM during suspend.
+ * brings the system back up to a safe starting point (SDRAM out of
+ * self-refresh, PLLC, PLLM and PLLP reenabled, CPU running on PLLP,
+ * system clock running on the same PLL that it suspended at), and
+ * jumps to tegra_lp2_startup to restore PLLX and virtual addressing.
+ * physical address of tegra_lp2_startup expected to be stored in
+ * PMC_SCRATCH41
+ *
+ * NOTE: THIS *MUST* BE RELOCATED TO TEGRA_IRAM_CODE_AREA AND MUST BE FIRST.
+ */
+
+/* !!!FIXME!!! Add LP1/LP1 code */
+
+/*
+ * tegra3_enter_sleep
+ *
+ * uses flow controller to enter sleep state
+ * executes from IRAM with SDRAM in selfrefresh when target state is LP0 or LP1
+ * executes from SDRAM with target state is LP2
+ */
+tegra3_enter_sleep:
+	mov32	r7, TEGRA_TMRUS_BASE
+	ldr	r1, [r7]
+	mov32	r4, TEGRA_PMC_BASE
+	str	r1, [r4, #PMC_SCRATCH38]
+	dsb
+	mov32	r6, TEGRA_FLOW_CTRL_BASE
+	cpu_id	r1
+
+	cpu_to_csr_reg	r2, r1
+	ldr	r0, [r6, r2]
+	orr	r0, r0, #FLOW_CTRL_CSR_INTR_FLAG | FLOW_CTRL_CSR_EVENT_FLAG
+	str	r0, [r6, r2]
+
+	mov	r0, #FLOW_CTRL_WAIT_FOR_INTERRUPT
+	orr	r0, r0, #FLOW_CTRL_HALT_CPU_IRQ | FLOW_CTRL_HALT_CPU_FIQ
+	cpu_to_halt_reg r2, r1
+	str	r0, [r6, r2]
+	dsb
+	ldr	r0, [r6, r2] /* memory barrier */
+
+halted:
+	dsb
+	isb
+	wfi	/* CPU should be power gated here */
+
+	/* !!!FIXME!!! Implement halt failure handler */
+	b	halted
+
+	.ltorg
+/* dummy symbol for end of IRAM */
+	.align L1_CACHE_SHIFT
+	.globl tegra3_iram_end
+tegra3_iram_end:
+	b	.
+#endif
diff --git a/arch/arm/mach-tegra/sleep.S b/arch/arm/mach-tegra/sleep.S
index 915a7b467f52..0c15989a4cd8 100644
--- a/arch/arm/mach-tegra/sleep.S
+++ b/arch/arm/mach-tegra/sleep.S
@@ -187,6 +187,8 @@ ENTRY(tegra_sleep_cpu)
 
 #ifdef CONFIG_ARCH_TEGRA_2x_SOC
 	mov32	r1, tegra2_tear_down_cpu
+#else
+	mov32	r1, tegra3_tear_down_cpu
 #endif
 	add	r1, r1, r0
 	b	tegra_turn_off_mmu
diff --git a/arch/arm/mach-tegra/sleep.h b/arch/arm/mach-tegra/sleep.h
index 5f418255de84..3caefe15f021 100644
--- a/arch/arm/mach-tegra/sleep.h
+++ b/arch/arm/mach-tegra/sleep.h
@@ -108,6 +108,9 @@ void tegra2_sleep_core(unsigned long v2p);
 void tegra2_hotplug_shutdown(void);
 void tegra2_sleep_wfi(unsigned long v2p);
 #else
+extern void tegra3_iram_start;
+extern void tegra3_iram_end;
+void tegra3_sleep_core(unsigned long v2p);
 void tegra3_hotplug_shutdown(void);
 #endif
 
@@ -115,6 +118,8 @@ static inline void *tegra_iram_start(void)
 {
 #ifdef CONFIG_ARCH_TEGRA_2x_SOC
 	return &tegra2_iram_start;
+#else
+	return &tegra3_iram_start;
 #endif
 }
 
@@ -122,6 +127,8 @@ static inline void *tegra_iram_end(void)
 {
 #ifdef CONFIG_ARCH_TEGRA_2x_SOC
 	return &tegra2_iram_end;
+#else
+	return &tegra3_iram_end;
 #endif
 }
 
@@ -129,6 +136,9 @@ static inline void tegra_sleep_core(unsigned long v2p)
 {
 #ifdef CONFIG_ARCH_TEGRA_2x_SOC
 	tegra2_sleep_core(v2p);
+#else
+	/* tegra3_sleep_core(v2p);    !!!FIXME!!! not supported yet */
+	BUG();
 #endif
 }