4 files changed, 31 insertions, 5 deletions

diff --git a/arch/arm/mach-omap2/common.h b/arch/arm/mach-omap2/common.h
index e51527835160..3adaa1d5cd10 100644
--- a/arch/arm/mach-omap2/common.h
+++ b/arch/arm/mach-omap2/common.h
@@ -236,6 +236,7 @@ static inline void __iomem *omap4_get_scu_base(void)
 
 extern void __init gic_init_irq(void);
 extern void gic_dist_disable(void);
+extern void gic_dist_enable(void);
 extern bool gic_dist_disabled(void);
 extern void gic_timer_retrigger(void);
 extern void omap_smc1(u32 fn, u32 arg);
diff --git a/arch/arm/mach-omap2/cpuidle44xx.c b/arch/arm/mach-omap2/cpuidle44xx.c
index 4c8982ae9529..4c158c838d40 100644
--- a/arch/arm/mach-omap2/cpuidle44xx.c
+++ b/arch/arm/mach-omap2/cpuidle44xx.c
@@ -80,6 +80,7 @@ static int omap_enter_idle_coupled(struct cpuidle_device *dev,
                         int index)
 {
         struct idle_statedata *cx = state_ptr + index;
+        u32 mpuss_can_lose_context = 0;
 
         /*
          * CPU0 has to wait and stay ON until CPU1 is OFF state.
@@ -104,6 +105,9 @@ static int omap_enter_idle_coupled(struct cpuidle_device *dev,
                 }
         }
 
+        mpuss_can_lose_context = (cx->mpu_state == PWRDM_POWER_RET) &&
+                                 (cx->mpu_logic_state == PWRDM_POWER_OFF);
+
         /*
          * Call idle CPU PM enter notifier chain so that
          * VFP and per CPU interrupt context is saved.
@@ -118,9 +122,8 @@ static int omap_enter_idle_coupled(struct cpuidle_device *dev,
                  * Call idle CPU cluster PM enter notifier chain
                  * to save GIC and wakeupgen context.
                  */
-                if ((cx->mpu_state == PWRDM_POWER_RET) &&
-                        (cx->mpu_logic_state == PWRDM_POWER_OFF))
-                                cpu_cluster_pm_enter();
+                if (mpuss_can_lose_context)
+                        cpu_cluster_pm_enter();
         }
 
         omap4_enter_lowpower(dev->cpu, cx->cpu_state);
@@ -128,9 +131,23 @@ static int omap_enter_idle_coupled(struct cpuidle_device *dev,
 
         /* Wakeup CPU1 only if it is not offlined */
         if (dev->cpu == 0 && cpumask_test_cpu(1, cpu_online_mask)) {
+
+                if (IS_PM44XX_ERRATUM(PM_OMAP4_ROM_SMP_BOOT_ERRATUM_GICD) &&
+                    mpuss_can_lose_context)
+                        gic_dist_disable();
+
                 clkdm_wakeup(cpu_clkdm[1]);
                 omap_set_pwrdm_state(cpu_pd[1], PWRDM_POWER_ON);
                 clkdm_allow_idle(cpu_clkdm[1]);
+
+                if (IS_PM44XX_ERRATUM(PM_OMAP4_ROM_SMP_BOOT_ERRATUM_GICD) &&
+                    mpuss_can_lose_context) {
+                        while (gic_dist_disabled()) {
+                                udelay(1);
+                                cpu_relax();
+                        }
+                        gic_timer_retrigger();
+                }
         }
 
         /*
@@ -143,8 +160,7 @@ static int omap_enter_idle_coupled(struct cpuidle_device *dev,
          * Call idle CPU cluster PM exit notifier chain
          * to restore GIC and wakeupgen context.
          */
-        if (dev->cpu == 0 && (cx->mpu_state == PWRDM_POWER_RET) &&
-                (cx->mpu_logic_state == PWRDM_POWER_OFF))
+        if (dev->cpu == 0 && mpuss_can_lose_context)
                 cpu_cluster_pm_exit();
 
 fail:
diff --git a/arch/arm/mach-omap2/omap-mpuss-lowpower.c b/arch/arm/mach-omap2/omap-mpuss-lowpower.c
index f991016e2a6a..667915d236f3 100644
--- a/arch/arm/mach-omap2/omap-mpuss-lowpower.c
+++ b/arch/arm/mach-omap2/omap-mpuss-lowpower.c
@@ -271,6 +271,9 @@ int omap4_enter_lowpower(unsigned int cpu, unsigned int power_state)
         else
                 omap_pm_ops.finish_suspend(save_state);
 
+        if (IS_PM44XX_ERRATUM(PM_OMAP4_ROM_SMP_BOOT_ERRATUM_GICD) && cpu)
+                gic_dist_enable();
+
         /*
          * Restore the CPUx power state to ON otherwise CPUx
          * power domain can transitions to programmed low power
diff --git a/arch/arm/mach-omap2/omap4-common.c b/arch/arm/mach-omap2/omap4-common.c
index dd893ec4c8f2..6cd3f3772ecf 100644
--- a/arch/arm/mach-omap2/omap4-common.c
+++ b/arch/arm/mach-omap2/omap4-common.c
@@ -127,6 +127,12 @@ void gic_dist_disable(void)
                 __raw_writel(0x0, gic_dist_base_addr + GIC_DIST_CTRL);
 }
 
+void gic_dist_enable(void)
+{
+        if (gic_dist_base_addr)
+                __raw_writel(0x1, gic_dist_base_addr + GIC_DIST_CTRL);
+}
+
 bool gic_dist_disabled(void)
 {
         return !(__raw_readl(gic_dist_base_addr + GIC_DIST_CTRL) & 0x1);