3 files changed, 26 insertions, 4 deletions
diff --git a/arch/arm/mach-tegra/pm-t3.c b/arch/arm/mach-tegra/pm-t3.c
index 93b5e0081e8b..ad129a49d59f 100644
--- a/arch/arm/mach-tegra/pm-t3.c
+++ b/arch/arm/mach-tegra/pm-t3.c
@@ -279,6 +279,7 @@ int tegra_cluster_control(unsigned int us, unsigned int flags)
 	unsigned int current_cluster = is_lp_cluster()
 					? TEGRA_POWER_CLUSTER_LP
 					: TEGRA_POWER_CLUSTER_G;
+	unsigned long irq_flags;
 
 	if ((target_cluster == TEGRA_POWER_CLUSTER_MASK) || !target_cluster)
 		return -EINVAL;
@@ -305,7 +306,7 @@ int tegra_cluster_control(unsigned int us, unsigned int flags)
 		(flags & TEGRA_POWER_CLUSTER_FORCE) ? "force" : "",
 	        us));
 
-	local_irq_disable();
+	local_irq_save(irq_flags);
 	if (flags & TEGRA_POWER_SDRAM_SELFREFRESH) {
 		if (us)
 			tegra_lp2_set_trigger(us);
@@ -321,7 +322,7 @@ int tegra_cluster_control(unsigned int us, unsigned int flags)
 		cpu_pm_exit();
 		tegra_clear_cpu_in_lp2(0);
 	}
-	local_irq_enable();
+	local_irq_restore(irq_flags);
 
 	DEBUG_CLUSTER(("%s: %s\r\n", __func__, is_lp_cluster() ? "LP" : "G"));
 
@@ -347,4 +348,19 @@ void tegra_lp0_resume_mc(void)
 	writel(mc_reserved_rsv, mc + MC_RESERVED_RSV);
 	writel(mc_emem_arb_override, mc + MC_EMEM_ARB_OVERRIDE);
 }
+
+void tegra_lp0_cpu_mode(bool enter)
+{
+	static bool entered_on_g = false;
+	unsigned int flags;
+
+	if (enter)
+		entered_on_g = !is_lp_cluster();
+
+	if (entered_on_g) {
+		flags = enter ? TEGRA_POWER_CLUSTER_LP : TEGRA_POWER_CLUSTER_G;
+		flags |= TEGRA_POWER_CLUSTER_IMMEDIATE;
+		tegra_cluster_control(0, flags);
+	}
+}
 #endif
diff --git a/arch/arm/mach-tegra/pm.c b/arch/arm/mach-tegra/pm.c
index a34ded6461d5..93347f9452f2 100644
--- a/arch/arm/mach-tegra/pm.c
+++ b/arch/arm/mach-tegra/pm.c
@@ -737,8 +737,10 @@ int tegra_suspend_dram(enum tegra_suspend_mode mode)
 	cpu_pm_enter();
 	cpu_complex_pm_enter();
 
-	if (mode == TEGRA_SUSPEND_LP0)
+	if (mode == TEGRA_SUSPEND_LP0) {
+		tegra_lp0_cpu_mode(true);
 		tegra_lp0_suspend_mc();
+	}
 
 	suspend_cpu_complex(0);
 	flush_cache_all();
@@ -752,8 +754,10 @@ int tegra_suspend_dram(enum tegra_suspend_mode mode)
 
 	tegra_init_cache();
 
-	if (mode == TEGRA_SUSPEND_LP0)
+	if (mode == TEGRA_SUSPEND_LP0) {
 		tegra_lp0_resume_mc();
+		tegra_lp0_cpu_mode(false);
+	}
 
 	restore_cpu_complex(0);
 
diff --git a/arch/arm/mach-tegra/pm.h b/arch/arm/mach-tegra/pm.h
index 87f248efa3ea..778a67054672 100644
--- a/arch/arm/mach-tegra/pm.h
+++ b/arch/arm/mach-tegra/pm.h
@@ -105,6 +105,7 @@ void tegra_cluster_switch_prolog(unsigned int flags);
 void tegra_cluster_switch_epilog(unsigned int flags);
 void tegra_lp0_suspend_mc(void);
 void tegra_lp0_resume_mc(void);
+void tegra_lp0_cpu_mode(bool enter);
 #else
 static inline int tegra_cluster_control(unsigned int us, unsigned int flags)
 #define INSTRUMENT_CLUSTER_SWITCH 0	/* Must be zero for ARCH_TEGRA_2x_SOC */
@@ -117,6 +118,7 @@ static inline void tegra_cluster_switch_prolog(unsigned int flags) {}
 static inline void tegra_cluster_switch_epilog(unsigned int flags) {}
 static inline void tegra_lp0_suspend_mc(void) {}
 static inline void tegra_lp0_resume_mc(void) {}
+static inline void tegra_lp0_cpu_mode(bool enter) {}
 #endif
 
 #ifdef CONFIG_ARCH_TEGRA_2x_SOC