arm: tegra3: Add SMP CPU idle support

Original-Change-Id: I96edd2706d959f04e9bc5cd4a841d9a582fbd469
Reviewed-on: http://git-master/r/14575
Reviewed-by: Scott Williams <scwilliams@nvidia.com>
Tested-by: Scott Williams <scwilliams@nvidia.com>
Change-Id: I3d49abf93947d09af9bc6faf2b15675303723515

9 files changed, 147 insertions, 36 deletions

diff --git a/arch/arm/common/gic.c b/arch/arm/common/gic.c
index 48a70afca283..54a6ed5592ea 100644
--- a/arch/arm/common/gic.c
+++ b/arch/arm/common/gic.c
@@ -388,6 +388,14 @@ void gic_dist_exit(unsigned int gic_nr)
 	_gic_dist_exit(gic_nr);
 }
 
+void gic_dist_enable(unsigned int gic_nr)
+{
+	if (gic_nr >= MAX_GIC_NR)
+		BUG();
+
+	writel(1, gic_data[gic_nr].dist_base + GIC_DIST_CTRL);
+}
+
 void __cpuinit gic_cpu_init(unsigned int gic_nr, void __iomem *base)
 {
 	if (gic_nr >= MAX_GIC_NR)
diff --git a/arch/arm/include/asm/hardware/gic.h b/arch/arm/include/asm/hardware/gic.h
index 5fd0ff2bb821..f61b9daa6b4b 100644
--- a/arch/arm/include/asm/hardware/gic.h
+++ b/arch/arm/include/asm/hardware/gic.h
@@ -37,6 +37,7 @@ void gic_dist_init(unsigned int gic_nr, void __iomem *base, unsigned int irq_sta
 void gic_dist_save(unsigned int gic_nr);
 void gic_dist_restore(unsigned int gic_nr);
 void gic_dist_exit(unsigned int gic_nr);
+void gic_dist_enable(unsigned int gic_nr);
 void gic_cpu_init(unsigned int gic_nr, void __iomem *base);
 void gic_cpu_exit(unsigned int gic_nr);
 void gic_cascade_irq(unsigned int gic_nr, unsigned int irq);
diff --git a/arch/arm/mach-tegra/cpuidle-t3.c b/arch/arm/mach-tegra/cpuidle-t3.c
index 560f683a65fd..1c74765e7d57 100644
--- a/arch/arm/mach-tegra/cpuidle-t3.c
+++ b/arch/arm/mach-tegra/cpuidle-t3.c
@@ -101,6 +101,18 @@ void tegra_idle_stats_lp2_time(unsigned int cpu, s64 us)
 	idle_stats.cpu_wants_lp2_time[cpu_number(cpu)] += us;
 }
 
+bool tegra_lp2_is_allowed(struct cpuidle_device *dev,
+	struct cpuidle_state *state)
+{
+	if (dev->cpu == 0) {
+		u32 reg = readl(CLK_RST_CONTROLLER_CPU_CMPLX_STATUS);
+		if ((reg & 0xE) != 0xE) {
+			return false;
+		}
+	}
+	return true;
+}
+
 void tegra_idle_enter_lp2_cpu_0(struct cpuidle_device *dev,
 	struct cpuidle_state *state)
 {
@@ -124,14 +136,37 @@ void tegra_idle_enter_lp2_cpu_0(struct cpuidle_device *dev,
 	enter = ktime_get();
 
 #ifdef CONFIG_SMP
-	{
-		int i;
+	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. */
+		gic_dist_exit(0);
+
+		/* Did an interrupt come in for another CPU before we
+		   could disable the distributor? */
+		if (!tegra_lp2_is_allowed(dev, state)) {
+			/* Yes, re-enable the distributor and LP3. */
+			gic_dist_enable(0);
+			tegra_flow_wfi(dev);
+			return;
+		}
+
+		/* Save and disable the affinity setting for the other
+		   CPUs and route all interrupts to CPU0. */
+		tegra_irq_disable_affinity();
+
+		/* Re-enable the distributor. */
+		gic_dist_enable(0);
 
 		/* LP2 initial targeted wake time */
-		s64 wake_time = ktime_to_us(enter) + request;
+		wake_time = ktime_to_us(enter) + request;
 
-		smp_rmb();
 		/* 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]);
@@ -157,9 +192,18 @@ void tegra_idle_enter_lp2_cpu_0(struct cpuidle_device *dev,
 	}
 
 #ifdef CONFIG_SMP
-	{
+	if (!is_lp_cluster() && (num_online_cpus() > 1)) {
 		int i;
 
+		/* Disable the distributor. */
+		gic_dist_exit(0);
+
+		/* Restore the other CPU's interrupt affinity. */
+		tegra_irq_restore_affinity();
+
+		/* Re-enable the distributor. */
+		gic_dist_enable(0);
+
 		/* Start the other CPUs if they were online. */
 		smp_wmb();
 		for (i = 1; i < CONFIG_NR_CPUS; i++) {
diff --git a/arch/arm/mach-tegra/cpuidle.c b/arch/arm/mach-tegra/cpuidle.c
index 5383e437fefb..6aa6ef89d571 100644
--- a/arch/arm/mach-tegra/cpuidle.c
+++ b/arch/arm/mach-tegra/cpuidle.c
@@ -106,18 +106,10 @@ static int tegra_idle_enter_lp2(struct cpuidle_device *dev,
 	ktime_t enter, exit;
 	s64 us;
 
-	if (!lp2_in_idle || lp2_disabled_by_suspend)
+	if (!lp2_in_idle || lp2_disabled_by_suspend ||
+	    !tegra_lp2_is_allowed(dev, state))
 		return tegra_idle_enter_lp3(dev, state);
 
-#if defined(CONFIG_SMP) && !defined(CONFIG_ARCH_TEGRA_2x_SOC)
-	if (dev->cpu == 0) {
-		u32 reg = readl(CLK_RST_CONTROLLER_CPU_CMPLX_STATUS);
-		if ((reg & 0xE) != 0xE) {
-			return tegra_idle_enter_lp3(dev, state);
-		}
-	}
-#endif
-
 	local_irq_disable();
 	clockevents_notify(CLOCK_EVT_NOTIFY_BROADCAST_ENTER, &dev->cpu);
 	local_fiq_disable();
diff --git a/arch/arm/mach-tegra/include/mach/irqs.h b/arch/arm/mach-tegra/include/mach/irqs.h
index 58e07d7cc528..ff3be56298b8 100644
--- a/arch/arm/mach-tegra/include/mach/irqs.h
+++ b/arch/arm/mach-tegra/include/mach/irqs.h
@@ -23,7 +23,7 @@
 
 #define INT_GIC_BASE			0
 
-#define IRQ_LOCALTIMER                  29
+#define IRQ_LOCALTIMER			29
 
 #ifdef CONFIG_ARCH_TEGRA_2x_SOC
 /* Primary Interrupt Controller */
@@ -167,7 +167,9 @@
 #define INT_QUAD_RES_30			(INT_QUAD_BASE + 30)
 #define INT_QUAD_RES_31			(INT_QUAD_BASE + 31)
 
-#define INT_MAIN_NR			(INT_QUAD_BASE + 32 - INT_PRI_BASE)
+#define INT_GIC_NR			(INT_QUAD_BASE + 32)
+
+#define INT_MAIN_NR			(INT_GIC_NR - INT_PRI_BASE)
 
 #define INT_SYNCPT_THRESH_BASE		(INT_QUAD_BASE + 32)
 #define INT_SYNCPT_THRESH_NR		32
@@ -353,7 +355,9 @@
 #define INT_QUINT_RES_30		(INT_QUINT_BASE + 30)
 #define INT_QUINT_RES_31		(INT_QUINT_BASE + 31)
 
-#define INT_MAIN_NR			(INT_QUINT_BASE + 32 - INT_PRI_BASE)
+#define INT_GIC_NR			(INT_QUINT_BASE + 32)
+
+#define INT_MAIN_NR			(INT_GIC_NR - INT_PRI_BASE)
 
 #define INT_SYNCPT_THRESH_BASE		(INT_QUINT_BASE + 32)
 #define INT_SYNCPT_THRESH_NR		32
@@ -372,6 +376,6 @@
 
 #define NR_BOARD_IRQS			32
 
-#define NR_IRQS				(INT_BOARD_BASE + NR_BOARD_IRQS)
+#define NR_IRQS			(INT_BOARD_BASE + NR_BOARD_IRQS)
 
 #endif
diff --git a/arch/arm/mach-tegra/include/mach/suspend.h b/arch/arm/mach-tegra/include/mach/suspend.h
index 056d91b0a57c..d33bc560d2b7 100644
--- a/arch/arm/mach-tegra/include/mach/suspend.h
+++ b/arch/arm/mach-tegra/include/mach/suspend.h
@@ -70,6 +70,12 @@ void tegra_timer_resume(void);
 int tegra_irq_to_wake(int irq);
 int tegra_wake_to_irq(int wake);
 
+#ifndef CONFIG_ARCH_TEGRA_2x_SOC
+void tegra_irq_disable_affinity(void);
+void tegra_irq_affinity_to_cpu0(void);
+void tegra_irq_restore_affinity(void);
+#endif
+
 int tegra_set_lp0_wake(int irq, int enable);
 int tegra_set_lp0_wake_type(int irq, int flow_type);
 int tegra_set_lp1_wake(int irq, int enable);
diff --git a/arch/arm/mach-tegra/irq.c b/arch/arm/mach-tegra/irq.c
index 102cf0c5d7f7..60af07af913b 100644
--- a/arch/arm/mach-tegra/irq.c
+++ b/arch/arm/mach-tegra/irq.c
@@ -33,12 +33,13 @@
 #include <mach/suspend.h>
 
 #include "board.h"
+#include "power.h"
 
 #define PMC_CTRL		0x0
 #define PMC_CTRL_LATCH_WAKEUPS	(1 << 5)
 #define PMC_WAKE_MASK		0xc
 #define PMC_WAKE_LEVEL		0x10
-#define PMC_WAKE_STATUS		0x14
+#define PMC_WAKE_STATUS	0x14
 #define PMC_SW_WAKE_STATUS	0x18
 #define PMC_DPD_SAMPLE  	0x20
 
@@ -268,6 +269,63 @@ void tegra_irq_resume(void)
 }
 #endif
 
+#ifndef CONFIG_ARCH_TEGRA_2x_SOC
+
+static u32 gic_affinity[INT_GIC_NR/4];
+
+void tegra_irq_disable_affinity(void)
+{
+	void __iomem *gic_base = IO_ADDRESS(TEGRA_ARM_INT_DIST_BASE);
+	unsigned int i;
+
+	BUG_ON(is_lp_cluster());
+
+       /* The GIC distributor TARGET register is one byte per IRQ. */
+	for (i = 32; i < INT_GIC_NR; i += 4) {
+		/* Save the affinity. */
+		gic_affinity[i/4] = __raw_readl(gic_base + GIC_DIST_TARGET + i);
+
+		/* Force this interrupt to CPU0. */
+		__raw_writel(0x01010101, gic_base + GIC_DIST_TARGET + i);
+	}
+
+       wmb();
+}
+
+void tegra_irq_restore_affinity(void)
+{
+	void __iomem *gic_base = IO_ADDRESS(TEGRA_ARM_INT_DIST_BASE);
+	unsigned int i;
+
+	BUG_ON(is_lp_cluster());
+
+       /* The GIC distributor TARGET register is one byte per IRQ. */
+	for (i = 32; i < INT_GIC_NR; i += 4) {
+#ifdef CONFIG_BUG
+		u32 reg = __raw_readl(gic_base + GIC_DIST_TARGET + i);
+		if (reg & 0xFEFEFEFE)
+			panic("GIC affinity changed!");
+#endif
+		/* Restore this interrupt's affinity. */
+		__raw_writel(gic_affinity[i/4], gic_base + GIC_DIST_TARGET + i);
+	}
+
+       wmb();
+}
+
+void tegra_irq_affinity_to_cpu0(void)
+{
+	void __iomem *gic_base = IO_ADDRESS(TEGRA_ARM_INT_DIST_BASE);
+	unsigned int i;
+
+	BUG_ON(is_lp_cluster());
+
+	for (i = 32; i < INT_GIC_NR; i += 4)
+		__raw_writel(0x01010101, gic_base + GIC_DIST_TARGET + i);
+       wmb();
+}
+#endif
+
 #ifdef CONFIG_DEBUG_FS
 static int tegra_wake_irq_debug_show(struct seq_file *s, void *data)
 {
diff --git a/arch/arm/mach-tegra/power.h b/arch/arm/mach-tegra/power.h
index d470ce662759..663e50b3b2fd 100644
--- a/arch/arm/mach-tegra/power.h
+++ b/arch/arm/mach-tegra/power.h
@@ -44,18 +44,18 @@
 #define TEGRA_POWER_CLUSTER_FORCE	0x8000	/* Force switch */
 
 /* CPU Context area (1KB per CPU) */
-#define CONTEXT_SIZE_BYTES_SHIFT 10
-#define CONTEXT_SIZE_BYTES (1<<CONTEXT_SIZE_BYTES_SHIFT)
+#define CONTEXT_SIZE_BYTES_SHIFT	10
+#define CONTEXT_SIZE_BYTES		(1<<CONTEXT_SIZE_BYTES_SHIFT)
 
 /* CPU Context area (1KB per CPU) */
-#define CONTEXT_SIZE_BYTES_SHIFT 10
-#define CONTEXT_SIZE_BYTES (1<<CONTEXT_SIZE_BYTES_SHIFT)
+#define CONTEXT_SIZE_BYTES_SHIFT	10
+#define CONTEXT_SIZE_BYTES		(1<<CONTEXT_SIZE_BYTES_SHIFT)
 
 /* layout of IRAM used for LP1 save & restore */
 #define TEGRA_IRAM_CODE_AREA		TEGRA_IRAM_BASE + SZ_4K
 #define TEGRA_IRAM_CODE_SIZE		SZ_4K
 
-#define CLK_RESET_CLK_MASK_ARM 0x44
+#define CLK_RESET_CLK_MASK_ARM		0x44
 
 #define FLOW_CTRL_WAITEVENT		(2<<29)
 #define FLOW_CTRL_WAIT_FOR_INTERRUPT	(4<<29)
@@ -81,8 +81,8 @@
 
 #ifndef __ASSEMBLY__
 
-#define FLOW_CTRL_HALT_CPUx_EVENTS(cpu) ((cpu)?((cpu-1)*0x8 + 0x14) : 0x0)
-#define FLOW_CTRL_CPUx_CSR(cpu)	 ((cpu)?(((cpu)-1)*0x8 + 0x18) : 0x8)
+#define FLOW_CTRL_HALT_CPUx_EVENTS(cpu)	((cpu)?(((cpu)-1)*0x8 + 0x14) : 0x0)
+#define FLOW_CTRL_CPUx_CSR(cpu)		((cpu)?(((cpu)-1)*0x8 + 0x18) : 0x8)
 
 static inline void flowctrl_writel(unsigned long val, unsigned int offs)
 {
@@ -122,6 +122,9 @@ static inline unsigned int is_lp_cluster(void)
 static inline unsigned long tegra_get_lpcpu_max_rate(void)
 { return 0; }
 int tegra_cpudile_init_soc(void);
+static inline bool tegra_lp2_is_allowed(struct cpuidle_device *dev,
+	struct cpuidle_state *state)
+{ return true; }
 #else
 int tegra_cluster_control(unsigned int us, unsigned int flags);
 void tegra_cluster_switch_prolog(unsigned int flags);
@@ -130,6 +133,8 @@ unsigned int is_lp_cluster(void);
 unsigned long tegra_get_lpcpu_max_rate(void);
 static inline int tegra_cpudile_init_soc(void)
 { return 0; }
+bool tegra_lp2_is_allowed(struct cpuidle_device *dev,
+	struct cpuidle_state *state);
 #endif
 #endif
 
diff --git a/arch/arm/mach-tegra/suspend-t3.c b/arch/arm/mach-tegra/suspend-t3.c
index 25461c33b531..4889c2adf046 100644
--- a/arch/arm/mach-tegra/suspend-t3.c
+++ b/arch/arm/mach-tegra/suspend-t3.c
@@ -30,6 +30,7 @@
 #include <mach/gpio.h>
 #include <mach/iomap.h>
 #include <mach/irqs.h>
+#include <mach/suspend.h>
 #include <asm/hardware/gic.h>
 
 #include "clock.h"
@@ -267,9 +268,6 @@ done:
 
 static void cluster_switch_epilog_gic(void)
 {
-	unsigned int max_irq, i;
-	void __iomem *gic_base = IO_ADDRESS(TEGRA_ARM_INT_DIST_BASE);
-
 	/* Nothing to do if currently running on the LP CPU. */
 	if (is_lp_cluster())
 		return;
@@ -281,12 +279,7 @@ static void cluster_switch_epilog_gic(void)
 	   as all zero. This causes all interrupts to be effectively
 	   disabled when back on the G CPU because they aren't routable
 	   to any CPU. See bug 667720 for details. */
-
-	max_irq = readl(gic_base + GIC_DIST_CTR) & 0x1f;
-	max_irq = (max_irq + 1) * 32;
-
-	for (i = 32; i < max_irq; i += 4)
-		writel(0x01010101, gic_base + GIC_DIST_TARGET + i * 4 / 4);
+	tegra_irq_affinity_to_cpu0();
 }
 
 void tegra_cluster_switch_epilog(unsigned int flags)