irqchip: gic-v3: Refactor gic_enable_redist to support both enabling and disabling

[cascardo/linux.git] / drivers / irqchip / irq-gic-v3.c

diff --git a/drivers/irqchip/irq-gic-v3.c b/drivers/irqchip/irq-gic-v3.c
index 57eaa5a..37062ba 100644 (file)
--- 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();