diff --git a/drivers/irqchip/irq-gic-v3.c b/drivers/irqchip/irq-gic-v3.c index 9e3144975696..7247a3cc7ba1 100644 --- a/drivers/irqchip/irq-gic-v3.c +++ b/drivers/irqchip/irq-gic-v3.c @@ -16,6 +16,7 @@ */ #include +#include #include #include #include @@ -155,7 +156,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 +164,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"); } /* @@ -372,20 +383,8 @@ static int gic_populate_rdist(void) return -ENODEV; } -static void gic_cpu_init(void) +static void gic_cpu_sys_reg_init(void) { - void __iomem *rbase; - - /* Register ourselves with the rest of the world */ - if (gic_populate_rdist()) - return; - - gic_enable_redist(); - - rbase = gic_data_rdist_sgi_base(); - - gic_cpu_config(rbase, gic_redist_wait_for_rwp); - /* Enable system registers */ gic_enable_sre(); @@ -399,6 +398,24 @@ static void gic_cpu_init(void) gic_write_grpen1(1); } +static void gic_cpu_init(void) +{ + void __iomem *rbase; + + /* Register ourselves with the rest of the world */ + if (gic_populate_rdist()) + return; + + gic_enable_redist(true); + + rbase = gic_data_rdist_sgi_base(); + + gic_cpu_config(rbase, gic_redist_wait_for_rwp); + + /* initialise system registers */ + gic_cpu_sys_reg_init(); +} + #ifdef CONFIG_SMP static int gic_secondary_init(struct notifier_block *nfb, unsigned long action, void *hcpu) @@ -532,6 +549,33 @@ static int gic_set_affinity(struct irq_data *d, const struct cpumask *mask_val, #define gic_smp_init() do { } while(0) #endif +#ifdef CONFIG_CPU_PM +static int gic_cpu_pm_notifier(struct notifier_block *self, + unsigned long cmd, void *v) +{ + if (cmd == CPU_PM_EXIT) { + gic_enable_redist(true); + gic_cpu_sys_reg_init(); + } else if (cmd == CPU_PM_ENTER) { + gic_write_grpen1(0); + gic_enable_redist(false); + } + return NOTIFY_OK; +} + +static struct notifier_block gic_cpu_pm_notifier_block = { + .notifier_call = gic_cpu_pm_notifier, +}; + +static void gic_cpu_pm_init(void) +{ + cpu_pm_register_notifier(&gic_cpu_pm_notifier_block); +} + +#else +static inline void gic_cpu_pm_init(void) { } +#endif /* CONFIG_CPU_PM */ + static struct irq_chip gic_chip = { .name = "GICv3", .irq_mask = gic_mask_irq, @@ -671,6 +715,7 @@ static int __init gic_of_init(struct device_node *node, struct device_node *pare gic_smp_init(); gic_dist_init(); gic_cpu_init(); + gic_cpu_pm_init(); return 0;