tools/testing/selftests/kvm/lib/x86_64/processor.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /*
   3  * tools/testing/selftests/kvm/lib/x86_64/processor.c
   4  *
   5  * Copyright (C) 2018, Google LLC.
   6  */
   7
   8 #include "test_util.h"
   9 #include "kvm_util.h"
  10 #include "../kvm_util_internal.h"
  11 #include "processor.h"
  12
  13 #ifndef NUM_INTERRUPTS
  14 #define NUM_INTERRUPTS 256
  15 #endif
  16
  17 #define DEFAULT_CODE_SELECTOR 0x8
  18 #define DEFAULT_DATA_SELECTOR 0x10
  19
  20 vm_vaddr_t exception_handlers;
  21
  22 /* Virtual translation table structure declarations */
  23 struct pageUpperEntry {
  24         uint64_t present:1;
  25         uint64_t writable:1;
  26         uint64_t user:1;
  27         uint64_t write_through:1;
  28         uint64_t cache_disable:1;
  29         uint64_t accessed:1;
  30         uint64_t ignored_06:1;
  31         uint64_t page_size:1;
  32         uint64_t ignored_11_08:4;
  33         uint64_t pfn:40;
  34         uint64_t ignored_62_52:11;
  35         uint64_t execute_disable:1;
  36 };
  37
  38 struct pageTableEntry {
  39         uint64_t present:1;
  40         uint64_t writable:1;
  41         uint64_t user:1;
  42         uint64_t write_through:1;
  43         uint64_t cache_disable:1;
  44         uint64_t accessed:1;
  45         uint64_t dirty:1;
  46         uint64_t reserved_07:1;
  47         uint64_t global:1;
  48         uint64_t ignored_11_09:3;
  49         uint64_t pfn:40;
  50         uint64_t ignored_62_52:11;
  51         uint64_t execute_disable:1;
  52 };
  53
  54 void regs_dump(FILE *stream, struct kvm_regs *regs,
  55                uint8_t indent)
  56 {
  57         fprintf(stream, "%*srax: 0x%.16llx rbx: 0x%.16llx "
  58                 "rcx: 0x%.16llx rdx: 0x%.16llx\n",
  59                 indent, "",
  60                 regs->rax, regs->rbx, regs->rcx, regs->rdx);
  61         fprintf(stream, "%*srsi: 0x%.16llx rdi: 0x%.16llx "
  62                 "rsp: 0x%.16llx rbp: 0x%.16llx\n",
  63                 indent, "",
  64                 regs->rsi, regs->rdi, regs->rsp, regs->rbp);
  65         fprintf(stream, "%*sr8:  0x%.16llx r9:  0x%.16llx "
  66                 "r10: 0x%.16llx r11: 0x%.16llx\n",
  67                 indent, "",
  68                 regs->r8, regs->r9, regs->r10, regs->r11);
  69         fprintf(stream, "%*sr12: 0x%.16llx r13: 0x%.16llx "
  70                 "r14: 0x%.16llx r15: 0x%.16llx\n",
  71                 indent, "",
  72                 regs->r12, regs->r13, regs->r14, regs->r15);
  73         fprintf(stream, "%*srip: 0x%.16llx rfl: 0x%.16llx\n",
  74                 indent, "",
  75                 regs->rip, regs->rflags);
  76 }
  77
  78 /*
  79  * Segment Dump
  80  *
  81  * Input Args:
  82  *   stream  - Output FILE stream
  83  *   segment - KVM segment
  84  *   indent  - Left margin indent amount
  85  *
  86  * Output Args: None
  87  *
  88  * Return: None
  89  *
  90  * Dumps the state of the KVM segment given by @segment, to the FILE stream
  91  * given by @stream.
  92  */
  93 static void segment_dump(FILE *stream, struct kvm_segment *segment,
  94                          uint8_t indent)
  95 {
  96         fprintf(stream, "%*sbase: 0x%.16llx limit: 0x%.8x "
  97                 "selector: 0x%.4x type: 0x%.2x\n",
  98                 indent, "", segment->base, segment->limit,
  99                 segment->selector, segment->type);
 100         fprintf(stream, "%*spresent: 0x%.2x dpl: 0x%.2x "
 101                 "db: 0x%.2x s: 0x%.2x l: 0x%.2x\n",
 102                 indent, "", segment->present, segment->dpl,
 103                 segment->db, segment->s, segment->l);
 104         fprintf(stream, "%*sg: 0x%.2x avl: 0x%.2x "
 105                 "unusable: 0x%.2x padding: 0x%.2x\n",
 106                 indent, "", segment->g, segment->avl,
 107                 segment->unusable, segment->padding);
 108 }
 109
 110 /*
 111  * dtable Dump
 112  *
 113  * Input Args:
 114  *   stream - Output FILE stream
 115  *   dtable - KVM dtable
 116  *   indent - Left margin indent amount
 117  *
 118  * Output Args: None
 119  *
 120  * Return: None
 121  *
 122  * Dumps the state of the KVM dtable given by @dtable, to the FILE stream
 123  * given by @stream.
 124  */
 125 static void dtable_dump(FILE *stream, struct kvm_dtable *dtable,
 126                         uint8_t indent)
 127 {
 128         fprintf(stream, "%*sbase: 0x%.16llx limit: 0x%.4x "
 129                 "padding: 0x%.4x 0x%.4x 0x%.4x\n",
 130                 indent, "", dtable->base, dtable->limit,
 131                 dtable->padding[0], dtable->padding[1], dtable->padding[2]);
 132 }
 133
 134 void sregs_dump(FILE *stream, struct kvm_sregs *sregs,
 135                 uint8_t indent)
 136 {
 137         unsigned int i;
 138
 139         fprintf(stream, "%*scs:\n", indent, "");
 140         segment_dump(stream, &sregs->cs, indent + 2);
 141         fprintf(stream, "%*sds:\n", indent, "");
 142         segment_dump(stream, &sregs->ds, indent + 2);
 143         fprintf(stream, "%*ses:\n", indent, "");
 144         segment_dump(stream, &sregs->es, indent + 2);
 145         fprintf(stream, "%*sfs:\n", indent, "");
 146         segment_dump(stream, &sregs->fs, indent + 2);
 147         fprintf(stream, "%*sgs:\n", indent, "");
 148         segment_dump(stream, &sregs->gs, indent + 2);
 149         fprintf(stream, "%*sss:\n", indent, "");
 150         segment_dump(stream, &sregs->ss, indent + 2);
 151         fprintf(stream, "%*str:\n", indent, "");
 152         segment_dump(stream, &sregs->tr, indent + 2);
 153         fprintf(stream, "%*sldt:\n", indent, "");
 154         segment_dump(stream, &sregs->ldt, indent + 2);
 155
 156         fprintf(stream, "%*sgdt:\n", indent, "");
 157         dtable_dump(stream, &sregs->gdt, indent + 2);
 158         fprintf(stream, "%*sidt:\n", indent, "");
 159         dtable_dump(stream, &sregs->idt, indent + 2);
 160
 161         fprintf(stream, "%*scr0: 0x%.16llx cr2: 0x%.16llx "
 162                 "cr3: 0x%.16llx cr4: 0x%.16llx\n",
 163                 indent, "",
 164                 sregs->cr0, sregs->cr2, sregs->cr3, sregs->cr4);
 165         fprintf(stream, "%*scr8: 0x%.16llx efer: 0x%.16llx "
 166                 "apic_base: 0x%.16llx\n",
 167                 indent, "",
 168                 sregs->cr8, sregs->efer, sregs->apic_base);
 169
 170         fprintf(stream, "%*sinterrupt_bitmap:\n", indent, "");
 171         for (i = 0; i < (KVM_NR_INTERRUPTS + 63) / 64; i++) {
 172                 fprintf(stream, "%*s%.16llx\n", indent + 2, "",
 173                         sregs->interrupt_bitmap[i]);
 174         }
 175 }
 176
 177 void virt_pgd_alloc(struct kvm_vm *vm)
 178 {
 179         TEST_ASSERT(vm->mode == VM_MODE_PXXV48_4K, "Attempt to use "
 180                 "unknown or unsupported guest mode, mode: 0x%x", vm->mode);
 181
 182         /* If needed, create page map l4 table. */
 183         if (!vm->pgd_created) {
 184                 vm->pgd = vm_alloc_page_table(vm);
 185                 vm->pgd_created = true;
 186         }
 187 }
 188
 189 static void *virt_get_pte(struct kvm_vm *vm, uint64_t pt_pfn, uint64_t vaddr,
 190                           int level)
 191 {
 192         uint64_t *page_table = addr_gpa2hva(vm, pt_pfn << vm->page_shift);
 193         int index = vaddr >> (vm->page_shift + level * 9) & 0x1ffu;
 194
 195         return &page_table[index];
 196 }
 197
 198 static struct pageUpperEntry *virt_create_upper_pte(struct kvm_vm *vm,
 199                                                     uint64_t pt_pfn,
 200                                                     uint64_t vaddr,
 201                                                     uint64_t paddr,
 202                                                     int level,
 203                                                     enum x86_page_size page_size)
 204 {
 205         struct pageUpperEntry *pte = virt_get_pte(vm, pt_pfn, vaddr, level);
 206
 207         if (!pte->present) {
 208                 pte->writable = true;
 209                 pte->present = true;
 210                 pte->page_size = (level == page_size);
 211                 if (pte->page_size)
 212                         pte->pfn = paddr >> vm->page_shift;
 213                 else
 214                         pte->pfn = vm_alloc_page_table(vm) >> vm->page_shift;
 215         } else {
 216                 /*
 217                  * Entry already present.  Assert that the caller doesn't want
 218                  * a hugepage at this level, and that there isn't a hugepage at
 219                  * this level.
 220                  */
 221                 TEST_ASSERT(level != page_size,
 222                             "Cannot create hugepage at level: %u, vaddr: 0x%lx\n",
 223                             page_size, vaddr);
 224                 TEST_ASSERT(!pte->page_size,
 225                             "Cannot create page table at level: %u, vaddr: 0x%lx\n",
 226                             level, vaddr);
 227         }
 228         return pte;
 229 }
 230
 231 void __virt_pg_map(struct kvm_vm *vm, uint64_t vaddr, uint64_t paddr,
 232                    enum x86_page_size page_size)
 233 {
 234         const uint64_t pg_size = 1ull << ((page_size * 9) + 12);
 235         struct pageUpperEntry *pml4e, *pdpe, *pde;
 236         struct pageTableEntry *pte;
 237
 238         TEST_ASSERT(vm->mode == VM_MODE_PXXV48_4K,
 239                     "Unknown or unsupported guest mode, mode: 0x%x", vm->mode);
 240
 241         TEST_ASSERT((vaddr % pg_size) == 0,
 242                     "Virtual address not aligned,\n"
 243                     "vaddr: 0x%lx page size: 0x%lx", vaddr, pg_size);
 244         TEST_ASSERT(sparsebit_is_set(vm->vpages_valid, (vaddr >> vm->page_shift)),
 245                     "Invalid virtual address, vaddr: 0x%lx", vaddr);
 246         TEST_ASSERT((paddr % pg_size) == 0,
 247                     "Physical address not aligned,\n"
 248                     "  paddr: 0x%lx page size: 0x%lx", paddr, pg_size);
 249         TEST_ASSERT((paddr >> vm->page_shift) <= vm->max_gfn,
 250                     "Physical address beyond maximum supported,\n"
 251                     "  paddr: 0x%lx vm->max_gfn: 0x%lx vm->page_size: 0x%x",
 252                     paddr, vm->max_gfn, vm->page_size);
 253
 254         /*
 255          * Allocate upper level page tables, if not already present.  Return
 256          * early if a hugepage was created.
 257          */
 258         pml4e = virt_create_upper_pte(vm, vm->pgd >> vm->page_shift,
 259                                       vaddr, paddr, 3, page_size);
 260         if (pml4e->page_size)
 261                 return;
 262
 263         pdpe = virt_create_upper_pte(vm, pml4e->pfn, vaddr, paddr, 2, page_size);
 264         if (pdpe->page_size)
 265                 return;
 266
 267         pde = virt_create_upper_pte(vm, pdpe->pfn, vaddr, paddr, 1, page_size);
 268         if (pde->page_size)
 269                 return;
 270
 271         /* Fill in page table entry. */
 272         pte = virt_get_pte(vm, pde->pfn, vaddr, 0);
 273         TEST_ASSERT(!pte->present,
 274                     "PTE already present for 4k page at vaddr: 0x%lx\n", vaddr);
 275         pte->pfn = paddr >> vm->page_shift;
 276         pte->writable = true;
 277         pte->present = 1;
 278 }
 279
 280 void virt_pg_map(struct kvm_vm *vm, uint64_t vaddr, uint64_t paddr)
 281 {
 282         __virt_pg_map(vm, vaddr, paddr, X86_PAGE_SIZE_4K);
 283 }
 284
 285 void virt_dump(FILE *stream, struct kvm_vm *vm, uint8_t indent)
 286 {
 287         struct pageUpperEntry *pml4e, *pml4e_start;
 288         struct pageUpperEntry *pdpe, *pdpe_start;
 289         struct pageUpperEntry *pde, *pde_start;
 290         struct pageTableEntry *pte, *pte_start;
 291
 292         if (!vm->pgd_created)
 293                 return;
 294
 295         fprintf(stream, "%*s                                          "
 296                 "                no\n", indent, "");
 297         fprintf(stream, "%*s      index hvaddr         gpaddr         "
 298                 "addr         w exec dirty\n",
 299                 indent, "");
 300         pml4e_start = (struct pageUpperEntry *) addr_gpa2hva(vm, vm->pgd);
 301         for (uint16_t n1 = 0; n1 <= 0x1ffu; n1++) {
 302                 pml4e = &pml4e_start[n1];
 303                 if (!pml4e->present)
 304                         continue;
 305                 fprintf(stream, "%*spml4e 0x%-3zx %p 0x%-12lx 0x%-10lx %u "
 306                         " %u\n",
 307                         indent, "",
 308                         pml4e - pml4e_start, pml4e,
 309                         addr_hva2gpa(vm, pml4e), (uint64_t) pml4e->pfn,
 310                         pml4e->writable, pml4e->execute_disable);
 311
 312                 pdpe_start = addr_gpa2hva(vm, pml4e->pfn * vm->page_size);
 313                 for (uint16_t n2 = 0; n2 <= 0x1ffu; n2++) {
 314                         pdpe = &pdpe_start[n2];
 315                         if (!pdpe->present)
 316                                 continue;
 317                         fprintf(stream, "%*spdpe  0x%-3zx %p 0x%-12lx 0x%-10lx "
 318                                 "%u  %u\n",
 319                                 indent, "",
 320                                 pdpe - pdpe_start, pdpe,
 321                                 addr_hva2gpa(vm, pdpe),
 322                                 (uint64_t) pdpe->pfn, pdpe->writable,
 323                                 pdpe->execute_disable);
 324
 325                         pde_start = addr_gpa2hva(vm, pdpe->pfn * vm->page_size);
 326                         for (uint16_t n3 = 0; n3 <= 0x1ffu; n3++) {
 327                                 pde = &pde_start[n3];
 328                                 if (!pde->present)
 329                                         continue;
 330                                 fprintf(stream, "%*spde   0x%-3zx %p "
 331                                         "0x%-12lx 0x%-10lx %u  %u\n",
 332                                         indent, "", pde - pde_start, pde,
 333                                         addr_hva2gpa(vm, pde),
 334                                         (uint64_t) pde->pfn, pde->writable,
 335                                         pde->execute_disable);
 336
 337                                 pte_start = addr_gpa2hva(vm, pde->pfn * vm->page_size);
 338                                 for (uint16_t n4 = 0; n4 <= 0x1ffu; n4++) {
 339                                         pte = &pte_start[n4];
 340                                         if (!pte->present)
 341                                                 continue;
 342                                         fprintf(stream, "%*spte   0x%-3zx %p "
 343                                                 "0x%-12lx 0x%-10lx %u  %u "
 344                                                 "    %u    0x%-10lx\n",
 345                                                 indent, "",
 346                                                 pte - pte_start, pte,
 347                                                 addr_hva2gpa(vm, pte),
 348                                                 (uint64_t) pte->pfn,
 349                                                 pte->writable,
 350                                                 pte->execute_disable,
 351                                                 pte->dirty,
 352                                                 ((uint64_t) n1 << 27)
 353                                                         | ((uint64_t) n2 << 18)
 354                                                         | ((uint64_t) n3 << 9)
 355                                                         | ((uint64_t) n4));
 356                                 }
 357                         }
 358                 }
 359         }
 360 }
 361
 362 /*
 363  * Set Unusable Segment
 364  *
 365  * Input Args: None
 366  *
 367  * Output Args:
 368  *   segp - Pointer to segment register
 369  *
 370  * Return: None
 371  *
 372  * Sets the segment register pointed to by @segp to an unusable state.
 373  */
 374 static void kvm_seg_set_unusable(struct kvm_segment *segp)
 375 {
 376         memset(segp, 0, sizeof(*segp));
 377         segp->unusable = true;
 378 }
 379
 380 static void kvm_seg_fill_gdt_64bit(struct kvm_vm *vm, struct kvm_segment *segp)
 381 {
 382         void *gdt = addr_gva2hva(vm, vm->gdt);
 383         struct desc64 *desc = gdt + (segp->selector >> 3) * 8;
 384
 385         desc->limit0 = segp->limit & 0xFFFF;
 386         desc->base0 = segp->base & 0xFFFF;
 387         desc->base1 = segp->base >> 16;
 388         desc->type = segp->type;
 389         desc->s = segp->s;
 390         desc->dpl = segp->dpl;
 391         desc->p = segp->present;
 392         desc->limit1 = segp->limit >> 16;
 393         desc->avl = segp->avl;
 394         desc->l = segp->l;
 395         desc->db = segp->db;
 396         desc->g = segp->g;
 397         desc->base2 = segp->base >> 24;
 398         if (!segp->s)
 399                 desc->base3 = segp->base >> 32;
 400 }
 401
 402
 403 /*
 404  * Set Long Mode Flat Kernel Code Segment
 405  *
 406  * Input Args:
 407  *   vm - VM whose GDT is being filled, or NULL to only write segp
 408  *   selector - selector value
 409  *
 410  * Output Args:
 411  *   segp - Pointer to KVM segment
 412  *
 413  * Return: None
 414  *
 415  * Sets up the KVM segment pointed to by @segp, to be a code segment
 416  * with the selector value given by @selector.
 417  */
 418 static void kvm_seg_set_kernel_code_64bit(struct kvm_vm *vm, uint16_t selector,
 419         struct kvm_segment *segp)
 420 {
 421         memset(segp, 0, sizeof(*segp));
 422         segp->selector = selector;
 423         segp->limit = 0xFFFFFFFFu;
 424         segp->s = 0x1; /* kTypeCodeData */
 425         segp->type = 0x08 | 0x01 | 0x02; /* kFlagCode | kFlagCodeAccessed
 426                                           * | kFlagCodeReadable
 427                                           */
 428         segp->g = true;
 429         segp->l = true;
 430         segp->present = 1;
 431         if (vm)
 432                 kvm_seg_fill_gdt_64bit(vm, segp);
 433 }
 434
 435 /*
 436  * Set Long Mode Flat Kernel Data Segment
 437  *
 438  * Input Args:
 439  *   vm - VM whose GDT is being filled, or NULL to only write segp
 440  *   selector - selector value
 441  *
 442  * Output Args:
 443  *   segp - Pointer to KVM segment
 444  *
 445  * Return: None
 446  *
 447  * Sets up the KVM segment pointed to by @segp, to be a data segment
 448  * with the selector value given by @selector.
 449  */
 450 static void kvm_seg_set_kernel_data_64bit(struct kvm_vm *vm, uint16_t selector,
 451         struct kvm_segment *segp)
 452 {
 453         memset(segp, 0, sizeof(*segp));
 454         segp->selector = selector;
 455         segp->limit = 0xFFFFFFFFu;
 456         segp->s = 0x1; /* kTypeCodeData */
 457         segp->type = 0x00 | 0x01 | 0x02; /* kFlagData | kFlagDataAccessed
 458                                           * | kFlagDataWritable
 459                                           */
 460         segp->g = true;
 461         segp->present = true;
 462         if (vm)
 463                 kvm_seg_fill_gdt_64bit(vm, segp);
 464 }
 465
 466 vm_paddr_t addr_gva2gpa(struct kvm_vm *vm, vm_vaddr_t gva)
 467 {
 468         uint16_t index[4];
 469         struct pageUpperEntry *pml4e, *pdpe, *pde;
 470         struct pageTableEntry *pte;
 471
 472         TEST_ASSERT(vm->mode == VM_MODE_PXXV48_4K, "Attempt to use "
 473                 "unknown or unsupported guest mode, mode: 0x%x", vm->mode);
 474
 475         index[0] = (gva >> 12) & 0x1ffu;
 476         index[1] = (gva >> 21) & 0x1ffu;
 477         index[2] = (gva >> 30) & 0x1ffu;
 478         index[3] = (gva >> 39) & 0x1ffu;
 479
 480         if (!vm->pgd_created)
 481                 goto unmapped_gva;
 482         pml4e = addr_gpa2hva(vm, vm->pgd);
 483         if (!pml4e[index[3]].present)
 484                 goto unmapped_gva;
 485
 486         pdpe = addr_gpa2hva(vm, pml4e[index[3]].pfn * vm->page_size);
 487         if (!pdpe[index[2]].present)
 488                 goto unmapped_gva;
 489
 490         pde = addr_gpa2hva(vm, pdpe[index[2]].pfn * vm->page_size);
 491         if (!pde[index[1]].present)
 492                 goto unmapped_gva;
 493
 494         pte = addr_gpa2hva(vm, pde[index[1]].pfn * vm->page_size);
 495         if (!pte[index[0]].present)
 496                 goto unmapped_gva;
 497
 498         return (pte[index[0]].pfn * vm->page_size) + (gva & 0xfffu);
 499
 500 unmapped_gva:
 501         TEST_FAIL("No mapping for vm virtual address, gva: 0x%lx", gva);
 502         exit(EXIT_FAILURE);
 503 }
 504
 505 static void kvm_setup_gdt(struct kvm_vm *vm, struct kvm_dtable *dt)
 506 {
 507         if (!vm->gdt)
 508                 vm->gdt = vm_vaddr_alloc_page(vm);
 509
 510         dt->base = vm->gdt;
 511         dt->limit = getpagesize();
 512 }
 513
 514 static void kvm_setup_tss_64bit(struct kvm_vm *vm, struct kvm_segment *segp,
 515                                 int selector)
 516 {
 517         if (!vm->tss)
 518                 vm->tss = vm_vaddr_alloc_page(vm);
 519
 520         memset(segp, 0, sizeof(*segp));
 521         segp->base = vm->tss;
 522         segp->limit = 0x67;
 523         segp->selector = selector;
 524         segp->type = 0xb;
 525         segp->present = 1;
 526         kvm_seg_fill_gdt_64bit(vm, segp);
 527 }
 528
 529 static void vcpu_setup(struct kvm_vm *vm, int vcpuid)
 530 {
 531         struct kvm_sregs sregs;
 532
 533         /* Set mode specific system register values. */
 534         vcpu_sregs_get(vm, vcpuid, &sregs);
 535
 536         sregs.idt.limit = 0;
 537
 538         kvm_setup_gdt(vm, &sregs.gdt);
 539
 540         switch (vm->mode) {
 541         case VM_MODE_PXXV48_4K:
 542                 sregs.cr0 = X86_CR0_PE | X86_CR0_NE | X86_CR0_PG;
 543                 sregs.cr4 |= X86_CR4_PAE | X86_CR4_OSFXSR;
 544                 sregs.efer |= (EFER_LME | EFER_LMA | EFER_NX);
 545
 546                 kvm_seg_set_unusable(&sregs.ldt);
 547                 kvm_seg_set_kernel_code_64bit(vm, DEFAULT_CODE_SELECTOR, &sregs.cs);
 548                 kvm_seg_set_kernel_data_64bit(vm, DEFAULT_DATA_SELECTOR, &sregs.ds);
 549                 kvm_seg_set_kernel_data_64bit(vm, DEFAULT_DATA_SELECTOR, &sregs.es);
 550                 kvm_setup_tss_64bit(vm, &sregs.tr, 0x18);
 551                 break;
 552
 553         default:
 554                 TEST_FAIL("Unknown guest mode, mode: 0x%x", vm->mode);
 555         }
 556
 557         sregs.cr3 = vm->pgd;
 558         vcpu_sregs_set(vm, vcpuid, &sregs);
 559 }
 560
 561 void vm_vcpu_add_default(struct kvm_vm *vm, uint32_t vcpuid, void *guest_code)
 562 {
 563         struct kvm_mp_state mp_state;
 564         struct kvm_regs regs;
 565         vm_vaddr_t stack_vaddr;
 566         stack_vaddr = vm_vaddr_alloc(vm, DEFAULT_STACK_PGS * getpagesize(),
 567                                      DEFAULT_GUEST_STACK_VADDR_MIN);
 568
 569         /* Create VCPU */
 570         vm_vcpu_add(vm, vcpuid);
 571         vcpu_setup(vm, vcpuid);
 572
 573         /* Setup guest general purpose registers */
 574         vcpu_regs_get(vm, vcpuid, &regs);
 575         regs.rflags = regs.rflags | 0x2;
 576         regs.rsp = stack_vaddr + (DEFAULT_STACK_PGS * getpagesize());
 577         regs.rip = (unsigned long) guest_code;
 578         vcpu_regs_set(vm, vcpuid, &regs);
 579
 580         /* Setup the MP state */
 581         mp_state.mp_state = 0;
 582         vcpu_set_mp_state(vm, vcpuid, &mp_state);
 583
 584         /* Setup supported CPUIDs */
 585         vcpu_set_cpuid(vm, vcpuid, kvm_get_supported_cpuid());
 586 }
 587
 588 /*
 589  * Allocate an instance of struct kvm_cpuid2
 590  *
 591  * Input Args: None
 592  *
 593  * Output Args: None
 594  *
 595  * Return: A pointer to the allocated struct. The caller is responsible
 596  * for freeing this struct.
 597  *
 598  * Since kvm_cpuid2 uses a 0-length array to allow a the size of the
 599  * array to be decided at allocation time, allocation is slightly
 600  * complicated. This function uses a reasonable default length for
 601  * the array and performs the appropriate allocation.
 602  */
 603 static struct kvm_cpuid2 *allocate_kvm_cpuid2(void)
 604 {
 605         struct kvm_cpuid2 *cpuid;
 606         int nent = 100;
 607         size_t size;
 608
 609         size = sizeof(*cpuid);
 610         size += nent * sizeof(struct kvm_cpuid_entry2);
 611         cpuid = malloc(size);
 612         if (!cpuid) {
 613                 perror("malloc");
 614                 abort();
 615         }
 616
 617         cpuid->nent = nent;
 618
 619         return cpuid;
 620 }
 621
 622 /*
 623  * KVM Supported CPUID Get
 624  *
 625  * Input Args: None
 626  *
 627  * Output Args:
 628  *
 629  * Return: The supported KVM CPUID
 630  *
 631  * Get the guest CPUID supported by KVM.
 632  */
 633 struct kvm_cpuid2 *kvm_get_supported_cpuid(void)
 634 {
 635         static struct kvm_cpuid2 *cpuid;
 636         int ret;
 637         int kvm_fd;
 638
 639         if (cpuid)
 640                 return cpuid;
 641
 642         cpuid = allocate_kvm_cpuid2();
 643         kvm_fd = open_kvm_dev_path_or_exit();
 644
 645         ret = ioctl(kvm_fd, KVM_GET_SUPPORTED_CPUID, cpuid);
 646         TEST_ASSERT(ret == 0, "KVM_GET_SUPPORTED_CPUID failed %d %d\n",
 647                     ret, errno);
 648
 649         close(kvm_fd);
 650         return cpuid;
 651 }
 652
 653 /*
 654  * KVM Get MSR
 655  *
 656  * Input Args:
 657  *   msr_index - Index of MSR
 658  *
 659  * Output Args: None
 660  *
 661  * Return: On success, value of the MSR. On failure a TEST_ASSERT is produced.
 662  *
 663  * Get value of MSR for VCPU.
 664  */
 665 uint64_t kvm_get_feature_msr(uint64_t msr_index)
 666 {
 667         struct {
 668                 struct kvm_msrs header;
 669                 struct kvm_msr_entry entry;
 670         } buffer = {};
 671         int r, kvm_fd;
 672
 673         buffer.header.nmsrs = 1;
 674         buffer.entry.index = msr_index;
 675         kvm_fd = open_kvm_dev_path_or_exit();
 676
 677         r = ioctl(kvm_fd, KVM_GET_MSRS, &buffer.header);
 678         TEST_ASSERT(r == 1, "KVM_GET_MSRS IOCTL failed,\n"
 679                 "  rc: %i errno: %i", r, errno);
 680
 681         close(kvm_fd);
 682         return buffer.entry.data;
 683 }
 684
 685 /*
 686  * VM VCPU CPUID Set
 687  *
 688  * Input Args:
 689  *   vm - Virtual Machine
 690  *   vcpuid - VCPU id
 691  *
 692  * Output Args: None
 693  *
 694  * Return: KVM CPUID (KVM_GET_CPUID2)
 695  *
 696  * Set the VCPU's CPUID.
 697  */
 698 struct kvm_cpuid2 *vcpu_get_cpuid(struct kvm_vm *vm, uint32_t vcpuid)
 699 {
 700         struct vcpu *vcpu = vcpu_find(vm, vcpuid);
 701         struct kvm_cpuid2 *cpuid;
 702         int max_ent;
 703         int rc = -1;
 704
 705         TEST_ASSERT(vcpu != NULL, "vcpu not found, vcpuid: %u", vcpuid);
 706
 707         cpuid = allocate_kvm_cpuid2();
 708         max_ent = cpuid->nent;
 709
 710         for (cpuid->nent = 1; cpuid->nent <= max_ent; cpuid->nent++) {
 711                 rc = ioctl(vcpu->fd, KVM_GET_CPUID2, cpuid);
 712                 if (!rc)
 713                         break;
 714
 715                 TEST_ASSERT(rc == -1 && errno == E2BIG,
 716                             "KVM_GET_CPUID2 should either succeed or give E2BIG: %d %d",
 717                             rc, errno);
 718         }
 719
 720         TEST_ASSERT(rc == 0, "KVM_GET_CPUID2 failed, rc: %i errno: %i",
 721                     rc, errno);
 722
 723         return cpuid;
 724 }
 725
 726
 727
 728 /*
 729  * Locate a cpuid entry.
 730  *
 731  * Input Args:
 732  *   function: The function of the cpuid entry to find.
 733  *   index: The index of the cpuid entry.
 734  *
 735  * Output Args: None
 736  *
 737  * Return: A pointer to the cpuid entry. Never returns NULL.
 738  */
 739 struct kvm_cpuid_entry2 *
 740 kvm_get_supported_cpuid_index(uint32_t function, uint32_t index)
 741 {
 742         struct kvm_cpuid2 *cpuid;
 743         struct kvm_cpuid_entry2 *entry = NULL;
 744         int i;
 745
 746         cpuid = kvm_get_supported_cpuid();
 747         for (i = 0; i < cpuid->nent; i++) {
 748                 if (cpuid->entries[i].function == function &&
 749                     cpuid->entries[i].index == index) {
 750                         entry = &cpuid->entries[i];
 751                         break;
 752                 }
 753         }
 754
 755         TEST_ASSERT(entry, "Guest CPUID entry not found: (EAX=%x, ECX=%x).",
 756                     function, index);
 757         return entry;
 758 }
 759
 760 /*
 761  * VM VCPU CPUID Set
 762  *
 763  * Input Args:
 764  *   vm - Virtual Machine
 765  *   vcpuid - VCPU id
 766  *   cpuid - The CPUID values to set.
 767  *
 768  * Output Args: None
 769  *
 770  * Return: void
 771  *
 772  * Set the VCPU's CPUID.
 773  */
 774 void vcpu_set_cpuid(struct kvm_vm *vm,
 775                 uint32_t vcpuid, struct kvm_cpuid2 *cpuid)
 776 {
 777         struct vcpu *vcpu = vcpu_find(vm, vcpuid);
 778         int rc;
 779
 780         TEST_ASSERT(vcpu != NULL, "vcpu not found, vcpuid: %u", vcpuid);
 781
 782         rc = ioctl(vcpu->fd, KVM_SET_CPUID2, cpuid);
 783         TEST_ASSERT(rc == 0, "KVM_SET_CPUID2 failed, rc: %i errno: %i",
 784                     rc, errno);
 785
 786 }
 787
 788 /*
 789  * VCPU Get MSR
 790  *
 791  * Input Args:
 792  *   vm - Virtual Machine
 793  *   vcpuid - VCPU ID
 794  *   msr_index - Index of MSR
 795  *
 796  * Output Args: None
 797  *
 798  * Return: On success, value of the MSR. On failure a TEST_ASSERT is produced.
 799  *
 800  * Get value of MSR for VCPU.
 801  */
 802 uint64_t vcpu_get_msr(struct kvm_vm *vm, uint32_t vcpuid, uint64_t msr_index)
 803 {
 804         struct vcpu *vcpu = vcpu_find(vm, vcpuid);
 805         struct {
 806                 struct kvm_msrs header;
 807                 struct kvm_msr_entry entry;
 808         } buffer = {};
 809         int r;
 810
 811         TEST_ASSERT(vcpu != NULL, "vcpu not found, vcpuid: %u", vcpuid);
 812         buffer.header.nmsrs = 1;
 813         buffer.entry.index = msr_index;
 814         r = ioctl(vcpu->fd, KVM_GET_MSRS, &buffer.header);
 815         TEST_ASSERT(r == 1, "KVM_GET_MSRS IOCTL failed,\n"
 816                 "  rc: %i errno: %i", r, errno);
 817
 818         return buffer.entry.data;
 819 }
 820
 821 /*
 822  * _VCPU Set MSR
 823  *
 824  * Input Args:
 825  *   vm - Virtual Machine
 826  *   vcpuid - VCPU ID
 827  *   msr_index - Index of MSR
 828  *   msr_value - New value of MSR
 829  *
 830  * Output Args: None
 831  *
 832  * Return: The result of KVM_SET_MSRS.
 833  *
 834  * Sets the value of an MSR for the given VCPU.
 835  */
 836 int _vcpu_set_msr(struct kvm_vm *vm, uint32_t vcpuid, uint64_t msr_index,
 837                   uint64_t msr_value)
 838 {
 839         struct vcpu *vcpu = vcpu_find(vm, vcpuid);
 840         struct {
 841                 struct kvm_msrs header;
 842                 struct kvm_msr_entry entry;
 843         } buffer = {};
 844         int r;
 845
 846         TEST_ASSERT(vcpu != NULL, "vcpu not found, vcpuid: %u", vcpuid);
 847         memset(&buffer, 0, sizeof(buffer));
 848         buffer.header.nmsrs = 1;
 849         buffer.entry.index = msr_index;
 850         buffer.entry.data = msr_value;
 851         r = ioctl(vcpu->fd, KVM_SET_MSRS, &buffer.header);
 852         return r;
 853 }
 854
 855 /*
 856  * VCPU Set MSR
 857  *
 858  * Input Args:
 859  *   vm - Virtual Machine
 860  *   vcpuid - VCPU ID
 861  *   msr_index - Index of MSR
 862  *   msr_value - New value of MSR
 863  *
 864  * Output Args: None
 865  *
 866  * Return: On success, nothing. On failure a TEST_ASSERT is produced.
 867  *
 868  * Set value of MSR for VCPU.
 869  */
 870 void vcpu_set_msr(struct kvm_vm *vm, uint32_t vcpuid, uint64_t msr_index,
 871         uint64_t msr_value)
 872 {
 873         int r;
 874
 875         r = _vcpu_set_msr(vm, vcpuid, msr_index, msr_value);
 876         TEST_ASSERT(r == 1, "KVM_SET_MSRS IOCTL failed,\n"
 877                 "  rc: %i errno: %i", r, errno);
 878 }
 879
 880 void vcpu_args_set(struct kvm_vm *vm, uint32_t vcpuid, unsigned int num, ...)
 881 {
 882         va_list ap;
 883         struct kvm_regs regs;
 884
 885         TEST_ASSERT(num >= 1 && num <= 6, "Unsupported number of args,\n"
 886                     "  num: %u\n",
 887                     num);
 888
 889         va_start(ap, num);
 890         vcpu_regs_get(vm, vcpuid, &regs);
 891
 892         if (num >= 1)
 893                 regs.rdi = va_arg(ap, uint64_t);
 894
 895         if (num >= 2)
 896                 regs.rsi = va_arg(ap, uint64_t);
 897
 898         if (num >= 3)
 899                 regs.rdx = va_arg(ap, uint64_t);
 900
 901         if (num >= 4)
 902                 regs.rcx = va_arg(ap, uint64_t);
 903
 904         if (num >= 5)
 905                 regs.r8 = va_arg(ap, uint64_t);
 906
 907         if (num >= 6)
 908                 regs.r9 = va_arg(ap, uint64_t);
 909
 910         vcpu_regs_set(vm, vcpuid, &regs);
 911         va_end(ap);
 912 }
 913
 914 void vcpu_dump(FILE *stream, struct kvm_vm *vm, uint32_t vcpuid, uint8_t indent)
 915 {
 916         struct kvm_regs regs;
 917         struct kvm_sregs sregs;
 918
 919         fprintf(stream, "%*scpuid: %u\n", indent, "", vcpuid);
 920
 921         fprintf(stream, "%*sregs:\n", indent + 2, "");
 922         vcpu_regs_get(vm, vcpuid, &regs);
 923         regs_dump(stream, &regs, indent + 4);
 924
 925         fprintf(stream, "%*ssregs:\n", indent + 2, "");
 926         vcpu_sregs_get(vm, vcpuid, &sregs);
 927         sregs_dump(stream, &sregs, indent + 4);
 928 }
 929
 930 struct kvm_x86_state {
 931         struct kvm_vcpu_events events;
 932         struct kvm_mp_state mp_state;
 933         struct kvm_regs regs;
 934         struct kvm_xsave xsave;
 935         struct kvm_xcrs xcrs;
 936         struct kvm_sregs sregs;
 937         struct kvm_debugregs debugregs;
 938         union {
 939                 struct kvm_nested_state nested;
 940                 char nested_[16384];
 941         };
 942         struct kvm_msrs msrs;
 943 };
 944
 945 static int kvm_get_num_msrs_fd(int kvm_fd)
 946 {
 947         struct kvm_msr_list nmsrs;
 948         int r;
 949
 950         nmsrs.nmsrs = 0;
 951         r = ioctl(kvm_fd, KVM_GET_MSR_INDEX_LIST, &nmsrs);
 952         TEST_ASSERT(r == -1 && errno == E2BIG, "Unexpected result from KVM_GET_MSR_INDEX_LIST probe, r: %i",
 953                 r);
 954
 955         return nmsrs.nmsrs;
 956 }
 957
 958 static int kvm_get_num_msrs(struct kvm_vm *vm)
 959 {
 960         return kvm_get_num_msrs_fd(vm->kvm_fd);
 961 }
 962
 963 struct kvm_msr_list *kvm_get_msr_index_list(void)
 964 {
 965         struct kvm_msr_list *list;
 966         int nmsrs, r, kvm_fd;
 967
 968         kvm_fd = open_kvm_dev_path_or_exit();
 969
 970         nmsrs = kvm_get_num_msrs_fd(kvm_fd);
 971         list = malloc(sizeof(*list) + nmsrs * sizeof(list->indices[0]));
 972         list->nmsrs = nmsrs;
 973         r = ioctl(kvm_fd, KVM_GET_MSR_INDEX_LIST, list);
 974         close(kvm_fd);
 975
 976         TEST_ASSERT(r == 0, "Unexpected result from KVM_GET_MSR_INDEX_LIST, r: %i",
 977                 r);
 978
 979         return list;
 980 }
 981
 982 struct kvm_x86_state *vcpu_save_state(struct kvm_vm *vm, uint32_t vcpuid)
 983 {
 984         struct vcpu *vcpu = vcpu_find(vm, vcpuid);
 985         struct kvm_msr_list *list;
 986         struct kvm_x86_state *state;
 987         int nmsrs, r, i;
 988         static int nested_size = -1;
 989
 990         if (nested_size == -1) {
 991                 nested_size = kvm_check_cap(KVM_CAP_NESTED_STATE);
 992                 TEST_ASSERT(nested_size <= sizeof(state->nested_),
 993                             "Nested state size too big, %i > %zi",
 994                             nested_size, sizeof(state->nested_));
 995         }
 996
 997         /*
 998          * When KVM exits to userspace with KVM_EXIT_IO, KVM guarantees
 999          * guest state is consistent only after userspace re-enters the
1000          * kernel with KVM_RUN.  Complete IO prior to migrating state
1001          * to a new VM.
1002          */
1003         vcpu_run_complete_io(vm, vcpuid);
1004
1005         nmsrs = kvm_get_num_msrs(vm);
1006         list = malloc(sizeof(*list) + nmsrs * sizeof(list->indices[0]));
1007         list->nmsrs = nmsrs;
1008         r = ioctl(vm->kvm_fd, KVM_GET_MSR_INDEX_LIST, list);
1009         TEST_ASSERT(r == 0, "Unexpected result from KVM_GET_MSR_INDEX_LIST, r: %i",
1010                 r);
1011
1012         state = malloc(sizeof(*state) + nmsrs * sizeof(state->msrs.entries[0]));
1013         r = ioctl(vcpu->fd, KVM_GET_VCPU_EVENTS, &state->events);
1014         TEST_ASSERT(r == 0, "Unexpected result from KVM_GET_VCPU_EVENTS, r: %i",
1015                 r);
1016
1017         r = ioctl(vcpu->fd, KVM_GET_MP_STATE, &state->mp_state);
1018         TEST_ASSERT(r == 0, "Unexpected result from KVM_GET_MP_STATE, r: %i",
1019                 r);
1020
1021         r = ioctl(vcpu->fd, KVM_GET_REGS, &state->regs);
1022         TEST_ASSERT(r == 0, "Unexpected result from KVM_GET_REGS, r: %i",
1023                 r);
1024
1025         r = ioctl(vcpu->fd, KVM_GET_XSAVE, &state->xsave);
1026         TEST_ASSERT(r == 0, "Unexpected result from KVM_GET_XSAVE, r: %i",
1027                 r);
1028
1029         if (kvm_check_cap(KVM_CAP_XCRS)) {
1030                 r = ioctl(vcpu->fd, KVM_GET_XCRS, &state->xcrs);
1031                 TEST_ASSERT(r == 0, "Unexpected result from KVM_GET_XCRS, r: %i",
1032                             r);
1033         }
1034
1035         r = ioctl(vcpu->fd, KVM_GET_SREGS, &state->sregs);
1036         TEST_ASSERT(r == 0, "Unexpected result from KVM_GET_SREGS, r: %i",
1037                 r);
1038
1039         if (nested_size) {
1040                 state->nested.size = sizeof(state->nested_);
1041                 r = ioctl(vcpu->fd, KVM_GET_NESTED_STATE, &state->nested);
1042                 TEST_ASSERT(r == 0, "Unexpected result from KVM_GET_NESTED_STATE, r: %i",
1043                         r);
1044                 TEST_ASSERT(state->nested.size <= nested_size,
1045                         "Nested state size too big, %i (KVM_CHECK_CAP gave %i)",
1046                         state->nested.size, nested_size);
1047         } else
1048                 state->nested.size = 0;
1049
1050         state->msrs.nmsrs = nmsrs;
1051         for (i = 0; i < nmsrs; i++)
1052                 state->msrs.entries[i].index = list->indices[i];
1053         r = ioctl(vcpu->fd, KVM_GET_MSRS, &state->msrs);
1054         TEST_ASSERT(r == nmsrs, "Unexpected result from KVM_GET_MSRS, r: %i (failed MSR was 0x%x)",
1055                 r, r == nmsrs ? -1 : list->indices[r]);
1056
1057         r = ioctl(vcpu->fd, KVM_GET_DEBUGREGS, &state->debugregs);
1058         TEST_ASSERT(r == 0, "Unexpected result from KVM_GET_DEBUGREGS, r: %i",
1059                 r);
1060
1061         free(list);
1062         return state;
1063 }
1064
1065 void vcpu_load_state(struct kvm_vm *vm, uint32_t vcpuid, struct kvm_x86_state *state)
1066 {
1067         struct vcpu *vcpu = vcpu_find(vm, vcpuid);
1068         int r;
1069
1070         r = ioctl(vcpu->fd, KVM_SET_XSAVE, &state->xsave);
1071         TEST_ASSERT(r == 0, "Unexpected result from KVM_SET_XSAVE, r: %i",
1072                 r);
1073
1074         if (kvm_check_cap(KVM_CAP_XCRS)) {
1075                 r = ioctl(vcpu->fd, KVM_SET_XCRS, &state->xcrs);
1076                 TEST_ASSERT(r == 0, "Unexpected result from KVM_SET_XCRS, r: %i",
1077                             r);
1078         }
1079
1080         r = ioctl(vcpu->fd, KVM_SET_SREGS, &state->sregs);
1081         TEST_ASSERT(r == 0, "Unexpected result from KVM_SET_SREGS, r: %i",
1082                 r);
1083
1084         r = ioctl(vcpu->fd, KVM_SET_MSRS, &state->msrs);
1085         TEST_ASSERT(r == state->msrs.nmsrs, "Unexpected result from KVM_SET_MSRS, r: %i (failed at %x)",
1086                 r, r == state->msrs.nmsrs ? -1 : state->msrs.entries[r].index);
1087
1088         r = ioctl(vcpu->fd, KVM_SET_VCPU_EVENTS, &state->events);
1089         TEST_ASSERT(r == 0, "Unexpected result from KVM_SET_VCPU_EVENTS, r: %i",
1090                 r);
1091
1092         r = ioctl(vcpu->fd, KVM_SET_MP_STATE, &state->mp_state);
1093         TEST_ASSERT(r == 0, "Unexpected result from KVM_SET_MP_STATE, r: %i",
1094                 r);
1095
1096         r = ioctl(vcpu->fd, KVM_SET_DEBUGREGS, &state->debugregs);
1097         TEST_ASSERT(r == 0, "Unexpected result from KVM_SET_DEBUGREGS, r: %i",
1098                 r);
1099
1100         r = ioctl(vcpu->fd, KVM_SET_REGS, &state->regs);
1101         TEST_ASSERT(r == 0, "Unexpected result from KVM_SET_REGS, r: %i",
1102                 r);
1103
1104         if (state->nested.size) {
1105                 r = ioctl(vcpu->fd, KVM_SET_NESTED_STATE, &state->nested);
1106                 TEST_ASSERT(r == 0, "Unexpected result from KVM_SET_NESTED_STATE, r: %i",
1107                         r);
1108         }
1109 }
1110
1111 bool is_intel_cpu(void)
1112 {
1113         int eax, ebx, ecx, edx;
1114         const uint32_t *chunk;
1115         const int leaf = 0;
1116
1117         __asm__ __volatile__(
1118                 "cpuid"
1119                 : /* output */ "=a"(eax), "=b"(ebx),
1120                   "=c"(ecx), "=d"(edx)
1121                 : /* input */ "0"(leaf), "2"(0));
1122
1123         chunk = (const uint32_t *)("GenuineIntel");
1124         return (ebx == chunk[0] && edx == chunk[1] && ecx == chunk[2]);
1125 }
1126
1127 uint32_t kvm_get_cpuid_max_basic(void)
1128 {
1129         return kvm_get_supported_cpuid_entry(0)->eax;
1130 }
1131
1132 uint32_t kvm_get_cpuid_max_extended(void)
1133 {
1134         return kvm_get_supported_cpuid_entry(0x80000000)->eax;
1135 }
1136
1137 void kvm_get_cpu_address_width(unsigned int *pa_bits, unsigned int *va_bits)
1138 {
1139         struct kvm_cpuid_entry2 *entry;
1140         bool pae;
1141
1142         /* SDM 4.1.4 */
1143         if (kvm_get_cpuid_max_extended() < 0x80000008) {
1144                 pae = kvm_get_supported_cpuid_entry(1)->edx & (1 << 6);
1145                 *pa_bits = pae ? 36 : 32;
1146                 *va_bits = 32;
1147         } else {
1148                 entry = kvm_get_supported_cpuid_entry(0x80000008);
1149                 *pa_bits = entry->eax & 0xff;
1150                 *va_bits = (entry->eax >> 8) & 0xff;
1151         }
1152 }
1153
1154 struct idt_entry {
1155         uint16_t offset0;
1156         uint16_t selector;
1157         uint16_t ist : 3;
1158         uint16_t : 5;
1159         uint16_t type : 4;
1160         uint16_t : 1;
1161         uint16_t dpl : 2;
1162         uint16_t p : 1;
1163         uint16_t offset1;
1164         uint32_t offset2; uint32_t reserved;
1165 };
1166
1167 static void set_idt_entry(struct kvm_vm *vm, int vector, unsigned long addr,
1168                           int dpl, unsigned short selector)
1169 {
1170         struct idt_entry *base =
1171                 (struct idt_entry *)addr_gva2hva(vm, vm->idt);
1172         struct idt_entry *e = &base[vector];
1173
1174         memset(e, 0, sizeof(*e));
1175         e->offset0 = addr;
1176         e->selector = selector;
1177         e->ist = 0;
1178         e->type = 14;
1179         e->dpl = dpl;
1180         e->p = 1;
1181         e->offset1 = addr >> 16;
1182         e->offset2 = addr >> 32;
1183 }
1184
1185 void kvm_exit_unexpected_vector(uint32_t value)
1186 {
1187         outl(UNEXPECTED_VECTOR_PORT, value);
1188 }
1189
1190 void route_exception(struct ex_regs *regs)
1191 {
1192         typedef void(*handler)(struct ex_regs *);
1193         handler *handlers = (handler *)exception_handlers;
1194
1195         if (handlers && handlers[regs->vector]) {
1196                 handlers[regs->vector](regs);
1197                 return;
1198         }
1199
1200         kvm_exit_unexpected_vector(regs->vector);
1201 }
1202
1203 void vm_init_descriptor_tables(struct kvm_vm *vm)
1204 {
1205         extern void *idt_handlers;
1206         int i;
1207
1208         vm->idt = vm_vaddr_alloc_page(vm);
1209         vm->handlers = vm_vaddr_alloc_page(vm);
1210         /* Handlers have the same address in both address spaces.*/
1211         for (i = 0; i < NUM_INTERRUPTS; i++)
1212                 set_idt_entry(vm, i, (unsigned long)(&idt_handlers)[i], 0,
1213                         DEFAULT_CODE_SELECTOR);
1214 }
1215
1216 void vcpu_init_descriptor_tables(struct kvm_vm *vm, uint32_t vcpuid)
1217 {
1218         struct kvm_sregs sregs;
1219
1220         vcpu_sregs_get(vm, vcpuid, &sregs);
1221         sregs.idt.base = vm->idt;
1222         sregs.idt.limit = NUM_INTERRUPTS * sizeof(struct idt_entry) - 1;
1223         sregs.gdt.base = vm->gdt;
1224         sregs.gdt.limit = getpagesize() - 1;
1225         kvm_seg_set_kernel_data_64bit(NULL, DEFAULT_DATA_SELECTOR, &sregs.gs);
1226         vcpu_sregs_set(vm, vcpuid, &sregs);
1227         *(vm_vaddr_t *)addr_gva2hva(vm, (vm_vaddr_t)(&exception_handlers)) = vm->handlers;
1228 }
1229
1230 void vm_handle_exception(struct kvm_vm *vm, int vector,
1231                          void (*handler)(struct ex_regs *))
1232 {
1233         vm_vaddr_t *handlers = (vm_vaddr_t *)addr_gva2hva(vm, vm->handlers);
1234
1235         handlers[vector] = (vm_vaddr_t)handler;
1236 }
1237
1238 void assert_on_unhandled_exception(struct kvm_vm *vm, uint32_t vcpuid)
1239 {
1240         if (vcpu_state(vm, vcpuid)->exit_reason == KVM_EXIT_IO
1241                 && vcpu_state(vm, vcpuid)->io.port == UNEXPECTED_VECTOR_PORT
1242                 && vcpu_state(vm, vcpuid)->io.size == 4) {
1243                 /* Grab pointer to io data */
1244                 uint32_t *data = (void *)vcpu_state(vm, vcpuid)
1245                         + vcpu_state(vm, vcpuid)->io.data_offset;
1246
1247                 TEST_ASSERT(false,
1248                             "Unexpected vectored event in guest (vector:0x%x)",
1249                             *data);
1250         }
1251 }
1252
1253 bool set_cpuid(struct kvm_cpuid2 *cpuid,
1254                struct kvm_cpuid_entry2 *ent)
1255 {
1256         int i;
1257
1258         for (i = 0; i < cpuid->nent; i++) {
1259                 struct kvm_cpuid_entry2 *cur = &cpuid->entries[i];
1260
1261                 if (cur->function != ent->function || cur->index != ent->index)
1262                         continue;
1263
1264                 memcpy(cur, ent, sizeof(struct kvm_cpuid_entry2));
1265                 return true;
1266         }
1267
1268         return false;
1269 }
1270
1271 uint64_t kvm_hypercall(uint64_t nr, uint64_t a0, uint64_t a1, uint64_t a2,
1272                        uint64_t a3)
1273 {
1274         uint64_t r;
1275
1276         asm volatile("vmcall"
1277                      : "=a"(r)
1278                      : "b"(a0), "c"(a1), "d"(a2), "S"(a3));
1279         return r;
1280 }
1281
1282 struct kvm_cpuid2 *kvm_get_supported_hv_cpuid(void)
1283 {
1284         static struct kvm_cpuid2 *cpuid;
1285         int ret;
1286         int kvm_fd;
1287
1288         if (cpuid)
1289                 return cpuid;
1290
1291         cpuid = allocate_kvm_cpuid2();
1292         kvm_fd = open_kvm_dev_path_or_exit();
1293
1294         ret = ioctl(kvm_fd, KVM_GET_SUPPORTED_HV_CPUID, cpuid);
1295         TEST_ASSERT(ret == 0, "KVM_GET_SUPPORTED_HV_CPUID failed %d %d\n",
1296                     ret, errno);
1297
1298         close(kvm_fd);
1299         return cpuid;
1300 }
1301
1302 void vcpu_set_hv_cpuid(struct kvm_vm *vm, uint32_t vcpuid)
1303 {
1304         static struct kvm_cpuid2 *cpuid_full;
1305         struct kvm_cpuid2 *cpuid_sys, *cpuid_hv;
1306         int i, nent = 0;
1307
1308         if (!cpuid_full) {
1309                 cpuid_sys = kvm_get_supported_cpuid();
1310                 cpuid_hv = kvm_get_supported_hv_cpuid();
1311
1312                 cpuid_full = malloc(sizeof(*cpuid_full) +
1313                                     (cpuid_sys->nent + cpuid_hv->nent) *
1314                                     sizeof(struct kvm_cpuid_entry2));
1315                 if (!cpuid_full) {
1316                         perror("malloc");
1317                         abort();
1318                 }
1319
1320                 /* Need to skip KVM CPUID leaves 0x400000xx */
1321                 for (i = 0; i < cpuid_sys->nent; i++) {
1322                         if (cpuid_sys->entries[i].function >= 0x40000000 &&
1323                             cpuid_sys->entries[i].function < 0x40000100)
1324                                 continue;
1325                         cpuid_full->entries[nent] = cpuid_sys->entries[i];
1326                         nent++;
1327                 }
1328
1329                 memcpy(&cpuid_full->entries[nent], cpuid_hv->entries,
1330                        cpuid_hv->nent * sizeof(struct kvm_cpuid_entry2));
1331                 cpuid_full->nent = nent + cpuid_hv->nent;
1332         }
1333
1334         vcpu_set_cpuid(vm, vcpuid, cpuid_full);
1335 }
1336
1337 struct kvm_cpuid2 *vcpu_get_supported_hv_cpuid(struct kvm_vm *vm, uint32_t vcpuid)
1338 {
1339         static struct kvm_cpuid2 *cpuid;
1340
1341         cpuid = allocate_kvm_cpuid2();
1342
1343         vcpu_ioctl(vm, vcpuid, KVM_GET_SUPPORTED_HV_CPUID, cpuid);
1344
1345         return cpuid;
1346 }