kvm/user/x86/vm.c

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