arch/arm/kvm/coproc.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /*
   3  * Copyright (C) 2012 - Virtual Open Systems and Columbia University
   4  * Authors: Rusty Russell <rusty@rustcorp.com.au>
   5  *          Christoffer Dall <c.dall@virtualopensystems.com>
   6  */
   7
   8 #include <linux/bsearch.h>
   9 #include <linux/mm.h>
  10 #include <linux/kvm_host.h>
  11 #include <linux/uaccess.h>
  12 #include <asm/kvm_arm.h>
  13 #include <asm/kvm_host.h>
  14 #include <asm/kvm_emulate.h>
  15 #include <asm/kvm_coproc.h>
  16 #include <asm/kvm_mmu.h>
  17 #include <asm/cacheflush.h>
  18 #include <asm/cputype.h>
  19 #include <trace/events/kvm.h>
  20 #include <asm/vfp.h>
  21 #include "../vfp/vfpinstr.h"
  22
  23 #define CREATE_TRACE_POINTS
  24 #include "trace.h"
  25 #include "coproc.h"
  26
  27
  28 /******************************************************************************
  29  * Co-processor emulation
  30  *****************************************************************************/
  31
  32 static bool write_to_read_only(struct kvm_vcpu *vcpu,
  33                                const struct coproc_params *params)
  34 {
  35         WARN_ONCE(1, "CP15 write to read-only register\n");
  36         print_cp_instr(params);
  37         kvm_inject_undefined(vcpu);
  38         return false;
  39 }
  40
  41 static bool read_from_write_only(struct kvm_vcpu *vcpu,
  42                                  const struct coproc_params *params)
  43 {
  44         WARN_ONCE(1, "CP15 read to write-only register\n");
  45         print_cp_instr(params);
  46         kvm_inject_undefined(vcpu);
  47         return false;
  48 }
  49
  50 /* 3 bits per cache level, as per CLIDR, but non-existent caches always 0 */
  51 static u32 cache_levels;
  52
  53 /* CSSELR values; used to index KVM_REG_ARM_DEMUX_ID_CCSIDR */
  54 #define CSSELR_MAX 12
  55
  56 /*
  57  * kvm_vcpu_arch.cp15 holds cp15 registers as an array of u32, but some
  58  * of cp15 registers can be viewed either as couple of two u32 registers
  59  * or one u64 register. Current u64 register encoding is that least
  60  * significant u32 word is followed by most significant u32 word.
  61  */
  62 static inline void vcpu_cp15_reg64_set(struct kvm_vcpu *vcpu,
  63                                        const struct coproc_reg *r,
  64                                        u64 val)
  65 {
  66         vcpu_cp15(vcpu, r->reg) = val & 0xffffffff;
  67         vcpu_cp15(vcpu, r->reg + 1) = val >> 32;
  68 }
  69
  70 static inline u64 vcpu_cp15_reg64_get(struct kvm_vcpu *vcpu,
  71                                       const struct coproc_reg *r)
  72 {
  73         u64 val;
  74
  75         val = vcpu_cp15(vcpu, r->reg + 1);
  76         val = val << 32;
  77         val = val | vcpu_cp15(vcpu, r->reg);
  78         return val;
  79 }
  80
  81 int kvm_handle_cp10_id(struct kvm_vcpu *vcpu, struct kvm_run *run)
  82 {
  83         kvm_inject_undefined(vcpu);
  84         return 1;
  85 }
  86
  87 int kvm_handle_cp_0_13_access(struct kvm_vcpu *vcpu, struct kvm_run *run)
  88 {
  89         /*
  90          * We can get here, if the host has been built without VFPv3 support,
  91          * but the guest attempted a floating point operation.
  92          */
  93         kvm_inject_undefined(vcpu);
  94         return 1;
  95 }
  96
  97 int kvm_handle_cp14_load_store(struct kvm_vcpu *vcpu, struct kvm_run *run)
  98 {
  99         kvm_inject_undefined(vcpu);
 100         return 1;
 101 }
 102
 103 static void reset_mpidr(struct kvm_vcpu *vcpu, const struct coproc_reg *r)
 104 {
 105         /*
 106          * Compute guest MPIDR. We build a virtual cluster out of the
 107          * vcpu_id, but we read the 'U' bit from the underlying
 108          * hardware directly.
 109          */
 110         vcpu_cp15(vcpu, c0_MPIDR) = ((read_cpuid_mpidr() & MPIDR_SMP_BITMASK) |
 111                                      ((vcpu->vcpu_id >> 2) << MPIDR_LEVEL_BITS) |
 112                                      (vcpu->vcpu_id & 3));
 113 }
 114
 115 /* TRM entries A7:4.3.31 A15:4.3.28 - RO WI */
 116 static bool access_actlr(struct kvm_vcpu *vcpu,
 117                          const struct coproc_params *p,
 118                          const struct coproc_reg *r)
 119 {
 120         if (p->is_write)
 121                 return ignore_write(vcpu, p);
 122
 123         *vcpu_reg(vcpu, p->Rt1) = vcpu_cp15(vcpu, c1_ACTLR);
 124         return true;
 125 }
 126
 127 /* TRM entries A7:4.3.56, A15:4.3.60 - R/O. */
 128 static bool access_cbar(struct kvm_vcpu *vcpu,
 129                         const struct coproc_params *p,
 130                         const struct coproc_reg *r)
 131 {
 132         if (p->is_write)
 133                 return write_to_read_only(vcpu, p);
 134         return read_zero(vcpu, p);
 135 }
 136
 137 /* TRM entries A7:4.3.49, A15:4.3.48 - R/O WI */
 138 static bool access_l2ctlr(struct kvm_vcpu *vcpu,
 139                           const struct coproc_params *p,
 140                           const struct coproc_reg *r)
 141 {
 142         if (p->is_write)
 143                 return ignore_write(vcpu, p);
 144
 145         *vcpu_reg(vcpu, p->Rt1) = vcpu_cp15(vcpu, c9_L2CTLR);
 146         return true;
 147 }
 148
 149 static void reset_l2ctlr(struct kvm_vcpu *vcpu, const struct coproc_reg *r)
 150 {
 151         u32 l2ctlr, ncores;
 152
 153         asm volatile("mrc p15, 1, %0, c9, c0, 2\n" : "=r" (l2ctlr));
 154         l2ctlr &= ~(3 << 24);
 155         ncores = atomic_read(&vcpu->kvm->online_vcpus) - 1;
 156         /* How many cores in the current cluster and the next ones */
 157         ncores -= (vcpu->vcpu_id & ~3);
 158         /* Cap it to the maximum number of cores in a single cluster */
 159         ncores = min(ncores, 3U);
 160         l2ctlr |= (ncores & 3) << 24;
 161
 162         vcpu_cp15(vcpu, c9_L2CTLR) = l2ctlr;
 163 }
 164
 165 static void reset_actlr(struct kvm_vcpu *vcpu, const struct coproc_reg *r)
 166 {
 167         u32 actlr;
 168
 169         /* ACTLR contains SMP bit: make sure you create all cpus first! */
 170         asm volatile("mrc p15, 0, %0, c1, c0, 1\n" : "=r" (actlr));
 171         /* Make the SMP bit consistent with the guest configuration */
 172         if (atomic_read(&vcpu->kvm->online_vcpus) > 1)
 173                 actlr |= 1U << 6;
 174         else
 175                 actlr &= ~(1U << 6);
 176
 177         vcpu_cp15(vcpu, c1_ACTLR) = actlr;
 178 }
 179
 180 /*
 181  * TRM entries: A7:4.3.50, A15:4.3.49
 182  * R/O WI (even if NSACR.NS_L2ERR, a write of 1 is ignored).
 183  */
 184 static bool access_l2ectlr(struct kvm_vcpu *vcpu,
 185                            const struct coproc_params *p,
 186                            const struct coproc_reg *r)
 187 {
 188         if (p->is_write)
 189                 return ignore_write(vcpu, p);
 190
 191         *vcpu_reg(vcpu, p->Rt1) = 0;
 192         return true;
 193 }
 194
 195 /*
 196  * See note at ARMv7 ARM B1.14.4 (TL;DR: S/W ops are not easily virtualized).
 197  */
 198 static bool access_dcsw(struct kvm_vcpu *vcpu,
 199                         const struct coproc_params *p,
 200                         const struct coproc_reg *r)
 201 {
 202         if (!p->is_write)
 203                 return read_from_write_only(vcpu, p);
 204
 205         kvm_set_way_flush(vcpu);
 206         return true;
 207 }
 208
 209 /*
 210  * Generic accessor for VM registers. Only called as long as HCR_TVM
 211  * is set.  If the guest enables the MMU, we stop trapping the VM
 212  * sys_regs and leave it in complete control of the caches.
 213  *
 214  * Used by the cpu-specific code.
 215  */
 216 bool access_vm_reg(struct kvm_vcpu *vcpu,
 217                    const struct coproc_params *p,
 218                    const struct coproc_reg *r)
 219 {
 220         bool was_enabled = vcpu_has_cache_enabled(vcpu);
 221
 222         BUG_ON(!p->is_write);
 223
 224         vcpu_cp15(vcpu, r->reg) = *vcpu_reg(vcpu, p->Rt1);
 225         if (p->is_64bit)
 226                 vcpu_cp15(vcpu, r->reg + 1) = *vcpu_reg(vcpu, p->Rt2);
 227
 228         kvm_toggle_cache(vcpu, was_enabled);
 229         return true;
 230 }
 231
 232 static bool access_gic_sgi(struct kvm_vcpu *vcpu,
 233                            const struct coproc_params *p,
 234                            const struct coproc_reg *r)
 235 {
 236         u64 reg;
 237         bool g1;
 238
 239         if (!p->is_write)
 240                 return read_from_write_only(vcpu, p);
 241
 242         reg = (u64)*vcpu_reg(vcpu, p->Rt2) << 32;
 243         reg |= *vcpu_reg(vcpu, p->Rt1) ;
 244
 245         /*
 246          * In a system where GICD_CTLR.DS=1, a ICC_SGI0R access generates
 247          * Group0 SGIs only, while ICC_SGI1R can generate either group,
 248          * depending on the SGI configuration. ICC_ASGI1R is effectively
 249          * equivalent to ICC_SGI0R, as there is no "alternative" secure
 250          * group.
 251          */
 252         switch (p->Op1) {
 253         default:                /* Keep GCC quiet */
 254         case 0:                 /* ICC_SGI1R */
 255                 g1 = true;
 256                 break;
 257         case 1:                 /* ICC_ASGI1R */
 258         case 2:                 /* ICC_SGI0R */
 259                 g1 = false;
 260                 break;
 261         }
 262
 263         vgic_v3_dispatch_sgi(vcpu, reg, g1);
 264
 265         return true;
 266 }
 267
 268 static bool access_gic_sre(struct kvm_vcpu *vcpu,
 269                            const struct coproc_params *p,
 270                            const struct coproc_reg *r)
 271 {
 272         if (p->is_write)
 273                 return ignore_write(vcpu, p);
 274
 275         *vcpu_reg(vcpu, p->Rt1) = vcpu->arch.vgic_cpu.vgic_v3.vgic_sre;
 276
 277         return true;
 278 }
 279
 280 static bool access_cntp_tval(struct kvm_vcpu *vcpu,
 281                              const struct coproc_params *p,
 282                              const struct coproc_reg *r)
 283 {
 284         u32 val;
 285
 286         if (p->is_write) {
 287                 val = *vcpu_reg(vcpu, p->Rt1);
 288                 kvm_arm_timer_write_sysreg(vcpu,
 289                                            TIMER_PTIMER, TIMER_REG_TVAL, val);
 290         } else {
 291                 val = kvm_arm_timer_read_sysreg(vcpu,
 292                                                 TIMER_PTIMER, TIMER_REG_TVAL);
 293                 *vcpu_reg(vcpu, p->Rt1) = val;
 294         }
 295
 296         return true;
 297 }
 298
 299 static bool access_cntp_ctl(struct kvm_vcpu *vcpu,
 300                             const struct coproc_params *p,
 301                             const struct coproc_reg *r)
 302 {
 303         u32 val;
 304
 305         if (p->is_write) {
 306                 val = *vcpu_reg(vcpu, p->Rt1);
 307                 kvm_arm_timer_write_sysreg(vcpu,
 308                                            TIMER_PTIMER, TIMER_REG_CTL, val);
 309         } else {
 310                 val = kvm_arm_timer_read_sysreg(vcpu,
 311                                                 TIMER_PTIMER, TIMER_REG_CTL);
 312                 *vcpu_reg(vcpu, p->Rt1) = val;
 313         }
 314
 315         return true;
 316 }
 317
 318 static bool access_cntp_cval(struct kvm_vcpu *vcpu,
 319                              const struct coproc_params *p,
 320                              const struct coproc_reg *r)
 321 {
 322         u64 val;
 323
 324         if (p->is_write) {
 325                 val = (u64)*vcpu_reg(vcpu, p->Rt2) << 32;
 326                 val |= *vcpu_reg(vcpu, p->Rt1);
 327                 kvm_arm_timer_write_sysreg(vcpu,
 328                                            TIMER_PTIMER, TIMER_REG_CVAL, val);
 329         } else {
 330                 val = kvm_arm_timer_read_sysreg(vcpu,
 331                                                 TIMER_PTIMER, TIMER_REG_CVAL);
 332                 *vcpu_reg(vcpu, p->Rt1) = val;
 333                 *vcpu_reg(vcpu, p->Rt2) = val >> 32;
 334         }
 335
 336         return true;
 337 }
 338
 339 /*
 340  * We could trap ID_DFR0 and tell the guest we don't support performance
 341  * monitoring.  Unfortunately the patch to make the kernel check ID_DFR0 was
 342  * NAKed, so it will read the PMCR anyway.
 343  *
 344  * Therefore we tell the guest we have 0 counters.  Unfortunately, we
 345  * must always support PMCCNTR (the cycle counter): we just RAZ/WI for
 346  * all PM registers, which doesn't crash the guest kernel at least.
 347  */
 348 static bool trap_raz_wi(struct kvm_vcpu *vcpu,
 349                     const struct coproc_params *p,
 350                     const struct coproc_reg *r)
 351 {
 352         if (p->is_write)
 353                 return ignore_write(vcpu, p);
 354         else
 355                 return read_zero(vcpu, p);
 356 }
 357
 358 #define access_pmcr trap_raz_wi
 359 #define access_pmcntenset trap_raz_wi
 360 #define access_pmcntenclr trap_raz_wi
 361 #define access_pmovsr trap_raz_wi
 362 #define access_pmselr trap_raz_wi
 363 #define access_pmceid0 trap_raz_wi
 364 #define access_pmceid1 trap_raz_wi
 365 #define access_pmccntr trap_raz_wi
 366 #define access_pmxevtyper trap_raz_wi
 367 #define access_pmxevcntr trap_raz_wi
 368 #define access_pmuserenr trap_raz_wi
 369 #define access_pmintenset trap_raz_wi
 370 #define access_pmintenclr trap_raz_wi
 371
 372 /* Architected CP15 registers.
 373  * CRn denotes the primary register number, but is copied to the CRm in the
 374  * user space API for 64-bit register access in line with the terminology used
 375  * in the ARM ARM.
 376  * Important: Must be sorted ascending by CRn, CRM, Op1, Op2 and with 64-bit
 377  *            registers preceding 32-bit ones.
 378  */
 379 static const struct coproc_reg cp15_regs[] = {
 380         /* MPIDR: we use VMPIDR for guest access. */
 381         { CRn( 0), CRm( 0), Op1( 0), Op2( 5), is32,
 382                         NULL, reset_mpidr, c0_MPIDR },
 383
 384         /* CSSELR: swapped by interrupt.S. */
 385         { CRn( 0), CRm( 0), Op1( 2), Op2( 0), is32,
 386                         NULL, reset_unknown, c0_CSSELR },
 387
 388         /* ACTLR: trapped by HCR.TAC bit. */
 389         { CRn( 1), CRm( 0), Op1( 0), Op2( 1), is32,
 390                         access_actlr, reset_actlr, c1_ACTLR },
 391
 392         /* CPACR: swapped by interrupt.S. */
 393         { CRn( 1), CRm( 0), Op1( 0), Op2( 2), is32,
 394                         NULL, reset_val, c1_CPACR, 0x00000000 },
 395
 396         /* TTBR0/TTBR1/TTBCR: swapped by interrupt.S. */
 397         { CRm64( 2), Op1( 0), is64, access_vm_reg, reset_unknown64, c2_TTBR0 },
 398         { CRn(2), CRm( 0), Op1( 0), Op2( 0), is32,
 399                         access_vm_reg, reset_unknown, c2_TTBR0 },
 400         { CRn(2), CRm( 0), Op1( 0), Op2( 1), is32,
 401                         access_vm_reg, reset_unknown, c2_TTBR1 },
 402         { CRn( 2), CRm( 0), Op1( 0), Op2( 2), is32,
 403                         access_vm_reg, reset_val, c2_TTBCR, 0x00000000 },
 404         { CRm64( 2), Op1( 1), is64, access_vm_reg, reset_unknown64, c2_TTBR1 },
 405
 406
 407         /* DACR: swapped by interrupt.S. */
 408         { CRn( 3), CRm( 0), Op1( 0), Op2( 0), is32,
 409                         access_vm_reg, reset_unknown, c3_DACR },
 410
 411         /* DFSR/IFSR/ADFSR/AIFSR: swapped by interrupt.S. */
 412         { CRn( 5), CRm( 0), Op1( 0), Op2( 0), is32,
 413                         access_vm_reg, reset_unknown, c5_DFSR },
 414         { CRn( 5), CRm( 0), Op1( 0), Op2( 1), is32,
 415                         access_vm_reg, reset_unknown, c5_IFSR },
 416         { CRn( 5), CRm( 1), Op1( 0), Op2( 0), is32,
 417                         access_vm_reg, reset_unknown, c5_ADFSR },
 418         { CRn( 5), CRm( 1), Op1( 0), Op2( 1), is32,
 419                         access_vm_reg, reset_unknown, c5_AIFSR },
 420
 421         /* DFAR/IFAR: swapped by interrupt.S. */
 422         { CRn( 6), CRm( 0), Op1( 0), Op2( 0), is32,
 423                         access_vm_reg, reset_unknown, c6_DFAR },
 424         { CRn( 6), CRm( 0), Op1( 0), Op2( 2), is32,
 425                         access_vm_reg, reset_unknown, c6_IFAR },
 426
 427         /* PAR swapped by interrupt.S */
 428         { CRm64( 7), Op1( 0), is64, NULL, reset_unknown64, c7_PAR },
 429
 430         /*
 431          * DC{C,I,CI}SW operations:
 432          */
 433         { CRn( 7), CRm( 6), Op1( 0), Op2( 2), is32, access_dcsw},
 434         { CRn( 7), CRm(10), Op1( 0), Op2( 2), is32, access_dcsw},
 435         { CRn( 7), CRm(14), Op1( 0), Op2( 2), is32, access_dcsw},
 436         /*
 437          * L2CTLR access (guest wants to know #CPUs).
 438          */
 439         { CRn( 9), CRm( 0), Op1( 1), Op2( 2), is32,
 440                         access_l2ctlr, reset_l2ctlr, c9_L2CTLR },
 441         { CRn( 9), CRm( 0), Op1( 1), Op2( 3), is32, access_l2ectlr},
 442
 443         /*
 444          * Dummy performance monitor implementation.
 445          */
 446         { CRn( 9), CRm(12), Op1( 0), Op2( 0), is32, access_pmcr},
 447         { CRn( 9), CRm(12), Op1( 0), Op2( 1), is32, access_pmcntenset},
 448         { CRn( 9), CRm(12), Op1( 0), Op2( 2), is32, access_pmcntenclr},
 449         { CRn( 9), CRm(12), Op1( 0), Op2( 3), is32, access_pmovsr},
 450         { CRn( 9), CRm(12), Op1( 0), Op2( 5), is32, access_pmselr},
 451         { CRn( 9), CRm(12), Op1( 0), Op2( 6), is32, access_pmceid0},
 452         { CRn( 9), CRm(12), Op1( 0), Op2( 7), is32, access_pmceid1},
 453         { CRn( 9), CRm(13), Op1( 0), Op2( 0), is32, access_pmccntr},
 454         { CRn( 9), CRm(13), Op1( 0), Op2( 1), is32, access_pmxevtyper},
 455         { CRn( 9), CRm(13), Op1( 0), Op2( 2), is32, access_pmxevcntr},
 456         { CRn( 9), CRm(14), Op1( 0), Op2( 0), is32, access_pmuserenr},
 457         { CRn( 9), CRm(14), Op1( 0), Op2( 1), is32, access_pmintenset},
 458         { CRn( 9), CRm(14), Op1( 0), Op2( 2), is32, access_pmintenclr},
 459
 460         /* PRRR/NMRR (aka MAIR0/MAIR1): swapped by interrupt.S. */
 461         { CRn(10), CRm( 2), Op1( 0), Op2( 0), is32,
 462                         access_vm_reg, reset_unknown, c10_PRRR},
 463         { CRn(10), CRm( 2), Op1( 0), Op2( 1), is32,
 464                         access_vm_reg, reset_unknown, c10_NMRR},
 465
 466         /* AMAIR0/AMAIR1: swapped by interrupt.S. */
 467         { CRn(10), CRm( 3), Op1( 0), Op2( 0), is32,
 468                         access_vm_reg, reset_unknown, c10_AMAIR0},
 469         { CRn(10), CRm( 3), Op1( 0), Op2( 1), is32,
 470                         access_vm_reg, reset_unknown, c10_AMAIR1},
 471
 472         /* ICC_SGI1R */
 473         { CRm64(12), Op1( 0), is64, access_gic_sgi},
 474
 475         /* VBAR: swapped by interrupt.S. */
 476         { CRn(12), CRm( 0), Op1( 0), Op2( 0), is32,
 477                         NULL, reset_val, c12_VBAR, 0x00000000 },
 478
 479         /* ICC_ASGI1R */
 480         { CRm64(12), Op1( 1), is64, access_gic_sgi},
 481         /* ICC_SGI0R */
 482         { CRm64(12), Op1( 2), is64, access_gic_sgi},
 483         /* ICC_SRE */
 484         { CRn(12), CRm(12), Op1( 0), Op2(5), is32, access_gic_sre },
 485
 486         /* CONTEXTIDR/TPIDRURW/TPIDRURO/TPIDRPRW: swapped by interrupt.S. */
 487         { CRn(13), CRm( 0), Op1( 0), Op2( 1), is32,
 488                         access_vm_reg, reset_val, c13_CID, 0x00000000 },
 489         { CRn(13), CRm( 0), Op1( 0), Op2( 2), is32,
 490                         NULL, reset_unknown, c13_TID_URW },
 491         { CRn(13), CRm( 0), Op1( 0), Op2( 3), is32,
 492                         NULL, reset_unknown, c13_TID_URO },
 493         { CRn(13), CRm( 0), Op1( 0), Op2( 4), is32,
 494                         NULL, reset_unknown, c13_TID_PRIV },
 495
 496         /* CNTP */
 497         { CRm64(14), Op1( 2), is64, access_cntp_cval},
 498
 499         /* CNTKCTL: swapped by interrupt.S. */
 500         { CRn(14), CRm( 1), Op1( 0), Op2( 0), is32,
 501                         NULL, reset_val, c14_CNTKCTL, 0x00000000 },
 502
 503         /* CNTP */
 504         { CRn(14), CRm( 2), Op1( 0), Op2( 0), is32, access_cntp_tval },
 505         { CRn(14), CRm( 2), Op1( 0), Op2( 1), is32, access_cntp_ctl },
 506
 507         /* The Configuration Base Address Register. */
 508         { CRn(15), CRm( 0), Op1( 4), Op2( 0), is32, access_cbar},
 509 };
 510
 511 static int check_reg_table(const struct coproc_reg *table, unsigned int n)
 512 {
 513         unsigned int i;
 514
 515         for (i = 1; i < n; i++) {
 516                 if (cmp_reg(&table[i-1], &table[i]) >= 0) {
 517                         kvm_err("reg table %p out of order (%d)\n", table, i - 1);
 518                         return 1;
 519                 }
 520         }
 521
 522         return 0;
 523 }
 524
 525 /* Target specific emulation tables */
 526 static struct kvm_coproc_target_table *target_tables[KVM_ARM_NUM_TARGETS];
 527
 528 void kvm_register_target_coproc_table(struct kvm_coproc_target_table *table)
 529 {
 530         BUG_ON(check_reg_table(table->table, table->num));
 531         target_tables[table->target] = table;
 532 }
 533
 534 /* Get specific register table for this target. */
 535 static const struct coproc_reg *get_target_table(unsigned target, size_t *num)
 536 {
 537         struct kvm_coproc_target_table *table;
 538
 539         table = target_tables[target];
 540         *num = table->num;
 541         return table->table;
 542 }
 543
 544 #define reg_to_match_value(x)                                           \
 545         ({                                                              \
 546                 unsigned long val;                                      \
 547                 val  = (x)->CRn << 11;                                  \
 548                 val |= (x)->CRm << 7;                                   \
 549                 val |= (x)->Op1 << 4;                                   \
 550                 val |= (x)->Op2 << 1;                                   \
 551                 val |= !(x)->is_64bit;                                  \
 552                 val;                                                    \
 553          })
 554
 555 static int match_reg(const void *key, const void *elt)
 556 {
 557         const unsigned long pval = (unsigned long)key;
 558         const struct coproc_reg *r = elt;
 559
 560         return pval - reg_to_match_value(r);
 561 }
 562
 563 static const struct coproc_reg *find_reg(const struct coproc_params *params,
 564                                          const struct coproc_reg table[],
 565                                          unsigned int num)
 566 {
 567         unsigned long pval = reg_to_match_value(params);
 568
 569         return bsearch((void *)pval, table, num, sizeof(table[0]), match_reg);
 570 }
 571
 572 static int emulate_cp15(struct kvm_vcpu *vcpu,
 573                         const struct coproc_params *params)
 574 {
 575         size_t num;
 576         const struct coproc_reg *table, *r;
 577
 578         trace_kvm_emulate_cp15_imp(params->Op1, params->Rt1, params->CRn,
 579                                    params->CRm, params->Op2, params->is_write);
 580
 581         table = get_target_table(vcpu->arch.target, &num);
 582
 583         /* Search target-specific then generic table. */
 584         r = find_reg(params, table, num);
 585         if (!r)
 586                 r = find_reg(params, cp15_regs, ARRAY_SIZE(cp15_regs));
 587
 588         if (likely(r)) {
 589                 /* If we don't have an accessor, we should never get here! */
 590                 BUG_ON(!r->access);
 591
 592                 if (likely(r->access(vcpu, params, r))) {
 593                         /* Skip instruction, since it was emulated */
 594                         kvm_skip_instr(vcpu, kvm_vcpu_trap_il_is32bit(vcpu));
 595                 }
 596         } else {
 597                 /* If access function fails, it should complain. */
 598                 kvm_err("Unsupported guest CP15 access at: %08lx [%08lx]\n",
 599                         *vcpu_pc(vcpu), *vcpu_cpsr(vcpu));
 600                 print_cp_instr(params);
 601                 kvm_inject_undefined(vcpu);
 602         }
 603
 604         return 1;
 605 }
 606
 607 static struct coproc_params decode_64bit_hsr(struct kvm_vcpu *vcpu)
 608 {
 609         struct coproc_params params;
 610
 611         params.CRn = (kvm_vcpu_get_hsr(vcpu) >> 1) & 0xf;
 612         params.Rt1 = (kvm_vcpu_get_hsr(vcpu) >> 5) & 0xf;
 613         params.is_write = ((kvm_vcpu_get_hsr(vcpu) & 1) == 0);
 614         params.is_64bit = true;
 615
 616         params.Op1 = (kvm_vcpu_get_hsr(vcpu) >> 16) & 0xf;
 617         params.Op2 = 0;
 618         params.Rt2 = (kvm_vcpu_get_hsr(vcpu) >> 10) & 0xf;
 619         params.CRm = 0;
 620
 621         return params;
 622 }
 623
 624 /**
 625  * kvm_handle_cp15_64 -- handles a mrrc/mcrr trap on a guest CP15 access
 626  * @vcpu: The VCPU pointer
 627  * @run:  The kvm_run struct
 628  */
 629 int kvm_handle_cp15_64(struct kvm_vcpu *vcpu, struct kvm_run *run)
 630 {
 631         struct coproc_params params = decode_64bit_hsr(vcpu);
 632
 633         return emulate_cp15(vcpu, &params);
 634 }
 635
 636 /**
 637  * kvm_handle_cp14_64 -- handles a mrrc/mcrr trap on a guest CP14 access
 638  * @vcpu: The VCPU pointer
 639  * @run:  The kvm_run struct
 640  */
 641 int kvm_handle_cp14_64(struct kvm_vcpu *vcpu, struct kvm_run *run)
 642 {
 643         struct coproc_params params = decode_64bit_hsr(vcpu);
 644
 645         /* raz_wi cp14 */
 646         trap_raz_wi(vcpu, &params, NULL);
 647
 648         /* handled */
 649         kvm_skip_instr(vcpu, kvm_vcpu_trap_il_is32bit(vcpu));
 650         return 1;
 651 }
 652
 653 static void reset_coproc_regs(struct kvm_vcpu *vcpu,
 654                               const struct coproc_reg *table, size_t num)
 655 {
 656         unsigned long i;
 657
 658         for (i = 0; i < num; i++)
 659                 if (table[i].reset)
 660                         table[i].reset(vcpu, &table[i]);
 661 }
 662
 663 static struct coproc_params decode_32bit_hsr(struct kvm_vcpu *vcpu)
 664 {
 665         struct coproc_params params;
 666
 667         params.CRm = (kvm_vcpu_get_hsr(vcpu) >> 1) & 0xf;
 668         params.Rt1 = (kvm_vcpu_get_hsr(vcpu) >> 5) & 0xf;
 669         params.is_write = ((kvm_vcpu_get_hsr(vcpu) & 1) == 0);
 670         params.is_64bit = false;
 671
 672         params.CRn = (kvm_vcpu_get_hsr(vcpu) >> 10) & 0xf;
 673         params.Op1 = (kvm_vcpu_get_hsr(vcpu) >> 14) & 0x7;
 674         params.Op2 = (kvm_vcpu_get_hsr(vcpu) >> 17) & 0x7;
 675         params.Rt2 = 0;
 676
 677         return params;
 678 }
 679
 680 /**
 681  * kvm_handle_cp15_32 -- handles a mrc/mcr trap on a guest CP15 access
 682  * @vcpu: The VCPU pointer
 683  * @run:  The kvm_run struct
 684  */
 685 int kvm_handle_cp15_32(struct kvm_vcpu *vcpu, struct kvm_run *run)
 686 {
 687         struct coproc_params params = decode_32bit_hsr(vcpu);
 688         return emulate_cp15(vcpu, &params);
 689 }
 690
 691 /**
 692  * kvm_handle_cp14_32 -- handles a mrc/mcr trap on a guest CP14 access
 693  * @vcpu: The VCPU pointer
 694  * @run:  The kvm_run struct
 695  */
 696 int kvm_handle_cp14_32(struct kvm_vcpu *vcpu, struct kvm_run *run)
 697 {
 698         struct coproc_params params = decode_32bit_hsr(vcpu);
 699
 700         /* raz_wi cp14 */
 701         trap_raz_wi(vcpu, &params, NULL);
 702
 703         /* handled */
 704         kvm_skip_instr(vcpu, kvm_vcpu_trap_il_is32bit(vcpu));
 705         return 1;
 706 }
 707
 708 /******************************************************************************
 709  * Userspace API
 710  *****************************************************************************/
 711
 712 static bool index_to_params(u64 id, struct coproc_params *params)
 713 {
 714         switch (id & KVM_REG_SIZE_MASK) {
 715         case KVM_REG_SIZE_U32:
 716                 /* Any unused index bits means it's not valid. */
 717                 if (id & ~(KVM_REG_ARCH_MASK | KVM_REG_SIZE_MASK
 718                            | KVM_REG_ARM_COPROC_MASK
 719                            | KVM_REG_ARM_32_CRN_MASK
 720                            | KVM_REG_ARM_CRM_MASK
 721                            | KVM_REG_ARM_OPC1_MASK
 722                            | KVM_REG_ARM_32_OPC2_MASK))
 723                         return false;
 724
 725                 params->is_64bit = false;
 726                 params->CRn = ((id & KVM_REG_ARM_32_CRN_MASK)
 727                                >> KVM_REG_ARM_32_CRN_SHIFT);
 728                 params->CRm = ((id & KVM_REG_ARM_CRM_MASK)
 729                                >> KVM_REG_ARM_CRM_SHIFT);
 730                 params->Op1 = ((id & KVM_REG_ARM_OPC1_MASK)
 731                                >> KVM_REG_ARM_OPC1_SHIFT);
 732                 params->Op2 = ((id & KVM_REG_ARM_32_OPC2_MASK)
 733                                >> KVM_REG_ARM_32_OPC2_SHIFT);
 734                 return true;
 735         case KVM_REG_SIZE_U64:
 736                 /* Any unused index bits means it's not valid. */
 737                 if (id & ~(KVM_REG_ARCH_MASK | KVM_REG_SIZE_MASK
 738                               | KVM_REG_ARM_COPROC_MASK
 739                               | KVM_REG_ARM_CRM_MASK
 740                               | KVM_REG_ARM_OPC1_MASK))
 741                         return false;
 742                 params->is_64bit = true;
 743                 /* CRm to CRn: see cp15_to_index for details */
 744                 params->CRn = ((id & KVM_REG_ARM_CRM_MASK)
 745                                >> KVM_REG_ARM_CRM_SHIFT);
 746                 params->Op1 = ((id & KVM_REG_ARM_OPC1_MASK)
 747                                >> KVM_REG_ARM_OPC1_SHIFT);
 748                 params->Op2 = 0;
 749                 params->CRm = 0;
 750                 return true;
 751         default:
 752                 return false;
 753         }
 754 }
 755
 756 /* Decode an index value, and find the cp15 coproc_reg entry. */
 757 static const struct coproc_reg *index_to_coproc_reg(struct kvm_vcpu *vcpu,
 758                                                     u64 id)
 759 {
 760         size_t num;
 761         const struct coproc_reg *table, *r;
 762         struct coproc_params params;
 763
 764         /* We only do cp15 for now. */
 765         if ((id & KVM_REG_ARM_COPROC_MASK) >> KVM_REG_ARM_COPROC_SHIFT != 15)
 766                 return NULL;
 767
 768         if (!index_to_params(id, &params))
 769                 return NULL;
 770
 771         table = get_target_table(vcpu->arch.target, &num);
 772         r = find_reg(&params, table, num);
 773         if (!r)
 774                 r = find_reg(&params, cp15_regs, ARRAY_SIZE(cp15_regs));
 775
 776         /* Not saved in the cp15 array? */
 777         if (r && !r->reg)
 778                 r = NULL;
 779
 780         return r;
 781 }
 782
 783 /*
 784  * These are the invariant cp15 registers: we let the guest see the host
 785  * versions of these, so they're part of the guest state.
 786  *
 787  * A future CPU may provide a mechanism to present different values to
 788  * the guest, or a future kvm may trap them.
 789  */
 790 /* Unfortunately, there's no register-argument for mrc, so generate. */
 791 #define FUNCTION_FOR32(crn, crm, op1, op2, name)                        \
 792         static void get_##name(struct kvm_vcpu *v,                      \
 793                                const struct coproc_reg *r)              \
 794         {                                                               \
 795                 u32 val;                                                \
 796                                                                         \
 797                 asm volatile("mrc p15, " __stringify(op1)               \
 798                              ", %0, c" __stringify(crn)                 \
 799                              ", c" __stringify(crm)                     \
 800                              ", " __stringify(op2) "\n" : "=r" (val));  \
 801                 ((struct coproc_reg *)r)->val = val;                    \
 802         }
 803
 804 FUNCTION_FOR32(0, 0, 0, 0, MIDR)
 805 FUNCTION_FOR32(0, 0, 0, 1, CTR)
 806 FUNCTION_FOR32(0, 0, 0, 2, TCMTR)
 807 FUNCTION_FOR32(0, 0, 0, 3, TLBTR)
 808 FUNCTION_FOR32(0, 0, 0, 6, REVIDR)
 809 FUNCTION_FOR32(0, 1, 0, 0, ID_PFR0)
 810 FUNCTION_FOR32(0, 1, 0, 1, ID_PFR1)
 811 FUNCTION_FOR32(0, 1, 0, 2, ID_DFR0)
 812 FUNCTION_FOR32(0, 1, 0, 3, ID_AFR0)
 813 FUNCTION_FOR32(0, 1, 0, 4, ID_MMFR0)
 814 FUNCTION_FOR32(0, 1, 0, 5, ID_MMFR1)
 815 FUNCTION_FOR32(0, 1, 0, 6, ID_MMFR2)
 816 FUNCTION_FOR32(0, 1, 0, 7, ID_MMFR3)
 817 FUNCTION_FOR32(0, 2, 0, 0, ID_ISAR0)
 818 FUNCTION_FOR32(0, 2, 0, 1, ID_ISAR1)
 819 FUNCTION_FOR32(0, 2, 0, 2, ID_ISAR2)
 820 FUNCTION_FOR32(0, 2, 0, 3, ID_ISAR3)
 821 FUNCTION_FOR32(0, 2, 0, 4, ID_ISAR4)
 822 FUNCTION_FOR32(0, 2, 0, 5, ID_ISAR5)
 823 FUNCTION_FOR32(0, 0, 1, 1, CLIDR)
 824 FUNCTION_FOR32(0, 0, 1, 7, AIDR)
 825
 826 /* ->val is filled in by kvm_invariant_coproc_table_init() */
 827 static struct coproc_reg invariant_cp15[] = {
 828         { CRn( 0), CRm( 0), Op1( 0), Op2( 0), is32, NULL, get_MIDR },
 829         { CRn( 0), CRm( 0), Op1( 0), Op2( 1), is32, NULL, get_CTR },
 830         { CRn( 0), CRm( 0), Op1( 0), Op2( 2), is32, NULL, get_TCMTR },
 831         { CRn( 0), CRm( 0), Op1( 0), Op2( 3), is32, NULL, get_TLBTR },
 832         { CRn( 0), CRm( 0), Op1( 0), Op2( 6), is32, NULL, get_REVIDR },
 833
 834         { CRn( 0), CRm( 0), Op1( 1), Op2( 1), is32, NULL, get_CLIDR },
 835         { CRn( 0), CRm( 0), Op1( 1), Op2( 7), is32, NULL, get_AIDR },
 836
 837         { CRn( 0), CRm( 1), Op1( 0), Op2( 0), is32, NULL, get_ID_PFR0 },
 838         { CRn( 0), CRm( 1), Op1( 0), Op2( 1), is32, NULL, get_ID_PFR1 },
 839         { CRn( 0), CRm( 1), Op1( 0), Op2( 2), is32, NULL, get_ID_DFR0 },
 840         { CRn( 0), CRm( 1), Op1( 0), Op2( 3), is32, NULL, get_ID_AFR0 },
 841         { CRn( 0), CRm( 1), Op1( 0), Op2( 4), is32, NULL, get_ID_MMFR0 },
 842         { CRn( 0), CRm( 1), Op1( 0), Op2( 5), is32, NULL, get_ID_MMFR1 },
 843         { CRn( 0), CRm( 1), Op1( 0), Op2( 6), is32, NULL, get_ID_MMFR2 },
 844         { CRn( 0), CRm( 1), Op1( 0), Op2( 7), is32, NULL, get_ID_MMFR3 },
 845
 846         { CRn( 0), CRm( 2), Op1( 0), Op2( 0), is32, NULL, get_ID_ISAR0 },
 847         { CRn( 0), CRm( 2), Op1( 0), Op2( 1), is32, NULL, get_ID_ISAR1 },
 848         { CRn( 0), CRm( 2), Op1( 0), Op2( 2), is32, NULL, get_ID_ISAR2 },
 849         { CRn( 0), CRm( 2), Op1( 0), Op2( 3), is32, NULL, get_ID_ISAR3 },
 850         { CRn( 0), CRm( 2), Op1( 0), Op2( 4), is32, NULL, get_ID_ISAR4 },
 851         { CRn( 0), CRm( 2), Op1( 0), Op2( 5), is32, NULL, get_ID_ISAR5 },
 852 };
 853
 854 /*
 855  * Reads a register value from a userspace address to a kernel
 856  * variable. Make sure that register size matches sizeof(*__val).
 857  */
 858 static int reg_from_user(void *val, const void __user *uaddr, u64 id)
 859 {
 860         if (copy_from_user(val, uaddr, KVM_REG_SIZE(id)) != 0)
 861                 return -EFAULT;
 862         return 0;
 863 }
 864
 865 /*
 866  * Writes a register value to a userspace address from a kernel variable.
 867  * Make sure that register size matches sizeof(*__val).
 868  */
 869 static int reg_to_user(void __user *uaddr, const void *val, u64 id)
 870 {
 871         if (copy_to_user(uaddr, val, KVM_REG_SIZE(id)) != 0)
 872                 return -EFAULT;
 873         return 0;
 874 }
 875
 876 static int get_invariant_cp15(u64 id, void __user *uaddr)
 877 {
 878         struct coproc_params params;
 879         const struct coproc_reg *r;
 880         int ret;
 881
 882         if (!index_to_params(id, &params))
 883                 return -ENOENT;
 884
 885         r = find_reg(&params, invariant_cp15, ARRAY_SIZE(invariant_cp15));
 886         if (!r)
 887                 return -ENOENT;
 888
 889         ret = -ENOENT;
 890         if (KVM_REG_SIZE(id) == 4) {
 891                 u32 val = r->val;
 892
 893                 ret = reg_to_user(uaddr, &val, id);
 894         } else if (KVM_REG_SIZE(id) == 8) {
 895                 ret = reg_to_user(uaddr, &r->val, id);
 896         }
 897         return ret;
 898 }
 899
 900 static int set_invariant_cp15(u64 id, void __user *uaddr)
 901 {
 902         struct coproc_params params;
 903         const struct coproc_reg *r;
 904         int err;
 905         u64 val;
 906
 907         if (!index_to_params(id, &params))
 908                 return -ENOENT;
 909         r = find_reg(&params, invariant_cp15, ARRAY_SIZE(invariant_cp15));
 910         if (!r)
 911                 return -ENOENT;
 912
 913         err = -ENOENT;
 914         if (KVM_REG_SIZE(id) == 4) {
 915                 u32 val32;
 916
 917                 err = reg_from_user(&val32, uaddr, id);
 918                 if (!err)
 919                         val = val32;
 920         } else if (KVM_REG_SIZE(id) == 8) {
 921                 err = reg_from_user(&val, uaddr, id);
 922         }
 923         if (err)
 924                 return err;
 925
 926         /* This is what we mean by invariant: you can't change it. */
 927         if (r->val != val)
 928                 return -EINVAL;
 929
 930         return 0;
 931 }
 932
 933 static bool is_valid_cache(u32 val)
 934 {
 935         u32 level, ctype;
 936
 937         if (val >= CSSELR_MAX)
 938                 return false;
 939
 940         /* Bottom bit is Instruction or Data bit.  Next 3 bits are level. */
 941         level = (val >> 1);
 942         ctype = (cache_levels >> (level * 3)) & 7;
 943
 944         switch (ctype) {
 945         case 0: /* No cache */
 946                 return false;
 947         case 1: /* Instruction cache only */
 948                 return (val & 1);
 949         case 2: /* Data cache only */
 950         case 4: /* Unified cache */
 951                 return !(val & 1);
 952         case 3: /* Separate instruction and data caches */
 953                 return true;
 954         default: /* Reserved: we can't know instruction or data. */
 955                 return false;
 956         }
 957 }
 958
 959 /* Which cache CCSIDR represents depends on CSSELR value. */
 960 static u32 get_ccsidr(u32 csselr)
 961 {
 962         u32 ccsidr;
 963
 964         /* Make sure noone else changes CSSELR during this! */
 965         local_irq_disable();
 966         /* Put value into CSSELR */
 967         asm volatile("mcr p15, 2, %0, c0, c0, 0" : : "r" (csselr));
 968         isb();
 969         /* Read result out of CCSIDR */
 970         asm volatile("mrc p15, 1, %0, c0, c0, 0" : "=r" (ccsidr));
 971         local_irq_enable();
 972
 973         return ccsidr;
 974 }
 975
 976 static int demux_c15_get(u64 id, void __user *uaddr)
 977 {
 978         u32 val;
 979         u32 __user *uval = uaddr;
 980
 981         /* Fail if we have unknown bits set. */
 982         if (id & ~(KVM_REG_ARCH_MASK|KVM_REG_SIZE_MASK|KVM_REG_ARM_COPROC_MASK
 983                    | ((1 << KVM_REG_ARM_COPROC_SHIFT)-1)))
 984                 return -ENOENT;
 985
 986         switch (id & KVM_REG_ARM_DEMUX_ID_MASK) {
 987         case KVM_REG_ARM_DEMUX_ID_CCSIDR:
 988                 if (KVM_REG_SIZE(id) != 4)
 989                         return -ENOENT;
 990                 val = (id & KVM_REG_ARM_DEMUX_VAL_MASK)
 991                         >> KVM_REG_ARM_DEMUX_VAL_SHIFT;
 992                 if (!is_valid_cache(val))
 993                         return -ENOENT;
 994
 995                 return put_user(get_ccsidr(val), uval);
 996         default:
 997                 return -ENOENT;
 998         }
 999 }
1000
1001 static int demux_c15_set(u64 id, void __user *uaddr)
1002 {
1003         u32 val, newval;
1004         u32 __user *uval = uaddr;
1005
1006         /* Fail if we have unknown bits set. */
1007         if (id & ~(KVM_REG_ARCH_MASK|KVM_REG_SIZE_MASK|KVM_REG_ARM_COPROC_MASK
1008                    | ((1 << KVM_REG_ARM_COPROC_SHIFT)-1)))
1009                 return -ENOENT;
1010
1011         switch (id & KVM_REG_ARM_DEMUX_ID_MASK) {
1012         case KVM_REG_ARM_DEMUX_ID_CCSIDR:
1013                 if (KVM_REG_SIZE(id) != 4)
1014                         return -ENOENT;
1015                 val = (id & KVM_REG_ARM_DEMUX_VAL_MASK)
1016                         >> KVM_REG_ARM_DEMUX_VAL_SHIFT;
1017                 if (!is_valid_cache(val))
1018                         return -ENOENT;
1019
1020                 if (get_user(newval, uval))
1021                         return -EFAULT;
1022
1023                 /* This is also invariant: you can't change it. */
1024                 if (newval != get_ccsidr(val))
1025                         return -EINVAL;
1026                 return 0;
1027         default:
1028                 return -ENOENT;
1029         }
1030 }
1031
1032 #ifdef CONFIG_VFPv3
1033 static const int vfp_sysregs[] = { KVM_REG_ARM_VFP_FPEXC,
1034                                    KVM_REG_ARM_VFP_FPSCR,
1035                                    KVM_REG_ARM_VFP_FPINST,
1036                                    KVM_REG_ARM_VFP_FPINST2,
1037                                    KVM_REG_ARM_VFP_MVFR0,
1038                                    KVM_REG_ARM_VFP_MVFR1,
1039                                    KVM_REG_ARM_VFP_FPSID };
1040
1041 static unsigned int num_fp_regs(void)
1042 {
1043         if (((fmrx(MVFR0) & MVFR0_A_SIMD_MASK) >> MVFR0_A_SIMD_BIT) == 2)
1044                 return 32;
1045         else
1046                 return 16;
1047 }
1048
1049 static unsigned int num_vfp_regs(void)
1050 {
1051         /* Normal FP regs + control regs. */
1052         return num_fp_regs() + ARRAY_SIZE(vfp_sysregs);
1053 }
1054
1055 static int copy_vfp_regids(u64 __user *uindices)
1056 {
1057         unsigned int i;
1058         const u64 u32reg = KVM_REG_ARM | KVM_REG_SIZE_U32 | KVM_REG_ARM_VFP;
1059         const u64 u64reg = KVM_REG_ARM | KVM_REG_SIZE_U64 | KVM_REG_ARM_VFP;
1060
1061         for (i = 0; i < num_fp_regs(); i++) {
1062                 if (put_user((u64reg | KVM_REG_ARM_VFP_BASE_REG) + i,
1063                              uindices))
1064                         return -EFAULT;
1065                 uindices++;
1066         }
1067
1068         for (i = 0; i < ARRAY_SIZE(vfp_sysregs); i++) {
1069                 if (put_user(u32reg | vfp_sysregs[i], uindices))
1070                         return -EFAULT;
1071                 uindices++;
1072         }
1073
1074         return num_vfp_regs();
1075 }
1076
1077 static int vfp_get_reg(const struct kvm_vcpu *vcpu, u64 id, void __user *uaddr)
1078 {
1079         u32 vfpid = (id & KVM_REG_ARM_VFP_MASK);
1080         u32 val;
1081
1082         /* Fail if we have unknown bits set. */
1083         if (id & ~(KVM_REG_ARCH_MASK|KVM_REG_SIZE_MASK|KVM_REG_ARM_COPROC_MASK
1084                    | ((1 << KVM_REG_ARM_COPROC_SHIFT)-1)))
1085                 return -ENOENT;
1086
1087         if (vfpid < num_fp_regs()) {
1088                 if (KVM_REG_SIZE(id) != 8)
1089                         return -ENOENT;
1090                 return reg_to_user(uaddr, &vcpu->arch.ctxt.vfp.fpregs[vfpid],
1091                                    id);
1092         }
1093
1094         /* FP control registers are all 32 bit. */
1095         if (KVM_REG_SIZE(id) != 4)
1096                 return -ENOENT;
1097
1098         switch (vfpid) {
1099         case KVM_REG_ARM_VFP_FPEXC:
1100                 return reg_to_user(uaddr, &vcpu->arch.ctxt.vfp.fpexc, id);
1101         case KVM_REG_ARM_VFP_FPSCR:
1102                 return reg_to_user(uaddr, &vcpu->arch.ctxt.vfp.fpscr, id);
1103         case KVM_REG_ARM_VFP_FPINST:
1104                 return reg_to_user(uaddr, &vcpu->arch.ctxt.vfp.fpinst, id);
1105         case KVM_REG_ARM_VFP_FPINST2:
1106                 return reg_to_user(uaddr, &vcpu->arch.ctxt.vfp.fpinst2, id);
1107         case KVM_REG_ARM_VFP_MVFR0:
1108                 val = fmrx(MVFR0);
1109                 return reg_to_user(uaddr, &val, id);
1110         case KVM_REG_ARM_VFP_MVFR1:
1111                 val = fmrx(MVFR1);
1112                 return reg_to_user(uaddr, &val, id);
1113         case KVM_REG_ARM_VFP_FPSID:
1114                 val = fmrx(FPSID);
1115                 return reg_to_user(uaddr, &val, id);
1116         default:
1117                 return -ENOENT;
1118         }
1119 }
1120
1121 static int vfp_set_reg(struct kvm_vcpu *vcpu, u64 id, const void __user *uaddr)
1122 {
1123         u32 vfpid = (id & KVM_REG_ARM_VFP_MASK);
1124         u32 val;
1125
1126         /* Fail if we have unknown bits set. */
1127         if (id & ~(KVM_REG_ARCH_MASK|KVM_REG_SIZE_MASK|KVM_REG_ARM_COPROC_MASK
1128                    | ((1 << KVM_REG_ARM_COPROC_SHIFT)-1)))
1129                 return -ENOENT;
1130
1131         if (vfpid < num_fp_regs()) {
1132                 if (KVM_REG_SIZE(id) != 8)
1133                         return -ENOENT;
1134                 return reg_from_user(&vcpu->arch.ctxt.vfp.fpregs[vfpid],
1135                                      uaddr, id);
1136         }
1137
1138         /* FP control registers are all 32 bit. */
1139         if (KVM_REG_SIZE(id) != 4)
1140                 return -ENOENT;
1141
1142         switch (vfpid) {
1143         case KVM_REG_ARM_VFP_FPEXC:
1144                 return reg_from_user(&vcpu->arch.ctxt.vfp.fpexc, uaddr, id);
1145         case KVM_REG_ARM_VFP_FPSCR:
1146                 return reg_from_user(&vcpu->arch.ctxt.vfp.fpscr, uaddr, id);
1147         case KVM_REG_ARM_VFP_FPINST:
1148                 return reg_from_user(&vcpu->arch.ctxt.vfp.fpinst, uaddr, id);
1149         case KVM_REG_ARM_VFP_FPINST2:
1150                 return reg_from_user(&vcpu->arch.ctxt.vfp.fpinst2, uaddr, id);
1151         /* These are invariant. */
1152         case KVM_REG_ARM_VFP_MVFR0:
1153                 if (reg_from_user(&val, uaddr, id))
1154                         return -EFAULT;
1155                 if (val != fmrx(MVFR0))
1156                         return -EINVAL;
1157                 return 0;
1158         case KVM_REG_ARM_VFP_MVFR1:
1159                 if (reg_from_user(&val, uaddr, id))
1160                         return -EFAULT;
1161                 if (val != fmrx(MVFR1))
1162                         return -EINVAL;
1163                 return 0;
1164         case KVM_REG_ARM_VFP_FPSID:
1165                 if (reg_from_user(&val, uaddr, id))
1166                         return -EFAULT;
1167                 if (val != fmrx(FPSID))
1168                         return -EINVAL;
1169                 return 0;
1170         default:
1171                 return -ENOENT;
1172         }
1173 }
1174 #else /* !CONFIG_VFPv3 */
1175 static unsigned int num_vfp_regs(void)
1176 {
1177         return 0;
1178 }
1179
1180 static int copy_vfp_regids(u64 __user *uindices)
1181 {
1182         return 0;
1183 }
1184
1185 static int vfp_get_reg(const struct kvm_vcpu *vcpu, u64 id, void __user *uaddr)
1186 {
1187         return -ENOENT;
1188 }
1189
1190 static int vfp_set_reg(struct kvm_vcpu *vcpu, u64 id, const void __user *uaddr)
1191 {
1192         return -ENOENT;
1193 }
1194 #endif /* !CONFIG_VFPv3 */
1195
1196 int kvm_arm_coproc_get_reg(struct kvm_vcpu *vcpu, const struct kvm_one_reg *reg)
1197 {
1198         const struct coproc_reg *r;
1199         void __user *uaddr = (void __user *)(long)reg->addr;
1200         int ret;
1201
1202         if ((reg->id & KVM_REG_ARM_COPROC_MASK) == KVM_REG_ARM_DEMUX)
1203                 return demux_c15_get(reg->id, uaddr);
1204
1205         if ((reg->id & KVM_REG_ARM_COPROC_MASK) == KVM_REG_ARM_VFP)
1206                 return vfp_get_reg(vcpu, reg->id, uaddr);
1207
1208         r = index_to_coproc_reg(vcpu, reg->id);
1209         if (!r)
1210                 return get_invariant_cp15(reg->id, uaddr);
1211
1212         ret = -ENOENT;
1213         if (KVM_REG_SIZE(reg->id) == 8) {
1214                 u64 val;
1215
1216                 val = vcpu_cp15_reg64_get(vcpu, r);
1217                 ret = reg_to_user(uaddr, &val, reg->id);
1218         } else if (KVM_REG_SIZE(reg->id) == 4) {
1219                 ret = reg_to_user(uaddr, &vcpu_cp15(vcpu, r->reg), reg->id);
1220         }
1221
1222         return ret;
1223 }
1224
1225 int kvm_arm_coproc_set_reg(struct kvm_vcpu *vcpu, const struct kvm_one_reg *reg)
1226 {
1227         const struct coproc_reg *r;
1228         void __user *uaddr = (void __user *)(long)reg->addr;
1229         int ret;
1230
1231         if ((reg->id & KVM_REG_ARM_COPROC_MASK) == KVM_REG_ARM_DEMUX)
1232                 return demux_c15_set(reg->id, uaddr);
1233
1234         if ((reg->id & KVM_REG_ARM_COPROC_MASK) == KVM_REG_ARM_VFP)
1235                 return vfp_set_reg(vcpu, reg->id, uaddr);
1236
1237         r = index_to_coproc_reg(vcpu, reg->id);
1238         if (!r)
1239                 return set_invariant_cp15(reg->id, uaddr);
1240
1241         ret = -ENOENT;
1242         if (KVM_REG_SIZE(reg->id) == 8) {
1243                 u64 val;
1244
1245                 ret = reg_from_user(&val, uaddr, reg->id);
1246                 if (!ret)
1247                         vcpu_cp15_reg64_set(vcpu, r, val);
1248         } else if (KVM_REG_SIZE(reg->id) == 4) {
1249                 ret = reg_from_user(&vcpu_cp15(vcpu, r->reg), uaddr, reg->id);
1250         }
1251
1252         return ret;
1253 }
1254
1255 static unsigned int num_demux_regs(void)
1256 {
1257         unsigned int i, count = 0;
1258
1259         for (i = 0; i < CSSELR_MAX; i++)
1260                 if (is_valid_cache(i))
1261                         count++;
1262
1263         return count;
1264 }
1265
1266 static int write_demux_regids(u64 __user *uindices)
1267 {
1268         u64 val = KVM_REG_ARM | KVM_REG_SIZE_U32 | KVM_REG_ARM_DEMUX;
1269         unsigned int i;
1270
1271         val |= KVM_REG_ARM_DEMUX_ID_CCSIDR;
1272         for (i = 0; i < CSSELR_MAX; i++) {
1273                 if (!is_valid_cache(i))
1274                         continue;
1275                 if (put_user(val | i, uindices))
1276                         return -EFAULT;
1277                 uindices++;
1278         }
1279         return 0;
1280 }
1281
1282 static u64 cp15_to_index(const struct coproc_reg *reg)
1283 {
1284         u64 val = KVM_REG_ARM | (15 << KVM_REG_ARM_COPROC_SHIFT);
1285         if (reg->is_64bit) {
1286                 val |= KVM_REG_SIZE_U64;
1287                 val |= (reg->Op1 << KVM_REG_ARM_OPC1_SHIFT);
1288                 /*
1289                  * CRn always denotes the primary coproc. reg. nr. for the
1290                  * in-kernel representation, but the user space API uses the
1291                  * CRm for the encoding, because it is modelled after the
1292                  * MRRC/MCRR instructions: see the ARM ARM rev. c page
1293                  * B3-1445
1294                  */
1295                 val |= (reg->CRn << KVM_REG_ARM_CRM_SHIFT);
1296         } else {
1297                 val |= KVM_REG_SIZE_U32;
1298                 val |= (reg->Op1 << KVM_REG_ARM_OPC1_SHIFT);
1299                 val |= (reg->Op2 << KVM_REG_ARM_32_OPC2_SHIFT);
1300                 val |= (reg->CRm << KVM_REG_ARM_CRM_SHIFT);
1301                 val |= (reg->CRn << KVM_REG_ARM_32_CRN_SHIFT);
1302         }
1303         return val;
1304 }
1305
1306 static bool copy_reg_to_user(const struct coproc_reg *reg, u64 __user **uind)
1307 {
1308         if (!*uind)
1309                 return true;
1310
1311         if (put_user(cp15_to_index(reg), *uind))
1312                 return false;
1313
1314         (*uind)++;
1315         return true;
1316 }
1317
1318 /* Assumed ordered tables, see kvm_coproc_table_init. */
1319 static int walk_cp15(struct kvm_vcpu *vcpu, u64 __user *uind)
1320 {
1321         const struct coproc_reg *i1, *i2, *end1, *end2;
1322         unsigned int total = 0;
1323         size_t num;
1324
1325         /* We check for duplicates here, to allow arch-specific overrides. */
1326         i1 = get_target_table(vcpu->arch.target, &num);
1327         end1 = i1 + num;
1328         i2 = cp15_regs;
1329         end2 = cp15_regs + ARRAY_SIZE(cp15_regs);
1330
1331         BUG_ON(i1 == end1 || i2 == end2);
1332
1333         /* Walk carefully, as both tables may refer to the same register. */
1334         while (i1 || i2) {
1335                 int cmp = cmp_reg(i1, i2);
1336                 /* target-specific overrides generic entry. */
1337                 if (cmp <= 0) {
1338                         /* Ignore registers we trap but don't save. */
1339                         if (i1->reg) {
1340                                 if (!copy_reg_to_user(i1, &uind))
1341                                         return -EFAULT;
1342                                 total++;
1343                         }
1344                 } else {
1345                         /* Ignore registers we trap but don't save. */
1346                         if (i2->reg) {
1347                                 if (!copy_reg_to_user(i2, &uind))
1348                                         return -EFAULT;
1349                                 total++;
1350                         }
1351                 }
1352
1353                 if (cmp <= 0 && ++i1 == end1)
1354                         i1 = NULL;
1355                 if (cmp >= 0 && ++i2 == end2)
1356                         i2 = NULL;
1357         }
1358         return total;
1359 }
1360
1361 unsigned long kvm_arm_num_coproc_regs(struct kvm_vcpu *vcpu)
1362 {
1363         return ARRAY_SIZE(invariant_cp15)
1364                 + num_demux_regs()
1365                 + num_vfp_regs()
1366                 + walk_cp15(vcpu, (u64 __user *)NULL);
1367 }
1368
1369 int kvm_arm_copy_coproc_indices(struct kvm_vcpu *vcpu, u64 __user *uindices)
1370 {
1371         unsigned int i;
1372         int err;
1373
1374         /* Then give them all the invariant registers' indices. */
1375         for (i = 0; i < ARRAY_SIZE(invariant_cp15); i++) {
1376                 if (put_user(cp15_to_index(&invariant_cp15[i]), uindices))
1377                         return -EFAULT;
1378                 uindices++;
1379         }
1380
1381         err = walk_cp15(vcpu, uindices);
1382         if (err < 0)
1383                 return err;
1384         uindices += err;
1385
1386         err = copy_vfp_regids(uindices);
1387         if (err < 0)
1388                 return err;
1389         uindices += err;
1390
1391         return write_demux_regids(uindices);
1392 }
1393
1394 void kvm_coproc_table_init(void)
1395 {
1396         unsigned int i;
1397
1398         /* Make sure tables are unique and in order. */
1399         BUG_ON(check_reg_table(cp15_regs, ARRAY_SIZE(cp15_regs)));
1400         BUG_ON(check_reg_table(invariant_cp15, ARRAY_SIZE(invariant_cp15)));
1401
1402         /* We abuse the reset function to overwrite the table itself. */
1403         for (i = 0; i < ARRAY_SIZE(invariant_cp15); i++)
1404                 invariant_cp15[i].reset(NULL, &invariant_cp15[i]);
1405
1406         /*
1407          * CLIDR format is awkward, so clean it up.  See ARM B4.1.20:
1408          *
1409          *   If software reads the Cache Type fields from Ctype1
1410          *   upwards, once it has seen a value of 0b000, no caches
1411          *   exist at further-out levels of the hierarchy. So, for
1412          *   example, if Ctype3 is the first Cache Type field with a
1413          *   value of 0b000, the values of Ctype4 to Ctype7 must be
1414          *   ignored.
1415          */
1416         asm volatile("mrc p15, 1, %0, c0, c0, 1" : "=r" (cache_levels));
1417         for (i = 0; i < 7; i++)
1418                 if (((cache_levels >> (i*3)) & 7) == 0)
1419                         break;
1420         /* Clear all higher bits. */
1421         cache_levels &= (1 << (i*3))-1;
1422 }
1423
1424 /**
1425  * kvm_reset_coprocs - sets cp15 registers to reset value
1426  * @vcpu: The VCPU pointer
1427  *
1428  * This function finds the right table above and sets the registers on the
1429  * virtual CPU struct to their architecturally defined reset values.
1430  */
1431 void kvm_reset_coprocs(struct kvm_vcpu *vcpu)
1432 {
1433         size_t num;
1434         const struct coproc_reg *table;
1435
1436         /* Catch someone adding a register without putting in reset entry. */
1437         memset(vcpu->arch.ctxt.cp15, 0x42, sizeof(vcpu->arch.ctxt.cp15));
1438
1439         /* Generic chip reset first (so target could override). */
1440         reset_coproc_regs(vcpu, cp15_regs, ARRAY_SIZE(cp15_regs));
1441
1442         table = get_target_table(vcpu->arch.target, &num);
1443         reset_coproc_regs(vcpu, table, num);
1444
1445         for (num = 1; num < NR_CP15_REGS; num++)
1446                 WARN(vcpu_cp15(vcpu, num) == 0x42424242,
1447                      "Didn't reset vcpu_cp15(vcpu, %zi)", num);
1448 }