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