qemu/contrib/elf2dmp/addrspace.c
<<
>>
Prefs
   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
  11static 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
  24static 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
  35int 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
  69void pa_space_destroy(struct pa_space *ps)
  70{
  71    ps->block_nr = 0;
  72    free(ps->block);
  73}
  74
  75void va_space_set_dtb(struct va_space *vs, uint64_t dtb)
  76{
  77    vs->dtb = dtb & 0x00ffffffffff000;
  78}
  79
  80void 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
  86static 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
  93static 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
 101static uint64_t pde_index(uint64_t va)
 102{
 103    return (va >> 21) & 0x1FF;
 104}
 105
 106static uint64_t pdba_base(uint64_t pdpe)
 107{
 108    return pdpe & 0xFFFFFFFFFF000;
 109}
 110
 111static 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
 118static uint64_t pte_index(uint64_t va)
 119{
 120    return (va >> 12) & 0x1FF;
 121}
 122
 123static uint64_t ptba_base(uint64_t pde)
 124{
 125    return pde & 0xFFFFFFFFFF000;
 126}
 127
 128static 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
 135static uint64_t get_paddr(uint64_t va, uint64_t pte)
 136{
 137    return (pte & 0xFFFFFFFFFF000) | (va & 0xFFF);
 138}
 139
 140static bool is_present(uint64_t entry)
 141{
 142    return entry & 0x1;
 143}
 144
 145static bool page_size_flag(uint64_t entry)
 146{
 147    return entry & (1 << 7);
 148}
 149
 150static uint64_t get_1GB_paddr(uint64_t va, uint64_t pdpte)
 151{
 152    return (pdpte & 0xfffffc0000000) | (va & 0x3fffffff);
 153}
 154
 155static uint64_t get_2MB_paddr(uint64_t va, uint64_t pgd_entry)
 156{
 157    return (pgd_entry & 0xfffffffe00000) | (va & 0x00000001fffff);
 158}
 159
 160static 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
 195void *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
 206int 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}
 234