TEMP: irqchip/irq-pruss-intc: Allow setting irq affinity for PRU events

The PRUSS INTC irqchip implementation provides a standard IRQ API
for consumers to directly use the PRU System Events as interrupts
and uses chained interrupts for connecting these to the main ARM
GIC interrupt controller. Add support to the irqchip driver to set
irq affinity for these consumer PRU interrupts.

These consumer PRU interrupts are not directly connected to the ARM
GIC, so it is not possible to directly set the affinity for these
interrupts. The affinity is instead set on the real system irq or
the actual chained interrupt that the PRU event is mapped to. The
system irq is looked up using the programmed channel map and host
map registers, and is validated against the configured channel and
hosts as the channel map and host map registers are not reset in
pruss_intc_unconfigure().

NOTE:
1. The affinity for a PRU event can only be set after its channel
   map and host map are configured.
2. The PRUSS INTC design allows multiple events on a single system
   irq, so the actual affinity on the corresponding chained interrupt
   will be set to the last programmed affinity on any of the connected
   PRU events. The IRQ affinity data for other events is not updated.

Signed-off-by: Tero Kristo <t-kristo@ti.com>
[s-anna@ti.com: add error checks, update affinity logic & patch description]
Signed-off-by: Suman Anna <s-anna@ti.com>

drivers/irqchip/irq-pruss-intc.c

@@ -403,6 +403,65 @@ static void pruss_intc_irq_relres(struct irq_data *data)
 	module_put(THIS_MODULE);
 }
 
+#ifdef CONFIG_SMP
+static int pruss_intc_irq_set_affinity(struct irq_data *data,
+				       const struct cpumask *mask_val,
+				       bool force)
+{
+	struct pruss_intc *intc = irq_data_get_irq_chip_data(data);
+	u32 ch, host;
+	s8 sch, shost;
+	unsigned int pirq;
+	struct irq_chip *pchip;
+	struct irq_data *pdata;
+	struct cpumask *eff_mask;
+	int ret;
+
+	/* check for stored channel & host config for this event */
+	sch = intc->config_map.sysev_to_ch[data->hwirq];
+	shost = sch != -1 ? intc->config_map.ch_to_host[sch] : -1;
+	if (sch == -1 || shost == -1) {
+		pr_err("%s: event %lu not configured: ch = %d, host = %d\n",
+		       __func__, data->hwirq, sch, shost);
+		return -EINVAL;
+	}
+
+	/* find programmed channel */
+	ch = pruss_intc_read_reg(intc, PRU_INTC_CMR(data->hwirq / 4));
+	ch >>= (data->hwirq % 4) * 8;
+	ch &= 0xf;
+
+	/* find programmed host interrupt */
+	host = pruss_intc_read_reg(intc, PRU_INTC_HMR(ch / 4));
+	host >>= (ch % 4) * 8;
+	host &= 0xf;
+
+	/* check programmed configuration for sanity */
+	if (ch != sch || host != shost) {
+		pr_err("%s: event %lu has mismatched configuration, ch = %d, host = %d\n",
+		       __func__, data->hwirq, sch, shost);
+		return -EINVAL;
+	}
+
+	/* program affinity using parent GIC irqchip and irqdata */
+	pirq = intc->irqs[host - MIN_PRU_HOST_INT];
+	pchip = irq_get_chip(pirq);
+	pdata = irq_get_irq_data(pirq);
+
+	if (pchip && pchip->irq_set_affinity) {
+		ret = pchip->irq_set_affinity(pdata, mask_val, force);
+		if (ret >= 0) {
+			eff_mask = irq_data_get_effective_affinity_mask(pdata);
+			irq_data_update_effective_affinity(data, eff_mask);
+		}
+
+		return ret;
+	}
+
+	return -EINVAL;
+}
+#endif
+
 /**
  * pruss_intc_trigger() - trigger a PRU system event
  * @irq: linux IRQ number associated with a PRU system event
@@ -549,6 +608,9 @@ static int pruss_intc_probe(struct platform_device *pdev)
 	irqchip->irq_retrigger = pruss_intc_irq_retrigger;
 	irqchip->irq_request_resources = pruss_intc_irq_reqres;
 	irqchip->irq_release_resources = pruss_intc_irq_relres;
+#ifdef CONFIG_SMP
+	irqchip->irq_set_affinity = pruss_intc_irq_set_affinity;
+#endif
 	irqchip->name = dev_name(dev);
 	intc->irqchip = irqchip;