[PATCH 3/3] irqchip/gic-v3: Save and restore distributor and re-distributor

[Florian Fainelli <f.fainelli@gmail.com>]

On platforms implementing Suspend to RAM where the GIC loses power, we
are not properly saving and restoring the GIC distributor and
re-distributor registers thus leading to the system resuming without any
functional interrupts.

Add support for saving and restoring the GIC distributor and
re-distributor in order to properly suspend and resume with a functional
system.

Signed-off-by: Florian Fainelli <f.fainelli@gmail.com>
---
 drivers/irqchip/irq-gic-v3.c       | 258 +++++++++++++++++++++++++++++
 include/linux/irqchip/arm-gic-v3.h |   4 +
 2 files changed, 262 insertions(+)

diff --git a/drivers/irqchip/irq-gic-v3.c b/drivers/irqchip/irq-gic-v3.c
index 48b0e9aba27c..4caab61268d0 100644
--- a/drivers/irqchip/irq-gic-v3.c
+++ b/drivers/irqchip/irq-gic-v3.c
@@ -11,6 +11,7 @@
 #include <linux/cpu_pm.h>
 #include <linux/delay.h>
 #include <linux/interrupt.h>
+#include <linux/io-64-nonatomic-lo-hi.h>
 #include <linux/irqdomain.h>
 #include <linux/kstrtox.h>
 #include <linux/of.h>
@@ -57,6 +58,25 @@ struct gic_chip_data {
 	bool			has_rss;
 	unsigned int		ppi_nr;
 	struct partition_desc	**ppi_descs;
+#ifdef CONFIG_CPU_PM
+	u32			*saved_spi_conf;
+	u64			*saved_spi_target;
+	u32			*saved_spi_enable;
+	u32			*saved_spi_active;
+
+	u32			*saved_espi_conf;
+	u64			*saved_espi_target;
+	u32			*saved_espi_enable;
+	u32			*saved_espi_active;
+
+	u32			saved_ppi_conf;
+	u32			saved_ppi_enable;
+	u32			saved_ppi_active;
+
+	u32			*saved_eppi_conf;
+	u32			*saved_eppi_enable;
+	u32			*saved_eppi_active;
+#endif
 };
 
 static struct gic_chip_data gic_data __read_mostly;
@@ -1371,6 +1391,143 @@ static int gic_retrigger(struct irq_data *data)
 }
 
 #ifdef CONFIG_CPU_PM
+static void gic_rdist_save(void)
+{
+	struct gic_chip_data *gic = &gic_data;
+	void __iomem *rbase = gic_data_rdist_sgi_base();
+	unsigned int i;
+
+	gic->saved_ppi_conf = readl_relaxed(rbase + GICR_ICFGR0 + 4);
+	gic->saved_ppi_enable = readl_relaxed(rbase + GICR_ISENABLER0);
+	gic->saved_ppi_active = readl_relaxed(rbase + GICR_ICACTIVER0);
+
+	for (i = 0; i < DIV_ROUND_UP(gic->ppi_nr - 16, 32); i++) {
+		gic->saved_eppi_conf[i] =
+			readl_relaxed(rbase + GICR_ICFGRnE + i * 4);
+		gic->saved_eppi_enable[i] =
+			readl_relaxed(rbase + GICR_ISENABLERnE + i * 4);
+		gic->saved_eppi_active[i] =
+			readl_relaxed(rbase + GICR_ICACTIVERnE + i * 4);
+	}
+}
+
+static void gic_dist_save(void)
+{
+	struct gic_chip_data *gic = &gic_data;
+	void __iomem *base = gic_data.dist_base;
+	unsigned int i;
+
+	/* Save the SPIs first */
+	for (i = 2; i < DIV_ROUND_UP(GIC_LINE_NR, 16); i++)
+		gic->saved_spi_conf[i] =
+			readl_relaxed(base + GICD_ICFGR + i * 4);
+
+	for (i = 32; i < GIC_LINE_NR; i++)
+		gic->saved_spi_target[i] =
+			readq_relaxed(base + GICD_IROUTER + i * 8);
+
+	for (i = 1; i < DIV_ROUND_UP(GIC_LINE_NR, 32); i++) {
+		gic->saved_spi_enable[i] =
+			readl_relaxed(base + GICD_ISENABLER + i * 4);
+		gic->saved_spi_active[i] =
+			readl_relaxed(base + GICD_ISACTIVER + i * 4);
+	}
+
+	/* Save the EPIs next */
+	for (i = 0; i < DIV_ROUND_UP(GIC_ESPI_NR, 16); i++)
+		gic->saved_espi_conf[i] =
+			readl_relaxed(base + GICD_ICFGRnE + i * 4);
+
+	for (i = 0; i < GIC_ESPI_NR; i++)
+		gic->saved_espi_target[i] =
+			readq_relaxed(base + GICD_IROUTERnE + i * 8);
+
+	for (i = 0; i < DIV_ROUND_UP(GIC_ESPI_NR, 32); i++) {
+		gic->saved_espi_enable[i] =
+			readl_relaxed(base + GICD_ISENABLERnE + i * 4);
+		gic->saved_espi_active[i] =
+			readl_relaxed(base + GICD_ISACTIVERnE + i * 4);
+	}
+}
+
+static void gic_rdist_restore(void)
+{
+	struct gic_chip_data *gic = &gic_data;
+	void __iomem *rbase = gic_data_rdist_sgi_base();
+	unsigned int i;
+
+	writel_relaxed(gic->saved_ppi_conf, rbase + GICR_ICFGR0 + 4);
+	writel_relaxed(gic->saved_ppi_enable, rbase + GICR_ISENABLER0);
+	writel_relaxed(gic->saved_ppi_active, rbase + GICR_ICACTIVER0);
+
+	for (i = 0; i < DIV_ROUND_UP(gic->ppi_nr - 16, 32); i++) {
+		writel_relaxed(gic->saved_eppi_conf[i],
+				rbase + GICR_ICFGRnE + i * 4);
+		writel_relaxed(gic->saved_eppi_enable[i],
+				rbase + GICR_ISENABLERnE + i * 4);
+		writel_relaxed(gic->saved_eppi_active[i],
+				rbase + GICR_ICACTIVERnE + i * 4);
+	}
+}
+
+static void gic_dist_restore(void)
+{
+	struct gic_chip_data *gic = &gic_data;
+	void __iomem *base = gic_data.dist_base;
+	unsigned int i;
+
+	/* Ensure distributor is disabled */
+	writel_relaxed(0, base + GICD_CTLR);
+	gic_dist_wait_for_rwp();
+
+	/* Configure SPIs as non-secure Group-1. */
+	for (i = 32; i < GIC_LINE_NR; i += 32)
+		writel_relaxed(~0, base + GICD_IGROUPR + i / 8);
+
+	/* Restore the SPIs */
+	for (i = 2; i < DIV_ROUND_UP(GIC_LINE_NR, 16); i++)
+		writel_relaxed(gic->saved_spi_conf[i],
+			       base + GICD_ICFGR + i * 4);
+
+	for (i = 32; i < GIC_LINE_NR; i++)
+		writel_relaxed(gic->saved_spi_target[i],
+				base + GICD_IROUTER + i * 8);
+
+	for (i = 1; i < DIV_ROUND_UP(GIC_LINE_NR, 32); i++) {
+		writel_relaxed(gic->saved_spi_enable[i],
+				base + GICD_ISENABLER + i * 4);
+		writel_relaxed(gic->saved_spi_active[i],
+				base + GICD_ISACTIVER + i * 4);
+	}
+
+	/* Configure ESPIs as non-secure Group-1. */
+	for (i = 0; i < GIC_ESPI_NR; i += 32)
+		writel_relaxed(~0U, base + GICD_IGROUPRnE + i / 8);
+
+	/* Restore the ESPIs */
+	for (i = 0; i < DIV_ROUND_UP(GIC_ESPI_NR, 16); i++)
+		writel_relaxed(gic->saved_espi_conf[i],
+				base + GICD_ICFGRnE + i * 4);
+
+	for (i = 0; i < GIC_ESPI_NR; i++)
+		writeq_relaxed(gic->saved_espi_target[i],
+				base + GICD_IROUTERnE + i * 8);
+
+	for (i = 0; i < DIV_ROUND_UP(GIC_ESPI_NR, 32); i++) {
+		writel_relaxed(gic->saved_espi_enable[i],
+				base + GICD_ISENABLERnE + i * 4);
+		writel_relaxed(gic->saved_espi_active[i],
+				base + GICD_ISACTIVERnE + i * 4);
+	}
+
+	for (i = 0; i < GIC_ESPI_NR; i += 4)
+		writel_relaxed(GICD_INT_DEF_PRI_X4, base + GICD_IPRIORITYRnE + i);
+
+	/* Enable distributor with ARE, Group1 */
+	writel_relaxed(GICD_CTLR_ARE_NS | GICD_CTLR_ENABLE_G1A | GICD_CTLR_ENABLE_G1,
+		       base + GICD_CTLR);
+}
+
 static int gic_cpu_pm_notifier(struct notifier_block *self,
 			       unsigned long cmd, void *v)
 {
@@ -1380,12 +1537,20 @@ static int gic_cpu_pm_notifier(struct notifier_block *self,
 			gic_write_grpen1(0);
 			gic_enable_redist(false);
 		}
+		gic_rdist_save();
+		break;
+	case CPU_CLUSTER_PM_ENTER:
+		gic_dist_save();
 		break;
 	case CPU_PM_EXIT:
+		gic_rdist_restore();
 		if (gic_dist_security_disabled())
 			gic_enable_redist(true);
 		gic_cpu_sys_reg_init();
 		break;
+	case CPU_CLUSTER_PM_EXIT:
+		gic_dist_restore();
+		break;
 	}
 
 	return NOTIFY_OK;
@@ -1397,9 +1562,102 @@ static struct notifier_block gic_cpu_pm_notifier_block = {
 
 static int gic_cpu_pm_init(void)
 {
+	struct gic_chip_data *gic = &gic_data;
+	unsigned int spi_size = DIV_ROUND_UP(GIC_LINE_NR, 32);
+	unsigned int espi_size = DIV_ROUND_UP(GIC_ESPI_NR, 32);
+	unsigned int eppi_size = DIV_ROUND_UP(gic->ppi_nr - 16, 32);
+
+	gic->saved_spi_conf = kcalloc(DIV_ROUND_UP(GIC_LINE_NR, 16),
+				      sizeof(*gic->saved_spi_conf),
+				      GFP_KERNEL);
+	if (WARN_ON(!gic->saved_spi_conf))
+		return -ENOMEM;
+
+	gic->saved_spi_target = kcalloc(GIC_LINE_NR,
+					sizeof(*gic->saved_spi_target),
+					GFP_KERNEL);
+	if (WARN_ON(!gic->saved_spi_target))
+		goto out_free_spi_conf;
+
+	gic->saved_spi_enable = kcalloc(spi_size,
+					sizeof(*gic->saved_spi_enable),
+					GFP_KERNEL);
+	if (WARN_ON(!gic->saved_spi_enable))
+		goto out_free_spi_target;
+
+	gic->saved_spi_active = kcalloc(spi_size,
+					sizeof(*gic->saved_spi_active),
+					GFP_KERNEL);
+	if (WARN_ON(!gic->saved_spi_active))
+		goto out_free_spi_enable;
+
+	gic->saved_espi_conf = kcalloc(DIV_ROUND_UP(GIC_ESPI_NR, 16),
+				       sizeof(*gic->saved_espi_conf),
+				       GFP_KERNEL);
+	if (WARN_ON(!gic->saved_espi_conf))
+		goto out_free_spi_active;
+
+	gic->saved_espi_target = kcalloc(GIC_ESPI_NR,
+					 sizeof(*gic->saved_espi_target),
+					 GFP_KERNEL);
+	if (WARN_ON(!gic->saved_espi_target))
+		goto out_free_espi_conf;
+
+	gic->saved_espi_enable = kcalloc(espi_size,
+					 sizeof(*gic->saved_espi_enable),
+					 GFP_KERNEL);
+	if (WARN_ON(!gic->saved_espi_enable))
+		goto out_free_espi_target;
+
+	gic->saved_espi_active = kcalloc(espi_size,
+					 sizeof(*gic->saved_espi_active),
+					 GFP_KERNEL);
+	if (WARN_ON(!gic->saved_espi_active))
+		goto out_free_espi_enable;
+
+	gic->saved_eppi_conf = kcalloc(DIV_ROUND_UP(gic->ppi_nr - 16, 16),
+				       sizeof(*gic->saved_eppi_conf),
+				       GFP_KERNEL);
+	if (WARN_ON(!gic->saved_eppi_conf))
+		goto out_free_espi_active;
+
+	gic->saved_eppi_enable = kcalloc(eppi_size,
+					 sizeof(*gic->saved_eppi_enable),
+					 GFP_KERNEL);
+	if (WARN_ON(!gic->saved_eppi_enable))
+		goto out_free_eppi_conf;
+
+	gic->saved_eppi_active = kcalloc(eppi_size,
+					 sizeof(*gic->saved_eppi_active),
+					 GFP_KERNEL);
+	if (WARN_ON(!gic->saved_eppi_active))
+		goto out_free_eppi_enable;
+
 	cpu_pm_register_notifier(&gic_cpu_pm_notifier_block);
 
 	return 0;
+
+out_free_eppi_enable:
+	kfree(gic->saved_eppi_enable);
+out_free_eppi_conf:
+	kfree(gic->saved_eppi_conf);
+out_free_espi_active:
+	kfree(gic->saved_espi_active);
+out_free_espi_enable:
+	kfree(gic->saved_espi_enable);
+out_free_espi_target:
+	kfree(gic->saved_espi_target);
+out_free_espi_conf:
+	kfree(gic->saved_espi_conf);
+out_free_spi_active:
+	kfree(gic->saved_spi_active);
+out_free_spi_enable:
+	kfree(gic->saved_spi_enable);
+out_free_spi_target:
+	kfree(gic->saved_spi_target);
+out_free_spi_conf:
+	kfree(gic->saved_spi_conf);
+	return -ENOMEM;
 }
 
 #else
diff --git a/include/linux/irqchip/arm-gic-v3.h b/include/linux/irqchip/arm-gic-v3.h
index 728691365464..40483530cadd 100644
--- a/include/linux/irqchip/arm-gic-v3.h
+++ b/include/linux/irqchip/arm-gic-v3.h
@@ -229,13 +229,17 @@
  */
 #define GICR_IGROUPR0			GICD_IGROUPR
 #define GICR_ISENABLER0			GICD_ISENABLER
+#define GICR_ISENABLERnE		GICD_ISENABLERnE
 #define GICR_ICENABLER0			GICD_ICENABLER
 #define GICR_ISPENDR0			GICD_ISPENDR
 #define GICR_ICPENDR0			GICD_ICPENDR
 #define GICR_ISACTIVER0			GICD_ISACTIVER
+#define GICR_ISACTIVERnE		GICD_ISACTIVERnE
 #define GICR_ICACTIVER0			GICD_ICACTIVER
+#define GICR_ICACTIVERnE		GICD_ICACTIVERnE
 #define GICR_IPRIORITYR0		GICD_IPRIORITYR
 #define GICR_ICFGR0			GICD_ICFGR
+#define GICR_ICFGRnE			GICD_ICFGRnE
 #define GICR_IGRPMODR0			GICD_IGRPMODR
 #define GICR_NSACR			GICD_NSACR
 
-- 
2.34.1