contrib/elf2dmp/addrspace.c

   1 /*
   2  * Copyright (c) 2018 Virtuozzo International GmbH
   3  *
   4  * This work is licensed under the terms of the GNU GPL, version 2 or later.
   5  *
   6  */
   7
   8 #include "qemu/osdep.h"
   9 #include "addrspace.h"
  10
  11 static struct pa_block *pa_space_find_block(struct pa_space *ps, uint64_t pa)
  12 {
  13     size_t i;
  14     for (i = 0; i < ps->block_nr; i++) {
  15         if (ps->block[i].paddr <= pa &&
  16                 pa <= ps->block[i].paddr + ps->block[i].size) {
  17             return ps->block + i;
  18         }
  19     }
  20
  21     return NULL;
  22 }
  23
  24 static uint8_t *pa_space_resolve(struct pa_space *ps, uint64_t pa)
  25 {
  26     struct pa_block *block = pa_space_find_block(ps, pa);
  27
  28     if (!block) {
  29         return NULL;
  30     }
  31
  32     return block->addr + (pa - block->paddr);
  33 }
  34
  35 int pa_space_create(struct pa_space *ps, QEMU_Elf *qemu_elf)
  36 {
  37     Elf64_Half phdr_nr = elf_getphdrnum(qemu_elf->map);
  38     Elf64_Phdr *phdr = elf64_getphdr(qemu_elf->map);
  39     size_t block_i = 0;
  40     size_t i;
  41
  42     ps->block_nr = 0;
  43
  44     for (i = 0; i < phdr_nr; i++) {
  45         if (phdr[i].p_type == PT_LOAD) {
  46             ps->block_nr++;
  47         }
  48     }
  49
  50     ps->block = malloc(sizeof(*ps->block) * ps->block_nr);
  51     if (!ps->block) {
  52         return 1;
  53     }
  54
  55     for (i = 0; i < phdr_nr; i++) {
  56         if (phdr[i].p_type == PT_LOAD) {
  57             ps->block[block_i] = (struct pa_block) {
  58                 .addr = (uint8_t *)qemu_elf->map + phdr[i].p_offset,
  59                 .paddr = phdr[i].p_paddr,
  60                 .size = phdr[i].p_filesz,
  61             };
  62             block_i++;
  63         }
  64     }
  65
  66     return 0;
  67 }
  68
  69 void pa_space_destroy(struct pa_space *ps)
  70 {
  71     ps->block_nr = 0;
  72     free(ps->block);
  73 }
  74
  75 void va_space_set_dtb(struct va_space *vs, uint64_t dtb)
  76 {
  77     vs->dtb = dtb & 0x00ffffffffff000;
  78 }
  79
  80 void va_space_create(struct va_space *vs, struct pa_space *ps, uint64_t dtb)
  81 {
  82     vs->ps = ps;
  83     va_space_set_dtb(vs, dtb);
  84 }
  85
  86 static uint64_t get_pml4e(struct va_space *vs, uint64_t va)
  87 {
  88     uint64_t pa = (vs->dtb & 0xffffffffff000) | ((va & 0xff8000000000) >> 36);
  89
  90     return *(uint64_t *)pa_space_resolve(vs->ps, pa);
  91 }
  92
  93 static uint64_t get_pdpi(struct va_space *vs, uint64_t va, uint64_t pml4e)
  94 {
  95     uint64_t pdpte_paddr = (pml4e & 0xffffffffff000) |
  96         ((va & 0x7FC0000000) >> 27);
  97
  98     return *(uint64_t *)pa_space_resolve(vs->ps, pdpte_paddr);
  99 }
 100
 101 static uint64_t pde_index(uint64_t va)
 102 {
 103     return (va >> 21) & 0x1FF;
 104 }
 105
 106 static uint64_t pdba_base(uint64_t pdpe)
 107 {
 108     return pdpe & 0xFFFFFFFFFF000;
 109 }
 110
 111 static uint64_t get_pgd(struct va_space *vs, uint64_t va, uint64_t pdpe)
 112 {
 113     uint64_t pgd_entry = pdba_base(pdpe) + pde_index(va) * 8;
 114
 115     return *(uint64_t *)pa_space_resolve(vs->ps, pgd_entry);
 116 }
 117
 118 static uint64_t pte_index(uint64_t va)
 119 {
 120     return (va >> 12) & 0x1FF;
 121 }
 122
 123 static uint64_t ptba_base(uint64_t pde)
 124 {
 125     return pde & 0xFFFFFFFFFF000;
 126 }
 127
 128 static uint64_t get_pte(struct va_space *vs, uint64_t va, uint64_t pgd)
 129 {
 130     uint64_t pgd_val = ptba_base(pgd) + pte_index(va) * 8;
 131
 132     return *(uint64_t *)pa_space_resolve(vs->ps, pgd_val);
 133 }
 134
 135 static uint64_t get_paddr(uint64_t va, uint64_t pte)
 136 {
 137     return (pte & 0xFFFFFFFFFF000) | (va & 0xFFF);
 138 }
 139
 140 static bool is_present(uint64_t entry)
 141 {
 142     return entry & 0x1;
 143 }
 144
 145 static bool page_size_flag(uint64_t entry)
 146 {
 147     return entry & (1 << 7);
 148 }
 149
 150 static uint64_t get_1GB_paddr(uint64_t va, uint64_t pdpte)
 151 {
 152     return (pdpte & 0xfffffc0000000) | (va & 0x3fffffff);
 153 }
 154
 155 static uint64_t get_2MB_paddr(uint64_t va, uint64_t pgd_entry)
 156 {
 157     return (pgd_entry & 0xfffffffe00000) | (va & 0x00000001fffff);
 158 }
 159
 160 static uint64_t va_space_va2pa(struct va_space *vs, uint64_t va)
 161 {
 162     uint64_t pml4e, pdpe, pgd, pte;
 163
 164     pml4e = get_pml4e(vs, va);
 165     if (!is_present(pml4e)) {
 166         return INVALID_PA;
 167     }
 168
 169     pdpe = get_pdpi(vs, va, pml4e);
 170     if (!is_present(pdpe)) {
 171         return INVALID_PA;
 172     }
 173
 174     if (page_size_flag(pdpe)) {
 175         return get_1GB_paddr(va, pdpe);
 176     }
 177
 178     pgd = get_pgd(vs, va, pdpe);
 179     if (!is_present(pgd)) {
 180         return INVALID_PA;
 181     }
 182
 183     if (page_size_flag(pgd)) {
 184         return get_2MB_paddr(va, pgd);
 185     }
 186
 187     pte = get_pte(vs, va, pgd);
 188     if (!is_present(pte)) {
 189         return INVALID_PA;
 190     }
 191
 192     return get_paddr(va, pte);
 193 }
 194
 195 void *va_space_resolve(struct va_space *vs, uint64_t va)
 196 {
 197     uint64_t pa = va_space_va2pa(vs, va);
 198
 199     if (pa == INVALID_PA) {
 200         return NULL;
 201     }
 202
 203     return pa_space_resolve(vs->ps, pa);
 204 }
 205
 206 int va_space_rw(struct va_space *vs, uint64_t addr,
 207         void *buf, size_t size, int is_write)
 208 {
 209     while (size) {
 210         uint64_t page = addr & ELF2DMP_PFN_MASK;
 211         size_t s = (page + ELF2DMP_PAGE_SIZE) - addr;
 212         void *ptr;
 213
 214         s = (s > size) ? size : s;
 215
 216         ptr = va_space_resolve(vs, addr);
 217         if (!ptr) {
 218             return 1;
 219         }
 220
 221         if (is_write) {
 222             memcpy(ptr, buf, s);
 223         } else {
 224             memcpy(buf, ptr, s);
 225         }
 226
 227         size -= s;
 228         buf = (uint8_t *)buf + s;
 229         addr += s;
 230     }
 231
 232     return 0;
 233 }