arch/arm/kvm/coproc.c

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