sparc32,leon: implement genirq CPU affinity

A simple implementation of CPU affinity, the first CPU in
the affinity CPU mask always takes the IRQ.

Signed-off-by: Daniel Hellstrom <daniel@gaisler.com>
Acked-by: Sam Ravnborg <sam@ravnborg.org>
Signed-off-by: David S. Miller <davem@davemloft.net>

diff --git a/arch/sparc/kernel/leon_kernel.c b/arch/sparc/kernel/leon_kernel.c
index d8fafeacb4961eb42c326f337e4f770fe59c7f21..ab1458454422de0f320e0e41fcf299beead87ec7 100644 (file)
--- a/arch/sparc/kernel/leon_kernel.c
+++ b/arch/sparc/kernel/leon_kernel.c
@@ -99,25 +99,68 @@ static inline unsigned long get_irqmask(unsigned int irq)
        return mask;
 }
 
+#ifdef CONFIG_SMP
+static int irq_choose_cpu(const struct cpumask *affinity)
+{
+       cpumask_t mask;
+
+       cpus_and(mask, cpu_online_map, *affinity);
+       if (cpus_equal(mask, cpu_online_map) || cpus_empty(mask))
+               return 0;
+       else
+               return first_cpu(mask);
+}
+#else
+#define irq_choose_cpu(affinity) 0
+#endif
+
+static int leon_set_affinity(struct irq_data *data, const struct cpumask *dest,
+                            bool force)
+{
+       unsigned long mask, oldmask, flags;
+       int oldcpu, newcpu;
+
+       mask = (unsigned long)data->chip_data;
+       oldcpu = irq_choose_cpu(data->affinity);
+       newcpu = irq_choose_cpu(dest);
+
+       if (oldcpu == newcpu)
+               goto out;
+
+       /* unmask on old CPU first before enabling on the selected CPU */
+       spin_lock_irqsave(&leon_irq_lock, flags);
+       oldmask = LEON3_BYPASS_LOAD_PA(LEON_IMASK(oldcpu));
+       LEON3_BYPASS_STORE_PA(LEON_IMASK(oldcpu), (oldmask & ~mask));
+       oldmask = LEON3_BYPASS_LOAD_PA(LEON_IMASK(newcpu));
+       LEON3_BYPASS_STORE_PA(LEON_IMASK(newcpu), (oldmask | mask));
+       spin_unlock_irqrestore(&leon_irq_lock, flags);
+out:
+       return IRQ_SET_MASK_OK;
+}
+
 static void leon_unmask_irq(struct irq_data *data)
 {
        unsigned long mask, oldmask, flags;
+       int cpu;
 
        mask = (unsigned long)data->chip_data;
+       cpu = irq_choose_cpu(data->affinity);
        spin_lock_irqsave(&leon_irq_lock, flags);
-       oldmask = LEON3_BYPASS_LOAD_PA(LEON_IMASK(0));
-       LEON3_BYPASS_STORE_PA(LEON_IMASK(0), (oldmask | mask));
+       oldmask = LEON3_BYPASS_LOAD_PA(LEON_IMASK(cpu));
+       LEON3_BYPASS_STORE_PA(LEON_IMASK(cpu), (oldmask | mask));
        spin_unlock_irqrestore(&leon_irq_lock, flags);
 }
 
 static void leon_mask_irq(struct irq_data *data)
 {
        unsigned long mask, oldmask, flags;
+       int cpu;
 
        mask = (unsigned long)data->chip_data;
+       cpu = irq_choose_cpu(data->affinity);
        spin_lock_irqsave(&leon_irq_lock, flags);
-       oldmask = LEON3_BYPASS_LOAD_PA(LEON_IMASK(0));
-       LEON3_BYPASS_STORE_PA(LEON_IMASK(0), (oldmask & ~mask));
+       oldmask = LEON3_BYPASS_LOAD_PA(LEON_IMASK(cpu));
+       LEON3_BYPASS_STORE_PA(LEON_IMASK(cpu), (oldmask & ~mask));
        spin_unlock_irqrestore(&leon_irq_lock, flags);
 }
 
@@ -144,12 +187,13 @@ static void leon_eoi_irq(struct irq_data *data)
 }
 
 static struct irq_chip leon_irq = {
-       .name           = "leon",
-       .irq_startup    = leon_startup_irq,
-       .irq_shutdown   = leon_shutdown_irq,
-       .irq_mask       = leon_mask_irq,
-       .irq_unmask     = leon_unmask_irq,
-       .irq_eoi        = leon_eoi_irq,
+       .name                   = "leon",
+       .irq_startup            = leon_startup_irq,
+       .irq_shutdown           = leon_shutdown_irq,
+       .irq_mask               = leon_mask_irq,
+       .irq_unmask             = leon_unmask_irq,
+       .irq_eoi                = leon_eoi_irq,
+       .irq_set_affinity       = leon_set_affinity,
 };
 
 /*