1 files changed, 41 insertions, 1 deletions

diff --git a/arch/arm/mach-omap2/omap4-common.c b/arch/arm/mach-omap2/omap4-common.c
index e1f289748c5d..6f94b4e7b18d 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>
 #include <linux/of_irq.h>
@@ -24,6 +25,7 @@
 #include <asm/hardware/cache-l2x0.h>
 #include <asm/mach/map.h>
 #include <asm/memblock.h>
+#include <asm/smp_twd.h>
 
 #include <plat/sram.h>
 #include <plat/omap-secure.h>
@@ -41,6 +43,10 @@ static void __iomem *l2cache_base;
 #endif
 
 static void __iomem *sar_ram_base;
+static void __iomem *gic_dist_base_addr;
+static void __iomem *twd_base;
+
+#define IRQ_LOCALTIMER          29
 
 #ifdef CONFIG_OMAP4_ERRATA_I688
 /* Used to implement memory barrier on DRAM path */
@@ -95,12 +101,14 @@ void __init omap_barriers_init(void)
 void __init gic_init_irq(void)
 {
         void __iomem *omap_irq_base;
-        void __iomem *gic_dist_base_addr;
 
         /* Static mapping, never released */
         gic_dist_base_addr = ioremap(OMAP44XX_GIC_DIST_BASE, SZ_4K);
         BUG_ON(!gic_dist_base_addr);
 
+        twd_base = ioremap(OMAP44XX_LOCAL_TWD_BASE, SZ_4K);
+        BUG_ON(!twd_base);
+
         /* Static mapping, never released */
         omap_irq_base = ioremap(OMAP44XX_GIC_CPU_BASE, SZ_512);
         BUG_ON(!omap_irq_base);
@@ -110,6 +118,38 @@ void __init gic_init_irq(void)
         gic_init(0, 29, gic_dist_base_addr, omap_irq_base);
 }
 
+void gic_dist_disable(void)
+{
+        if (gic_dist_base_addr)
+                __raw_writel(0x0, gic_dist_base_addr + GIC_DIST_CTRL);
+}
+
+bool gic_dist_disabled(void)
+{
+        return !(__raw_readl(gic_dist_base_addr + GIC_DIST_CTRL) & 0x1);
+}
+
+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(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)