kvm/user/test/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     memset(cr3, 0, PAGE_SIZE);
 193     while (phys + LARGE_PAGE_SIZE <= len) {
 194         install_large_page(cr3, phys, (void *)phys);
 195         phys += LARGE_PAGE_SIZE;
 196     }
 197     while (phys + PAGE_SIZE <= len) {
 198         install_page(cr3, phys, (void *)phys);
 199         phys += PAGE_SIZE;
 200     }
 201
 202     load_cr3(virt_to_phys(cr3));
 203     print("paging enabled\n");
 204 }
 205
 206 static unsigned int inl(unsigned short port)
 207 {
 208     unsigned int val;
 209     asm volatile("inl %w1, %0" : "=a"(val) : "Nd"(port));
 210     return val;
 211 }
 212
 213 void setup_vm()
 214 {
 215     end_of_memory = inl(0xd1);
 216     free_memory(&edata, end_of_memory - (unsigned long)&edata);
 217     setup_mmu(end_of_memory);
 218 }
 219
 220 void *vmalloc(unsigned long size)
 221 {
 222     void *mem, *p;
 223     unsigned pages;
 224
 225     size += sizeof(unsigned long);
 226
 227     size = (size + PAGE_SIZE - 1) & ~(PAGE_SIZE - 1);
 228     vfree_top -= size;
 229     mem = p = vfree_top;
 230     pages = size / PAGE_SIZE;
 231     while (pages--) {
 232         install_page(phys_to_virt(read_cr3()), virt_to_phys(alloc_page()), p);
 233         p += PAGE_SIZE;
 234     }
 235     *(unsigned long *)mem = size;
 236     mem += sizeof(unsigned long);
 237     return mem;
 238 }
 239
 240 void vfree(void *mem)
 241 {
 242     unsigned long size = ((unsigned long *)mem)[-1];
 243
 244     while (size) {
 245         free_page(phys_to_virt(get_pte(phys_to_virt(read_cr3()), mem) & PTE_ADDR));
 246         mem += PAGE_SIZE;
 247         size -= PAGE_SIZE;
 248     }
 249 }