--- a/arch/mips/lantiq/irq.c 2019-02-16 16:06:12.647092281 +0100 +++ b/arch/mips/lantiq/irq.c 2019-03-14 02:00:47.721984102 +0100 @@ -23,13 +23,13 @@ #include /* register definitions - internal irqs */ -#define LTQ_ICU_IM0_ISR 0x0000 -#define LTQ_ICU_IM0_IER 0x0008 -#define LTQ_ICU_IM0_IOSR 0x0010 -#define LTQ_ICU_IM0_IRSR 0x0018 -#define LTQ_ICU_IM0_IMR 0x0020 -#define LTQ_ICU_IM1_ISR 0x0028 -#define LTQ_ICU_OFFSET (LTQ_ICU_IM1_ISR - LTQ_ICU_IM0_ISR) +#define LTQ_ICU_ISR 0x0000 +#define LTQ_ICU_IER 0x0008 +#define LTQ_ICU_IOSR 0x0010 +#define LTQ_ICU_IRSR 0x0018 +#define LTQ_ICU_IMR 0x0020 +//#define LTQ_ICU_IM1_ISR 0x0028 +//#define LTQ_ICU_OFFSET (LTQ_ICU_IM1_ISR - LTQ_ICU_ISR) /* register definitions - external irqs */ #define LTQ_EIU_EXIN_C 0x0000 @@ -49,8 +49,8 @@ */ #define LTQ_ICU_EBU_IRQ 22 -#define ltq_icu_w32(m, x, y) ltq_w32((x), ltq_icu_membase[m] + (y)) -#define ltq_icu_r32(m, x) ltq_r32(ltq_icu_membase[m] + (x)) +#define ltq_icu_w32(vpe, m, x, y) ltq_w32((x), ltq_icu_membase[vpe] + m*0x28 + (y)) +#define ltq_icu_r32(vpe, m, x) ltq_r32(ltq_icu_membase[vpe] + m*0x28 + (x)) #define ltq_eiu_w32(x, y) ltq_w32((x), ltq_eiu_membase + (y)) #define ltq_eiu_r32(x) ltq_r32(ltq_eiu_membase + (x)) @@ -62,11 +62,21 @@ /* we have a cascade of 8 irqs */ #define MIPS_CPU_IRQ_CASCADE 8 +#define MAX_VPES 2 + +/* + * Convenience Macro. Should be somewhere generic. + */ +#define get_current_vpe() \ + ((read_c0_tcbind() >> TCBIND_CURVPE_SHIFT) & TCBIND_CURVPE) + static int exin_avail; static u32 ltq_eiu_irq[MAX_EIU]; -static void __iomem *ltq_icu_membase[MAX_IM]; +static void __iomem *ltq_icu_membase[MAX_VPES]; static void __iomem *ltq_eiu_membase; static struct irq_domain *ltq_domain; +static DEFINE_SPINLOCK(ltq_eiu_lock); +static DEFINE_RAW_SPINLOCK(ltq_icu_lock); static int ltq_perfcount_irq; int ltq_eiu_get_irq(int exin) @@ -78,50 +88,87 @@ void ltq_disable_irq(struct irq_data *d) { - u32 ier = LTQ_ICU_IM0_IER; int offset = d->hwirq - MIPS_CPU_IRQ_CASCADE; int im = offset / INT_NUM_IM_OFFSET; + int vpe; + unsigned long flags; offset %= INT_NUM_IM_OFFSET; - ltq_icu_w32(im, ltq_icu_r32(im, ier) & ~BIT(offset), ier); + + raw_spin_lock_irqsave(<q_icu_lock, flags); + for_each_present_cpu(vpe) { + ltq_icu_w32(vpe, im, ltq_icu_r32(vpe, im, LTQ_ICU_IER) & ~BIT(offset), LTQ_ICU_IER); +//mb(); + } + raw_spin_unlock_irqrestore(<q_icu_lock, flags); } void ltq_mask_and_ack_irq(struct irq_data *d) { - u32 ier = LTQ_ICU_IM0_IER; - u32 isr = LTQ_ICU_IM0_ISR; int offset = d->hwirq - MIPS_CPU_IRQ_CASCADE; int im = offset / INT_NUM_IM_OFFSET; - + int vpe; + unsigned long flags; + offset %= INT_NUM_IM_OFFSET; - ltq_icu_w32(im, ltq_icu_r32(im, ier) & ~BIT(offset), ier); - ltq_icu_w32(im, BIT(offset), isr); + + raw_spin_lock_irqsave(<q_icu_lock, flags); + for_each_present_cpu(vpe) { + ltq_icu_w32(vpe, im, ltq_icu_r32(vpe, im, LTQ_ICU_IER) & ~BIT(offset), LTQ_ICU_IER); +//mb(); + ltq_icu_w32(vpe, im, BIT(offset), LTQ_ICU_ISR); +//mb(); + } + raw_spin_unlock_irqrestore(<q_icu_lock, flags); } EXPORT_SYMBOL(ltq_mask_and_ack_irq); static void ltq_ack_irq(struct irq_data *d) { - u32 isr = LTQ_ICU_IM0_ISR; int offset = d->hwirq - MIPS_CPU_IRQ_CASCADE; int im = offset / INT_NUM_IM_OFFSET; + int vpe; + unsigned long flags; offset %= INT_NUM_IM_OFFSET; - ltq_icu_w32(im, BIT(offset), isr); + + raw_spin_lock_irqsave(<q_icu_lock, flags); + for_each_present_cpu(vpe) { + ltq_icu_w32(vpe, im, BIT(offset), LTQ_ICU_ISR); +//mb(); + } + raw_spin_unlock_irqrestore(<q_icu_lock, flags); } +//struct cpumask *mask_val + void ltq_enable_irq(struct irq_data *d) { - u32 ier = LTQ_ICU_IM0_IER; int offset = d->hwirq - MIPS_CPU_IRQ_CASCADE; int im = offset / INT_NUM_IM_OFFSET; + int vpe; + unsigned long flags; offset %= INT_NUM_IM_OFFSET; - ltq_icu_w32(im, ltq_icu_r32(im, ier) | BIT(offset), ier); + + raw_spin_lock_irqsave(<q_icu_lock, flags); + +#if 1 + vpe = cpumask_next(get_current_vpe(), irq_data_get_effective_affinity_mask(d)); + if (vpe >= nr_cpu_ids) { + vpe = cpumask_first(irq_data_get_effective_affinity_mask(d)); + } +#endif + + ltq_icu_w32(vpe, im, ltq_icu_r32(vpe, im, LTQ_ICU_IER) | BIT(offset), LTQ_ICU_IER); + + raw_spin_unlock_irqrestore(<q_icu_lock, flags); } static int ltq_eiu_settype(struct irq_data *d, unsigned int type) { int i; + unsigned long flags; for (i = 0; i < exin_avail; i++) { if (d->hwirq == ltq_eiu_irq[i]) { @@ -158,8 +205,11 @@ if (edge) irq_set_handler(d->hwirq, handle_edge_irq); - ltq_eiu_w32(ltq_eiu_r32(LTQ_EIU_EXIN_C) | - (val << (i * 4)), LTQ_EIU_EXIN_C); + // v3.10 kernel has this atomic for SMP + spin_lock_irqsave(<q_eiu_lock, flags); + ltq_eiu_w32((ltq_eiu_r32(LTQ_EIU_EXIN_C) & (~ (val << (i * 4)))) | + (val << (i * 4)), LTQ_EIU_EXIN_C); + spin_unlock_irqrestore(<q_eiu_lock, flags); } } @@ -203,14 +253,46 @@ } } +#if defined(CONFIG_SMP) +static int ltq_icu_irq_set_affinity(struct irq_data *d, + const struct cpumask *cpumask, bool force) +{ +// int offset = d->hwirq - MIPS_CPU_IRQ_CASCADE; +// int im = offset / INT_NUM_IM_OFFSET; + unsigned long flags; + struct cpumask tmask; + +// offset %= INT_NUM_IM_OFFSET; + + raw_spin_lock_irqsave(<q_icu_lock, flags); + + if (!cpumask_and(&tmask, cpumask, cpu_online_mask)) { + raw_spin_unlock_irqrestore(<q_icu_lock, flags); + return -EINVAL; + } + + irq_data_update_effective_affinity(d, &tmask); + + raw_spin_unlock_irqrestore(<q_icu_lock, flags); + + return IRQ_SET_MASK_OK; +} +#endif + static struct irq_chip ltq_irq_type = { .name = "icu", .irq_enable = ltq_enable_irq, .irq_disable = ltq_disable_irq, - .irq_unmask = ltq_enable_irq, + +.irq_unmask = ltq_enable_irq, //puvodne enable +.irq_mask = ltq_disable_irq, //puvodne disable + .irq_ack = ltq_ack_irq, - .irq_mask = ltq_disable_irq, + .irq_mask_ack = ltq_mask_and_ack_irq, +#if defined(CONFIG_SMP) + .irq_set_affinity = ltq_icu_irq_set_affinity, +#endif }; static struct irq_chip ltq_eiu_type = { @@ -219,10 +301,17 @@ .irq_shutdown = ltq_shutdown_eiu_irq, .irq_enable = ltq_enable_irq, .irq_disable = ltq_disable_irq, - .irq_unmask = ltq_enable_irq, .irq_ack = ltq_ack_irq, + + .irq_unmask = ltq_enable_irq, .irq_mask = ltq_disable_irq, + .irq_mask_ack = ltq_mask_and_ack_irq, + +#if defined(CONFIG_SMP) + .irq_set_affinity = ltq_icu_irq_set_affinity, +#endif + .irq_set_type = ltq_eiu_settype, }; @@ -231,8 +320,10 @@ int module = irq_desc_get_irq(desc) - 2; u32 irq; int hwirq; + int vpe = get_current_vpe(); - irq = ltq_icu_r32(module, LTQ_ICU_IM0_IOSR); +//v3.10 has lock_vpe around this, is it really necessary?, no MT + irq = ltq_icu_r32(vpe, module, LTQ_ICU_IOSR); if (irq == 0) return; @@ -253,6 +344,7 @@ static int icu_map(struct irq_domain *d, unsigned int irq, irq_hw_number_t hw) { struct irq_chip *chip = <q_irq_type; + struct irq_data *data; int i; if (hw < MIPS_CPU_IRQ_CASCADE) @@ -262,6 +354,10 @@ if (hw == ltq_eiu_irq[i]) chip = <q_eiu_type; + data = irq_get_irq_data(irq); + + irq_data_update_effective_affinity(data, cpumask_of(0)); + irq_set_chip_and_handler(irq, chip, handle_level_irq); return 0; @@ -276,28 +372,37 @@ { struct device_node *eiu_node; struct resource res; - int i, ret; - - for (i = 0; i < MAX_IM; i++) { - if (of_address_to_resource(node, i, &res)) - panic("Failed to get icu memory range"); + int i, ret, vpe; +//TODO two resources, one for VPE0, second for VPE1, one icu is continuous array +/* TODO assumes 2 VPEs equals 2 ICU, is it true for all boards? */ + for_each_possible_cpu(vpe) { + if (of_address_to_resource(node, vpe, &res)) + panic("Failed to get icu%i memory range", vpe); + if (!request_mem_region(res.start, resource_size(&res), res.name)) - pr_err("Failed to request icu memory"); - - ltq_icu_membase[i] = ioremap_nocache(res.start, + pr_err("Failed to request icu%i memory\n", vpe); + + ltq_icu_membase[vpe] = ioremap_nocache(res.start, resource_size(&res)); - if (!ltq_icu_membase[i]) - panic("Failed to remap icu memory"); + + if (!ltq_icu_membase[vpe]) + panic("Failed to remap icu%i memory", vpe); } /* turn off all irqs by default */ - for (i = 0; i < MAX_IM; i++) { - /* make sure all irqs are turned off by default */ - ltq_icu_w32(i, 0, LTQ_ICU_IM0_IER); - /* clear all possibly pending interrupts */ - ltq_icu_w32(i, ~0, LTQ_ICU_IM0_ISR); + for_each_possible_cpu(vpe) { + for (i = 0; i < MAX_IM; i++) { + /* make sure all irqs are turned off by default */ + ltq_icu_w32(vpe, i, 0, LTQ_ICU_IER); + + /* clear all possibly pending interrupts */ + ltq_icu_w32(vpe, i, ~0, LTQ_ICU_ISR); + ltq_icu_w32(vpe, i, ~0, LTQ_ICU_IMR); + + ltq_icu_w32(vpe, i, 0, LTQ_ICU_IRSR); + } } mips_cpu_irq_init();