ARM: tegra2: Move LP2 into cpuidle-t2.c

Move Tegra2 SOC-specific CPU idle functionality to cpuidle-t2.c

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

Rebase-Id: R1246e3942623458f5121ccdac3e6d4a1d40ad624

5 files changed, 160 insertions, 140 deletions

diff --git a/arch/arm/mach-tegra/cpuidle-t2.c b/arch/arm/mach-tegra/cpuidle-t2.c
index 12cfea1cb38f..badeeccf98fa 100644
--- a/arch/arm/mach-tegra/cpuidle-t2.c
+++ b/arch/arm/mach-tegra/cpuidle-t2.c
@@ -60,6 +60,128 @@ static struct {
 	unsigned int last_lp2_int_count[NR_IRQS];
 } idle_stats;
 
+#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 int tegra2_reset_sleeping_cpu(int cpu)
+{
+	int ret = 0;
+
+	BUG_ON(cpu == smp_processor_id());
+	tegra_pen_lock();
+
+	if (readl(pmc + PMC_SCRATCH41) == CPU_RESETTABLE)
+		tegra_cpu_reset(cpu);
+	else
+		ret = -EINVAL;
+
+	tegra_pen_unlock();
+
+	return ret;
+}
+
+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);
+
+	reg = 0x1111 << cpu;
+	writel(reg, clk_rst + 0x344);
+
+	/* unhalt the cpu */
+	flowctrl_writel(0, FLOW_CTRL_HALT_CPU(1));
+}
+
+static int tegra2_reset_other_cpus(int cpu)
+{
+	int i;
+	int abort = -1;
+
+	for_each_online_cpu(i) {
+		if (i != cpu) {
+			if (tegra2_reset_sleeping_cpu(i)) {
+				abort = i;
+				break;
+			}
+		}
+	}
+
+	if (abort >= 0) {
+		for_each_online_cpu(i) {
+			if (i != cpu && i < abort)
+				tegra2_wake_reset_cpu(i);
+		}
+		return -EINVAL;
+	}
+
+	return 0;
+}
+#else
+static void tegra2_wake_reset_cpu(int cpu)
+{
+}
+
+static int tegra2_reset_other_cpus(int cpu)
+{
+	return 0;
+}
+#endif
+
+static int tegra2_idle_lp2_last(struct cpuidle_device *dev,
+			struct cpuidle_state *state)
+{
+	int i;
+
+	while (tegra_cpu_is_resettable_soon())
+		cpu_relax();
+
+	if (tegra2_reset_other_cpus(dev->cpu))
+		return -EBUSY;
+
+	tegra_idle_lp2_last(0);
+
+	for_each_online_cpu(i) {
+		if (i != dev->cpu) {
+			tegra2_wake_reset_cpu(i);
+			tegra_clear_cpu_in_lp2(i);
+		}
+	}
+
+	return 0;
+}
+
+void tegra2_idle_lp2(struct cpuidle_device *dev,
+			struct cpuidle_state *state)
+{
+	bool last_cpu = tegra_set_cpu_in_lp2(dev->cpu);
+
+	cpu_pm_enter();
+
+	if (last_cpu) {
+		if (tegra2_idle_lp2_last(dev, state) < 0) {
+			int i;
+			for_each_online_cpu(i) {
+				if (i != dev->cpu) {
+					tegra2_wake_reset_cpu(i);
+					tegra_clear_cpu_in_lp2(i);
+				}
+			}
+		}
+	} else
+		tegra_sleep_wfi(PLAT_PHYS_OFFSET - PAGE_OFFSET);
+
+	cpu_pm_exit();
+	tegra_clear_cpu_in_lp2(dev->cpu);
+}
+
 void tegra2_cpu_idle_stats_lp2_ready(unsigned int cpu)
 {
 	idle_stats.cpu_ready_count[cpu]++;
diff --git a/arch/arm/mach-tegra/cpuidle.c b/arch/arm/mach-tegra/cpuidle.c
index 519ed7ba8aca..6f70bf3e014b 100644
--- a/arch/arm/mach-tegra/cpuidle.c
+++ b/arch/arm/mach-tegra/cpuidle.c
@@ -103,7 +103,7 @@ static int tegra_idle_enter_lp2(struct cpuidle_device *dev,
 	tegra_cpu_idle_stats_lp2_ready(dev->cpu);
 
 	clockevents_notify(CLOCK_EVT_NOTIFY_BROADCAST_ENTER, &dev->cpu);
-	tegra_idle_lp2();
+	tegra_idle_lp2(dev, state);
 	clockevents_notify(CLOCK_EVT_NOTIFY_BROADCAST_EXIT, &dev->cpu);
 
 	exit = ktime_sub(ktime_get(), enter);
diff --git a/arch/arm/mach-tegra/cpuidle.h b/arch/arm/mach-tegra/cpuidle.h
index f05d71d80ac9..f3f80ad05ba0 100644
--- a/arch/arm/mach-tegra/cpuidle.h
+++ b/arch/arm/mach-tegra/cpuidle.h
@@ -19,7 +19,10 @@
 #ifndef __MACH_TEGRA_CPUIDLE_H
 #define __MACH_TEGRA_CPUIDLE_H
 
+#include <linux/cpuidle.h>
+
 #ifdef CONFIG_ARCH_TEGRA_2x_SOC
+void tegra2_idle_lp2(struct cpuidle_device *dev, struct cpuidle_state *state);
 void tegra2_cpu_idle_stats_lp2_ready(unsigned int cpu);
 void tegra2_cpu_idle_stats_lp2_time(unsigned int cpu, s64 us);
 #ifdef CONFIG_DEBUG_FS
@@ -41,6 +44,14 @@ static inline void tegra_cpu_idle_stats_lp2_time(unsigned int cpu, s64 us)
 #endif
 }
 
+static inline void tegra_idle_lp2(struct cpuidle_device *dev,
+			struct cpuidle_state *state)
+{
+#ifdef CONFIG_ARCH_TEGRA_2x_SOC
+	tegra2_idle_lp2(dev, state);
+#endif
+}
+
 #ifdef CONFIG_DEBUG_FS
 static inline int tegra_lp2_debug_show(struct seq_file *s, void *data)
 {
diff --git a/arch/arm/mach-tegra/pm.c b/arch/arm/mach-tegra/pm.c
index acf020764496..0bb354a2b3b2 100644
--- a/arch/arm/mach-tegra/pm.c
+++ b/arch/arm/mach-tegra/pm.c
@@ -86,6 +86,10 @@ static unsigned long iram_save_size;
 
 struct suspend_context tegra_sctx;
 
+#ifdef CONFIG_PM_SLEEP
+static DEFINE_SPINLOCK(tegra_lp2_lock);
+static cpumask_t tegra_in_lp2;
+#endif
 static void __iomem *iram_code = IO_ADDRESS(TEGRA_IRAM_CODE_AREA);
 static void __iomem *pmc = IO_ADDRESS(TEGRA_PMC_BASE);
 #ifdef CONFIG_PM_SLEEP
@@ -167,9 +171,6 @@ static struct clk *tegra_pclk;
 static const struct tegra_suspend_platform_data *pdata;
 static enum tegra_suspend_mode current_suspend_mode = TEGRA_SUSPEND_NONE;
 
-static DEFINE_SPINLOCK(tegra_lp2_lock);
-static cpumask_t tegra_in_lp2;
-
 static struct kobject *suspend_kobj;
 
 static const char *tegra_suspend_name[TEGRA_MAX_SUSPEND_MODE] = {
@@ -310,51 +311,6 @@ static int create_suspend_pgtable(void)
 	return 0;
 }
 
-#ifdef CONFIG_SMP
-static int tegra_reset_sleeping_cpu(int cpu)
-{
-	int ret = 0;
-
-	BUG_ON(cpu == smp_processor_id());
-	tegra_pen_lock();
-
-	if (readl(pmc + PMC_SCRATCH41) == CPU_RESETTABLE)
-		tegra_cpu_reset(cpu);
-	else
-		ret = -EINVAL;
-
-	tegra_pen_unlock();
-
-	return ret;
-}
-
-static void tegra_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);
-
-	reg = 0x1111 << cpu;
-	writel(reg, clk_rst + 0x344);
-
-	/* unhalt the cpu */
-	flowctrl_writel(0, FLOW_CTRL_HALT_CPU(1));
-}
-#else
-static int tegra_reset_sleeping_cpu(int cpu)
-{
-	return 0;
-}
-
-static void tegra_wake_reset_cpu(int cpu)
-{
-}
-#endif
-
 #ifdef CONFIG_PM_SLEEP
 /*
  * restore_cpu_complex
@@ -473,50 +429,34 @@ static void suspend_cpu_complex(void)
 	}
 }
 
-#ifdef CONFIG_SMP
-int tegra_reset_other_cpus(int cpu)
+void tegra_clear_cpu_in_lp2(int cpu)
 {
-	int i;
-	int abort = -1;
-
-	for_each_online_cpu(i) {
-		if (i != cpu) {
-			if (tegra_reset_sleeping_cpu(i)) {
-				abort = i;
-				break;
-			}
-		}
-	}
-
-	if (abort >= 0) {
-		for_each_online_cpu(i) {
-			if (i != cpu && i < abort)
-				tegra_wake_reset_cpu(i);
-		}
-		return -EINVAL;
-	}
-
-	return 0;
+	spin_lock(&tegra_lp2_lock);
+	cpumask_clear_cpu(cpu, &tegra_in_lp2);
+	spin_unlock(&tegra_lp2_lock);
 }
-#else
-int tegra_reset_other_cpus(int cpu)
+
+bool tegra_set_cpu_in_lp2(int cpu)
 {
-	return 0;
-}
+	bool last_cpu = false;
+
+	spin_lock(&tegra_lp2_lock);
+
+	cpumask_set_cpu(cpu, &tegra_in_lp2);
+	if (cpumask_equal(&tegra_in_lp2, cpu_online_mask))
+		last_cpu = true;
+#ifdef CONFIG_ARCH_TEGRA_2x_SOC
+	else
+		tegra_cpu_set_resettable_soon();
 #endif
 
-#ifdef CONFIG_SMP
+	spin_unlock(&tegra_lp2_lock);
+	return last_cpu;
+}
+
 void tegra_idle_lp2_last(unsigned int flags)
 {
 	u32 reg;
-	int i;
-	int cpu = smp_processor_id();
-
-	while (tegra_cpu_is_resettable_soon())
-		cpu_relax();
-
-	if (tegra_reset_other_cpus(cpu))
-		return;
 
 	/* Only the last cpu down does the final suspend steps */
 	reg = readl(pmc + PMC_CTRL);
@@ -558,10 +498,6 @@ void tegra_idle_lp2_last(unsigned int flags)
 	if (flags & TEGRA_POWER_CLUSTER_MASK)
 		tegra_cluster_switch_epilog(reg);
 
-	for_each_online_cpu(i)
-		if (i != cpu)
-			tegra_wake_reset_cpu(i);
-
 	tegra_cluster_switch_time(flags, tegra_cluster_switch_time_id_epilog);
 
 #if INSTRUMENT_CLUSTER_SWITCH
@@ -579,55 +515,6 @@ void tegra_idle_lp2_last(unsigned int flags)
 	}
 #endif
 }
-#else
-void tegra_idle_lp2_last(unsigned int flags)
-{
-}
-#endif
-
-void tegra_idle_lp2(void)
-{
-	bool last_cpu = false;
-	int cpu = smp_processor_id();
-
-	spin_lock(&tegra_lp2_lock);
-
-	cpumask_set_cpu(cpu, &tegra_in_lp2);
-	if (cpumask_equal(&tegra_in_lp2, cpu_online_mask))
-		last_cpu = true;
-	else
-		tegra_cpu_set_resettable_soon();
-
-	spin_unlock(&tegra_lp2_lock);
-
-	cpu_pm_enter();
-
-#ifdef CONFIG_SMP
-	if (last_cpu)
-		tegra_idle_lp2_last(0);
-	else
-#endif
-		tegra_sleep_wfi(PLAT_PHYS_OFFSET - PAGE_OFFSET);
-
-	cpu_pm_exit();
-
-	spin_lock(&tegra_lp2_lock);
-	cpumask_clear_cpu(cpu, &tegra_in_lp2);
-
-	/*
-	 * cpus coming out of idle muck with page tables that belong to the
-	 * last process executed before idle.  Don't release any cpus back to
-	 * the scheduler until all cpus have booted to avoid modifying the
-	 * page table of a running process on another cpu.
-	 */
-	while (!cpumask_empty(&tegra_in_lp2)) {
-		spin_unlock(&tegra_lp2_lock);
-		cpu_relax();
-		spin_lock(&tegra_lp2_lock);
-	}
-
-	spin_unlock(&tegra_lp2_lock);
-}
 
 static int tegra_common_suspend(void)
 {
diff --git a/arch/arm/mach-tegra/pm.h b/arch/arm/mach-tegra/pm.h
index a8f991d1a756..8729f1260201 100644
--- a/arch/arm/mach-tegra/pm.h
+++ b/arch/arm/mach-tegra/pm.h
@@ -46,6 +46,8 @@ struct tegra_suspend_platform_data {
 unsigned long tegra_cpu_power_good_time(void);
 unsigned long tegra_cpu_power_off_time(void);
 unsigned long tegra_cpu_lp2_min_residency(void);
+void tegra_clear_cpu_in_lp2(int cpu);
+bool tegra_set_cpu_in_lp2(int cpu);
 
 int tegra_suspend_dram(enum tegra_suspend_mode mode);
 
@@ -81,8 +83,6 @@ static inline void tegra2_lp0_suspend_init(void)
 #endif
 void __init tegra_init_suspend(struct tegra_suspend_platform_data *plat);
 
-void tegra_idle_lp2(void);
-
 unsigned int tegra_count_slow_cpus(unsigned long speed_limit);
 unsigned int tegra_get_slowest_cpu_n(void);
 unsigned long tegra_cpu_lowest_speed(void);