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(unsigned long len) 139 { 140 unsigned long *cr3 = alloc_page(); 141 unsigned long phys = 0; 142 143 if (len < (1ul << 32)) 144 len = 1ul << 32; /* map mmio 1:1 */ 145 146 memset(cr3, 0, PAGE_SIZE); 147 while (phys + LARGE_PAGE_SIZE <= len) { 148 install_large_page(cr3, phys, (void *)phys); 149 phys += LARGE_PAGE_SIZE; 150 } 151 while (phys + PAGE_SIZE <= len) { 152 install_page(cr3, phys, (void *)phys); 153 phys += PAGE_SIZE; 154 } 155 write_cr3(virt_to_phys(cr3)); 156 #ifndef __x86_64__ 157 write_cr4(X86_CR4_PSE); 158 #endif 159 write_cr0(X86_CR0_PG |X86_CR0_PE); 160 161 printf("paging enabled\n"); 162 printf("cr0 = %x\n", read_cr0()); 163 printf("cr3 = %x\n", read_cr3()); 164 printf("cr4 = %x\n", read_cr4()); 165 } 166 167 static unsigned int inl(unsigned short port) 168 { 169 unsigned int val; 170 asm volatile("inl %w1, %0" : "=a"(val) : "Nd"(port)); 171 return val; 172 } 173 174 void setup_vm() 175 { 176 end_of_memory = inl(0xd1); 177 free_memory(&edata, end_of_memory - (unsigned long)&edata); 178 setup_mmu(end_of_memory); 179 } 180 181 void *vmalloc(unsigned long size) 182 { 183 void *mem, *p; 184 unsigned pages; 185 186 size += sizeof(unsigned long); 187 188 size = (size + PAGE_SIZE - 1) & ~(PAGE_SIZE - 1); 189 vfree_top -= size; 190 mem = p = vfree_top; 191 pages = size / PAGE_SIZE; 192 while (pages--) { 193 install_page(phys_to_virt(read_cr3()), virt_to_phys(alloc_page()), p); 194 p += PAGE_SIZE; 195 } 196 *(unsigned long *)mem = size; 197 mem += sizeof(unsigned long); 198 return mem; 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