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

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