[PATCH 1/2] gpiolib: Add irq_set_affinity helper for GPIO line IRQs

[Liang Hao <haohlliang@gmail.com>]

GPIO line IRQs need irq_chip->irq_set_affinity so user space can steer
affinity to the upstream controller. Hierarchical domains use
parent_data; chained controllers (parent_handler, no parent_data on the
line) store parent virqs in gpio_irq_chip instead.

Add gpiochip_irq_set_affinity() and GPIOCHIP_IRQ_AFFINITY_HELPERS.
Hierarchical lines delegate via irq_chip_set_affinity_parent().
Chained lines resolve the parent virq and call the parent
irq_chip->irq_set_affinity() directly. Return -EINVAL if the parent
virq cannot be resolved.

Signed-off-by: Liang Hao <haohlliang@gmail.com>
---
 drivers/gpio/gpiolib.c      | 79 +++++++++++++++++++++++++++++++++++++
 include/linux/gpio/driver.h |  5 +++
 2 files changed, 84 insertions(+)

diff --git a/drivers/gpio/gpiolib.c b/drivers/gpio/gpiolib.c
index 2987051d9904..0a4626579963 100644
--- a/drivers/gpio/gpiolib.c
+++ b/drivers/gpio/gpiolib.c
@@ -2104,6 +2104,85 @@ void gpiochip_irq_relres(struct irq_data *d)
 }
 EXPORT_SYMBOL(gpiochip_irq_relres);
 
+static unsigned int gpiochip_chained_parent_virq(struct gpio_chip *gc,
+						 struct irq_data *d)
+{
+	struct gpio_irq_chip *girq = &gc->irq;
+	unsigned int hwirq = irqd_to_hwirq(d);
+	struct irq_desc *desc;
+
+	if (!girq->parent_handler)
+		return 0;
+
+	if (!girq->initialized || !girq->num_parents || !girq->parents)
+		return 0;
+
+	if (girq->num_parents == 1)
+		return girq->parents[0];
+
+	if (girq->map) {
+		if (!gpiochip_irqchip_irq_valid(gc, hwirq))
+			return 0;
+		return girq->map[hwirq];
+	}
+
+	desc = irq_to_desc(d->irq);
+	if (desc && desc->parent_irq)
+		return desc->parent_irq;
+
+	return 0;
+}
+
+/**
+ * gpiochip_irq_set_affinity() - irq_chip set_affinity helper for GPIO lines
+ * @d: Pointer to the GPIO line irq_data
+ * @dest: Affinity mask to set
+ * @force: True to enforce the mask without online CPU checks
+ *
+ * Hierarchical irqdomains with parent_data delegate through
+ * irq_chip_set_affinity_parent(). GPIO irqchips that use a chained parent
+ * handler resolve the parent virq and apply the mask through the parent
+ * irq_chip as described in gpiochip_chained_parent_virq().
+ *
+ * Affinity is programmed on the parent irq, not independently per line. GPIO
+ * lines that share one parent irq also share its affinity; updating one line
+ * replaces the mask for all lines on that parent.
+ *
+ * Return: IRQ_SET_MASK_* on success, or a negative errno on error.
+ */
+int gpiochip_irq_set_affinity(struct irq_data *d,
+			      const struct cpumask *dest, bool force)
+{
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct irq_data *pdata;
+	unsigned int parent;
+	int ret;
+
+	if (d->parent_data)
+		return irq_chip_set_affinity_parent(d, dest, force);
+
+	parent = gpiochip_chained_parent_virq(gc, d);
+	if (!parent)
+		return -EINVAL;
+
+	pdata = irq_get_irq_data(parent);
+	if (!pdata || !pdata->chip || !pdata->chip->irq_set_affinity)
+		return -EINVAL;
+
+	ret = pdata->chip->irq_set_affinity(pdata, dest, force);
+	switch (ret) {
+	case IRQ_SET_MASK_OK:
+	case IRQ_SET_MASK_OK_NOCOPY:
+	case IRQ_SET_MASK_OK_DONE:
+		irq_data_update_effective_affinity(d,
+			irq_data_get_effective_affinity_mask(pdata));
+		return IRQ_SET_MASK_OK;
+	default:
+		return ret;
+	}
+}
+EXPORT_SYMBOL_GPL(gpiochip_irq_set_affinity);
+
 static void gpiochip_irq_mask(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
diff --git a/include/linux/gpio/driver.h b/include/linux/gpio/driver.h
index 4077dc678cae..a772bf1707aa 100644
--- a/include/linux/gpio/driver.h
+++ b/include/linux/gpio/driver.h
@@ -658,12 +658,17 @@ void gpiochip_enable_irq(struct gpio_chip *gc, unsigned int offset);
 /* irq_data versions of the above */
 int gpiochip_irq_reqres(struct irq_data *data);
 void gpiochip_irq_relres(struct irq_data *data);
+int gpiochip_irq_set_affinity(struct irq_data *data,
+			      const struct cpumask *dest, bool force);
 
 /* Paste this in your irq_chip structure  */
 #define	GPIOCHIP_IRQ_RESOURCE_HELPERS					\
 		.irq_request_resources  = gpiochip_irq_reqres,		\
 		.irq_release_resources  = gpiochip_irq_relres
 
+#define GPIOCHIP_IRQ_AFFINITY_HELPERS					\
+		.irq_set_affinity = gpiochip_irq_set_affinity
+
 static inline void gpio_irq_chip_set_chip(struct gpio_irq_chip *girq,
 					  const struct irq_chip *chip)
 {
-- 
2.54.0 (Apple Git-157)