[PATCH 2/2] irqchip/irq-realtek-rtl: Add multicore support

[Markus Stockhausen]

The Realtek IRQ driver currently supports only single core
systems. So the higher end devices like RTL839x and RTL930x
with dual VPEs must be driven with NR_CPU=1. Enhance the
driver to support multicore (dual VPE) systems. For this:

- Extend the register map for multiple cores
- Search for multiple CPU cores in the devicetree
- Improve the register helpers to support multiple cores
- Add an affinity setter
- Enhance the IRQ handler for multiple cores

Signed-off-by: Markus Stockhausen <markus.stockhausen@xxxxxx>
---
 drivers/irqchip/irq-realtek-rtl.c | 91 +++++++++++++++++++++++--------
 1 file changed, 69 insertions(+), 22 deletions(-)

diff --git a/drivers/irqchip/irq-realtek-rtl.c b/drivers/irqchip/irq-realtek-rtl.c
index 7fb8dd4c5670..e0e07074bf6f 100644
--- a/drivers/irqchip/irq-realtek-rtl.c
+++ b/drivers/irqchip/irq-realtek-rtl.c
@@ -23,10 +23,11 @@
 
 #define RTL_ICTL_NUM_INPUTS	32
 
-#define REG(x)		(realtek_ictl_base + x)
+#define REG(cpu, x)	(realtek_ictl_base[cpu] + x)
 
 static DEFINE_RAW_SPINLOCK(irq_lock);
-static void __iomem *realtek_ictl_base;
+static void __iomem *realtek_ictl_base[NR_CPUS];
+static cpumask_t realtek_ictl_cpu_configurable;
 
 /*
  * IRR0-IRR3 store 4 bits per interrupt, but Realtek uses inverted numbering,
@@ -37,27 +38,27 @@ static void __iomem *realtek_ictl_base;
 #define IRR_OFFSET(idx)		(4 * (3 - (idx * 4) / 32))
 #define IRR_SHIFT(idx)		((idx * 4) % 32)
 
-static inline void enable_gimr(int hw_irq)
+static inline void enable_gimr(int cpu, int hw_irq)
 {
 	u32 gimr;
 
-	gimr = readl(REG(RTL_ICTL_GIMR));
+	gimr = readl(REG(cpu, RTL_ICTL_GIMR));
 	gimr |= BIT(hw_irq);
-	writel(gimr, REG(RTL_ICTL_GIMR));
+	writel(gimr, REG(cpu, RTL_ICTL_GIMR));
 }
 
-static inline void disable_gimr(int hwirq)
+static inline void disable_gimr(int cpu, int hwirq)
 {
 	u32 gimr;
 
-	gimr = readl(REG(RTL_ICTL_GIMR));
+	gimr = readl(REG(cpu, RTL_ICTL_GIMR));
 	gimr &= ~BIT(hwirq);
-	writel(gimr, REG(RTL_ICTL_GIMR));
+	writel(gimr, REG(cpu, RTL_ICTL_GIMR));
 }
 
-static void write_irr(int hw_irq, u32 value)
+static void write_irr(int cpu, int hw_irq, u32 value)
 {
-	void __iomem *irr0 = REG(RTL_ICTL_IRR0);
+	void __iomem *irr0 = REG(cpu, RTL_ICTL_IRR0);
 	unsigned int offset = IRR_OFFSET(hw_irq);
 	unsigned int shift = IRR_SHIFT(hw_irq);
 	u32 irr;
@@ -70,35 +71,73 @@ static void write_irr(int hw_irq, u32 value)
 static void realtek_ictl_unmask_irq(struct irq_data *i)
 {
 	unsigned long flags;
+	cpumask_t cpus;
+	int cpu;
+
+	cpumask_and(&cpus, &realtek_ictl_cpu_configurable,
+		    irq_data_get_effective_affinity_mask(i));
 
 	raw_spin_lock_irqsave(&irq_lock, flags);
-	enable_gimr(i->hwirq);
+	for_each_cpu(cpu, &cpus)
+		enable_gimr(cpu, i->hwirq);
 	raw_spin_unlock_irqrestore(&irq_lock, flags);
 }
 
 static void realtek_ictl_mask_irq(struct irq_data *i)
 {
 	unsigned long flags;
+	int cpu;
 
 	raw_spin_lock_irqsave(&irq_lock, flags);
-	disable_gimr(i->hwirq);
+	for_each_cpu(cpu, &realtek_ictl_cpu_configurable)
+		disable_gimr(cpu, i->hwirq);
 	raw_spin_unlock_irqrestore(&irq_lock, flags);
 }
 
+static int realtek_ictl_irq_affinity(struct irq_data *i,
+				     const struct cpumask *dest,
+				     bool force)
+{
+	cpumask_t cpu_configure;
+	cpumask_t cpu_disable;
+	cpumask_t cpu_enable;
+	unsigned long flags;
+	int cpu;
+
+	cpumask_and(&cpu_configure, cpu_present_mask, &realtek_ictl_cpu_configurable);
+	cpumask_and(&cpu_enable, &cpu_configure, dest);
+	cpumask_andnot(&cpu_disable, &cpu_configure, dest);
+
+	raw_spin_lock_irqsave(&irq_lock, flags);
+	for_each_cpu(cpu, &cpu_disable)
+		disable_gimr(cpu, i->hwirq);
+	for_each_cpu(cpu, &cpu_enable)
+		if (!irqd_irq_masked(i))
+			enable_gimr(cpu, i->hwirq);
+	raw_spin_unlock_irqrestore(&irq_lock, flags);
+
+	irq_data_update_effective_affinity(i, &cpu_enable);
+
+	return IRQ_SET_MASK_OK;
+}
+
 static struct irq_chip realtek_ictl_irq = {
 	.name = "realtek-rtl-intc",
 	.irq_mask = realtek_ictl_mask_irq,
 	.irq_unmask = realtek_ictl_unmask_irq,
+	.irq_set_affinity = realtek_ictl_irq_affinity,
 };
 
 static int intc_map(struct irq_domain *d, unsigned int irq, irq_hw_number_t hw)
 {
 	unsigned long flags;
+	int cpu;
 
 	irq_set_chip_and_handler(irq, &realtek_ictl_irq, handle_level_irq);
 
 	raw_spin_lock_irqsave(&irq_lock, flags);
-	write_irr(hw, 1);
+	for_each_cpu(cpu, &realtek_ictl_cpu_configurable)
+		write_irr(cpu, hw, 1);
 	raw_spin_unlock_irqrestore(&irq_lock, flags);
 
 	return 0;
@@ -112,12 +151,13 @@ static const struct irq_domain_ops irq_domain_ops = {
 static void realtek_irq_dispatch(struct irq_desc *desc)
 {
 	struct irq_chip *chip = irq_desc_get_chip(desc);
+	int cpu = smp_processor_id();
 	struct irq_domain *domain;
 	unsigned long pending;
 	unsigned int soc_int;
 
 	chained_irq_enter(chip, desc);
-	pending = readl(REG(RTL_ICTL_GIMR)) & readl(REG(RTL_ICTL_GISR));
+	pending = readl(REG(cpu, RTL_ICTL_GIMR)) & readl(REG(cpu, RTL_ICTL_GISR));
 
 	if (unlikely(!pending)) {
 		spurious_interrupt();
@@ -139,16 +179,23 @@ static int __init realtek_rtl_of_init(struct device_node *node, struct device_no
 	unsigned int soc_irq;
 	int parent_irq;
 
-	realtek_ictl_base = of_iomap(node, 0);
-	if (!realtek_ictl_base)
-		return -ENXIO;
-
-	/* Disable all cascaded interrupts and clear routing */
-	for (soc_irq = 0; soc_irq < RTL_ICTL_NUM_INPUTS; soc_irq++) {
-		disable_gimr(soc_irq);
-		write_irr(soc_irq, 0);
+	cpumask_clear(&realtek_ictl_cpu_configurable);
+
+	for (int cpu = 0; cpu < NR_CPUS; cpu++) {
+		realtek_ictl_base[cpu] = of_iomap(node, cpu);
+		if (realtek_ictl_base[cpu]) {
+			cpumask_set_cpu(cpu, &realtek_ictl_cpu_configurable);
+			/* Disable all cascaded interrupts and clear routing */
+			for (soc_irq = 0; soc_irq < RTL_ICTL_NUM_INPUTS; soc_irq++) {
+				disable_gimr(cpu, soc_irq);
+				write_irr(cpu, soc_irq, 0);
+			}
+		}
 	}
 
+	if (cpumask_empty(&realtek_ictl_cpu_configurable))
+		return -ENXIO;
+
 	if (WARN_ON(!of_irq_count(node))) {
 		/*
 		 * If DT contains no parent interrupts, assume MIPS CPU IRQ 2
-- 
2.54.0