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