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