1 files changed, 40 insertions, 4 deletions

diff --git a/arch/arm/mach-tegra/cpuidle-tegra30.c b/arch/arm/mach-tegra/cpuidle-tegra30.c
index cc48d7fa3358..5e8cbf5b799f 100644
--- a/arch/arm/mach-tegra/cpuidle-tegra30.c
+++ b/arch/arm/mach-tegra/cpuidle-tegra30.c
@@ -32,6 +32,7 @@
 
 #include "pm.h"
 #include "sleep.h"
+#include "tegra_cpu_car.h"
 
 #ifdef CONFIG_PM_SLEEP
 static int tegra30_idle_lp2(struct cpuidle_device *dev,
@@ -67,6 +68,31 @@ static struct cpuidle_driver tegra_idle_driver = {
 static DEFINE_PER_CPU(struct cpuidle_device, tegra_idle_device);
 
 #ifdef CONFIG_PM_SLEEP
+static bool tegra30_cpu_cluster_power_down(struct cpuidle_device *dev,
+					   struct cpuidle_driver *drv,
+					   int index)
+{
+	struct cpuidle_state *state = &drv->states[index];
+	u32 cpu_on_time = state->exit_latency;
+	u32 cpu_off_time = state->target_residency - state->exit_latency;
+
+	/* All CPUs entering LP2 is not working.
+	 * Don't let CPU0 enter LP2 when any secondary CPU is online.
+	 */
+	if (num_online_cpus() > 1 || !tegra_cpu_rail_off_ready()) {
+		cpu_do_idle();
+		return false;
+	}
+
+	clockevents_notify(CLOCK_EVT_NOTIFY_BROADCAST_ENTER, &dev->cpu);
+
+	tegra_idle_lp2_last(cpu_on_time, cpu_off_time);
+
+	clockevents_notify(CLOCK_EVT_NOTIFY_BROADCAST_EXIT, &dev->cpu);
+
+	return true;
+}
+
 #ifdef CONFIG_SMP
 static bool tegra30_cpu_core_power_down(struct cpuidle_device *dev,
 					struct cpuidle_driver *drv,
@@ -101,16 +127,22 @@ static int __cpuinit tegra30_idle_lp2(struct cpuidle_device *dev,
 {
 	u32 cpu = is_smp() ? cpu_logical_map(dev->cpu) : dev->cpu;
 	bool entered_lp2 = false;
+	bool last_cpu;
 
 	local_fiq_disable();
 
-	tegra_set_cpu_in_lp2(cpu);
+	last_cpu = tegra_set_cpu_in_lp2(cpu);
 	cpu_pm_enter();
 
-	if (cpu == 0)
-		cpu_do_idle();
-	else
+	if (cpu == 0) {
+		if (last_cpu)
+			entered_lp2 = tegra30_cpu_cluster_power_down(dev, drv,
+								     index);
+		else
+			cpu_do_idle();
+	} else {
 		entered_lp2 = tegra30_cpu_core_power_down(dev, drv, index);
+	}
 
 	cpu_pm_exit();
 	tegra_clear_cpu_in_lp2(cpu);
@@ -130,6 +162,10 @@ int __init tegra30_cpuidle_init(void)
 	struct cpuidle_device *dev;
 	struct cpuidle_driver *drv = &tegra_idle_driver;
 
+#ifdef CONFIG_PM_SLEEP
+	tegra_tear_down_cpu = tegra30_tear_down_cpu;
+#endif
+
 	ret = cpuidle_register_driver(&tegra_idle_driver);
 	if (ret) {
 		pr_err("CPUidle driver registration failed\n");