xref: /openbmc/qemu/contrib/elf2dmp/addrspace.c (revision 05caa062) 
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 }
270