1 files changed, 20 insertions, 10 deletions

diff --git a/drivers/irqchip/irq-gic-v3.c b/drivers/irqchip/irq-gic-v3.c
index 57eaa5a0b1e3..37062ba6704b 100644
--- a/drivers/irqchip/irq-gic-v3.c
+++ b/drivers/irqchip/irq-gic-v3.c
@@ -155,7 +155,7 @@ static void gic_enable_sre(void)
                 pr_err("GIC: unable to set SRE (disabled at EL2), panic ahead\n");
 }
 
-static void gic_enable_redist(void)
+static void gic_enable_redist(bool enable)
 {
         void __iomem *rbase;
         u32 count = 1000000;    /* 1s! */
@@ -163,20 +163,30 @@ static void gic_enable_redist(void)
 
         rbase = gic_data_rdist_rd_base();
 
-        /* Wake up this CPU redistributor */
         val = readl_relaxed(rbase + GICR_WAKER);
-        val &= ~GICR_WAKER_ProcessorSleep;
+        if (enable)
+                /* Wake up this CPU redistributor */
+                val &= ~GICR_WAKER_ProcessorSleep;
+        else
+                val |= GICR_WAKER_ProcessorSleep;
         writel_relaxed(val, rbase + GICR_WAKER);
 
-        while (readl_relaxed(rbase + GICR_WAKER) & GICR_WAKER_ChildrenAsleep) {
-                count--;
-                if (!count) {
-                        pr_err_ratelimited("redist didn't wake up...\n");
-                        return;
-                }
+        if (!enable) {          /* Check that GICR_WAKER is writeable */
+                val = readl_relaxed(rbase + GICR_WAKER);
+                if (!(val & GICR_WAKER_ProcessorSleep))
+                        return; /* No PM support in this redistributor */
+        }
+
+        while (count--) {
+                val = readl_relaxed(rbase + GICR_WAKER);
+                if (enable ^ (val & GICR_WAKER_ChildrenAsleep))
+                        break;
                 cpu_relax();
                 udelay(1);
         };
+        if (!count)
+                pr_err_ratelimited("redistributor failed to %s...\n",
+                                   enable ? "wakeup" : "sleep");
 }
 
 /*
@@ -381,7 +391,7 @@ static void gic_cpu_init(void)
         if (gic_populate_rdist())
                 return;
 
-        gic_enable_redist();
+        gic_enable_redist(true);
 
         rbase = gic_data_rdist_sgi_base();