target/linux/realtek/patches-5.10/315-irqchip-irq-realtek-rtl-add-VPE-support.patch

   1 --- a/drivers/irqchip/irq-realtek-rtl.c
   2 +++ b/drivers/irqchip/irq-realtek-rtl.c
   3 @@ -21,21 +21,63 @@
   4  #define RTL_ICTL_IRR2          0x10
   5  #define RTL_ICTL_IRR3          0x14
   6
   7 -#define REG(x)         (realtek_ictl_base + x)
   8 +#define RTL_ICTL_NUM_INPUTS    32
   9 +#define RTL_ICTL_NUM_OUTPUTS   15
  10
  11  static DEFINE_RAW_SPINLOCK(irq_lock);
  12 -static void __iomem *realtek_ictl_base;
  13 +
  14 +#define REG(offset, cpu)       (realtek_ictl_base[cpu] + offset)
  15 +
  16 +static void __iomem *realtek_ictl_base[NR_CPUS];
  17 +static cpumask_t realtek_ictl_cpu_configurable;
  18 +
  19 +struct realtek_ictl_output {
  20 +       /* IRQ controller data */
  21 +       struct fwnode_handle *fwnode;
  22 +       /* Output specific data */
  23 +       unsigned int output_index;
  24 +       struct irq_domain *domain;
  25 +       u32 child_mask;
  26 +};
  27 +
  28 +/*
  29 + * IRR0-IRR3 store 4 bits per interrupt, but Realtek uses inverted numbering,
  30 + * placing IRQ 31 in the first four bits. A routing value of '0' means the
  31 + * interrupt is left disconnected. Routing values {1..15} connect to output
  32 + * lines {0..14}.
  33 + */
  34 +#define IRR_OFFSET(idx)                (4 * (3 - (idx * 4) / 32))
  35 +#define IRR_SHIFT(idx)         ((idx * 4) % 32)
  36 +
  37 +static inline u32 read_irr(void __iomem *irr0, int idx)
  38 +{
  39 +       return (readl(irr0 + IRR_OFFSET(idx)) >> IRR_SHIFT(idx)) & 0xf;
  40 +}
  41 +
  42 +static inline void write_irr(void __iomem *irr0, int idx, u32 value)
  43 +{
  44 +       unsigned int offset = IRR_OFFSET(idx);
  45 +       unsigned int shift = IRR_SHIFT(idx);
  46 +       u32 irr;
  47 +
  48 +       irr = readl(irr0 + offset) & ~(0xf << shift);
  49 +       irr |= (value & 0xf) << shift;
  50 +       writel(irr, irr0 + offset);
  51 +}
  52
  53  static void realtek_ictl_unmask_irq(struct irq_data *i)
  54  {
  55         unsigned long flags;
  56         u32 value;
  57 +       int cpu;
  58
  59         raw_spin_lock_irqsave(&irq_lock, flags);
  60
  61 -       value = readl(REG(RTL_ICTL_GIMR));
  62 -       value |= BIT(i->hwirq);
  63 -       writel(value, REG(RTL_ICTL_GIMR));
  64 +       for_each_cpu(cpu, &realtek_ictl_cpu_configurable) {
  65 +               value = readl(REG(RTL_ICTL_GIMR, cpu));
  66 +               value |= BIT(i->hwirq);
  67 +               writel(value, REG(RTL_ICTL_GIMR, cpu));
  68 +       }
  69
  70         raw_spin_unlock_irqrestore(&irq_lock, flags);
  71  }
  72 @@ -44,137 +86,247 @@ static void realtek_ictl_mask_irq(struct
  73  {
  74         unsigned long flags;
  75         u32 value;
  76 +       int cpu;
  77
  78         raw_spin_lock_irqsave(&irq_lock, flags);
  79
  80 -       value = readl(REG(RTL_ICTL_GIMR));
  81 -       value &= ~BIT(i->hwirq);
  82 -       writel(value, REG(RTL_ICTL_GIMR));
  83 +       for_each_cpu(cpu, &realtek_ictl_cpu_configurable) {
  84 +               value = readl(REG(RTL_ICTL_GIMR, cpu));
  85 +               value &= ~BIT(i->hwirq);
  86 +               writel(value, REG(RTL_ICTL_GIMR, cpu));
  87 +       }
  88
  89         raw_spin_unlock_irqrestore(&irq_lock, flags);
  90  }
  91
  92 +static int __maybe_unused realtek_ictl_irq_affinity(struct irq_data *i,
  93 +       const struct cpumask *dest, bool force)
  94 +{
  95 +       struct realtek_ictl_output *output = i->domain->host_data;
  96 +       cpumask_t cpu_configure;
  97 +       cpumask_t cpu_disable;
  98 +       cpumask_t cpu_enable;
  99 +       unsigned long flags;
 100 +       int cpu;
 101 +
 102 +       raw_spin_lock_irqsave(&irq_lock, flags);
 103 +
 104 +       cpumask_and(&cpu_configure, cpu_present_mask, &realtek_ictl_cpu_configurable);
 105 +
 106 +       cpumask_and(&cpu_enable, &cpu_configure, dest);
 107 +       cpumask_andnot(&cpu_disable, &cpu_configure, dest);
 108 +
 109 +       for_each_cpu(cpu, &cpu_disable)
 110 +               write_irr(REG(RTL_ICTL_IRR0, cpu), i->hwirq, 0);
 111 +
 112 +       for_each_cpu(cpu, &cpu_enable)
 113 +               write_irr(REG(RTL_ICTL_IRR0, cpu), i->hwirq, output->output_index + 1);
 114 +
 115 +       irq_data_update_effective_affinity(i, &cpu_enable);
 116 +
 117 +       raw_spin_unlock_irqrestore(&irq_lock, flags);
 118 +
 119 +       return IRQ_SET_MASK_OK;
 120 +}
 121 +
 122  static struct irq_chip realtek_ictl_irq = {
 123         .name = "realtek-rtl-intc",
 124         .irq_mask = realtek_ictl_mask_irq,
 125         .irq_unmask = realtek_ictl_unmask_irq,
 126 +#ifdef CONFIG_SMP
 127 +       .irq_set_affinity = realtek_ictl_irq_affinity,
 128 +#endif
 129  };
 130
 131  static int intc_map(struct irq_domain *d, unsigned int irq, irq_hw_number_t hw)
 132  {
 133 -       irq_set_chip_and_handler(hw, &realtek_ictl_irq, handle_level_irq);
 134 +       struct realtek_ictl_output *output = d->host_data;
 135 +       unsigned long flags;
 136 +
 137 +       irq_set_chip_and_handler(irq, &realtek_ictl_irq, handle_level_irq);
 138 +
 139 +       raw_spin_lock_irqsave(&irq_lock, flags);
 140 +
 141 +       output->child_mask |= BIT(hw);
 142 +       write_irr(REG(RTL_ICTL_IRR0, 0), hw, output->output_index + 1);
 143 +
 144 +       raw_spin_unlock_irqrestore(&irq_lock, flags);
 145
 146         return 0;
 147  }
 148
 149 +static int intc_select(struct irq_domain *d, struct irq_fwspec *fwspec,
 150 +       enum irq_domain_bus_token bus_token)
 151 +{
 152 +       struct realtek_ictl_output *output = d->host_data;
 153 +       bool routed_elsewhere;
 154 +       unsigned long flags;
 155 +       u32 routing_old;
 156 +       int cpu;
 157 +
 158 +       if (fwspec->fwnode != output->fwnode)
 159 +               return false;
 160 +
 161 +       /* Original specifiers had only one parameter */
 162 +       if (fwspec->param_count < 2)
 163 +               return true;
 164 +
 165 +       raw_spin_lock_irqsave(&irq_lock, flags);
 166 +
 167 +       /*
 168 +        * Inputs can only be routed to one output, so they shouldn't be
 169 +        * allowed to end up in multiple domains.
 170 +        */
 171 +       for_each_cpu(cpu, &realtek_ictl_cpu_configurable) {
 172 +               routing_old = read_irr(REG(RTL_ICTL_IRR0, cpu), fwspec->param[0]);
 173 +               routed_elsewhere = routing_old && fwspec->param[1] != routing_old - 1;
 174 +               if (routed_elsewhere) {
 175 +                       pr_warn("soc int %d already routed to output %d\n",
 176 +                               fwspec->param[0], routing_old - 1);
 177 +                       break;
 178 +               }
 179 +       }
 180 +
 181 +       raw_spin_unlock_irqrestore(&irq_lock, flags);
 182 +
 183 +       return !routed_elsewhere && fwspec->param[1] == output->output_index;
 184 +}
 185 +
 186  static const struct irq_domain_ops irq_domain_ops = {
 187         .map = intc_map,
 188 +       .select = intc_select,
 189         .xlate = irq_domain_xlate_onecell,
 190  };
 191
 192  static void realtek_irq_dispatch(struct irq_desc *desc)
 193  {
 194 +       struct realtek_ictl_output *output = irq_desc_get_handler_data(desc);
 195         struct irq_chip *chip = irq_desc_get_chip(desc);
 196 -       struct irq_domain *domain;
 197 -       unsigned int pending;
 198 +       int cpu = smp_processor_id();
 199 +       unsigned long pending;
 200 +       unsigned int soc_int;
 201
 202         chained_irq_enter(chip, desc);
 203 -       pending = readl(REG(RTL_ICTL_GIMR)) & readl(REG(RTL_ICTL_GISR));
 204 +       pending = readl(REG(RTL_ICTL_GIMR, cpu)) & readl(REG(RTL_ICTL_GISR, cpu))
 205 +               & output->child_mask;
 206 +
 207         if (unlikely(!pending)) {
 208                 spurious_interrupt();
 209                 goto out;
 210         }
 211 -       domain = irq_desc_get_handler_data(desc);
 212 -       generic_handle_irq(irq_find_mapping(domain, __ffs(pending)));
 213 +
 214 +       for_each_set_bit(soc_int, &pending, RTL_ICTL_NUM_INPUTS)
 215 +               generic_handle_irq(irq_find_mapping(output->domain, soc_int));
 216 +//             generic_handle_domain_irq(output->domain, soc_int);
 217
 218  out:
 219         chained_irq_exit(chip, desc);
 220  }
 221
 222 -/*
 223 - * SoC interrupts are cascaded to MIPS CPU interrupts according to the
 224 - * interrupt-map in the device tree. Each SoC interrupt gets 4 bits for
 225 - * the CPU interrupt in an Interrupt Routing Register. Max 32 SoC interrupts
 226 - * thus go into 4 IRRs.
 227 - */
 228 -static int __init map_interrupts(struct device_node *node, struct irq_domain *domain)
 229 +static int __init setup_parent_interrupts(struct device_node *node, int *parents,
 230 +       unsigned int num_parents)
 231  {
 232 -       struct device_node *cpu_ictl;
 233 -       const __be32 *imap;
 234 -       u32 imaplen, soc_int, cpu_int, tmp, regs[4];
 235 -       int ret, i, irr_regs[] = {
 236 -               RTL_ICTL_IRR3,
 237 -               RTL_ICTL_IRR2,
 238 -               RTL_ICTL_IRR1,
 239 -               RTL_ICTL_IRR0,
 240 -       };
 241 -       u8 mips_irqs_set;
 242 +       struct realtek_ictl_output *outputs;
 243 +       struct realtek_ictl_output *output;
 244 +       struct irq_domain *domain;
 245 +       unsigned int p;
 246
 247 -       ret = of_property_read_u32(node, "#address-cells", &tmp);
 248 -       if (ret || tmp)
 249 -               return -EINVAL;
 250 +       outputs = kcalloc(num_parents, sizeof(*outputs), GFP_KERNEL);
 251 +       if (!outputs)
 252 +               return -ENOMEM;
 253
 254 -       imap = of_get_property(node, "interrupt-map", &imaplen);
 255 -       if (!imap || imaplen % 3)
 256 -               return -EINVAL;
 257 +       for (p = 0; p < num_parents; p++) {
 258 +               output = outputs + p;
 259
 260 -       mips_irqs_set = 0;
 261 -       memset(regs, 0, sizeof(regs));
 262 -       for (i = 0; i < imaplen; i += 3 * sizeof(u32)) {
 263 -               soc_int = be32_to_cpup(imap);
 264 -               if (soc_int > 31)
 265 -                       return -EINVAL;
 266 -
 267 -               cpu_ictl = of_find_node_by_phandle(be32_to_cpup(imap + 1));
 268 -               if (!cpu_ictl)
 269 -                       return -EINVAL;
 270 -               ret = of_property_read_u32(cpu_ictl, "#interrupt-cells", &tmp);
 271 -               if (ret || tmp != 1)
 272 -                       return -EINVAL;
 273 -               of_node_put(cpu_ictl);
 274 -
 275 -               cpu_int = be32_to_cpup(imap + 2);
 276 -               if (cpu_int > 7)
 277 -                       return -EINVAL;
 278 -
 279 -               if (!(mips_irqs_set & BIT(cpu_int))) {
 280 -                       irq_set_chained_handler_and_data(cpu_int, realtek_irq_dispatch,
 281 -                                                        domain);
 282 -                       mips_irqs_set |= BIT(cpu_int);
 283 -               }
 284 +               domain = irq_domain_add_linear(node, RTL_ICTL_NUM_INPUTS, &irq_domain_ops, output);
 285 +               if (!domain)
 286 +                       goto domain_err;
 287
 288 -               regs[(soc_int * 4) / 32] |= cpu_int << (soc_int * 4) % 32;
 289 -               imap += 3;
 290 -       }
 291 +               output->fwnode = of_node_to_fwnode(node);
 292 +               output->output_index = p;
 293 +               output->domain = domain;
 294
 295 -       for (i = 0; i < 4; i++)
 296 -               writel(regs[i], REG(irr_regs[i]));
 297 +               irq_set_chained_handler_and_data(parents[p], realtek_irq_dispatch, output);
 298 +       }
 299
 300         return 0;
 301 +
 302 +domain_err:
 303 +       while (p--) {
 304 +               irq_set_chained_handler_and_data(parents[p], NULL, NULL);
 305 +               irq_domain_remove(outputs[p].domain);
 306 +       }
 307 +
 308 +       kfree(outputs);
 309 +
 310 +       return -ENOMEM;
 311  }
 312
 313  static int __init realtek_rtl_of_init(struct device_node *node, struct device_node *parent)
 314  {
 315 -       struct irq_domain *domain;
 316 -       int ret;
 317 +       int parent_irqs[RTL_ICTL_NUM_OUTPUTS];
 318 +       struct of_phandle_args oirq;
 319 +       unsigned int num_parents;
 320 +       unsigned int soc_irq;
 321 +       unsigned int p;
 322 +       int cpu;
 323 +
 324 +       cpumask_clear(&realtek_ictl_cpu_configurable);
 325 +
 326 +       for (cpu = 0; cpu < NR_CPUS; cpu++) {
 327 +               realtek_ictl_base[cpu] = of_iomap(node, cpu);
 328 +               if (realtek_ictl_base[cpu]) {
 329 +                       cpumask_set_cpu(cpu, &realtek_ictl_cpu_configurable);
 330 +
 331 +                       /* Disable all cascaded interrupts and clear routing */
 332 +                       writel(0, REG(RTL_ICTL_GIMR, cpu));
 333 +                       for (soc_irq = 0; soc_irq < RTL_ICTL_NUM_INPUTS; soc_irq++)
 334 +                               write_irr(REG(RTL_ICTL_IRR0, cpu), soc_irq, 0);
 335 +               }
 336 +       }
 337
 338 -       realtek_ictl_base = of_iomap(node, 0);
 339 -       if (!realtek_ictl_base)
 340 +       if (cpumask_empty(&realtek_ictl_cpu_configurable))
 341                 return -ENXIO;
 342
 343 -       /* Disable all cascaded interrupts */
 344 -       writel(0, REG(RTL_ICTL_GIMR));
 345 +       num_parents = of_irq_count(node);
 346 +       if (num_parents > RTL_ICTL_NUM_OUTPUTS) {
 347 +               pr_err("too many parent interrupts\n");
 348 +               return -EINVAL;
 349 +       }
 350
 351 -       domain = irq_domain_add_simple(node, 32, 0,
 352 -                                      &irq_domain_ops, NULL);
 353 +       for (p = 0; p < num_parents; p++)
 354 +               parent_irqs[p] = of_irq_get(node, p);
 355
 356 -       ret = map_interrupts(node, domain);
 357 -       if (ret) {
 358 -               pr_err("invalid interrupt map\n");
 359 -               return ret;
 360 +       if (WARN_ON(!num_parents)) {
 361 +               /*
 362 +                * If DT contains no parent interrupts, assume MIPS CPU IRQ 2
 363 +                * (HW0) is connected to the first output. This is the case for
 364 +                * all known hardware anyway. "interrupt-map" is deprecated, so
 365 +                * don't bother trying to parse that.
 366 +                * Since this is to account for old devicetrees with one-cell
 367 +                * interrupt specifiers, only one output domain is needed.
 368 +                */
 369 +               oirq.np = of_find_compatible_node(NULL, NULL, "mti,cpu-interrupt-controller");
 370 +               if (oirq.np) {
 371 +                       oirq.args_count = 1;
 372 +                       oirq.args[0] = 2;
 373 +
 374 +                       parent_irqs[0] = irq_create_of_mapping(&oirq);
 375 +                       num_parents = 1;
 376 +               }
 377 +
 378 +               of_node_put(oirq.np);
 379         }
 380
 381 -       return 0;
 382 +       /* Ensure we haven't collected any errors before proceeding */
 383 +       for (p = 0; p < num_parents; p++) {
 384 +               if (parent_irqs[p] < 0)
 385 +                       return parent_irqs[p];
 386 +               if (!parent_irqs[p])
 387 +                       return -ENODEV;
 388 +       }
 389 +
 390 +       return setup_parent_interrupts(node, &parent_irqs[0], num_parents);
 391  }
 392
 393  IRQCHIP_DECLARE(realtek_rtl_intc, "realtek,rtl-intc", realtek_rtl_of_init);