irqchip/irq-sifive-plic: Add syscore callbacks for hibernation

The priority and enable registers of plic will be reset
during hibernation power cycle in poweroff mode,
add the syscore callbacks to save/restore those registers.

Signed-off-by: Mason Huo <mason.huo@starfivetech.com>
Reviewed-by: Ley Foon Tan <leyfoon.tan@starfivetech.com>
Reviewed-by: Sia Jee Heng <jeeheng.sia@starfivetech.com>
Reported-by: Dan Carpenter <error27@gmail.com>
Link: https://lore.kernel.org/r/202302140709.CdkxgtPi-lkp@intel.com/
Signed-off-by: Marc Zyngier <maz@kernel.org>
Link: https://lore.kernel.org/r/20230404032908.89638-1-mason.huo@starfivetech.com
[hoegeun.kwon: cherry-pick the commit e80f0b6a2cf30 from mainline]
Signed-off-by: Hoegeun Kwon <hoegeun.kwon@samsung.com>
Change-Id: Ic549e1d59da713ac7b2bc0d1d827c6332cbdc3c8

diff --git a/drivers/irqchip/irq-sifive-plic.c b/drivers/irqchip/irq-sifive-plic.c
index 2f47848..2323606 100644 (file)
--- a/drivers/irqchip/irq-sifive-plic.c
+++ b/drivers/irqchip/irq-sifive-plic.c
@@ -17,6 +17,7 @@
 #include <linux/of_irq.h>
 #include <linux/platform_device.h>
 #include <linux/spinlock.h>
+#include <linux/syscore_ops.h>
 #include <asm/smp.h>
 
 /*
@@ -67,6 +68,8 @@ struct plic_priv {
        struct irq_domain *irqdomain;
        void __iomem *regs;
        unsigned long plic_quirks;
+       unsigned int nr_irqs;
+       unsigned long *prio_save;
 };
 
 struct plic_handler {
@@ -78,6 +81,7 @@ struct plic_handler {
         */
        raw_spinlock_t          enable_lock;
        void __iomem            *enable_base;
+       u32                     *enable_save;
        struct plic_priv        *priv;
 };
 static int plic_parent_irq __ro_after_init;
@@ -227,6 +231,71 @@ static int plic_irq_set_type(struct irq_data *d, unsigned int type)
        return IRQ_SET_MASK_OK;
 }
 
+static int plic_irq_suspend(void)
+{
+       unsigned int i, cpu;
+       u32 __iomem *reg;
+       struct plic_priv *priv;
+
+       priv = per_cpu_ptr(&plic_handlers, smp_processor_id())->priv;
+
+       for (i = 0; i < priv->nr_irqs; i++)
+               if (readl(priv->regs + PRIORITY_BASE + i * PRIORITY_PER_ID))
+                       __set_bit(i, priv->prio_save);
+               else
+                       __clear_bit(i, priv->prio_save);
+
+       for_each_cpu(cpu, cpu_present_mask) {
+               struct plic_handler *handler = per_cpu_ptr(&plic_handlers, cpu);
+
+               if (!handler->present)
+                       continue;
+
+               raw_spin_lock(&handler->enable_lock);
+               for (i = 0; i < DIV_ROUND_UP(priv->nr_irqs, 32); i++) {
+                       reg = handler->enable_base + i * sizeof(u32);
+                       handler->enable_save[i] = readl(reg);
+               }
+               raw_spin_unlock(&handler->enable_lock);
+       }
+
+       return 0;
+}
+
+static void plic_irq_resume(void)
+{
+       unsigned int i, index, cpu;
+       u32 __iomem *reg;
+       struct plic_priv *priv;
+
+       priv = per_cpu_ptr(&plic_handlers, smp_processor_id())->priv;
+
+       for (i = 0; i < priv->nr_irqs; i++) {
+               index = BIT_WORD(i);
+               writel((priv->prio_save[index] & BIT_MASK(i)) ? 1 : 0,
+                      priv->regs + PRIORITY_BASE + i * PRIORITY_PER_ID);
+       }
+
+       for_each_cpu(cpu, cpu_present_mask) {
+               struct plic_handler *handler = per_cpu_ptr(&plic_handlers, cpu);
+
+               if (!handler->present)
+                       continue;
+
+               raw_spin_lock(&handler->enable_lock);
+               for (i = 0; i < DIV_ROUND_UP(priv->nr_irqs, 32); i++) {
+                       reg = handler->enable_base + i * sizeof(u32);
+                       writel(handler->enable_save[i], reg);
+               }
+               raw_spin_unlock(&handler->enable_lock);
+       }
+}
+
+static struct syscore_ops plic_irq_syscore_ops = {
+       .suspend        = plic_irq_suspend,
+       .resume         = plic_irq_resume,
+};
+
 static int plic_irqdomain_map(struct irq_domain *d, unsigned int irq,
                              irq_hw_number_t hwirq)
 {
@@ -343,6 +412,7 @@ static int __init __plic_init(struct device_node *node,
        u32 nr_irqs;
        struct plic_priv *priv;
        struct plic_handler *handler;
+       unsigned int cpu;
 
        priv = kzalloc(sizeof(*priv), GFP_KERNEL);
        if (!priv)
@@ -361,15 +431,21 @@ static int __init __plic_init(struct device_node *node,
        if (WARN_ON(!nr_irqs))
                goto out_iounmap;
 
+       priv->nr_irqs = nr_irqs;
+
+       priv->prio_save = bitmap_alloc(nr_irqs, GFP_KERNEL);
+       if (!priv->prio_save)
+               goto out_free_priority_reg;
+
        nr_contexts = of_irq_count(node);
        if (WARN_ON(!nr_contexts))
-               goto out_iounmap;
+               goto out_free_priority_reg;
 
        error = -ENOMEM;
        priv->irqdomain = irq_domain_add_linear(node, nr_irqs + 1,
                        &plic_irqdomain_ops, priv);
        if (WARN_ON(!priv->irqdomain))
-               goto out_iounmap;
+               goto out_free_priority_reg;
 
        for (i = 0; i < nr_contexts; i++) {
                struct of_phandle_args parent;
@@ -439,6 +515,11 @@ static int __init __plic_init(struct device_node *node,
                handler->enable_base = priv->regs + CONTEXT_ENABLE_BASE +
                        i * CONTEXT_ENABLE_SIZE;
                handler->priv = priv;
+
+               handler->enable_save =  kcalloc(DIV_ROUND_UP(nr_irqs, 32),
+                                               sizeof(*handler->enable_save), GFP_KERNEL);
+               if (!handler->enable_save)
+                       goto out_free_enable_reg;
 done:
                for (hwirq = 1; hwirq <= nr_irqs; hwirq++) {
                        plic_toggle(handler, hwirq, 0);
@@ -459,11 +540,19 @@ done:
                                  plic_starting_cpu, plic_dying_cpu);
                plic_cpuhp_setup_done = true;
        }
+       register_syscore_ops(&plic_irq_syscore_ops);
 
        pr_info("%pOFP: mapped %d interrupts with %d handlers for"
                " %d contexts.\n", node, nr_irqs, nr_handlers, nr_contexts);
        return 0;
 
+out_free_enable_reg:
+       for_each_cpu(cpu, cpu_present_mask) {
+               handler = per_cpu_ptr(&plic_handlers, cpu);
+               kfree(handler->enable_save);
+       }
+out_free_priority_reg:
+       kfree(priv->prio_save);
 out_iounmap:
        iounmap(priv->regs);
 out_free_priv: