1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
|
/*
* Copyright (c) 2018 Virtuozzo International GmbH
*
* This work is licensed under the terms of the GNU GPL, version 2 or later.
*
*/
#include "qemu/osdep.h"
#include "addrspace.h"
static struct pa_block *pa_space_find_block(struct pa_space *ps, uint64_t pa)
{
size_t i;
for (i = 0; i < ps->block_nr; i++) {
if (ps->block[i].paddr <= pa &&
pa <= ps->block[i].paddr + ps->block[i].size) {
return ps->block + i;
}
}
return NULL;
}
static uint8_t *pa_space_resolve(struct pa_space *ps, uint64_t pa)
{
struct pa_block *block = pa_space_find_block(ps, pa);
if (!block) {
return NULL;
}
return block->addr + (pa - block->paddr);
}
int pa_space_create(struct pa_space *ps, QEMU_Elf *qemu_elf)
{
Elf64_Half phdr_nr = elf_getphdrnum(qemu_elf->map);
Elf64_Phdr *phdr = elf64_getphdr(qemu_elf->map);
size_t block_i = 0;
size_t i;
ps->block_nr = 0;
for (i = 0; i < phdr_nr; i++) {
if (phdr[i].p_type == PT_LOAD) {
ps->block_nr++;
}
}
ps->block = malloc(sizeof(*ps->block) * ps->block_nr);
if (!ps->block) {
return 1;
}
for (i = 0; i < phdr_nr; i++) {
if (phdr[i].p_type == PT_LOAD) {
ps->block[block_i] = (struct pa_block) {
.addr = (uint8_t *)qemu_elf->map + phdr[i].p_offset,
.paddr = phdr[i].p_paddr,
.size = phdr[i].p_filesz,
};
block_i++;
}
}
return 0;
}
void pa_space_destroy(struct pa_space *ps)
{
ps->block_nr = 0;
free(ps->block);
}
void va_space_set_dtb(struct va_space *vs, uint64_t dtb)
{
vs->dtb = dtb & 0x00ffffffffff000;
}
void va_space_create(struct va_space *vs, struct pa_space *ps, uint64_t dtb)
{
vs->ps = ps;
va_space_set_dtb(vs, dtb);
}
static uint64_t get_pml4e(struct va_space *vs, uint64_t va)
{
uint64_t pa = (vs->dtb & 0xffffffffff000) | ((va & 0xff8000000000) >> 36);
return *(uint64_t *)pa_space_resolve(vs->ps, pa);
}
static uint64_t get_pdpi(struct va_space *vs, uint64_t va, uint64_t pml4e)
{
uint64_t pdpte_paddr = (pml4e & 0xffffffffff000) |
((va & 0x7FC0000000) >> 27);
return *(uint64_t *)pa_space_resolve(vs->ps, pdpte_paddr);
}
static uint64_t pde_index(uint64_t va)
{
return (va >> 21) & 0x1FF;
}
static uint64_t pdba_base(uint64_t pdpe)
{
return pdpe & 0xFFFFFFFFFF000;
}
static uint64_t get_pgd(struct va_space *vs, uint64_t va, uint64_t pdpe)
{
uint64_t pgd_entry = pdba_base(pdpe) + pde_index(va) * 8;
return *(uint64_t *)pa_space_resolve(vs->ps, pgd_entry);
}
static uint64_t pte_index(uint64_t va)
{
return (va >> 12) & 0x1FF;
}
static uint64_t ptba_base(uint64_t pde)
{
return pde & 0xFFFFFFFFFF000;
}
static uint64_t get_pte(struct va_space *vs, uint64_t va, uint64_t pgd)
{
uint64_t pgd_val = ptba_base(pgd) + pte_index(va) * 8;
return *(uint64_t *)pa_space_resolve(vs->ps, pgd_val);
}
static uint64_t get_paddr(uint64_t va, uint64_t pte)
{
return (pte & 0xFFFFFFFFFF000) | (va & 0xFFF);
}
static bool is_present(uint64_t entry)
{
return entry & 0x1;
}
static bool page_size_flag(uint64_t entry)
{
return entry & (1 << 7);
}
static uint64_t get_1GB_paddr(uint64_t va, uint64_t pdpte)
{
return (pdpte & 0xfffffc0000000) | (va & 0x3fffffff);
}
static uint64_t get_2MB_paddr(uint64_t va, uint64_t pgd_entry)
{
return (pgd_entry & 0xfffffffe00000) | (va & 0x00000001fffff);
}
static uint64_t va_space_va2pa(struct va_space *vs, uint64_t va)
{
uint64_t pml4e, pdpe, pgd, pte;
pml4e = get_pml4e(vs, va);
if (!is_present(pml4e)) {
return INVALID_PA;
}
pdpe = get_pdpi(vs, va, pml4e);
if (!is_present(pdpe)) {
return INVALID_PA;
}
if (page_size_flag(pdpe)) {
return get_1GB_paddr(va, pdpe);
}
pgd = get_pgd(vs, va, pdpe);
if (!is_present(pgd)) {
return INVALID_PA;
}
if (page_size_flag(pgd)) {
return get_2MB_paddr(va, pgd);
}
pte = get_pte(vs, va, pgd);
if (!is_present(pte)) {
return INVALID_PA;
}
return get_paddr(va, pte);
}
void *va_space_resolve(struct va_space *vs, uint64_t va)
{
uint64_t pa = va_space_va2pa(vs, va);
if (pa == INVALID_PA) {
return NULL;
}
return pa_space_resolve(vs->ps, pa);
}
int va_space_rw(struct va_space *vs, uint64_t addr,
void *buf, size_t size, int is_write)
{
while (size) {
uint64_t page = addr & PFN_MASK;
size_t s = (page + PAGE_SIZE) - addr;
void *ptr;
s = (s > size) ? size : s;
ptr = va_space_resolve(vs, addr);
if (!ptr) {
return 1;
}
if (is_write) {
memcpy(ptr, buf, s);
} else {
memcpy(buf, ptr, s);
}
size -= s;
buf = (uint8_t *)buf + s;
addr += s;
}
return 0;
}
|
