arch/arm64/kvm/hyp/vgic-v3-sr.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /*
   3  * Copyright (C) 2012-2015 - ARM Ltd
   4  * Author: Marc Zyngier <marc.zyngier@arm.com>
   5  */
   6
   7 #include <hyp/adjust_pc.h>
   8
   9 #include <linux/compiler.h>
  10 #include <linux/irqchip/arm-gic-v3.h>
  11 #include <linux/kvm_host.h>
  12
  13 #include <asm/kvm_emulate.h>
  14 #include <asm/kvm_hyp.h>
  15 #include <asm/kvm_mmu.h>
  16
  17 #define vtr_to_max_lr_idx(v)            ((v) & 0xf)
  18 #define vtr_to_nr_pre_bits(v)           ((((u32)(v) >> 26) & 7) + 1)
  19 #define vtr_to_nr_apr_regs(v)           (1 << (vtr_to_nr_pre_bits(v) - 5))
  20
  21 static u64 __gic_v3_get_lr(unsigned int lr)
  22 {
  23         switch (lr & 0xf) {
  24         case 0:
  25                 return read_gicreg(ICH_LR0_EL2);
  26         case 1:
  27                 return read_gicreg(ICH_LR1_EL2);
  28         case 2:
  29                 return read_gicreg(ICH_LR2_EL2);
  30         case 3:
  31                 return read_gicreg(ICH_LR3_EL2);
  32         case 4:
  33                 return read_gicreg(ICH_LR4_EL2);
  34         case 5:
  35                 return read_gicreg(ICH_LR5_EL2);
  36         case 6:
  37                 return read_gicreg(ICH_LR6_EL2);
  38         case 7:
  39                 return read_gicreg(ICH_LR7_EL2);
  40         case 8:
  41                 return read_gicreg(ICH_LR8_EL2);
  42         case 9:
  43                 return read_gicreg(ICH_LR9_EL2);
  44         case 10:
  45                 return read_gicreg(ICH_LR10_EL2);
  46         case 11:
  47                 return read_gicreg(ICH_LR11_EL2);
  48         case 12:
  49                 return read_gicreg(ICH_LR12_EL2);
  50         case 13:
  51                 return read_gicreg(ICH_LR13_EL2);
  52         case 14:
  53                 return read_gicreg(ICH_LR14_EL2);
  54         case 15:
  55                 return read_gicreg(ICH_LR15_EL2);
  56         }
  57
  58         unreachable();
  59 }
  60
  61 static void __gic_v3_set_lr(u64 val, int lr)
  62 {
  63         switch (lr & 0xf) {
  64         case 0:
  65                 write_gicreg(val, ICH_LR0_EL2);
  66                 break;
  67         case 1:
  68                 write_gicreg(val, ICH_LR1_EL2);
  69                 break;
  70         case 2:
  71                 write_gicreg(val, ICH_LR2_EL2);
  72                 break;
  73         case 3:
  74                 write_gicreg(val, ICH_LR3_EL2);
  75                 break;
  76         case 4:
  77                 write_gicreg(val, ICH_LR4_EL2);
  78                 break;
  79         case 5:
  80                 write_gicreg(val, ICH_LR5_EL2);
  81                 break;
  82         case 6:
  83                 write_gicreg(val, ICH_LR6_EL2);
  84                 break;
  85         case 7:
  86                 write_gicreg(val, ICH_LR7_EL2);
  87                 break;
  88         case 8:
  89                 write_gicreg(val, ICH_LR8_EL2);
  90                 break;
  91         case 9:
  92                 write_gicreg(val, ICH_LR9_EL2);
  93                 break;
  94         case 10:
  95                 write_gicreg(val, ICH_LR10_EL2);
  96                 break;
  97         case 11:
  98                 write_gicreg(val, ICH_LR11_EL2);
  99                 break;
 100         case 12:
 101                 write_gicreg(val, ICH_LR12_EL2);
 102                 break;
 103         case 13:
 104                 write_gicreg(val, ICH_LR13_EL2);
 105                 break;
 106         case 14:
 107                 write_gicreg(val, ICH_LR14_EL2);
 108                 break;
 109         case 15:
 110                 write_gicreg(val, ICH_LR15_EL2);
 111                 break;
 112         }
 113 }
 114
 115 static void __vgic_v3_write_ap0rn(u32 val, int n)
 116 {
 117         switch (n) {
 118         case 0:
 119                 write_gicreg(val, ICH_AP0R0_EL2);
 120                 break;
 121         case 1:
 122                 write_gicreg(val, ICH_AP0R1_EL2);
 123                 break;
 124         case 2:
 125                 write_gicreg(val, ICH_AP0R2_EL2);
 126                 break;
 127         case 3:
 128                 write_gicreg(val, ICH_AP0R3_EL2);
 129                 break;
 130         }
 131 }
 132
 133 static void __vgic_v3_write_ap1rn(u32 val, int n)
 134 {
 135         switch (n) {
 136         case 0:
 137                 write_gicreg(val, ICH_AP1R0_EL2);
 138                 break;
 139         case 1:
 140                 write_gicreg(val, ICH_AP1R1_EL2);
 141                 break;
 142         case 2:
 143                 write_gicreg(val, ICH_AP1R2_EL2);
 144                 break;
 145         case 3:
 146                 write_gicreg(val, ICH_AP1R3_EL2);
 147                 break;
 148         }
 149 }
 150
 151 static u32 __vgic_v3_read_ap0rn(int n)
 152 {
 153         u32 val;
 154
 155         switch (n) {
 156         case 0:
 157                 val = read_gicreg(ICH_AP0R0_EL2);
 158                 break;
 159         case 1:
 160                 val = read_gicreg(ICH_AP0R1_EL2);
 161                 break;
 162         case 2:
 163                 val = read_gicreg(ICH_AP0R2_EL2);
 164                 break;
 165         case 3:
 166                 val = read_gicreg(ICH_AP0R3_EL2);
 167                 break;
 168         default:
 169                 unreachable();
 170         }
 171
 172         return val;
 173 }
 174
 175 static u32 __vgic_v3_read_ap1rn(int n)
 176 {
 177         u32 val;
 178
 179         switch (n) {
 180         case 0:
 181                 val = read_gicreg(ICH_AP1R0_EL2);
 182                 break;
 183         case 1:
 184                 val = read_gicreg(ICH_AP1R1_EL2);
 185                 break;
 186         case 2:
 187                 val = read_gicreg(ICH_AP1R2_EL2);
 188                 break;
 189         case 3:
 190                 val = read_gicreg(ICH_AP1R3_EL2);
 191                 break;
 192         default:
 193                 unreachable();
 194         }
 195
 196         return val;
 197 }
 198
 199 void __vgic_v3_save_state(struct vgic_v3_cpu_if *cpu_if)
 200 {
 201         u64 used_lrs = cpu_if->used_lrs;
 202
 203         /*
 204          * Make sure stores to the GIC via the memory mapped interface
 205          * are now visible to the system register interface when reading the
 206          * LRs, and when reading back the VMCR on non-VHE systems.
 207          */
 208         if (used_lrs || !has_vhe()) {
 209                 if (!cpu_if->vgic_sre) {
 210                         dsb(sy);
 211                         isb();
 212                 }
 213         }
 214
 215         if (used_lrs || cpu_if->its_vpe.its_vm) {
 216                 int i;
 217                 u32 elrsr;
 218
 219                 elrsr = read_gicreg(ICH_ELRSR_EL2);
 220
 221                 write_gicreg(cpu_if->vgic_hcr & ~ICH_HCR_EN, ICH_HCR_EL2);
 222
 223                 for (i = 0; i < used_lrs; i++) {
 224                         if (elrsr & (1 << i))
 225                                 cpu_if->vgic_lr[i] &= ~ICH_LR_STATE;
 226                         else
 227                                 cpu_if->vgic_lr[i] = __gic_v3_get_lr(i);
 228
 229                         __gic_v3_set_lr(0, i);
 230                 }
 231         }
 232 }
 233
 234 void __vgic_v3_restore_state(struct vgic_v3_cpu_if *cpu_if)
 235 {
 236         u64 used_lrs = cpu_if->used_lrs;
 237         int i;
 238
 239         if (used_lrs || cpu_if->its_vpe.its_vm) {
 240                 write_gicreg(cpu_if->vgic_hcr, ICH_HCR_EL2);
 241
 242                 for (i = 0; i < used_lrs; i++)
 243                         __gic_v3_set_lr(cpu_if->vgic_lr[i], i);
 244         }
 245
 246         /*
 247          * Ensure that writes to the LRs, and on non-VHE systems ensure that
 248          * the write to the VMCR in __vgic_v3_activate_traps(), will have
 249          * reached the (re)distributors. This ensure the guest will read the
 250          * correct values from the memory-mapped interface.
 251          */
 252         if (used_lrs || !has_vhe()) {
 253                 if (!cpu_if->vgic_sre) {
 254                         isb();
 255                         dsb(sy);
 256                 }
 257         }
 258 }
 259
 260 void __vgic_v3_activate_traps(struct vgic_v3_cpu_if *cpu_if)
 261 {
 262         /*
 263          * VFIQEn is RES1 if ICC_SRE_EL1.SRE is 1. This causes a
 264          * Group0 interrupt (as generated in GICv2 mode) to be
 265          * delivered as a FIQ to the guest, with potentially fatal
 266          * consequences. So we must make sure that ICC_SRE_EL1 has
 267          * been actually programmed with the value we want before
 268          * starting to mess with the rest of the GIC, and VMCR_EL2 in
 269          * particular.  This logic must be called before
 270          * __vgic_v3_restore_state().
 271          */
 272         if (!cpu_if->vgic_sre) {
 273                 write_gicreg(0, ICC_SRE_EL1);
 274                 isb();
 275                 write_gicreg(cpu_if->vgic_vmcr, ICH_VMCR_EL2);
 276
 277
 278                 if (has_vhe()) {
 279                         /*
 280                          * Ensure that the write to the VMCR will have reached
 281                          * the (re)distributors. This ensure the guest will
 282                          * read the correct values from the memory-mapped
 283                          * interface.
 284                          */
 285                         isb();
 286                         dsb(sy);
 287                 }
 288         }
 289
 290         /*
 291          * Prevent the guest from touching the GIC system registers if
 292          * SRE isn't enabled for GICv3 emulation.
 293          */
 294         write_gicreg(read_gicreg(ICC_SRE_EL2) & ~ICC_SRE_EL2_ENABLE,
 295                      ICC_SRE_EL2);
 296
 297         /*
 298          * If we need to trap system registers, we must write
 299          * ICH_HCR_EL2 anyway, even if no interrupts are being
 300          * injected,
 301          */
 302         if (static_branch_unlikely(&vgic_v3_cpuif_trap) ||
 303             cpu_if->its_vpe.its_vm)
 304                 write_gicreg(cpu_if->vgic_hcr, ICH_HCR_EL2);
 305 }
 306
 307 void __vgic_v3_deactivate_traps(struct vgic_v3_cpu_if *cpu_if)
 308 {
 309         u64 val;
 310
 311         if (!cpu_if->vgic_sre) {
 312                 cpu_if->vgic_vmcr = read_gicreg(ICH_VMCR_EL2);
 313         }
 314
 315         val = read_gicreg(ICC_SRE_EL2);
 316         write_gicreg(val | ICC_SRE_EL2_ENABLE, ICC_SRE_EL2);
 317
 318         if (!cpu_if->vgic_sre) {
 319                 /* Make sure ENABLE is set at EL2 before setting SRE at EL1 */
 320                 isb();
 321                 write_gicreg(1, ICC_SRE_EL1);
 322         }
 323
 324         /*
 325          * If we were trapping system registers, we enabled the VGIC even if
 326          * no interrupts were being injected, and we disable it again here.
 327          */
 328         if (static_branch_unlikely(&vgic_v3_cpuif_trap) ||
 329             cpu_if->its_vpe.its_vm)
 330                 write_gicreg(0, ICH_HCR_EL2);
 331 }
 332
 333 void __vgic_v3_save_aprs(struct vgic_v3_cpu_if *cpu_if)
 334 {
 335         u64 val;
 336         u32 nr_pre_bits;
 337
 338         val = read_gicreg(ICH_VTR_EL2);
 339         nr_pre_bits = vtr_to_nr_pre_bits(val);
 340
 341         switch (nr_pre_bits) {
 342         case 7:
 343                 cpu_if->vgic_ap0r[3] = __vgic_v3_read_ap0rn(3);
 344                 cpu_if->vgic_ap0r[2] = __vgic_v3_read_ap0rn(2);
 345                 fallthrough;
 346         case 6:
 347                 cpu_if->vgic_ap0r[1] = __vgic_v3_read_ap0rn(1);
 348                 fallthrough;
 349         default:
 350                 cpu_if->vgic_ap0r[0] = __vgic_v3_read_ap0rn(0);
 351         }
 352
 353         switch (nr_pre_bits) {
 354         case 7:
 355                 cpu_if->vgic_ap1r[3] = __vgic_v3_read_ap1rn(3);
 356                 cpu_if->vgic_ap1r[2] = __vgic_v3_read_ap1rn(2);
 357                 fallthrough;
 358         case 6:
 359                 cpu_if->vgic_ap1r[1] = __vgic_v3_read_ap1rn(1);
 360                 fallthrough;
 361         default:
 362                 cpu_if->vgic_ap1r[0] = __vgic_v3_read_ap1rn(0);
 363         }
 364 }
 365
 366 void __vgic_v3_restore_aprs(struct vgic_v3_cpu_if *cpu_if)
 367 {
 368         u64 val;
 369         u32 nr_pre_bits;
 370
 371         val = read_gicreg(ICH_VTR_EL2);
 372         nr_pre_bits = vtr_to_nr_pre_bits(val);
 373
 374         switch (nr_pre_bits) {
 375         case 7:
 376                 __vgic_v3_write_ap0rn(cpu_if->vgic_ap0r[3], 3);
 377                 __vgic_v3_write_ap0rn(cpu_if->vgic_ap0r[2], 2);
 378                 fallthrough;
 379         case 6:
 380                 __vgic_v3_write_ap0rn(cpu_if->vgic_ap0r[1], 1);
 381                 fallthrough;
 382         default:
 383                 __vgic_v3_write_ap0rn(cpu_if->vgic_ap0r[0], 0);
 384         }
 385
 386         switch (nr_pre_bits) {
 387         case 7:
 388                 __vgic_v3_write_ap1rn(cpu_if->vgic_ap1r[3], 3);
 389                 __vgic_v3_write_ap1rn(cpu_if->vgic_ap1r[2], 2);
 390                 fallthrough;
 391         case 6:
 392                 __vgic_v3_write_ap1rn(cpu_if->vgic_ap1r[1], 1);
 393                 fallthrough;
 394         default:
 395                 __vgic_v3_write_ap1rn(cpu_if->vgic_ap1r[0], 0);
 396         }
 397 }
 398
 399 void __vgic_v3_init_lrs(void)
 400 {
 401         int max_lr_idx = vtr_to_max_lr_idx(read_gicreg(ICH_VTR_EL2));
 402         int i;
 403
 404         for (i = 0; i <= max_lr_idx; i++)
 405                 __gic_v3_set_lr(0, i);
 406 }
 407
 408 /*
 409  * Return the GIC CPU configuration:
 410  * - [31:0]  ICH_VTR_EL2
 411  * - [62:32] RES0
 412  * - [63]    MMIO (GICv2) capable
 413  */
 414 u64 __vgic_v3_get_gic_config(void)
 415 {
 416         u64 val, sre = read_gicreg(ICC_SRE_EL1);
 417         unsigned long flags = 0;
 418
 419         /*
 420          * To check whether we have a MMIO-based (GICv2 compatible)
 421          * CPU interface, we need to disable the system register
 422          * view. To do that safely, we have to prevent any interrupt
 423          * from firing (which would be deadly).
 424          *
 425          * Note that this only makes sense on VHE, as interrupts are
 426          * already masked for nVHE as part of the exception entry to
 427          * EL2.
 428          */
 429         if (has_vhe())
 430                 flags = local_daif_save();
 431
 432         /*
 433          * Table 11-2 "Permitted ICC_SRE_ELx.SRE settings" indicates
 434          * that to be able to set ICC_SRE_EL1.SRE to 0, all the
 435          * interrupt overrides must be set. You've got to love this.
 436          */
 437         sysreg_clear_set(hcr_el2, 0, HCR_AMO | HCR_FMO | HCR_IMO);
 438         isb();
 439         write_gicreg(0, ICC_SRE_EL1);
 440         isb();
 441
 442         val = read_gicreg(ICC_SRE_EL1);
 443
 444         write_gicreg(sre, ICC_SRE_EL1);
 445         isb();
 446         sysreg_clear_set(hcr_el2, HCR_AMO | HCR_FMO | HCR_IMO, 0);
 447         isb();
 448
 449         if (has_vhe())
 450                 local_daif_restore(flags);
 451
 452         val  = (val & ICC_SRE_EL1_SRE) ? 0 : (1ULL << 63);
 453         val |= read_gicreg(ICH_VTR_EL2);
 454
 455         return val;
 456 }
 457
 458 u64 __vgic_v3_read_vmcr(void)
 459 {
 460         return read_gicreg(ICH_VMCR_EL2);
 461 }
 462
 463 void __vgic_v3_write_vmcr(u32 vmcr)
 464 {
 465         write_gicreg(vmcr, ICH_VMCR_EL2);
 466 }
 467
 468 static int __vgic_v3_bpr_min(void)
 469 {
 470         /* See Pseudocode for VPriorityGroup */
 471         return 8 - vtr_to_nr_pre_bits(read_gicreg(ICH_VTR_EL2));
 472 }
 473
 474 static int __vgic_v3_get_group(struct kvm_vcpu *vcpu)
 475 {
 476         u32 esr = kvm_vcpu_get_esr(vcpu);
 477         u8 crm = (esr & ESR_ELx_SYS64_ISS_CRM_MASK) >> ESR_ELx_SYS64_ISS_CRM_SHIFT;
 478
 479         return crm != 8;
 480 }
 481
 482 #define GICv3_IDLE_PRIORITY     0xff
 483
 484 static int __vgic_v3_highest_priority_lr(struct kvm_vcpu *vcpu, u32 vmcr,
 485                                          u64 *lr_val)
 486 {
 487         unsigned int used_lrs = vcpu->arch.vgic_cpu.vgic_v3.used_lrs;
 488         u8 priority = GICv3_IDLE_PRIORITY;
 489         int i, lr = -1;
 490
 491         for (i = 0; i < used_lrs; i++) {
 492                 u64 val = __gic_v3_get_lr(i);
 493                 u8 lr_prio = (val & ICH_LR_PRIORITY_MASK) >> ICH_LR_PRIORITY_SHIFT;
 494
 495                 /* Not pending in the state? */
 496                 if ((val & ICH_LR_STATE) != ICH_LR_PENDING_BIT)
 497                         continue;
 498
 499                 /* Group-0 interrupt, but Group-0 disabled? */
 500                 if (!(val & ICH_LR_GROUP) && !(vmcr & ICH_VMCR_ENG0_MASK))
 501                         continue;
 502
 503                 /* Group-1 interrupt, but Group-1 disabled? */
 504                 if ((val & ICH_LR_GROUP) && !(vmcr & ICH_VMCR_ENG1_MASK))
 505                         continue;
 506
 507                 /* Not the highest priority? */
 508                 if (lr_prio >= priority)
 509                         continue;
 510
 511                 /* This is a candidate */
 512                 priority = lr_prio;
 513                 *lr_val = val;
 514                 lr = i;
 515         }
 516
 517         if (lr == -1)
 518                 *lr_val = ICC_IAR1_EL1_SPURIOUS;
 519
 520         return lr;
 521 }
 522
 523 static int __vgic_v3_find_active_lr(struct kvm_vcpu *vcpu, int intid,
 524                                     u64 *lr_val)
 525 {
 526         unsigned int used_lrs = vcpu->arch.vgic_cpu.vgic_v3.used_lrs;
 527         int i;
 528
 529         for (i = 0; i < used_lrs; i++) {
 530                 u64 val = __gic_v3_get_lr(i);
 531
 532                 if ((val & ICH_LR_VIRTUAL_ID_MASK) == intid &&
 533                     (val & ICH_LR_ACTIVE_BIT)) {
 534                         *lr_val = val;
 535                         return i;
 536                 }
 537         }
 538
 539         *lr_val = ICC_IAR1_EL1_SPURIOUS;
 540         return -1;
 541 }
 542
 543 static int __vgic_v3_get_highest_active_priority(void)
 544 {
 545         u8 nr_apr_regs = vtr_to_nr_apr_regs(read_gicreg(ICH_VTR_EL2));
 546         u32 hap = 0;
 547         int i;
 548
 549         for (i = 0; i < nr_apr_regs; i++) {
 550                 u32 val;
 551
 552                 /*
 553                  * The ICH_AP0Rn_EL2 and ICH_AP1Rn_EL2 registers
 554                  * contain the active priority levels for this VCPU
 555                  * for the maximum number of supported priority
 556                  * levels, and we return the full priority level only
 557                  * if the BPR is programmed to its minimum, otherwise
 558                  * we return a combination of the priority level and
 559                  * subpriority, as determined by the setting of the
 560                  * BPR, but without the full subpriority.
 561                  */
 562                 val  = __vgic_v3_read_ap0rn(i);
 563                 val |= __vgic_v3_read_ap1rn(i);
 564                 if (!val) {
 565                         hap += 32;
 566                         continue;
 567                 }
 568
 569                 return (hap + __ffs(val)) << __vgic_v3_bpr_min();
 570         }
 571
 572         return GICv3_IDLE_PRIORITY;
 573 }
 574
 575 static unsigned int __vgic_v3_get_bpr0(u32 vmcr)
 576 {
 577         return (vmcr & ICH_VMCR_BPR0_MASK) >> ICH_VMCR_BPR0_SHIFT;
 578 }
 579
 580 static unsigned int __vgic_v3_get_bpr1(u32 vmcr)
 581 {
 582         unsigned int bpr;
 583
 584         if (vmcr & ICH_VMCR_CBPR_MASK) {
 585                 bpr = __vgic_v3_get_bpr0(vmcr);
 586                 if (bpr < 7)
 587                         bpr++;
 588         } else {
 589                 bpr = (vmcr & ICH_VMCR_BPR1_MASK) >> ICH_VMCR_BPR1_SHIFT;
 590         }
 591
 592         return bpr;
 593 }
 594
 595 /*
 596  * Convert a priority to a preemption level, taking the relevant BPR
 597  * into account by zeroing the sub-priority bits.
 598  */
 599 static u8 __vgic_v3_pri_to_pre(u8 pri, u32 vmcr, int grp)
 600 {
 601         unsigned int bpr;
 602
 603         if (!grp)
 604                 bpr = __vgic_v3_get_bpr0(vmcr) + 1;
 605         else
 606                 bpr = __vgic_v3_get_bpr1(vmcr);
 607
 608         return pri & (GENMASK(7, 0) << bpr);
 609 }
 610
 611 /*
 612  * The priority value is independent of any of the BPR values, so we
 613  * normalize it using the minimal BPR value. This guarantees that no
 614  * matter what the guest does with its BPR, we can always set/get the
 615  * same value of a priority.
 616  */
 617 static void __vgic_v3_set_active_priority(u8 pri, u32 vmcr, int grp)
 618 {
 619         u8 pre, ap;
 620         u32 val;
 621         int apr;
 622
 623         pre = __vgic_v3_pri_to_pre(pri, vmcr, grp);
 624         ap = pre >> __vgic_v3_bpr_min();
 625         apr = ap / 32;
 626
 627         if (!grp) {
 628                 val = __vgic_v3_read_ap0rn(apr);
 629                 __vgic_v3_write_ap0rn(val | BIT(ap % 32), apr);
 630         } else {
 631                 val = __vgic_v3_read_ap1rn(apr);
 632                 __vgic_v3_write_ap1rn(val | BIT(ap % 32), apr);
 633         }
 634 }
 635
 636 static int __vgic_v3_clear_highest_active_priority(void)
 637 {
 638         u8 nr_apr_regs = vtr_to_nr_apr_regs(read_gicreg(ICH_VTR_EL2));
 639         u32 hap = 0;
 640         int i;
 641
 642         for (i = 0; i < nr_apr_regs; i++) {
 643                 u32 ap0, ap1;
 644                 int c0, c1;
 645
 646                 ap0 = __vgic_v3_read_ap0rn(i);
 647                 ap1 = __vgic_v3_read_ap1rn(i);
 648                 if (!ap0 && !ap1) {
 649                         hap += 32;
 650                         continue;
 651                 }
 652
 653                 c0 = ap0 ? __ffs(ap0) : 32;
 654                 c1 = ap1 ? __ffs(ap1) : 32;
 655
 656                 /* Always clear the LSB, which is the highest priority */
 657                 if (c0 < c1) {
 658                         ap0 &= ~BIT(c0);
 659                         __vgic_v3_write_ap0rn(ap0, i);
 660                         hap += c0;
 661                 } else {
 662                         ap1 &= ~BIT(c1);
 663                         __vgic_v3_write_ap1rn(ap1, i);
 664                         hap += c1;
 665                 }
 666
 667                 /* Rescale to 8 bits of priority */
 668                 return hap << __vgic_v3_bpr_min();
 669         }
 670
 671         return GICv3_IDLE_PRIORITY;
 672 }
 673
 674 static void __vgic_v3_read_iar(struct kvm_vcpu *vcpu, u32 vmcr, int rt)
 675 {
 676         u64 lr_val;
 677         u8 lr_prio, pmr;
 678         int lr, grp;
 679
 680         grp = __vgic_v3_get_group(vcpu);
 681
 682         lr = __vgic_v3_highest_priority_lr(vcpu, vmcr, &lr_val);
 683         if (lr < 0)
 684                 goto spurious;
 685
 686         if (grp != !!(lr_val & ICH_LR_GROUP))
 687                 goto spurious;
 688
 689         pmr = (vmcr & ICH_VMCR_PMR_MASK) >> ICH_VMCR_PMR_SHIFT;
 690         lr_prio = (lr_val & ICH_LR_PRIORITY_MASK) >> ICH_LR_PRIORITY_SHIFT;
 691         if (pmr <= lr_prio)
 692                 goto spurious;
 693
 694         if (__vgic_v3_get_highest_active_priority() <= __vgic_v3_pri_to_pre(lr_prio, vmcr, grp))
 695                 goto spurious;
 696
 697         lr_val &= ~ICH_LR_STATE;
 698         /* No active state for LPIs */
 699         if ((lr_val & ICH_LR_VIRTUAL_ID_MASK) <= VGIC_MAX_SPI)
 700                 lr_val |= ICH_LR_ACTIVE_BIT;
 701         __gic_v3_set_lr(lr_val, lr);
 702         __vgic_v3_set_active_priority(lr_prio, vmcr, grp);
 703         vcpu_set_reg(vcpu, rt, lr_val & ICH_LR_VIRTUAL_ID_MASK);
 704         return;
 705
 706 spurious:
 707         vcpu_set_reg(vcpu, rt, ICC_IAR1_EL1_SPURIOUS);
 708 }
 709
 710 static void __vgic_v3_clear_active_lr(int lr, u64 lr_val)
 711 {
 712         lr_val &= ~ICH_LR_ACTIVE_BIT;
 713         if (lr_val & ICH_LR_HW) {
 714                 u32 pid;
 715
 716                 pid = (lr_val & ICH_LR_PHYS_ID_MASK) >> ICH_LR_PHYS_ID_SHIFT;
 717                 gic_write_dir(pid);
 718         }
 719
 720         __gic_v3_set_lr(lr_val, lr);
 721 }
 722
 723 static void __vgic_v3_bump_eoicount(void)
 724 {
 725         u32 hcr;
 726
 727         hcr = read_gicreg(ICH_HCR_EL2);
 728         hcr += 1 << ICH_HCR_EOIcount_SHIFT;
 729         write_gicreg(hcr, ICH_HCR_EL2);
 730 }
 731
 732 static void __vgic_v3_write_dir(struct kvm_vcpu *vcpu, u32 vmcr, int rt)
 733 {
 734         u32 vid = vcpu_get_reg(vcpu, rt);
 735         u64 lr_val;
 736         int lr;
 737
 738         /* EOImode == 0, nothing to be done here */
 739         if (!(vmcr & ICH_VMCR_EOIM_MASK))
 740                 return;
 741
 742         /* No deactivate to be performed on an LPI */
 743         if (vid >= VGIC_MIN_LPI)
 744                 return;
 745
 746         lr = __vgic_v3_find_active_lr(vcpu, vid, &lr_val);
 747         if (lr == -1) {
 748                 __vgic_v3_bump_eoicount();
 749                 return;
 750         }
 751
 752         __vgic_v3_clear_active_lr(lr, lr_val);
 753 }
 754
 755 static void __vgic_v3_write_eoir(struct kvm_vcpu *vcpu, u32 vmcr, int rt)
 756 {
 757         u32 vid = vcpu_get_reg(vcpu, rt);
 758         u64 lr_val;
 759         u8 lr_prio, act_prio;
 760         int lr, grp;
 761
 762         grp = __vgic_v3_get_group(vcpu);
 763
 764         /* Drop priority in any case */
 765         act_prio = __vgic_v3_clear_highest_active_priority();
 766
 767         /* If EOIing an LPI, no deactivate to be performed */
 768         if (vid >= VGIC_MIN_LPI)
 769                 return;
 770
 771         /* EOImode == 1, nothing to be done here */
 772         if (vmcr & ICH_VMCR_EOIM_MASK)
 773                 return;
 774
 775         lr = __vgic_v3_find_active_lr(vcpu, vid, &lr_val);
 776         if (lr == -1) {
 777                 __vgic_v3_bump_eoicount();
 778                 return;
 779         }
 780
 781         lr_prio = (lr_val & ICH_LR_PRIORITY_MASK) >> ICH_LR_PRIORITY_SHIFT;
 782
 783         /* If priorities or group do not match, the guest has fscked-up. */
 784         if (grp != !!(lr_val & ICH_LR_GROUP) ||
 785             __vgic_v3_pri_to_pre(lr_prio, vmcr, grp) != act_prio)
 786                 return;
 787
 788         /* Let's now perform the deactivation */
 789         __vgic_v3_clear_active_lr(lr, lr_val);
 790 }
 791
 792 static void __vgic_v3_read_igrpen0(struct kvm_vcpu *vcpu, u32 vmcr, int rt)
 793 {
 794         vcpu_set_reg(vcpu, rt, !!(vmcr & ICH_VMCR_ENG0_MASK));
 795 }
 796
 797 static void __vgic_v3_read_igrpen1(struct kvm_vcpu *vcpu, u32 vmcr, int rt)
 798 {
 799         vcpu_set_reg(vcpu, rt, !!(vmcr & ICH_VMCR_ENG1_MASK));
 800 }
 801
 802 static void __vgic_v3_write_igrpen0(struct kvm_vcpu *vcpu, u32 vmcr, int rt)
 803 {
 804         u64 val = vcpu_get_reg(vcpu, rt);
 805
 806         if (val & 1)
 807                 vmcr |= ICH_VMCR_ENG0_MASK;
 808         else
 809                 vmcr &= ~ICH_VMCR_ENG0_MASK;
 810
 811         __vgic_v3_write_vmcr(vmcr);
 812 }
 813
 814 static void __vgic_v3_write_igrpen1(struct kvm_vcpu *vcpu, u32 vmcr, int rt)
 815 {
 816         u64 val = vcpu_get_reg(vcpu, rt);
 817
 818         if (val & 1)
 819                 vmcr |= ICH_VMCR_ENG1_MASK;
 820         else
 821                 vmcr &= ~ICH_VMCR_ENG1_MASK;
 822
 823         __vgic_v3_write_vmcr(vmcr);
 824 }
 825
 826 static void __vgic_v3_read_bpr0(struct kvm_vcpu *vcpu, u32 vmcr, int rt)
 827 {
 828         vcpu_set_reg(vcpu, rt, __vgic_v3_get_bpr0(vmcr));
 829 }
 830
 831 static void __vgic_v3_read_bpr1(struct kvm_vcpu *vcpu, u32 vmcr, int rt)
 832 {
 833         vcpu_set_reg(vcpu, rt, __vgic_v3_get_bpr1(vmcr));
 834 }
 835
 836 static void __vgic_v3_write_bpr0(struct kvm_vcpu *vcpu, u32 vmcr, int rt)
 837 {
 838         u64 val = vcpu_get_reg(vcpu, rt);
 839         u8 bpr_min = __vgic_v3_bpr_min() - 1;
 840
 841         /* Enforce BPR limiting */
 842         if (val < bpr_min)
 843                 val = bpr_min;
 844
 845         val <<= ICH_VMCR_BPR0_SHIFT;
 846         val &= ICH_VMCR_BPR0_MASK;
 847         vmcr &= ~ICH_VMCR_BPR0_MASK;
 848         vmcr |= val;
 849
 850         __vgic_v3_write_vmcr(vmcr);
 851 }
 852
 853 static void __vgic_v3_write_bpr1(struct kvm_vcpu *vcpu, u32 vmcr, int rt)
 854 {
 855         u64 val = vcpu_get_reg(vcpu, rt);
 856         u8 bpr_min = __vgic_v3_bpr_min();
 857
 858         if (vmcr & ICH_VMCR_CBPR_MASK)
 859                 return;
 860
 861         /* Enforce BPR limiting */
 862         if (val < bpr_min)
 863                 val = bpr_min;
 864
 865         val <<= ICH_VMCR_BPR1_SHIFT;
 866         val &= ICH_VMCR_BPR1_MASK;
 867         vmcr &= ~ICH_VMCR_BPR1_MASK;
 868         vmcr |= val;
 869
 870         __vgic_v3_write_vmcr(vmcr);
 871 }
 872
 873 static void __vgic_v3_read_apxrn(struct kvm_vcpu *vcpu, int rt, int n)
 874 {
 875         u32 val;
 876
 877         if (!__vgic_v3_get_group(vcpu))
 878                 val = __vgic_v3_read_ap0rn(n);
 879         else
 880                 val = __vgic_v3_read_ap1rn(n);
 881
 882         vcpu_set_reg(vcpu, rt, val);
 883 }
 884
 885 static void __vgic_v3_write_apxrn(struct kvm_vcpu *vcpu, int rt, int n)
 886 {
 887         u32 val = vcpu_get_reg(vcpu, rt);
 888
 889         if (!__vgic_v3_get_group(vcpu))
 890                 __vgic_v3_write_ap0rn(val, n);
 891         else
 892                 __vgic_v3_write_ap1rn(val, n);
 893 }
 894
 895 static void __vgic_v3_read_apxr0(struct kvm_vcpu *vcpu,
 896                                             u32 vmcr, int rt)
 897 {
 898         __vgic_v3_read_apxrn(vcpu, rt, 0);
 899 }
 900
 901 static void __vgic_v3_read_apxr1(struct kvm_vcpu *vcpu,
 902                                             u32 vmcr, int rt)
 903 {
 904         __vgic_v3_read_apxrn(vcpu, rt, 1);
 905 }
 906
 907 static void __vgic_v3_read_apxr2(struct kvm_vcpu *vcpu, u32 vmcr, int rt)
 908 {
 909         __vgic_v3_read_apxrn(vcpu, rt, 2);
 910 }
 911
 912 static void __vgic_v3_read_apxr3(struct kvm_vcpu *vcpu, u32 vmcr, int rt)
 913 {
 914         __vgic_v3_read_apxrn(vcpu, rt, 3);
 915 }
 916
 917 static void __vgic_v3_write_apxr0(struct kvm_vcpu *vcpu, u32 vmcr, int rt)
 918 {
 919         __vgic_v3_write_apxrn(vcpu, rt, 0);
 920 }
 921
 922 static void __vgic_v3_write_apxr1(struct kvm_vcpu *vcpu, u32 vmcr, int rt)
 923 {
 924         __vgic_v3_write_apxrn(vcpu, rt, 1);
 925 }
 926
 927 static void __vgic_v3_write_apxr2(struct kvm_vcpu *vcpu, u32 vmcr, int rt)
 928 {
 929         __vgic_v3_write_apxrn(vcpu, rt, 2);
 930 }
 931
 932 static void __vgic_v3_write_apxr3(struct kvm_vcpu *vcpu, u32 vmcr, int rt)
 933 {
 934         __vgic_v3_write_apxrn(vcpu, rt, 3);
 935 }
 936
 937 static void __vgic_v3_read_hppir(struct kvm_vcpu *vcpu, u32 vmcr, int rt)
 938 {
 939         u64 lr_val;
 940         int lr, lr_grp, grp;
 941
 942         grp = __vgic_v3_get_group(vcpu);
 943
 944         lr = __vgic_v3_highest_priority_lr(vcpu, vmcr, &lr_val);
 945         if (lr == -1)
 946                 goto spurious;
 947
 948         lr_grp = !!(lr_val & ICH_LR_GROUP);
 949         if (lr_grp != grp)
 950                 lr_val = ICC_IAR1_EL1_SPURIOUS;
 951
 952 spurious:
 953         vcpu_set_reg(vcpu, rt, lr_val & ICH_LR_VIRTUAL_ID_MASK);
 954 }
 955
 956 static void __vgic_v3_read_pmr(struct kvm_vcpu *vcpu, u32 vmcr, int rt)
 957 {
 958         vmcr &= ICH_VMCR_PMR_MASK;
 959         vmcr >>= ICH_VMCR_PMR_SHIFT;
 960         vcpu_set_reg(vcpu, rt, vmcr);
 961 }
 962
 963 static void __vgic_v3_write_pmr(struct kvm_vcpu *vcpu, u32 vmcr, int rt)
 964 {
 965         u32 val = vcpu_get_reg(vcpu, rt);
 966
 967         val <<= ICH_VMCR_PMR_SHIFT;
 968         val &= ICH_VMCR_PMR_MASK;
 969         vmcr &= ~ICH_VMCR_PMR_MASK;
 970         vmcr |= val;
 971
 972         write_gicreg(vmcr, ICH_VMCR_EL2);
 973 }
 974
 975 static void __vgic_v3_read_rpr(struct kvm_vcpu *vcpu, u32 vmcr, int rt)
 976 {
 977         u32 val = __vgic_v3_get_highest_active_priority();
 978         vcpu_set_reg(vcpu, rt, val);
 979 }
 980
 981 static void __vgic_v3_read_ctlr(struct kvm_vcpu *vcpu, u32 vmcr, int rt)
 982 {
 983         u32 vtr, val;
 984
 985         vtr = read_gicreg(ICH_VTR_EL2);
 986         /* PRIbits */
 987         val = ((vtr >> 29) & 7) << ICC_CTLR_EL1_PRI_BITS_SHIFT;
 988         /* IDbits */
 989         val |= ((vtr >> 23) & 7) << ICC_CTLR_EL1_ID_BITS_SHIFT;
 990         /* SEIS */
 991         val |= ((vtr >> 22) & 1) << ICC_CTLR_EL1_SEIS_SHIFT;
 992         /* A3V */
 993         val |= ((vtr >> 21) & 1) << ICC_CTLR_EL1_A3V_SHIFT;
 994         /* EOImode */
 995         val |= ((vmcr & ICH_VMCR_EOIM_MASK) >> ICH_VMCR_EOIM_SHIFT) << ICC_CTLR_EL1_EOImode_SHIFT;
 996         /* CBPR */
 997         val |= (vmcr & ICH_VMCR_CBPR_MASK) >> ICH_VMCR_CBPR_SHIFT;
 998
 999         vcpu_set_reg(vcpu, rt, val);
1000 }
1001
1002 static void __vgic_v3_write_ctlr(struct kvm_vcpu *vcpu, u32 vmcr, int rt)
1003 {
1004         u32 val = vcpu_get_reg(vcpu, rt);
1005
1006         if (val & ICC_CTLR_EL1_CBPR_MASK)
1007                 vmcr |= ICH_VMCR_CBPR_MASK;
1008         else
1009                 vmcr &= ~ICH_VMCR_CBPR_MASK;
1010
1011         if (val & ICC_CTLR_EL1_EOImode_MASK)
1012                 vmcr |= ICH_VMCR_EOIM_MASK;
1013         else
1014                 vmcr &= ~ICH_VMCR_EOIM_MASK;
1015
1016         write_gicreg(vmcr, ICH_VMCR_EL2);
1017 }
1018
1019 int __vgic_v3_perform_cpuif_access(struct kvm_vcpu *vcpu)
1020 {
1021         int rt;
1022         u32 esr;
1023         u32 vmcr;
1024         void (*fn)(struct kvm_vcpu *, u32, int);
1025         bool is_read;
1026         u32 sysreg;
1027
1028         esr = kvm_vcpu_get_esr(vcpu);
1029         if (vcpu_mode_is_32bit(vcpu)) {
1030                 if (!kvm_condition_valid(vcpu)) {
1031                         __kvm_skip_instr(vcpu);
1032                         return 1;
1033                 }
1034
1035                 sysreg = esr_cp15_to_sysreg(esr);
1036         } else {
1037                 sysreg = esr_sys64_to_sysreg(esr);
1038         }
1039
1040         is_read = (esr & ESR_ELx_SYS64_ISS_DIR_MASK) == ESR_ELx_SYS64_ISS_DIR_READ;
1041
1042         switch (sysreg) {
1043         case SYS_ICC_IAR0_EL1:
1044         case SYS_ICC_IAR1_EL1:
1045                 if (unlikely(!is_read))
1046                         return 0;
1047                 fn = __vgic_v3_read_iar;
1048                 break;
1049         case SYS_ICC_EOIR0_EL1:
1050         case SYS_ICC_EOIR1_EL1:
1051                 if (unlikely(is_read))
1052                         return 0;
1053                 fn = __vgic_v3_write_eoir;
1054                 break;
1055         case SYS_ICC_IGRPEN1_EL1:
1056                 if (is_read)
1057                         fn = __vgic_v3_read_igrpen1;
1058                 else
1059                         fn = __vgic_v3_write_igrpen1;
1060                 break;
1061         case SYS_ICC_BPR1_EL1:
1062                 if (is_read)
1063                         fn = __vgic_v3_read_bpr1;
1064                 else
1065                         fn = __vgic_v3_write_bpr1;
1066                 break;
1067         case SYS_ICC_AP0Rn_EL1(0):
1068         case SYS_ICC_AP1Rn_EL1(0):
1069                 if (is_read)
1070                         fn = __vgic_v3_read_apxr0;
1071                 else
1072                         fn = __vgic_v3_write_apxr0;
1073                 break;
1074         case SYS_ICC_AP0Rn_EL1(1):
1075         case SYS_ICC_AP1Rn_EL1(1):
1076                 if (is_read)
1077                         fn = __vgic_v3_read_apxr1;
1078                 else
1079                         fn = __vgic_v3_write_apxr1;
1080                 break;
1081         case SYS_ICC_AP0Rn_EL1(2):
1082         case SYS_ICC_AP1Rn_EL1(2):
1083                 if (is_read)
1084                         fn = __vgic_v3_read_apxr2;
1085                 else
1086                         fn = __vgic_v3_write_apxr2;
1087                 break;
1088         case SYS_ICC_AP0Rn_EL1(3):
1089         case SYS_ICC_AP1Rn_EL1(3):
1090                 if (is_read)
1091                         fn = __vgic_v3_read_apxr3;
1092                 else
1093                         fn = __vgic_v3_write_apxr3;
1094                 break;
1095         case SYS_ICC_HPPIR0_EL1:
1096         case SYS_ICC_HPPIR1_EL1:
1097                 if (unlikely(!is_read))
1098                         return 0;
1099                 fn = __vgic_v3_read_hppir;
1100                 break;
1101         case SYS_ICC_IGRPEN0_EL1:
1102                 if (is_read)
1103                         fn = __vgic_v3_read_igrpen0;
1104                 else
1105                         fn = __vgic_v3_write_igrpen0;
1106                 break;
1107         case SYS_ICC_BPR0_EL1:
1108                 if (is_read)
1109                         fn = __vgic_v3_read_bpr0;
1110                 else
1111                         fn = __vgic_v3_write_bpr0;
1112                 break;
1113         case SYS_ICC_DIR_EL1:
1114                 if (unlikely(is_read))
1115                         return 0;
1116                 fn = __vgic_v3_write_dir;
1117                 break;
1118         case SYS_ICC_RPR_EL1:
1119                 if (unlikely(!is_read))
1120                         return 0;
1121                 fn = __vgic_v3_read_rpr;
1122                 break;
1123         case SYS_ICC_CTLR_EL1:
1124                 if (is_read)
1125                         fn = __vgic_v3_read_ctlr;
1126                 else
1127                         fn = __vgic_v3_write_ctlr;
1128                 break;
1129         case SYS_ICC_PMR_EL1:
1130                 if (is_read)
1131                         fn = __vgic_v3_read_pmr;
1132                 else
1133                         fn = __vgic_v3_write_pmr;
1134                 break;
1135         default:
1136                 return 0;
1137         }
1138
1139         vmcr = __vgic_v3_read_vmcr();
1140         rt = kvm_vcpu_sys_get_rt(vcpu);
1141         fn(vcpu, vmcr, rt);
1142
1143         __kvm_skip_instr(vcpu);
1144
1145         return 1;
1146 }