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