| 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 | } |