1 #include "fwcfg.h" 2 #include "vm.h" 3 #include "libcflat.h" 4 5 #define PAGE_SIZE 4096ul 6 #ifdef __x86_64__ 7 #define LARGE_PAGE_SIZE (512 * PAGE_SIZE) 8 #else 9 #define LARGE_PAGE_SIZE (1024 * PAGE_SIZE) 10 #endif 11 12 static void *free = 0; 13 static void *vfree_top = 0; 14 15 static void free_memory(void *mem, unsigned long size) 16 { 17 while (size >= PAGE_SIZE) { 18 *(void **)mem = free; 19 free = mem; 20 mem += PAGE_SIZE; 21 size -= PAGE_SIZE; 22 } 23 } 24 25 void *alloc_page() 26 { 27 void *p; 28 29 if (!free) 30 return 0; 31 32 p = free; 33 free = *(void **)free; 34 35 return p; 36 } 37 38 void free_page(void *page) 39 { 40 *(void **)page = free; 41 free = page; 42 } 43 44 extern char edata; 45 static unsigned long end_of_memory; 46 47 #ifdef __x86_64__ 48 #define PAGE_LEVEL 4 49 #define PGDIR_WIDTH 9 50 #define PGDIR_MASK 511 51 #else 52 #define PAGE_LEVEL 2 53 #define PGDIR_WIDTH 10 54 #define PGDIR_MASK 1023 55 #endif 56 57 unsigned long *install_pte(unsigned long *cr3, 58 int pte_level, 59 void *virt, 60 unsigned long pte, 61 unsigned long *pt_page) 62 { 63 int level; 64 unsigned long *pt = cr3; 65 unsigned offset; 66 67 for (level = PAGE_LEVEL; level > pte_level; --level) { 68 offset = ((unsigned long)virt >> ((level-1) * PGDIR_WIDTH + 12)) & PGDIR_MASK; 69 if (!(pt[offset] & PTE_PRESENT)) { 70 unsigned long *new_pt = pt_page; 71 if (!new_pt) 72 new_pt = alloc_page(); 73 else 74 pt_page = 0; 75 memset(new_pt, 0, PAGE_SIZE); 76 pt[offset] = virt_to_phys(new_pt) | PTE_PRESENT | PTE_WRITE | PTE_USER; 77 } 78 pt = phys_to_virt(pt[offset] & 0xffffffffff000ull); 79 } 80 offset = ((unsigned long)virt >> ((level-1) * PGDIR_WIDTH + 12)) & PGDIR_MASK; 81 pt[offset] = pte; 82 return &pt[offset]; 83 } 84 85 unsigned long *get_pte(unsigned long *cr3, void *virt) 86 { 87 int level; 88 unsigned long *pt = cr3, pte; 89 unsigned offset; 90 91 for (level = PAGE_LEVEL; level > 1; --level) { 92 offset = ((unsigned long)virt >> (((level-1) * PGDIR_WIDTH) + 12)) & PGDIR_MASK; 93 pte = pt[offset]; 94 if (!(pte & PTE_PRESENT)) 95 return NULL; 96 if (level == 2 && (pte & PTE_PSE)) 97 return &pt[offset]; 98 pt = phys_to_virt(pte & 0xffffffffff000ull); 99 } 100 offset = ((unsigned long)virt >> (((level-1) * PGDIR_WIDTH) + 12)) & PGDIR_MASK; 101 return &pt[offset]; 102 } 103 104 unsigned long *install_large_page(unsigned long *cr3, 105 unsigned long phys, 106 void *virt) 107 { 108 return install_pte(cr3, 2, virt, 109 phys | PTE_PRESENT | PTE_WRITE | PTE_USER | PTE_PSE, 0); 110 } 111 112 unsigned long *install_page(unsigned long *cr3, 113 unsigned long phys, 114 void *virt) 115 { 116 return install_pte(cr3, 1, virt, phys | PTE_PRESENT | PTE_WRITE | PTE_USER, 0); 117 } 118 119 120 static void setup_mmu_range(unsigned long *cr3, unsigned long start, 121 unsigned long len) 122 { 123 u64 max = (u64)len + (u64)start; 124 u64 phys = start; 125 126 while (phys + LARGE_PAGE_SIZE <= max) { 127 install_large_page(cr3, phys, (void *)(ulong)phys); 128 phys += LARGE_PAGE_SIZE; 129 } 130 while (phys + PAGE_SIZE <= max) { 131 install_page(cr3, phys, (void *)(ulong)phys); 132 phys += PAGE_SIZE; 133 } 134 } 135 136 static void setup_mmu(unsigned long len) 137 { 138 unsigned long *cr3 = alloc_page(); 139 140 memset(cr3, 0, PAGE_SIZE); 141 142 #ifdef __x86_64__ 143 if (len < (1ul << 32)) 144 len = (1ul << 32); /* map mmio 1:1 */ 145 146 setup_mmu_range(cr3, 0, len); 147 #else 148 if (len > (1ul << 31)) 149 len = (1ul << 31); 150 151 /* 0 - 2G memory, 2G-3G valloc area, 3G-4G mmio */ 152 setup_mmu_range(cr3, 0, len); 153 setup_mmu_range(cr3, 3ul << 30, (1ul << 30)); 154 vfree_top = (void*)(3ul << 30); 155 #endif 156 157 write_cr3(virt_to_phys(cr3)); 158 #ifndef __x86_64__ 159 write_cr4(X86_CR4_PSE); 160 #endif 161 write_cr0(X86_CR0_PG |X86_CR0_PE | X86_CR0_WP); 162 163 printf("paging enabled\n"); 164 printf("cr0 = %x\n", read_cr0()); 165 printf("cr3 = %x\n", read_cr3()); 166 printf("cr4 = %x\n", read_cr4()); 167 } 168 169 void setup_vm() 170 { 171 end_of_memory = fwcfg_get_u64(FW_CFG_RAM_SIZE); 172 free_memory(&edata, end_of_memory - (unsigned long)&edata); 173 setup_mmu(end_of_memory); 174 } 175 176 void *vmalloc(unsigned long size) 177 { 178 void *mem, *p; 179 unsigned pages; 180 181 size += sizeof(unsigned long); 182 183 size = (size + PAGE_SIZE - 1) & ~(PAGE_SIZE - 1); 184 vfree_top -= size; 185 mem = p = vfree_top; 186 pages = size / PAGE_SIZE; 187 while (pages--) { 188 install_page(phys_to_virt(read_cr3()), virt_to_phys(alloc_page()), p); 189 p += PAGE_SIZE; 190 } 191 *(unsigned long *)mem = size; 192 mem += sizeof(unsigned long); 193 return mem; 194 } 195 196 uint64_t virt_to_phys_cr3(void *mem) 197 { 198 return (*get_pte(phys_to_virt(read_cr3()), mem) & PTE_ADDR) + ((ulong)mem & (PAGE_SIZE - 1)); 199 } 200 201 void vfree(void *mem) 202 { 203 unsigned long size = ((unsigned long *)mem)[-1]; 204 205 while (size) { 206 free_page(phys_to_virt(*get_pte(phys_to_virt(read_cr3()), mem) & PTE_ADDR)); 207 mem += PAGE_SIZE; 208 size -= PAGE_SIZE; 209 } 210 } 211 212 void *vmap(unsigned long long phys, unsigned long size) 213 { 214 void *mem, *p; 215 unsigned pages; 216 217 size = (size + PAGE_SIZE - 1) & ~(PAGE_SIZE - 1); 218 vfree_top -= size; 219 phys &= ~(unsigned long long)(PAGE_SIZE - 1); 220 221 mem = p = vfree_top; 222 pages = size / PAGE_SIZE; 223 while (pages--) { 224 install_page(phys_to_virt(read_cr3()), phys, p); 225 phys += PAGE_SIZE; 226 p += PAGE_SIZE; 227 } 228 return mem; 229 } 230 231 void *alloc_vpages(ulong nr) 232 { 233 vfree_top -= PAGE_SIZE * nr; 234 return vfree_top; 235 } 236 237 void *alloc_vpage(void) 238 { 239 return alloc_vpages(1); 240 } 241