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
  15     for (i = 0; i < ps->block_nr; i++) {
  16         if (ps->block[i].paddr <= pa &&
  17                 pa < ps->block[i].paddr + ps->block[i].size) {
  18             return ps->block + i;
  19         }
  20     }
  21
  22     return NULL;
  23 }
  24
  25 static void *pa_space_resolve(struct pa_space *ps, uint64_t pa)
  26 {
  27     struct pa_block *block = pa_space_find_block(ps, pa);
  28
  29     if (!block) {
  30         return NULL;
  31     }
  32
  33     return block->addr + (pa - block->paddr);
  34 }
  35
  36 static bool pa_space_read64(struct pa_space *ps, uint64_t pa, uint64_t *value)
  37 {
  38     uint64_t *resolved = pa_space_resolve(ps, pa);
  39
  40     if (!resolved) {
  41         return false;
  42     }
  43
  44     *value = *resolved;
  45
  46     return true;
  47 }
  48
  49 static void pa_block_align(struct pa_block *b)
  50 {
  51     uint64_t low_align = ((b->paddr - 1) | ELF2DMP_PAGE_MASK) + 1 - b->paddr;
  52     uint64_t high_align = (b->paddr + b->size) & ELF2DMP_PAGE_MASK;
  53
  54     if (low_align == 0 && high_align == 0) {
  55         return;
  56     }
  57
  58     if (low_align + high_align < b->size) {
  59         printf("Block 0x%"PRIx64"+:0x%"PRIx64" will be aligned to "
  60                 "0x%"PRIx64"+:0x%"PRIx64"\n", b->paddr, b->size,
  61                 b->paddr + low_align, b->size - low_align - high_align);
  62         b->size -= low_align + high_align;
  63     } else {
  64         printf("Block 0x%"PRIx64"+:0x%"PRIx64" is too small to align\n",
  65                 b->paddr, b->size);
  66         b->size = 0;
  67     }
  68
  69     b->addr += low_align;
  70     b->paddr += low_align;
  71 }
  72
  73 void pa_space_create(struct pa_space *ps, QEMU_Elf *qemu_elf)
  74 {
  75     Elf64_Half phdr_nr = elf_getphdrnum(qemu_elf->map);
  76     Elf64_Phdr *phdr = elf64_getphdr(qemu_elf->map);
  77     size_t block_i = 0;
  78     size_t i;
  79
  80     ps->block_nr = 0;
  81
  82     for (i = 0; i < phdr_nr; i++) {
  83         if (phdr[i].p_type == PT_LOAD) {
  84             ps->block_nr++;
  85         }
  86     }
  87
  88     ps->block = g_new(struct pa_block, ps->block_nr);
  89
  90     for (i = 0; i < phdr_nr; i++) {
  91         if (phdr[i].p_type == PT_LOAD && phdr[i].p_offset < qemu_elf->size) {
  92             ps->block[block_i] = (struct pa_block) {
  93                 .addr = (uint8_t *)qemu_elf->map + phdr[i].p_offset,
  94                 .paddr = phdr[i].p_paddr,
  95                 .size = MIN(phdr[i].p_filesz,
  96                             qemu_elf->size - phdr[i].p_offset),
  97             };
  98             pa_block_align(&ps->block[block_i]);
  99             block_i = ps->block[block_i].size ? (block_i + 1) : block_i;
 100         }
 101     }
 102
 103     ps->block_nr = block_i;
 104 }
 105
 106 void pa_space_destroy(struct pa_space *ps)
 107 {
 108     ps->block_nr = 0;
 109     g_free(ps->block);
 110 }
 111
 112 void va_space_set_dtb(struct va_space *vs, uint64_t dtb)
 113 {
 114     vs->dtb = dtb & 0x00ffffffffff000;
 115 }
 116
 117 void va_space_create(struct va_space *vs, struct pa_space *ps, uint64_t dtb)
 118 {
 119     vs->ps = ps;
 120     va_space_set_dtb(vs, dtb);
 121 }
 122
 123 static bool get_pml4e(struct va_space *vs, uint64_t va, uint64_t *value)
 124 {
 125     uint64_t pa = (vs->dtb & 0xffffffffff000) | ((va & 0xff8000000000) >> 36);
 126
 127     return pa_space_read64(vs->ps, pa, value);
 128 }
 129
 130 static bool get_pdpi(struct va_space *vs, uint64_t va, uint64_t pml4e,
 131                     uint64_t *value)
 132 {
 133     uint64_t pdpte_paddr = (pml4e & 0xffffffffff000) |
 134         ((va & 0x7FC0000000) >> 27);
 135
 136     return pa_space_read64(vs->ps, pdpte_paddr, value);
 137 }
 138
 139 static uint64_t pde_index(uint64_t va)
 140 {
 141     return (va >> 21) & 0x1FF;
 142 }
 143
 144 static uint64_t pdba_base(uint64_t pdpe)
 145 {
 146     return pdpe & 0xFFFFFFFFFF000;
 147 }
 148
 149 static bool get_pgd(struct va_space *vs, uint64_t va, uint64_t pdpe,
 150                    uint64_t *value)
 151 {
 152     uint64_t pgd_entry = pdba_base(pdpe) + pde_index(va) * 8;
 153
 154     return pa_space_read64(vs->ps, pgd_entry, value);
 155 }
 156
 157 static uint64_t pte_index(uint64_t va)
 158 {
 159     return (va >> 12) & 0x1FF;
 160 }
 161
 162 static uint64_t ptba_base(uint64_t pde)
 163 {
 164     return pde & 0xFFFFFFFFFF000;
 165 }
 166
 167 static bool get_pte(struct va_space *vs, uint64_t va, uint64_t pgd,
 168                    uint64_t *value)
 169 {
 170     uint64_t pgd_val = ptba_base(pgd) + pte_index(va) * 8;
 171
 172     return pa_space_read64(vs->ps, pgd_val, value);
 173 }
 174
 175 static uint64_t get_paddr(uint64_t va, uint64_t pte)
 176 {
 177     return (pte & 0xFFFFFFFFFF000) | (va & 0xFFF);
 178 }
 179
 180 static bool is_present(uint64_t entry)
 181 {
 182     return entry & 0x1;
 183 }
 184
 185 static bool page_size_flag(uint64_t entry)
 186 {
 187     return entry & (1 << 7);
 188 }
 189
 190 static uint64_t get_1GB_paddr(uint64_t va, uint64_t pdpte)
 191 {
 192     return (pdpte & 0xfffffc0000000) | (va & 0x3fffffff);
 193 }
 194
 195 static uint64_t get_2MB_paddr(uint64_t va, uint64_t pgd_entry)
 196 {
 197     return (pgd_entry & 0xfffffffe00000) | (va & 0x00000001fffff);
 198 }
 199
 200 static uint64_t va_space_va2pa(struct va_space *vs, uint64_t va)
 201 {
 202     uint64_t pml4e, pdpe, pgd, pte;
 203
 204     if (!get_pml4e(vs, va, &pml4e) || !is_present(pml4e)) {
 205         return INVALID_PA;
 206     }
 207
 208     if (!get_pdpi(vs, va, pml4e, &pdpe) || !is_present(pdpe)) {
 209         return INVALID_PA;
 210     }
 211
 212     if (page_size_flag(pdpe)) {
 213         return get_1GB_paddr(va, pdpe);
 214     }
 215
 216     if (!get_pgd(vs, va, pdpe, &pgd) || !is_present(pgd)) {
 217         return INVALID_PA;
 218     }
 219
 220     if (page_size_flag(pgd)) {
 221         return get_2MB_paddr(va, pgd);
 222     }
 223
 224     if (!get_pte(vs, va, pgd, &pte) || !is_present(pte)) {
 225         return INVALID_PA;
 226     }
 227
 228     return get_paddr(va, pte);
 229 }
 230
 231 void *va_space_resolve(struct va_space *vs, uint64_t va)
 232 {
 233     uint64_t pa = va_space_va2pa(vs, va);
 234
 235     if (pa == INVALID_PA) {
 236         return NULL;
 237     }
 238
 239     return pa_space_resolve(vs->ps, pa);
 240 }
 241
 242 bool va_space_rw(struct va_space *vs, uint64_t addr,
 243                  void *buf, size_t size, int is_write)
 244 {
 245     while (size) {
 246         uint64_t page = addr & ELF2DMP_PFN_MASK;
 247         size_t s = (page + ELF2DMP_PAGE_SIZE) - addr;
 248         void *ptr;
 249
 250         s = (s > size) ? size : s;
 251
 252         ptr = va_space_resolve(vs, addr);
 253         if (!ptr) {
 254             return false;
 255         }
 256
 257         if (is_write) {
 258             memcpy(ptr, buf, s);
 259         } else {
 260             memcpy(buf, ptr, s);
 261         }
 262
 263         size -= s;
 264         buf = (uint8_t *)buf + s;
 265         addr += s;
 266     }
 267
 268     return true;
 269 }