drivers/irqchip/irq-gic-v3.c

   1 /*
   2  * Copyright (C) 2013, 2014 ARM Limited, All Rights Reserved.
   3  * Author: Marc Zyngier <marc.zyngier@arm.com>
   4  *
   5  * This program is free software; you can redistribute it and/or modify
   6  * it under the terms of the GNU General Public License version 2 as
   7  * published by the Free Software Foundation.
   8  *
   9  * This program is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <linux/cpu.h>
  19 #include <linux/cpu_pm.h>
  20 #include <linux/delay.h>
  21 #include <linux/interrupt.h>
  22 #include <linux/of.h>
  23 #include <linux/of_address.h>
  24 #include <linux/of_irq.h>
  25 #include <linux/percpu.h>
  26 #include <linux/slab.h>
  27
  28 #include <linux/irqchip.h>
  29 #include <linux/irqchip/arm-gic-v3.h>
  30 #include <linux/irqchip/irq-partition-percpu.h>
  31
  32 #include <asm/cputype.h>
  33 #include <asm/exception.h>
  34 #include <asm/smp_plat.h>
  35 #include <asm/virt.h>
  36
  37 #include "irq-gic-common.h"
  38
  39 struct redist_region {
  40         void __iomem            *redist_base;
  41         phys_addr_t             phys_base;
  42 };
  43
  44 struct gic_chip_data {
  45         struct fwnode_handle    *fwnode;
  46         void __iomem            *dist_base;
  47         struct redist_region    *redist_regions;
  48         struct rdists           rdists;
  49         struct irq_domain       *domain;
  50         u64                     redist_stride;
  51         u32                     nr_redist_regions;
  52         unsigned int            irq_nr;
  53         struct partition_desc   *ppi_descs[16];
  54 };
  55
  56 static struct gic_chip_data gic_data __read_mostly;
  57 static struct static_key supports_deactivate = STATIC_KEY_INIT_TRUE;
  58
  59 #define gic_data_rdist()                (this_cpu_ptr(gic_data.rdists.rdist))
  60 #define gic_data_rdist_rd_base()        (gic_data_rdist()->rd_base)
  61 #define gic_data_rdist_sgi_base()       (gic_data_rdist_rd_base() + SZ_64K)
  62
  63 /* Our default, arbitrary priority value. Linux only uses one anyway. */
  64 #define DEFAULT_PMR_VALUE       0xf0
  65
  66 static inline unsigned int gic_irq(struct irq_data *d)
  67 {
  68         return d->hwirq;
  69 }
  70
  71 static inline int gic_irq_in_rdist(struct irq_data *d)
  72 {
  73         return gic_irq(d) < 32;
  74 }
  75
  76 static inline void __iomem *gic_dist_base(struct irq_data *d)
  77 {
  78         if (gic_irq_in_rdist(d))        /* SGI+PPI -> SGI_base for this CPU */
  79                 return gic_data_rdist_sgi_base();
  80
  81         if (d->hwirq <= 1023)           /* SPI -> dist_base */
  82                 return gic_data.dist_base;
  83
  84         return NULL;
  85 }
  86
  87 static void gic_do_wait_for_rwp(void __iomem *base)
  88 {
  89         u32 count = 1000000;    /* 1s! */
  90
  91         while (readl_relaxed(base + GICD_CTLR) & GICD_CTLR_RWP) {
  92                 count--;
  93                 if (!count) {
  94                         pr_err_ratelimited("RWP timeout, gone fishing\n");
  95                         return;
  96                 }
  97                 cpu_relax();
  98                 udelay(1);
  99         };
 100 }
 101
 102 /* Wait for completion of a distributor change */
 103 static void gic_dist_wait_for_rwp(void)
 104 {
 105         gic_do_wait_for_rwp(gic_data.dist_base);
 106 }
 107
 108 /* Wait for completion of a redistributor change */
 109 static void gic_redist_wait_for_rwp(void)
 110 {
 111         gic_do_wait_for_rwp(gic_data_rdist_rd_base());
 112 }
 113
 114 #ifdef CONFIG_ARM64
 115 static DEFINE_STATIC_KEY_FALSE(is_cavium_thunderx);
 116
 117 static u64 __maybe_unused gic_read_iar(void)
 118 {
 119         if (static_branch_unlikely(&is_cavium_thunderx))
 120                 return gic_read_iar_cavium_thunderx();
 121         else
 122                 return gic_read_iar_common();
 123 }
 124 #endif
 125
 126 static void gic_enable_redist(bool enable)
 127 {
 128         void __iomem *rbase;
 129         u32 count = 1000000;    /* 1s! */
 130         u32 val;
 131
 132         rbase = gic_data_rdist_rd_base();
 133
 134         val = readl_relaxed(rbase + GICR_WAKER);
 135         if (enable)
 136                 /* Wake up this CPU redistributor */
 137                 val &= ~GICR_WAKER_ProcessorSleep;
 138         else
 139                 val |= GICR_WAKER_ProcessorSleep;
 140         writel_relaxed(val, rbase + GICR_WAKER);
 141
 142         if (!enable) {          /* Check that GICR_WAKER is writeable */
 143                 val = readl_relaxed(rbase + GICR_WAKER);
 144                 if (!(val & GICR_WAKER_ProcessorSleep))
 145                         return; /* No PM support in this redistributor */
 146         }
 147
 148         while (--count) {
 149                 val = readl_relaxed(rbase + GICR_WAKER);
 150                 if (enable ^ (val & GICR_WAKER_ChildrenAsleep))
 151                         break;
 152                 cpu_relax();
 153                 udelay(1);
 154         };
 155         if (!count)
 156                 pr_err_ratelimited("redistributor failed to %s...\n",
 157                                    enable ? "wakeup" : "sleep");
 158 }
 159
 160 /*
 161  * Routines to disable, enable, EOI and route interrupts
 162  */
 163 static int gic_peek_irq(struct irq_data *d, u32 offset)
 164 {
 165         u32 mask = 1 << (gic_irq(d) % 32);
 166         void __iomem *base;
 167
 168         if (gic_irq_in_rdist(d))
 169                 base = gic_data_rdist_sgi_base();
 170         else
 171                 base = gic_data.dist_base;
 172
 173         return !!(readl_relaxed(base + offset + (gic_irq(d) / 32) * 4) & mask);
 174 }
 175
 176 static void gic_poke_irq(struct irq_data *d, u32 offset)
 177 {
 178         u32 mask = 1 << (gic_irq(d) % 32);
 179         void (*rwp_wait)(void);
 180         void __iomem *base;
 181
 182         if (gic_irq_in_rdist(d)) {
 183                 base = gic_data_rdist_sgi_base();
 184                 rwp_wait = gic_redist_wait_for_rwp;
 185         } else {
 186                 base = gic_data.dist_base;
 187                 rwp_wait = gic_dist_wait_for_rwp;
 188         }
 189
 190         writel_relaxed(mask, base + offset + (gic_irq(d) / 32) * 4);
 191         rwp_wait();
 192 }
 193
 194 static void gic_mask_irq(struct irq_data *d)
 195 {
 196         gic_poke_irq(d, GICD_ICENABLER);
 197 }
 198
 199 static void gic_eoimode1_mask_irq(struct irq_data *d)
 200 {
 201         gic_mask_irq(d);
 202         /*
 203          * When masking a forwarded interrupt, make sure it is
 204          * deactivated as well.
 205          *
 206          * This ensures that an interrupt that is getting
 207          * disabled/masked will not get "stuck", because there is
 208          * noone to deactivate it (guest is being terminated).
 209          */
 210         if (irqd_is_forwarded_to_vcpu(d))
 211                 gic_poke_irq(d, GICD_ICACTIVER);
 212 }
 213
 214 static void gic_unmask_irq(struct irq_data *d)
 215 {
 216         gic_poke_irq(d, GICD_ISENABLER);
 217 }
 218
 219 #ifdef CONFIG_ARCH_ROCKCHIP
 220 static int gic_retrigger(struct irq_data *d)
 221 {
 222         gic_poke_irq(d, GICD_ISPENDR);
 223         /* the genirq layer expects 0 if we can't retrigger in hardware */
 224         return 0;
 225 }
 226 #endif
 227
 228 static int gic_irq_set_irqchip_state(struct irq_data *d,
 229                                      enum irqchip_irq_state which, bool val)
 230 {
 231         u32 reg;
 232
 233         if (d->hwirq >= gic_data.irq_nr) /* PPI/SPI only */
 234                 return -EINVAL;
 235
 236         switch (which) {
 237         case IRQCHIP_STATE_PENDING:
 238                 reg = val ? GICD_ISPENDR : GICD_ICPENDR;
 239                 break;
 240
 241         case IRQCHIP_STATE_ACTIVE:
 242                 reg = val ? GICD_ISACTIVER : GICD_ICACTIVER;
 243                 break;
 244
 245         case IRQCHIP_STATE_MASKED:
 246                 reg = val ? GICD_ICENABLER : GICD_ISENABLER;
 247                 break;
 248
 249         default:
 250                 return -EINVAL;
 251         }
 252
 253         gic_poke_irq(d, reg);
 254         return 0;
 255 }
 256
 257 static int gic_irq_get_irqchip_state(struct irq_data *d,
 258                                      enum irqchip_irq_state which, bool *val)
 259 {
 260         if (d->hwirq >= gic_data.irq_nr) /* PPI/SPI only */
 261                 return -EINVAL;
 262
 263         switch (which) {
 264         case IRQCHIP_STATE_PENDING:
 265                 *val = gic_peek_irq(d, GICD_ISPENDR);
 266                 break;
 267
 268         case IRQCHIP_STATE_ACTIVE:
 269                 *val = gic_peek_irq(d, GICD_ISACTIVER);
 270                 break;
 271
 272         case IRQCHIP_STATE_MASKED:
 273                 *val = !gic_peek_irq(d, GICD_ISENABLER);
 274                 break;
 275
 276         default:
 277                 return -EINVAL;
 278         }
 279
 280         return 0;
 281 }
 282
 283 static void gic_eoi_irq(struct irq_data *d)
 284 {
 285         gic_write_eoir(gic_irq(d));
 286 }
 287
 288 static void gic_eoimode1_eoi_irq(struct irq_data *d)
 289 {
 290         /*
 291          * No need to deactivate an LPI, or an interrupt that
 292          * is is getting forwarded to a vcpu.
 293          */
 294         if (gic_irq(d) >= 8192 || irqd_is_forwarded_to_vcpu(d))
 295                 return;
 296         gic_write_dir(gic_irq(d));
 297 }
 298
 299 static int gic_set_type(struct irq_data *d, unsigned int type)
 300 {
 301         unsigned int irq = gic_irq(d);
 302         void (*rwp_wait)(void);
 303         void __iomem *base;
 304
 305         /* Interrupt configuration for SGIs can't be changed */
 306         if (irq < 16)
 307                 return -EINVAL;
 308
 309         /* SPIs have restrictions on the supported types */
 310         if (irq >= 32 && type != IRQ_TYPE_LEVEL_HIGH &&
 311                          type != IRQ_TYPE_EDGE_RISING)
 312                 return -EINVAL;
 313
 314         if (gic_irq_in_rdist(d)) {
 315                 base = gic_data_rdist_sgi_base();
 316                 rwp_wait = gic_redist_wait_for_rwp;
 317         } else {
 318                 base = gic_data.dist_base;
 319                 rwp_wait = gic_dist_wait_for_rwp;
 320         }
 321
 322         return gic_configure_irq(irq, type, base, rwp_wait);
 323 }
 324
 325 static int gic_irq_set_vcpu_affinity(struct irq_data *d, void *vcpu)
 326 {
 327         if (vcpu)
 328                 irqd_set_forwarded_to_vcpu(d);
 329         else
 330                 irqd_clr_forwarded_to_vcpu(d);
 331         return 0;
 332 }
 333
 334 static u64 gic_mpidr_to_affinity(unsigned long mpidr)
 335 {
 336         u64 aff;
 337
 338         aff = ((u64)MPIDR_AFFINITY_LEVEL(mpidr, 3) << 32 |
 339                MPIDR_AFFINITY_LEVEL(mpidr, 2) << 16 |
 340                MPIDR_AFFINITY_LEVEL(mpidr, 1) << 8  |
 341                MPIDR_AFFINITY_LEVEL(mpidr, 0));
 342
 343         return aff;
 344 }
 345
 346 static asmlinkage void __exception_irq_entry gic_handle_irq(struct pt_regs *regs)
 347 {
 348         u32 irqnr;
 349
 350         do {
 351                 irqnr = gic_read_iar();
 352
 353                 if (likely(irqnr > 15 && irqnr < 1020) || irqnr >= 8192) {
 354                         int err;
 355
 356                         if (static_key_true(&supports_deactivate))
 357                                 gic_write_eoir(irqnr);
 358
 359                         err = handle_domain_irq(gic_data.domain, irqnr, regs);
 360                         if (err) {
 361                                 WARN_ONCE(true, "Unexpected interrupt received!\n");
 362                                 if (static_key_true(&supports_deactivate)) {
 363                                         if (irqnr < 8192)
 364                                                 gic_write_dir(irqnr);
 365                                 } else {
 366                                         gic_write_eoir(irqnr);
 367                                 }
 368                         }
 369                         continue;
 370                 }
 371                 if (irqnr < 16) {
 372                         gic_write_eoir(irqnr);
 373                         if (static_key_true(&supports_deactivate))
 374                                 gic_write_dir(irqnr);
 375 #ifdef CONFIG_SMP
 376                         /*
 377                          * Unlike GICv2, we don't need an smp_rmb() here.
 378                          * The control dependency from gic_read_iar to
 379                          * the ISB in gic_write_eoir is enough to ensure
 380                          * that any shared data read by handle_IPI will
 381                          * be read after the ACK.
 382                          */
 383                         handle_IPI(irqnr, regs);
 384 #else
 385                         WARN_ONCE(true, "Unexpected SGI received!\n");
 386 #endif
 387                         continue;
 388                 }
 389         } while (irqnr != ICC_IAR1_EL1_SPURIOUS);
 390 }
 391
 392 static void __init gic_dist_init(void)
 393 {
 394         unsigned int i;
 395         u64 affinity;
 396         void __iomem *base = gic_data.dist_base;
 397
 398         /* Disable the distributor */
 399         writel_relaxed(0, base + GICD_CTLR);
 400         gic_dist_wait_for_rwp();
 401
 402         /*
 403          * Configure SPIs as non-secure Group-1. This will only matter
 404          * if the GIC only has a single security state. This will not
 405          * do the right thing if the kernel is running in secure mode,
 406          * but that's not the intended use case anyway.
 407          */
 408         for (i = 32; i < gic_data.irq_nr; i += 32)
 409                 writel_relaxed(~0, base + GICD_IGROUPR + i / 8);
 410
 411         gic_dist_config(base, gic_data.irq_nr, gic_dist_wait_for_rwp);
 412
 413         /* Enable distributor with ARE, Group1 */
 414         writel_relaxed(GICD_CTLR_ARE_NS | GICD_CTLR_ENABLE_G1A | GICD_CTLR_ENABLE_G1,
 415                        base + GICD_CTLR);
 416
 417         /*
 418          * Set all global interrupts to the boot CPU only. ARE must be
 419          * enabled.
 420          */
 421         affinity = gic_mpidr_to_affinity(cpu_logical_map(smp_processor_id()));
 422         for (i = 32; i < gic_data.irq_nr; i++)
 423                 gic_write_irouter(affinity, base + GICD_IROUTER + i * 8);
 424 }
 425
 426 static int gic_populate_rdist(void)
 427 {
 428         unsigned long mpidr = cpu_logical_map(smp_processor_id());
 429         u64 typer;
 430         u32 aff;
 431         int i;
 432
 433         /*
 434          * Convert affinity to a 32bit value that can be matched to
 435          * GICR_TYPER bits [63:32].
 436          */
 437         aff = (MPIDR_AFFINITY_LEVEL(mpidr, 3) << 24 |
 438                MPIDR_AFFINITY_LEVEL(mpidr, 2) << 16 |
 439                MPIDR_AFFINITY_LEVEL(mpidr, 1) << 8 |
 440                MPIDR_AFFINITY_LEVEL(mpidr, 0));
 441
 442         for (i = 0; i < gic_data.nr_redist_regions; i++) {
 443                 void __iomem *ptr = gic_data.redist_regions[i].redist_base;
 444                 u32 reg;
 445
 446                 reg = readl_relaxed(ptr + GICR_PIDR2) & GIC_PIDR2_ARCH_MASK;
 447                 if (reg != GIC_PIDR2_ARCH_GICv3 &&
 448                     reg != GIC_PIDR2_ARCH_GICv4) { /* We're in trouble... */
 449                         pr_warn("No redistributor present @%p\n", ptr);
 450                         break;
 451                 }
 452
 453                 do {
 454                         typer = gic_read_typer(ptr + GICR_TYPER);
 455                         if ((typer >> 32) == aff) {
 456                                 u64 offset = ptr - gic_data.redist_regions[i].redist_base;
 457                                 gic_data_rdist_rd_base() = ptr;
 458                                 gic_data_rdist()->phys_base = gic_data.redist_regions[i].phys_base + offset;
 459                                 pr_info("CPU%d: found redistributor %lx region %d:%pa\n",
 460                                         smp_processor_id(), mpidr, i,
 461                                         &gic_data_rdist()->phys_base);
 462                                 return 0;
 463                         }
 464
 465                         if (gic_data.redist_stride) {
 466                                 ptr += gic_data.redist_stride;
 467                         } else {
 468                                 ptr += SZ_64K * 2; /* Skip RD_base + SGI_base */
 469                                 if (typer & GICR_TYPER_VLPIS)
 470                                         ptr += SZ_64K * 2; /* Skip VLPI_base + reserved page */
 471                         }
 472                 } while (!(typer & GICR_TYPER_LAST));
 473         }
 474
 475         /* We couldn't even deal with ourselves... */
 476         WARN(true, "CPU%d: mpidr %lx has no re-distributor!\n",
 477              smp_processor_id(), mpidr);
 478         return -ENODEV;
 479 }
 480
 481 static void gic_cpu_sys_reg_init(void)
 482 {
 483         /*
 484          * Need to check that the SRE bit has actually been set. If
 485          * not, it means that SRE is disabled at EL2. We're going to
 486          * die painfully, and there is nothing we can do about it.
 487          *
 488          * Kindly inform the luser.
 489          */
 490         if (!gic_enable_sre())
 491                 pr_err("GIC: unable to set SRE (disabled at EL2), panic ahead\n");
 492
 493         /* Set priority mask register */
 494         gic_write_pmr(DEFAULT_PMR_VALUE);
 495
 496         if (static_key_true(&supports_deactivate)) {
 497                 /* EOI drops priority only (mode 1) */
 498                 gic_write_ctlr(ICC_CTLR_EL1_EOImode_drop);
 499         } else {
 500                 /* EOI deactivates interrupt too (mode 0) */
 501                 gic_write_ctlr(ICC_CTLR_EL1_EOImode_drop_dir);
 502         }
 503
 504         /* ... and let's hit the road... */
 505         gic_write_grpen1(1);
 506 }
 507
 508 static int gic_dist_supports_lpis(void)
 509 {
 510         return !!(readl_relaxed(gic_data.dist_base + GICD_TYPER) & GICD_TYPER_LPIS);
 511 }
 512
 513 static void gic_cpu_init(void)
 514 {
 515         void __iomem *rbase;
 516
 517         /* Register ourselves with the rest of the world */
 518         if (gic_populate_rdist())
 519                 return;
 520
 521         gic_enable_redist(true);
 522
 523         rbase = gic_data_rdist_sgi_base();
 524
 525         /* Configure SGIs/PPIs as non-secure Group-1 */
 526         writel_relaxed(~0, rbase + GICR_IGROUPR0);
 527
 528         gic_cpu_config(rbase, gic_redist_wait_for_rwp);
 529
 530         /* Give LPIs a spin */
 531         if (IS_ENABLED(CONFIG_ARM_GIC_V3_ITS) && gic_dist_supports_lpis())
 532                 its_cpu_init();
 533
 534         /* initialise system registers */
 535         gic_cpu_sys_reg_init();
 536 }
 537
 538 #ifdef CONFIG_SMP
 539 static int gic_secondary_init(struct notifier_block *nfb,
 540                               unsigned long action, void *hcpu)
 541 {
 542         if (action == CPU_STARTING || action == CPU_STARTING_FROZEN)
 543                 gic_cpu_init();
 544         return NOTIFY_OK;
 545 }
 546
 547 /*
 548  * Notifier for enabling the GIC CPU interface. Set an arbitrarily high
 549  * priority because the GIC needs to be up before the ARM generic timers.
 550  */
 551 static struct notifier_block gic_cpu_notifier = {
 552         .notifier_call = gic_secondary_init,
 553         .priority = 100,
 554 };
 555
 556 static u16 gic_compute_target_list(int *base_cpu, const struct cpumask *mask,
 557                                    unsigned long cluster_id)
 558 {
 559         int next_cpu, cpu = *base_cpu;
 560         unsigned long mpidr = cpu_logical_map(cpu);
 561         u16 tlist = 0;
 562
 563         while (cpu < nr_cpu_ids) {
 564                 /*
 565                  * If we ever get a cluster of more than 16 CPUs, just
 566                  * scream and skip that CPU.
 567                  */
 568                 if (WARN_ON((mpidr & 0xff) >= 16))
 569                         goto out;
 570
 571                 tlist |= 1 << (mpidr & 0xf);
 572
 573                 next_cpu = cpumask_next(cpu, mask);
 574                 if (next_cpu >= nr_cpu_ids)
 575                         goto out;
 576                 cpu = next_cpu;
 577
 578                 mpidr = cpu_logical_map(cpu);
 579
 580                 if (cluster_id != (mpidr & ~0xffUL)) {
 581                         cpu--;
 582                         goto out;
 583                 }
 584         }
 585 out:
 586         *base_cpu = cpu;
 587         return tlist;
 588 }
 589
 590 #define MPIDR_TO_SGI_AFFINITY(cluster_id, level) \
 591         (MPIDR_AFFINITY_LEVEL(cluster_id, level) \
 592                 << ICC_SGI1R_AFFINITY_## level ##_SHIFT)
 593
 594 static void gic_send_sgi(u64 cluster_id, u16 tlist, unsigned int irq)
 595 {
 596         u64 val;
 597
 598         val = (MPIDR_TO_SGI_AFFINITY(cluster_id, 3)     |
 599                MPIDR_TO_SGI_AFFINITY(cluster_id, 2)     |
 600                irq << ICC_SGI1R_SGI_ID_SHIFT            |
 601                MPIDR_TO_SGI_AFFINITY(cluster_id, 1)     |
 602                tlist << ICC_SGI1R_TARGET_LIST_SHIFT);
 603
 604         pr_debug("CPU%d: ICC_SGI1R_EL1 %llx\n", smp_processor_id(), val);
 605         gic_write_sgi1r(val);
 606 }
 607
 608 static void gic_raise_softirq(const struct cpumask *mask, unsigned int irq)
 609 {
 610         int cpu;
 611
 612         if (WARN_ON(irq >= 16))
 613                 return;
 614
 615         /*
 616          * Ensure that stores to Normal memory are visible to the
 617          * other CPUs before issuing the IPI.
 618          */
 619         smp_wmb();
 620
 621         for_each_cpu(cpu, mask) {
 622                 unsigned long cluster_id = cpu_logical_map(cpu) & ~0xffUL;
 623                 u16 tlist;
 624
 625                 tlist = gic_compute_target_list(&cpu, mask, cluster_id);
 626                 gic_send_sgi(cluster_id, tlist, irq);
 627         }
 628
 629         /* Force the above writes to ICC_SGI1R_EL1 to be executed */
 630         isb();
 631 }
 632
 633 static void gic_smp_init(void)
 634 {
 635         set_smp_cross_call(gic_raise_softirq);
 636         register_cpu_notifier(&gic_cpu_notifier);
 637 }
 638
 639 static int gic_set_affinity(struct irq_data *d, const struct cpumask *mask_val,
 640                             bool force)
 641 {
 642         unsigned int cpu = cpumask_any_and(mask_val, cpu_online_mask);
 643         void __iomem *reg;
 644         int enabled;
 645         u64 val;
 646
 647         if (gic_irq_in_rdist(d))
 648                 return -EINVAL;
 649
 650         /* If interrupt was enabled, disable it first */
 651         enabled = gic_peek_irq(d, GICD_ISENABLER);
 652         if (enabled)
 653                 gic_mask_irq(d);
 654
 655         reg = gic_dist_base(d) + GICD_IROUTER + (gic_irq(d) * 8);
 656         val = gic_mpidr_to_affinity(cpu_logical_map(cpu));
 657
 658         gic_write_irouter(val, reg);
 659
 660         /*
 661          * If the interrupt was enabled, enabled it again. Otherwise,
 662          * just wait for the distributor to have digested our changes.
 663          */
 664         if (enabled)
 665                 gic_unmask_irq(d);
 666         else
 667                 gic_dist_wait_for_rwp();
 668
 669         return IRQ_SET_MASK_OK;
 670 }
 671 #else
 672 #define gic_set_affinity        NULL
 673 #define gic_smp_init()          do { } while(0)
 674 #endif
 675
 676 #ifdef CONFIG_CPU_PM
 677 static int gic_cpu_pm_notifier(struct notifier_block *self,
 678                                unsigned long cmd, void *v)
 679 {
 680         if (cmd == CPU_PM_EXIT) {
 681                 gic_enable_redist(true);
 682                 gic_cpu_sys_reg_init();
 683         } else if (cmd == CPU_PM_ENTER) {
 684                 gic_write_grpen1(0);
 685                 gic_enable_redist(false);
 686         }
 687         return NOTIFY_OK;
 688 }
 689
 690 static struct notifier_block gic_cpu_pm_notifier_block = {
 691         .notifier_call = gic_cpu_pm_notifier,
 692 };
 693
 694 static void gic_cpu_pm_init(void)
 695 {
 696         cpu_pm_register_notifier(&gic_cpu_pm_notifier_block);
 697 }
 698
 699 #else
 700 static inline void gic_cpu_pm_init(void) { }
 701 #endif /* CONFIG_CPU_PM */
 702
 703 static struct irq_chip gic_chip = {
 704         .name                   = "GICv3",
 705         .irq_mask               = gic_mask_irq,
 706         .irq_unmask             = gic_unmask_irq,
 707         .irq_eoi                = gic_eoi_irq,
 708         .irq_set_type           = gic_set_type,
 709 #ifdef CONFIG_ARCH_ROCKCHIP
 710         .irq_retrigger          = gic_retrigger,
 711 #endif
 712         .irq_set_affinity       = gic_set_affinity,
 713         .irq_get_irqchip_state  = gic_irq_get_irqchip_state,
 714         .irq_set_irqchip_state  = gic_irq_set_irqchip_state,
 715         .flags                  = IRQCHIP_SET_TYPE_MASKED,
 716 };
 717
 718 static struct irq_chip gic_eoimode1_chip = {
 719         .name                   = "GICv3",
 720         .irq_mask               = gic_eoimode1_mask_irq,
 721         .irq_unmask             = gic_unmask_irq,
 722         .irq_eoi                = gic_eoimode1_eoi_irq,
 723 #ifdef CONFIG_ARCH_ROCKCHIP
 724         .irq_retrigger          = gic_retrigger,
 725 #endif
 726         .irq_set_type           = gic_set_type,
 727         .irq_set_affinity       = gic_set_affinity,
 728         .irq_get_irqchip_state  = gic_irq_get_irqchip_state,
 729         .irq_set_irqchip_state  = gic_irq_set_irqchip_state,
 730         .irq_set_vcpu_affinity  = gic_irq_set_vcpu_affinity,
 731         .flags                  = IRQCHIP_SET_TYPE_MASKED,
 732 };
 733
 734 #define GIC_ID_NR               (1U << gic_data.rdists.id_bits)
 735
 736 static int gic_irq_domain_map(struct irq_domain *d, unsigned int irq,
 737                               irq_hw_number_t hw)
 738 {
 739         struct irq_chip *chip = &gic_chip;
 740
 741         if (static_key_true(&supports_deactivate))
 742                 chip = &gic_eoimode1_chip;
 743
 744         /* SGIs are private to the core kernel */
 745         if (hw < 16)
 746                 return -EPERM;
 747         /* Nothing here */
 748         if (hw >= gic_data.irq_nr && hw < 8192)
 749                 return -EPERM;
 750         /* Off limits */
 751         if (hw >= GIC_ID_NR)
 752                 return -EPERM;
 753
 754         /* PPIs */
 755         if (hw < 32) {
 756                 irq_set_percpu_devid(irq);
 757                 irq_domain_set_info(d, irq, hw, chip, d->host_data,
 758                                     handle_percpu_devid_irq, NULL, NULL);
 759                 irq_set_status_flags(irq, IRQ_NOAUTOEN);
 760         }
 761         /* SPIs */
 762         if (hw >= 32 && hw < gic_data.irq_nr) {
 763                 irq_domain_set_info(d, irq, hw, chip, d->host_data,
 764                                     handle_fasteoi_irq, NULL, NULL);
 765                 irq_set_probe(irq);
 766         }
 767         /* LPIs */
 768         if (hw >= 8192 && hw < GIC_ID_NR) {
 769                 if (!gic_dist_supports_lpis())
 770                         return -EPERM;
 771                 irq_domain_set_info(d, irq, hw, chip, d->host_data,
 772                                     handle_fasteoi_irq, NULL, NULL);
 773         }
 774
 775         return 0;
 776 }
 777
 778 static int gic_irq_domain_translate(struct irq_domain *d,
 779                                     struct irq_fwspec *fwspec,
 780                                     unsigned long *hwirq,
 781                                     unsigned int *type)
 782 {
 783         if (is_of_node(fwspec->fwnode)) {
 784                 if (fwspec->param_count < 3)
 785                         return -EINVAL;
 786
 787                 switch (fwspec->param[0]) {
 788                 case 0:                 /* SPI */
 789                         *hwirq = fwspec->param[1] + 32;
 790                         break;
 791                 case 1:                 /* PPI */
 792                         *hwirq = fwspec->param[1] + 16;
 793                         break;
 794                 case GIC_IRQ_TYPE_LPI:  /* LPI */
 795                         *hwirq = fwspec->param[1];
 796                         break;
 797                 default:
 798                         return -EINVAL;
 799                 }
 800
 801                 *type = fwspec->param[2] & IRQ_TYPE_SENSE_MASK;
 802                 return 0;
 803         }
 804
 805         return -EINVAL;
 806 }
 807
 808 static int gic_irq_domain_alloc(struct irq_domain *domain, unsigned int virq,
 809                                 unsigned int nr_irqs, void *arg)
 810 {
 811         int i, ret;
 812         irq_hw_number_t hwirq;
 813         unsigned int type = IRQ_TYPE_NONE;
 814         struct irq_fwspec *fwspec = arg;
 815
 816         ret = gic_irq_domain_translate(domain, fwspec, &hwirq, &type);
 817         if (ret)
 818                 return ret;
 819
 820         for (i = 0; i < nr_irqs; i++)
 821                 gic_irq_domain_map(domain, virq + i, hwirq + i);
 822
 823         return 0;
 824 }
 825
 826 static void gic_irq_domain_free(struct irq_domain *domain, unsigned int virq,
 827                                 unsigned int nr_irqs)
 828 {
 829         int i;
 830
 831         for (i = 0; i < nr_irqs; i++) {
 832                 struct irq_data *d = irq_domain_get_irq_data(domain, virq + i);
 833                 irq_set_handler(virq + i, NULL);
 834                 irq_domain_reset_irq_data(d);
 835         }
 836 }
 837
 838 static int gic_irq_domain_select(struct irq_domain *d,
 839                                  struct irq_fwspec *fwspec,
 840                                  enum irq_domain_bus_token bus_token)
 841 {
 842         /* Not for us */
 843         if (fwspec->fwnode != d->fwnode)
 844                 return 0;
 845
 846         /* If this is not DT, then we have a single domain */
 847         if (!is_of_node(fwspec->fwnode))
 848                 return 1;
 849
 850         /*
 851          * If this is a PPI and we have a 4th (non-null) parameter,
 852          * then we need to match the partition domain.
 853          */
 854         if (fwspec->param_count >= 4 &&
 855             fwspec->param[0] == 1 && fwspec->param[3] != 0)
 856                 return d == partition_get_domain(gic_data.ppi_descs[fwspec->param[1]]);
 857
 858         return d == gic_data.domain;
 859 }
 860
 861 static const struct irq_domain_ops gic_irq_domain_ops = {
 862         .translate = gic_irq_domain_translate,
 863         .alloc = gic_irq_domain_alloc,
 864         .free = gic_irq_domain_free,
 865         .select = gic_irq_domain_select,
 866 };
 867
 868 static int partition_domain_translate(struct irq_domain *d,
 869                                       struct irq_fwspec *fwspec,
 870                                       unsigned long *hwirq,
 871                                       unsigned int *type)
 872 {
 873         struct device_node *np;
 874         int ret;
 875
 876         np = of_find_node_by_phandle(fwspec->param[3]);
 877         if (WARN_ON(!np))
 878                 return -EINVAL;
 879
 880         ret = partition_translate_id(gic_data.ppi_descs[fwspec->param[1]],
 881                                      of_node_to_fwnode(np));
 882         if (ret < 0)
 883                 return ret;
 884
 885         *hwirq = ret;
 886         *type = fwspec->param[2] & IRQ_TYPE_SENSE_MASK;
 887
 888         return 0;
 889 }
 890
 891 static const struct irq_domain_ops partition_domain_ops = {
 892         .translate = partition_domain_translate,
 893         .select = gic_irq_domain_select,
 894 };
 895
 896 static void gicv3_enable_quirks(void)
 897 {
 898 #ifdef CONFIG_ARM64
 899         if (cpus_have_cap(ARM64_WORKAROUND_CAVIUM_23154))
 900                 static_branch_enable(&is_cavium_thunderx);
 901 #endif
 902 }
 903
 904 static int __init gic_init_bases(void __iomem *dist_base,
 905                                  struct redist_region *rdist_regs,
 906                                  u32 nr_redist_regions,
 907                                  u64 redist_stride,
 908                                  struct fwnode_handle *handle)
 909 {
 910         struct device_node *node;
 911         u32 typer;
 912         int gic_irqs;
 913         int err;
 914
 915         if (!is_hyp_mode_available())
 916                 static_key_slow_dec(&supports_deactivate);
 917
 918         if (static_key_true(&supports_deactivate))
 919                 pr_info("GIC: Using split EOI/Deactivate mode\n");
 920
 921         gic_data.fwnode = handle;
 922         gic_data.dist_base = dist_base;
 923         gic_data.redist_regions = rdist_regs;
 924         gic_data.nr_redist_regions = nr_redist_regions;
 925         gic_data.redist_stride = redist_stride;
 926
 927         gicv3_enable_quirks();
 928
 929         /*
 930          * Find out how many interrupts are supported.
 931          * The GIC only supports up to 1020 interrupt sources (SGI+PPI+SPI)
 932          */
 933         typer = readl_relaxed(gic_data.dist_base + GICD_TYPER);
 934         gic_data.rdists.id_bits = GICD_TYPER_ID_BITS(typer);
 935         gic_irqs = GICD_TYPER_IRQS(typer);
 936         if (gic_irqs > 1020)
 937                 gic_irqs = 1020;
 938         gic_data.irq_nr = gic_irqs;
 939
 940         gic_data.domain = irq_domain_create_tree(handle, &gic_irq_domain_ops,
 941                                                  &gic_data);
 942         gic_data.rdists.rdist = alloc_percpu(typeof(*gic_data.rdists.rdist));
 943
 944         if (WARN_ON(!gic_data.domain) || WARN_ON(!gic_data.rdists.rdist)) {
 945                 err = -ENOMEM;
 946                 goto out_free;
 947         }
 948
 949         set_handle_irq(gic_handle_irq);
 950
 951         node = to_of_node(handle);
 952         if (IS_ENABLED(CONFIG_ARM_GIC_V3_ITS) && gic_dist_supports_lpis() &&
 953             node) /* Temp hack to prevent ITS init for ACPI */
 954                 its_init(node, &gic_data.rdists, gic_data.domain);
 955
 956         gic_smp_init();
 957         gic_dist_init();
 958         gic_cpu_init();
 959         gic_cpu_pm_init();
 960
 961         return 0;
 962
 963 out_free:
 964         if (gic_data.domain)
 965                 irq_domain_remove(gic_data.domain);
 966         free_percpu(gic_data.rdists.rdist);
 967         return err;
 968 }
 969
 970 static int __init gic_validate_dist_version(void __iomem *dist_base)
 971 {
 972         u32 reg = readl_relaxed(dist_base + GICD_PIDR2) & GIC_PIDR2_ARCH_MASK;
 973
 974         if (reg != GIC_PIDR2_ARCH_GICv3 && reg != GIC_PIDR2_ARCH_GICv4)
 975                 return -ENODEV;
 976
 977         return 0;
 978 }
 979
 980 static int get_cpu_number(struct device_node *dn)
 981 {
 982         const __be32 *cell;
 983         u64 hwid;
 984         int i;
 985
 986         cell = of_get_property(dn, "reg", NULL);
 987         if (!cell)
 988                 return -1;
 989
 990         hwid = of_read_number(cell, of_n_addr_cells(dn));
 991
 992         /*
 993          * Non affinity bits must be set to 0 in the DT
 994          */
 995         if (hwid & ~MPIDR_HWID_BITMASK)
 996                 return -1;
 997
 998         for (i = 0; i < num_possible_cpus(); i++)
 999                 if (cpu_logical_map(i) == hwid)
1000                         return i;
1001
1002         return -1;
1003 }
1004
1005 /* Create all possible partitions at boot time */
1006 static void gic_populate_ppi_partitions(struct device_node *gic_node)
1007 {
1008         struct device_node *parts_node, *child_part;
1009         int part_idx = 0, i;
1010         int nr_parts;
1011         struct partition_affinity *parts;
1012
1013         parts_node = of_find_node_by_name(gic_node, "ppi-partitions");
1014         if (!parts_node)
1015                 return;
1016
1017         nr_parts = of_get_child_count(parts_node);
1018
1019         if (!nr_parts)
1020                 return;
1021
1022         parts = kzalloc(sizeof(*parts) * nr_parts, GFP_KERNEL);
1023         if (WARN_ON(!parts))
1024                 return;
1025
1026         for_each_child_of_node(parts_node, child_part) {
1027                 struct partition_affinity *part;
1028                 int n;
1029
1030                 part = &parts[part_idx];
1031
1032                 part->partition_id = of_node_to_fwnode(child_part);
1033
1034                 pr_info("GIC: PPI partition %s[%d] { ",
1035                         child_part->name, part_idx);
1036
1037                 n = of_property_count_elems_of_size(child_part, "affinity",
1038                                                     sizeof(u32));
1039                 WARN_ON(n <= 0);
1040
1041                 for (i = 0; i < n; i++) {
1042                         int err, cpu;
1043                         u32 cpu_phandle;
1044                         struct device_node *cpu_node;
1045
1046                         err = of_property_read_u32_index(child_part, "affinity",
1047                                                          i, &cpu_phandle);
1048                         if (WARN_ON(err))
1049                                 continue;
1050
1051                         cpu_node = of_find_node_by_phandle(cpu_phandle);
1052                         if (WARN_ON(!cpu_node))
1053                                 continue;
1054
1055                         cpu = get_cpu_number(cpu_node);
1056                         if (WARN_ON(cpu == -1))
1057                                 continue;
1058
1059                         pr_cont("%s[%d] ", cpu_node->full_name, cpu);
1060
1061                         cpumask_set_cpu(cpu, &part->mask);
1062                 }
1063
1064                 pr_cont("}\n");
1065                 part_idx++;
1066         }
1067
1068         for (i = 0; i < 16; i++) {
1069                 unsigned int irq;
1070                 struct partition_desc *desc;
1071                 struct irq_fwspec ppi_fwspec = {
1072                         .fwnode         = gic_data.fwnode,
1073                         .param_count    = 3,
1074                         .param          = {
1075                                 [0]     = 1,
1076                                 [1]     = i,
1077                                 [2]     = IRQ_TYPE_NONE,
1078                         },
1079                 };
1080
1081                 irq = irq_create_fwspec_mapping(&ppi_fwspec);
1082                 if (WARN_ON(!irq))
1083                         continue;
1084                 desc = partition_create_desc(gic_data.fwnode, parts, nr_parts,
1085                                              irq, &partition_domain_ops);
1086                 if (WARN_ON(!desc))
1087                         continue;
1088
1089                 gic_data.ppi_descs[i] = desc;
1090         }
1091 }
1092
1093 static int __init gic_of_init(struct device_node *node, struct device_node *parent)
1094 {
1095         void __iomem *dist_base;
1096         struct redist_region *rdist_regs;
1097         u64 redist_stride;
1098         u32 nr_redist_regions;
1099         int err, i;
1100
1101         dist_base = of_iomap(node, 0);
1102         if (!dist_base) {
1103                 pr_err("%s: unable to map gic dist registers\n",
1104                         node->full_name);
1105                 return -ENXIO;
1106         }
1107
1108         err = gic_validate_dist_version(dist_base);
1109         if (err) {
1110                 pr_err("%s: no distributor detected, giving up\n",
1111                         node->full_name);
1112                 goto out_unmap_dist;
1113         }
1114
1115         if (of_property_read_u32(node, "#redistributor-regions", &nr_redist_regions))
1116                 nr_redist_regions = 1;
1117
1118         rdist_regs = kzalloc(sizeof(*rdist_regs) * nr_redist_regions, GFP_KERNEL);
1119         if (!rdist_regs) {
1120                 err = -ENOMEM;
1121                 goto out_unmap_dist;
1122         }
1123
1124         for (i = 0; i < nr_redist_regions; i++) {
1125                 struct resource res;
1126                 int ret;
1127
1128                 ret = of_address_to_resource(node, 1 + i, &res);
1129                 rdist_regs[i].redist_base = of_iomap(node, 1 + i);
1130                 if (ret || !rdist_regs[i].redist_base) {
1131                         pr_err("%s: couldn't map region %d\n",
1132                                node->full_name, i);
1133                         err = -ENODEV;
1134                         goto out_unmap_rdist;
1135                 }
1136                 rdist_regs[i].phys_base = res.start;
1137         }
1138
1139         if (of_property_read_u64(node, "redistributor-stride", &redist_stride))
1140                 redist_stride = 0;
1141
1142         err = gic_init_bases(dist_base, rdist_regs, nr_redist_regions,
1143                              redist_stride, &node->fwnode);
1144         if (err)
1145                 goto out_unmap_rdist;
1146
1147         gic_populate_ppi_partitions(node);
1148         return 0;
1149
1150 out_unmap_rdist:
1151         for (i = 0; i < nr_redist_regions; i++)
1152                 if (rdist_regs[i].redist_base)
1153                         iounmap(rdist_regs[i].redist_base);
1154         kfree(rdist_regs);
1155 out_unmap_dist:
1156         iounmap(dist_base);
1157         return err;
1158 }
1159
1160 IRQCHIP_DECLARE(gic_v3, "arm,gic-v3", gic_of_init);