target/linux/realtek/patches-5.10/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,137 +86,247 @@ 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 -       irq_set_chip_and_handler(hw, &realtek_ictl_irq, handle_level_irq);
 148 +       struct realtek_ictl_output *output = d->host_data;
 149 +       unsigned long flags;
 150 +
 151 +       irq_set_chip_and_handler(irq, &realtek_ictl_irq, handle_level_irq);
 152 +
 153 +       raw_spin_lock_irqsave(&irq_lock, flags);
 154 +
 155 +       output->child_mask |= BIT(hw);
 156 +       write_irr(REG(RTL_ICTL_IRR0, 0), hw, output->output_index + 1);
 157 +
 158 +       raw_spin_unlock_irqrestore(&irq_lock, flags);
 159
 160         return 0;
 161  }
 162
 163 +static int intc_select(struct irq_domain *d, struct irq_fwspec *fwspec,
 164 +       enum irq_domain_bus_token bus_token)
 165 +{
 166 +       struct realtek_ictl_output *output = d->host_data;
 167 +       bool routed_elsewhere;
 168 +       unsigned long flags;
 169 +       u32 routing_old;
 170 +       int cpu;
 171 +
 172 +       if (fwspec->fwnode != output->fwnode)
 173 +               return false;
 174 +
 175 +       /* Original specifiers had only one parameter */
 176 +       if (fwspec->param_count < 2)
 177 +               return true;
 178 +
 179 +       raw_spin_lock_irqsave(&irq_lock, flags);
 180 +
 181 +       /*
 182 +        * Inputs can only be routed to one output, so they shouldn't be
 183 +        * allowed to end up in multiple domains.
 184 +        */
 185 +       for_each_cpu(cpu, &realtek_ictl_cpu_configurable) {
 186 +               routing_old = read_irr(REG(RTL_ICTL_IRR0, cpu), fwspec->param[0]);
 187 +               routed_elsewhere = routing_old && fwspec->param[1] != routing_old - 1;
 188 +               if (routed_elsewhere) {
 189 +                       pr_warn("soc int %d already routed to output %d\n",
 190 +                               fwspec->param[0], routing_old - 1);
 191 +                       break;
 192 +               }
 193 +       }
 194 +
 195 +       raw_spin_unlock_irqrestore(&irq_lock, flags);
 196 +
 197 +       return !routed_elsewhere && fwspec->param[1] == output->output_index;
 198 +}
 199 +
 200  static const struct irq_domain_ops irq_domain_ops = {
 201         .map = intc_map,
 202 +       .select = intc_select,
 203         .xlate = irq_domain_xlate_onecell,
 204  };
 205
 206  static void realtek_irq_dispatch(struct irq_desc *desc)
 207  {
 208 +       struct realtek_ictl_output *output = irq_desc_get_handler_data(desc);
 209         struct irq_chip *chip = irq_desc_get_chip(desc);
 210 -       struct irq_domain *domain;
 211 -       unsigned int pending;
 212 +       int cpu = smp_processor_id();
 213 +       unsigned long pending;
 214 +       unsigned int soc_int;
 215
 216         chained_irq_enter(chip, desc);
 217 -       pending = readl(REG(RTL_ICTL_GIMR)) & readl(REG(RTL_ICTL_GISR));
 218 +       pending = readl(REG(RTL_ICTL_GIMR, cpu)) & readl(REG(RTL_ICTL_GISR, cpu))
 219 +               & output->child_mask;
 220 +
 221         if (unlikely(!pending)) {
 222                 spurious_interrupt();
 223                 goto out;
 224         }
 225 -       domain = irq_desc_get_handler_data(desc);
 226 -       generic_handle_irq(irq_find_mapping(domain, __ffs(pending)));
 227 +
 228 +       for_each_set_bit(soc_int, &pending, RTL_ICTL_NUM_INPUTS)
 229 +               generic_handle_irq(irq_find_mapping(output->domain, soc_int));
 230 +//             generic_handle_domain_irq(output->domain, soc_int);
 231
 232  out:
 233         chained_irq_exit(chip, desc);
 234  }
 235
 236 -/*
 237 - * SoC interrupts are cascaded to MIPS CPU interrupts according to the
 238 - * interrupt-map in the device tree. Each SoC interrupt gets 4 bits for
 239 - * the CPU interrupt in an Interrupt Routing Register. Max 32 SoC interrupts
 240 - * thus go into 4 IRRs.
 241 - */
 242 -static int __init map_interrupts(struct device_node *node, struct irq_domain *domain)
 243 +static int __init setup_parent_interrupts(struct device_node *node, int *parents,
 244 +       unsigned int num_parents)
 245  {
 246 -       struct device_node *cpu_ictl;
 247 -       const __be32 *imap;
 248 -       u32 imaplen, soc_int, cpu_int, tmp, regs[4];
 249 -       int ret, i, irr_regs[] = {
 250 -               RTL_ICTL_IRR3,
 251 -               RTL_ICTL_IRR2,
 252 -               RTL_ICTL_IRR1,
 253 -               RTL_ICTL_IRR0,
 254 -       };
 255 -       u8 mips_irqs_set;
 256 +       struct realtek_ictl_output *outputs;
 257 +       struct realtek_ictl_output *output;
 258 +       struct irq_domain *domain;
 259 +       unsigned int p;
 260
 261 -       ret = of_property_read_u32(node, "#address-cells", &tmp);
 262 -       if (ret || tmp)
 263 -               return -EINVAL;
 264 +       outputs = kcalloc(num_parents, sizeof(*outputs), GFP_KERNEL);
 265 +       if (!outputs)
 266 +               return -ENOMEM;
 267
 268 -       imap = of_get_property(node, "interrupt-map", &imaplen);
 269 -       if (!imap || imaplen % 3)
 270 -               return -EINVAL;
 271 +       for (p = 0; p < num_parents; p++) {
 272 +               output = outputs + p;
 273
 274 -       mips_irqs_set = 0;
 275 -       memset(regs, 0, sizeof(regs));
 276 -       for (i = 0; i < imaplen; i += 3 * sizeof(u32)) {
 277 -               soc_int = be32_to_cpup(imap);
 278 -               if (soc_int > 31)
 279 -                       return -EINVAL;
 280 -
 281 -               cpu_ictl = of_find_node_by_phandle(be32_to_cpup(imap + 1));
 282 -               if (!cpu_ictl)
 283 -                       return -EINVAL;
 284 -               ret = of_property_read_u32(cpu_ictl, "#interrupt-cells", &tmp);
 285 -               if (ret || tmp != 1)
 286 -                       return -EINVAL;
 287 -               of_node_put(cpu_ictl);
 288 -
 289 -               cpu_int = be32_to_cpup(imap + 2);
 290 -               if (cpu_int > 7)
 291 -                       return -EINVAL;
 292 -
 293 -               if (!(mips_irqs_set & BIT(cpu_int))) {
 294 -                       irq_set_chained_handler_and_data(cpu_int, realtek_irq_dispatch,
 295 -                                                        domain);
 296 -                       mips_irqs_set |= BIT(cpu_int);
 297 -               }
 298 +               domain = irq_domain_add_linear(node, RTL_ICTL_NUM_INPUTS, &irq_domain_ops, output);
 299 +               if (!domain)
 300 +                       goto domain_err;
 301
 302 -               regs[(soc_int * 4) / 32] |= cpu_int << (soc_int * 4) % 32;
 303 -               imap += 3;
 304 -       }
 305 +               output->fwnode = of_node_to_fwnode(node);
 306 +               output->output_index = p;
 307 +               output->domain = domain;
 308
 309 -       for (i = 0; i < 4; i++)
 310 -               writel(regs[i], REG(irr_regs[i]));
 311 +               irq_set_chained_handler_and_data(parents[p], realtek_irq_dispatch, output);
 312 +       }
 313
 314         return 0;
 315 +
 316 +domain_err:
 317 +       while (p--) {
 318 +               irq_set_chained_handler_and_data(parents[p], NULL, NULL);
 319 +               irq_domain_remove(outputs[p].domain);
 320 +       }
 321 +
 322 +       kfree(outputs);
 323 +
 324 +       return -ENOMEM;
 325  }
 326
 327  static int __init realtek_rtl_of_init(struct device_node *node, struct device_node *parent)
 328  {
 329 -       struct irq_domain *domain;
 330 -       int ret;
 331 +       int parent_irqs[RTL_ICTL_NUM_OUTPUTS];
 332 +       struct of_phandle_args oirq;
 333 +       unsigned int num_parents;
 334 +       unsigned int soc_irq;
 335 +       unsigned int p;
 336 +       int cpu;
 337 +
 338 +       cpumask_clear(&realtek_ictl_cpu_configurable);
 339 +
 340 +       for (cpu = 0; cpu < NR_CPUS; cpu++) {
 341 +               realtek_ictl_base[cpu] = of_iomap(node, cpu);
 342 +               if (realtek_ictl_base[cpu]) {
 343 +                       cpumask_set_cpu(cpu, &realtek_ictl_cpu_configurable);
 344 +
 345 +                       /* Disable all cascaded interrupts and clear routing */
 346 +                       writel(0, REG(RTL_ICTL_GIMR, cpu));
 347 +                       for (soc_irq = 0; soc_irq < RTL_ICTL_NUM_INPUTS; soc_irq++)
 348 +                               write_irr(REG(RTL_ICTL_IRR0, cpu), soc_irq, 0);
 349 +               }
 350 +       }
 351
 352 -       realtek_ictl_base = of_iomap(node, 0);
 353 -       if (!realtek_ictl_base)
 354 +       if (cpumask_empty(&realtek_ictl_cpu_configurable))
 355                 return -ENXIO;
 356
 357 -       /* Disable all cascaded interrupts */
 358 -       writel(0, REG(RTL_ICTL_GIMR));
 359 +       num_parents = of_irq_count(node);
 360 +       if (num_parents > RTL_ICTL_NUM_OUTPUTS) {
 361 +               pr_err("too many parent interrupts\n");
 362 +               return -EINVAL;
 363 +       }
 364
 365 -       domain = irq_domain_add_simple(node, 32, 0,
 366 -                                      &irq_domain_ops, NULL);
 367 +       for (p = 0; p < num_parents; p++)
 368 +               parent_irqs[p] = of_irq_get(node, p);
 369
 370 -       ret = map_interrupts(node, domain);
 371 -       if (ret) {
 372 -               pr_err("invalid interrupt map\n");
 373 -               return ret;
 374 +       if (WARN_ON(!num_parents)) {
 375 +               /*
 376 +                * If DT contains no parent interrupts, assume MIPS CPU IRQ 2
 377 +                * (HW0) is connected to the first output. This is the case for
 378 +                * all known hardware anyway. "interrupt-map" is deprecated, so
 379 +                * don't bother trying to parse that.
 380 +                * Since this is to account for old devicetrees with one-cell
 381 +                * interrupt specifiers, only one output domain is needed.
 382 +                */
 383 +               oirq.np = of_find_compatible_node(NULL, NULL, "mti,cpu-interrupt-controller");
 384 +               if (oirq.np) {
 385 +                       oirq.args_count = 1;
 386 +                       oirq.args[0] = 2;
 387 +
 388 +                       parent_irqs[0] = irq_create_of_mapping(&oirq);
 389 +                       num_parents = 1;
 390 +               }
 391 +
 392 +               of_node_put(oirq.np);
 393         }
 394
 395 -       return 0;
 396 +       /* Ensure we haven't collected any errors before proceeding */
 397 +       for (p = 0; p < num_parents; p++) {
 398 +               if (parent_irqs[p] < 0)
 399 +                       return parent_irqs[p];
 400 +               if (!parent_irqs[p])
 401 +                       return -ENODEV;
 402 +       }
 403 +
 404 +       return setup_parent_interrupts(node, &parent_irqs[0], num_parents);
 405  }
 406
 407  IRQCHIP_DECLARE(realtek_rtl_intc, "realtek,rtl-intc", realtek_rtl_of_init);