1 #include "fwcfg.h" 2 #include "vm.h" 3 #include "libcflat.h" 4 5 static void *free = 0; 6 static void *vfree_top = 0; 7 8 static void free_memory(void *mem, unsigned long size) 9 { 10 void *end; 11 12 assert_msg((unsigned long) mem % PAGE_SIZE == 0, 13 "mem not page aligned: %p", mem); 14 15 assert_msg(size % PAGE_SIZE == 0, "size not page aligned: 0x%lx", size); 16 17 assert_msg(size == 0 || mem + size > mem, 18 "mem + size overflow: %p + 0x%lx", mem, size); 19 20 if (size == 0) { 21 free = NULL; 22 return; 23 } 24 25 free = mem; 26 end = mem + size; 27 while (mem + PAGE_SIZE != end) { 28 *(void **)mem = (mem + PAGE_SIZE); 29 mem += PAGE_SIZE; 30 } 31 32 *(void **)mem = NULL; 33 } 34 35 void *alloc_page() 36 { 37 void *p; 38 39 if (!free) 40 return 0; 41 42 p = free; 43 free = *(void **)free; 44 45 return p; 46 } 47 48 /* 49 * Allocates (1 << order) physically contiguous and naturally aligned pages. 50 * Returns NULL if there's no memory left. 51 */ 52 void *alloc_pages(unsigned long order) 53 { 54 /* Generic list traversal. */ 55 void *prev; 56 void *curr = NULL; 57 void *next = free; 58 59 /* Looking for a run of length (1 << order). */ 60 unsigned long run = 0; 61 const unsigned long n = 1ul << order; 62 const unsigned long align_mask = (n << PAGE_SHIFT) - 1; 63 void *run_start = NULL; 64 void *run_prev = NULL; 65 unsigned long run_next_pa = 0; 66 unsigned long pa; 67 68 assert(order < sizeof(unsigned long) * 8); 69 70 for (;;) { 71 prev = curr; 72 curr = next; 73 next = curr ? *((void **) curr) : NULL; 74 75 if (!curr) 76 return 0; 77 78 pa = virt_to_phys(curr); 79 80 if (run == 0) { 81 if (!(pa & align_mask)) { 82 run_start = curr; 83 run_prev = prev; 84 run_next_pa = pa + PAGE_SIZE; 85 run = 1; 86 } 87 } else if (pa == run_next_pa) { 88 run_next_pa += PAGE_SIZE; 89 run += 1; 90 } else { 91 run = 0; 92 } 93 94 if (run == n) { 95 if (run_prev) 96 *((void **) run_prev) = next; 97 else 98 free = next; 99 return run_start; 100 } 101 } 102 } 103 104 105 void free_page(void *page) 106 { 107 *(void **)page = free; 108 free = page; 109 } 110 111 extern char edata; 112 static unsigned long end_of_memory; 113 114 unsigned long *install_pte(unsigned long *cr3, 115 int pte_level, 116 void *virt, 117 unsigned long pte, 118 unsigned long *pt_page) 119 { 120 int level; 121 unsigned long *pt = cr3; 122 unsigned offset; 123 124 for (level = PAGE_LEVEL; level > pte_level; --level) { 125 offset = PGDIR_OFFSET((unsigned long)virt, level); 126 if (!(pt[offset] & PT_PRESENT_MASK)) { 127 unsigned long *new_pt = pt_page; 128 if (!new_pt) 129 new_pt = alloc_page(); 130 else 131 pt_page = 0; 132 memset(new_pt, 0, PAGE_SIZE); 133 pt[offset] = virt_to_phys(new_pt) | PT_PRESENT_MASK | PT_WRITABLE_MASK | PT_USER_MASK; 134 } 135 pt = phys_to_virt(pt[offset] & PT_ADDR_MASK); 136 } 137 offset = PGDIR_OFFSET((unsigned long)virt, level); 138 pt[offset] = pte; 139 return &pt[offset]; 140 } 141 142 unsigned long *get_pte(unsigned long *cr3, void *virt) 143 { 144 int level; 145 unsigned long *pt = cr3, pte; 146 unsigned offset; 147 148 for (level = PAGE_LEVEL; level > 1; --level) { 149 offset = ((unsigned long)virt >> (((level-1) * PGDIR_WIDTH) + 12)) & PGDIR_MASK; 150 pte = pt[offset]; 151 if (!(pte & PT_PRESENT_MASK)) 152 return NULL; 153 if (level == 2 && (pte & PT_PAGE_SIZE_MASK)) 154 return &pt[offset]; 155 pt = phys_to_virt(pte & PT_ADDR_MASK); 156 } 157 offset = ((unsigned long)virt >> (((level-1) * PGDIR_WIDTH) + 12)) & PGDIR_MASK; 158 return &pt[offset]; 159 } 160 161 unsigned long *install_large_page(unsigned long *cr3, 162 unsigned long phys, 163 void *virt) 164 { 165 return install_pte(cr3, 2, virt, 166 phys | PT_PRESENT_MASK | PT_WRITABLE_MASK | PT_USER_MASK | PT_PAGE_SIZE_MASK, 0); 167 } 168 169 unsigned long *install_page(unsigned long *cr3, 170 unsigned long phys, 171 void *virt) 172 { 173 return install_pte(cr3, 1, virt, phys | PT_PRESENT_MASK | PT_WRITABLE_MASK | PT_USER_MASK, 0); 174 } 175 176 177 static void setup_mmu_range(unsigned long *cr3, unsigned long start, 178 unsigned long len) 179 { 180 u64 max = (u64)len + (u64)start; 181 u64 phys = start; 182 183 while (phys + LARGE_PAGE_SIZE <= max) { 184 install_large_page(cr3, phys, (void *)(ulong)phys); 185 phys += LARGE_PAGE_SIZE; 186 } 187 while (phys + PAGE_SIZE <= max) { 188 install_page(cr3, phys, (void *)(ulong)phys); 189 phys += PAGE_SIZE; 190 } 191 } 192 193 static void setup_mmu(unsigned long len) 194 { 195 unsigned long *cr3 = alloc_page(); 196 197 memset(cr3, 0, PAGE_SIZE); 198 199 #ifdef __x86_64__ 200 if (len < (1ul << 32)) 201 len = (1ul << 32); /* map mmio 1:1 */ 202 203 setup_mmu_range(cr3, 0, len); 204 #else 205 if (len > (1ul << 31)) 206 len = (1ul << 31); 207 208 /* 0 - 2G memory, 2G-3G valloc area, 3G-4G mmio */ 209 setup_mmu_range(cr3, 0, len); 210 setup_mmu_range(cr3, 3ul << 30, (1ul << 30)); 211 vfree_top = (void*)(3ul << 30); 212 #endif 213 214 write_cr3(virt_to_phys(cr3)); 215 #ifndef __x86_64__ 216 write_cr4(X86_CR4_PSE); 217 #endif 218 write_cr0(X86_CR0_PG |X86_CR0_PE | X86_CR0_WP); 219 220 printf("paging enabled\n"); 221 printf("cr0 = %lx\n", read_cr0()); 222 printf("cr3 = %lx\n", read_cr3()); 223 printf("cr4 = %lx\n", read_cr4()); 224 } 225 226 void setup_vm() 227 { 228 assert(!end_of_memory); 229 end_of_memory = fwcfg_get_u64(FW_CFG_RAM_SIZE); 230 free_memory(&edata, end_of_memory - (unsigned long)&edata); 231 setup_mmu(end_of_memory); 232 } 233 234 void *vmalloc(unsigned long size) 235 { 236 void *mem, *p; 237 unsigned pages; 238 239 size += sizeof(unsigned long); 240 241 size = (size + PAGE_SIZE - 1) & ~(PAGE_SIZE - 1); 242 vfree_top -= size; 243 mem = p = vfree_top; 244 pages = size / PAGE_SIZE; 245 while (pages--) { 246 install_page(phys_to_virt(read_cr3()), virt_to_phys(alloc_page()), p); 247 p += PAGE_SIZE; 248 } 249 *(unsigned long *)mem = size; 250 mem += sizeof(unsigned long); 251 return mem; 252 } 253 254 uint64_t virt_to_phys_cr3(void *mem) 255 { 256 return (*get_pte(phys_to_virt(read_cr3()), mem) & PT_ADDR_MASK) + ((ulong)mem & (PAGE_SIZE - 1)); 257 } 258 259 void vfree(void *mem) 260 { 261 unsigned long size = ((unsigned long *)mem)[-1]; 262 263 while (size) { 264 free_page(phys_to_virt(*get_pte(phys_to_virt(read_cr3()), mem) & PT_ADDR_MASK)); 265 mem += PAGE_SIZE; 266 size -= PAGE_SIZE; 267 } 268 } 269 270 void *vmap(unsigned long long phys, unsigned long size) 271 { 272 void *mem, *p; 273 unsigned pages; 274 275 size = (size + PAGE_SIZE - 1) & ~(PAGE_SIZE - 1); 276 vfree_top -= size; 277 phys &= ~(unsigned long long)(PAGE_SIZE - 1); 278 279 mem = p = vfree_top; 280 pages = size / PAGE_SIZE; 281 while (pages--) { 282 install_page(phys_to_virt(read_cr3()), phys, p); 283 phys += PAGE_SIZE; 284 p += PAGE_SIZE; 285 } 286 return mem; 287 } 288 289 void *alloc_vpages(ulong nr) 290 { 291 vfree_top -= PAGE_SIZE * nr; 292 return vfree_top; 293 } 294 295 void *alloc_vpage(void) 296 { 297 return alloc_vpages(1); 298 } 299