HACK: ARM: omap4: retrigger localtimers after re-enabling gic

commit 09391f5c332bb1c521141a34f3f8687cf93a7125
(OMAP4460: Workaround for ROM bug because of CA9 r2pX gic control
register change) disables the gic distributor while the secondary
cpu is being booted.  If a localtimer interrupt on the primary cpu
occurs when the distributor is turned off, the interrupt is lost,
and the localtimer never fires again.

Make the primary cpu wait for the secondary cpu to reenable the
gic distributor (with interrupts off for safety), and then
check if the pending bit is set in the localtimer but not the
gic.  If so, ack it in the localtimer, and reset the timer with
the minimum timeout to trigger a new timer interrupt.

Signed-off-by: Colin Cross <ccross@android.com>
[s-jan@ti.com: adapted to k3.4 + validated functionality]
Signed-off-by: Sebastien Jan <s-jan@ti.com>

5 files changed, 49 insertions, 2 deletions

diff --git a/arch/arm/include/asm/smp_twd.h b/arch/arm/include/asm/smp_twd.h
index c9df3d2746d9..c17fe6b295c4 100644
--- a/arch/arm/include/asm/smp_twd.h
+++ b/arch/arm/include/asm/smp_twd.h
@@ -27,6 +27,8 @@ struct twd_local_timer {
 	struct resource	res[2];
 };
 
+extern void __iomem *twd_base;
+
 #define DEFINE_TWD_LOCAL_TIMER(name,base,irq)	\
 struct twd_local_timer name __initdata = {	\
 	.res	= {				\
diff --git a/arch/arm/kernel/smp_twd.c b/arch/arm/kernel/smp_twd.c
index 562c63063c37..4a0c599c94b3 100644
--- a/arch/arm/kernel/smp_twd.c
+++ b/arch/arm/kernel/smp_twd.c
@@ -30,7 +30,7 @@
 #include <asm/localtimer.h>
 
 /* set up by the platform code */
-static void __iomem *twd_base;
+void __iomem *twd_base;
 
 static struct clk *twd_clk;
 static unsigned long twd_timer_rate;
diff --git a/arch/arm/mach-omap2/common.h b/arch/arm/mach-omap2/common.h
index 29542307ee79..e705d3abaaa0 100644
--- a/arch/arm/mach-omap2/common.h
+++ b/arch/arm/mach-omap2/common.h
@@ -245,8 +245,10 @@ extern int omap_hotplug_cpu(unsigned int cpu, unsigned int power_state);
 extern u32 omap_mpuss_read_prev_context_state(void);
 extern u32 omap_mpuss_read_prev_context_state(void);                            
 extern void omap_mpuss_timer_init(void);
+extern bool gic_dist_disabled(void);
 extern void gic_dist_enable(void);
 extern void gic_dist_disable(void);
+extern void gic_timer_retrigger(void);
 extern u32 gic_readl(u32 offset, u8 idx);
 #else
 static inline int omap_enter_lowpower(unsigned int cpu,
diff --git a/arch/arm/mach-omap2/omap-smp.c b/arch/arm/mach-omap2/omap-smp.c
index 7839c956e3ac..c6c685be8ff7 100644
--- a/arch/arm/mach-omap2/omap-smp.c
+++ b/arch/arm/mach-omap2/omap-smp.c
@@ -17,7 +17,9 @@
  */
 #include <linux/init.h>
 #include <linux/device.h>
+#include <linux/delay.h>
 #include <linux/smp.h>
+#include <linux/hrtimer.h>
 #include <linux/io.h>
 
 #include <asm/cacheflush.h>
@@ -150,11 +152,23 @@ int __cpuinit boot_secondary(unsigned int cpu, struct task_struct *idle)
 		 * 2) CPU1 must re-enable the GIC distributor on
 		 * it's wakeup path.
 		 */
-		if (omap4_smp_romcode_errata)
+		if (omap4_smp_romcode_errata) {
+			local_irq_disable();
 			gic_dist_disable();
+		}
 
 		clkdm_wakeup(cpu1_clkdm);
 		clkdm_allow_idle(cpu1_clkdm);
+
+		if (omap4_smp_romcode_errata) {
+			while (gic_dist_disabled()) {
+				udelay(1);
+				cpu_relax();
+			}
+			gic_timer_retrigger();
+			local_irq_enable();
+		}
+
 	} else {
 		dsb_sev();
 		booted = true;
diff --git a/arch/arm/mach-omap2/omap4-common.c b/arch/arm/mach-omap2/omap4-common.c
index 691ee96b34c1..42a9a91789c1 100644
--- a/arch/arm/mach-omap2/omap4-common.c
+++ b/arch/arm/mach-omap2/omap4-common.c
@@ -14,6 +14,7 @@
 #include <linux/kernel.h>
 #include <linux/init.h>
 #include <linux/io.h>
+#include <linux/irq.h>
 #include <linux/platform_device.h>
 #include <linux/memblock.h>
 
@@ -21,6 +22,7 @@
 #include <asm/hardware/cache-l2x0.h>
 #include <asm/mach/map.h>
 #include <asm/memblock.h>
+#include <asm/smp_twd.h>
 
 #include <plat/irqs.h>
 #include <plat/sram.h>
@@ -125,11 +127,38 @@ u32 gic_readl(u32 offset, u8 idx)
 	return __raw_readl(gic_dist_base_addr + offset + 4 * idx);
 }
 
+
+bool gic_dist_disabled(void)
+{
+	return !(__raw_readl(gic_dist_base_addr + GIC_DIST_CTRL) & 0x1);
+}
+
 void gic_dist_enable(void)
 {
 	__raw_writel(0x1, gic_dist_base_addr + GIC_DIST_CTRL);
 }
 
+void gic_timer_retrigger(void)
+{
+	u32 twd_int = __raw_readl(twd_base + TWD_TIMER_INTSTAT);
+	u32 gic_int = __raw_readl(gic_dist_base_addr + GIC_DIST_PENDING_SET);
+	u32 twd_ctrl = __raw_readl(twd_base + TWD_TIMER_CONTROL);
+
+	if (twd_int && !(gic_int & BIT(OMAP44XX_IRQ_LOCALTIMER))) {
+		/*
+		 * The local timer interrupt got lost while the distributor was
+		 * disabled.  Ack the pending interrupt, and retrigger it.
+		 */
+		pr_warn("%s: lost localtimer interrupt\n", __func__);
+		__raw_writel(1, twd_base + TWD_TIMER_INTSTAT);
+		if (!(twd_ctrl & TWD_TIMER_CONTROL_PERIODIC)) {
+			__raw_writel(1, twd_base + TWD_TIMER_COUNTER);
+			twd_ctrl |= TWD_TIMER_CONTROL_ENABLE;
+			__raw_writel(twd_ctrl, twd_base + TWD_TIMER_CONTROL);
+		}
+	}
+}
+
 #ifdef CONFIG_CACHE_L2X0
 
 void __iomem *omap4_get_l2cache_base(void)