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) { 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 = phdr[i].p_filesz, 96 }; 97 pa_block_align(&ps->block[block_i]); 98 block_i = ps->block[block_i].size ? (block_i + 1) : block_i; 99 } 100 } 101 102 ps->block_nr = block_i; 103 } 104 105 void pa_space_destroy(struct pa_space *ps) 106 { 107 ps->block_nr = 0; 108 g_free(ps->block); 109 } 110 111 void va_space_set_dtb(struct va_space *vs, uint64_t dtb) 112 { 113 vs->dtb = dtb & 0x00ffffffffff000; 114 } 115 116 void va_space_create(struct va_space *vs, struct pa_space *ps, uint64_t dtb) 117 { 118 vs->ps = ps; 119 va_space_set_dtb(vs, dtb); 120 } 121 122 static bool get_pml4e(struct va_space *vs, uint64_t va, uint64_t *value) 123 { 124 uint64_t pa = (vs->dtb & 0xffffffffff000) | ((va & 0xff8000000000) >> 36); 125 126 return pa_space_read64(vs->ps, pa, value); 127 } 128 129 static bool get_pdpi(struct va_space *vs, uint64_t va, uint64_t pml4e, 130 uint64_t *value) 131 { 132 uint64_t pdpte_paddr = (pml4e & 0xffffffffff000) | 133 ((va & 0x7FC0000000) >> 27); 134 135 return pa_space_read64(vs->ps, pdpte_paddr, value); 136 } 137 138 static uint64_t pde_index(uint64_t va) 139 { 140 return (va >> 21) & 0x1FF; 141 } 142 143 static uint64_t pdba_base(uint64_t pdpe) 144 { 145 return pdpe & 0xFFFFFFFFFF000; 146 } 147 148 static bool get_pgd(struct va_space *vs, uint64_t va, uint64_t pdpe, 149 uint64_t *value) 150 { 151 uint64_t pgd_entry = pdba_base(pdpe) + pde_index(va) * 8; 152 153 return pa_space_read64(vs->ps, pgd_entry, value); 154 } 155 156 static uint64_t pte_index(uint64_t va) 157 { 158 return (va >> 12) & 0x1FF; 159 } 160 161 static uint64_t ptba_base(uint64_t pde) 162 { 163 return pde & 0xFFFFFFFFFF000; 164 } 165 166 static bool get_pte(struct va_space *vs, uint64_t va, uint64_t pgd, 167 uint64_t *value) 168 { 169 uint64_t pgd_val = ptba_base(pgd) + pte_index(va) * 8; 170 171 return pa_space_read64(vs->ps, pgd_val, value); 172 } 173 174 static uint64_t get_paddr(uint64_t va, uint64_t pte) 175 { 176 return (pte & 0xFFFFFFFFFF000) | (va & 0xFFF); 177 } 178 179 static bool is_present(uint64_t entry) 180 { 181 return entry & 0x1; 182 } 183 184 static bool page_size_flag(uint64_t entry) 185 { 186 return entry & (1 << 7); 187 } 188 189 static uint64_t get_1GB_paddr(uint64_t va, uint64_t pdpte) 190 { 191 return (pdpte & 0xfffffc0000000) | (va & 0x3fffffff); 192 } 193 194 static uint64_t get_2MB_paddr(uint64_t va, uint64_t pgd_entry) 195 { 196 return (pgd_entry & 0xfffffffe00000) | (va & 0x00000001fffff); 197 } 198 199 static uint64_t va_space_va2pa(struct va_space *vs, uint64_t va) 200 { 201 uint64_t pml4e, pdpe, pgd, pte; 202 203 if (!get_pml4e(vs, va, &pml4e) || !is_present(pml4e)) { 204 return INVALID_PA; 205 } 206 207 if (!get_pdpi(vs, va, pml4e, &pdpe) || !is_present(pdpe)) { 208 return INVALID_PA; 209 } 210 211 if (page_size_flag(pdpe)) { 212 return get_1GB_paddr(va, pdpe); 213 } 214 215 if (!get_pgd(vs, va, pdpe, &pgd) || !is_present(pgd)) { 216 return INVALID_PA; 217 } 218 219 if (page_size_flag(pgd)) { 220 return get_2MB_paddr(va, pgd); 221 } 222 223 if (!get_pte(vs, va, pgd, &pte) || !is_present(pte)) { 224 return INVALID_PA; 225 } 226 227 return get_paddr(va, pte); 228 } 229 230 void *va_space_resolve(struct va_space *vs, uint64_t va) 231 { 232 uint64_t pa = va_space_va2pa(vs, va); 233 234 if (pa == INVALID_PA) { 235 return NULL; 236 } 237 238 return pa_space_resolve(vs->ps, pa); 239 } 240 241 bool va_space_rw(struct va_space *vs, uint64_t addr, 242 void *buf, size_t size, int is_write) 243 { 244 while (size) { 245 uint64_t page = addr & ELF2DMP_PFN_MASK; 246 size_t s = (page + ELF2DMP_PAGE_SIZE) - addr; 247 void *ptr; 248 249 s = (s > size) ? size : s; 250 251 ptr = va_space_resolve(vs, addr); 252 if (!ptr) { 253 return false; 254 } 255 256 if (is_write) { 257 memcpy(ptr, buf, s); 258 } else { 259 memcpy(buf, ptr, s); 260 } 261 262 size -= s; 263 buf = (uint8_t *)buf + s; 264 addr += s; 265 } 266 267 return true; 268 } 269