[MEMORY] Memory pages and paging management

.gdbinit

@@ -1,6 +1,7 @@
 file sysroot/boot/kernel
 target remote localhost:1234
 break int_handler_breakpoint
+break page_fault_breakpoint
 
 define q
 monitor quit

kernel/Link.ld

@@ -7,6 +7,7 @@ KERNEL_START = 0x100000;
 SECTIONS
 {
   . = KERNEL_START + KERNEL_OFFSET;
+  kernel_start = .;
 
   .text : AT(ADDR(.text) - KERNEL_OFFSET)
   {
@@ -30,4 +31,6 @@ SECTIONS
     *(.COMMON)
     *(.bss)
   }
+  . = ALIGN(4096);
+  kernel_end = .;
 }

kernel/arch/registers.S

@@ -17,6 +17,10 @@ read_cr2:
 read_cr3:
   mov rax, cr3
   ret
+.global write_cr3
+write_cr3:
+  mov cr3, rdi
+  ret
 .global read_cr4
 read_cr4:
   mov rax, cr4

kernel/boot/kmain.c

@@ -12,12 +12,13 @@ int kmain(uint64_t multiboot_magic, void *multiboot_data)
 
   interrupt_init();
   multiboot_init(multiboot_magic, P2V(multiboot_data));
+  vmm_init();
+  pmm_init();
+
+  // We still need the GDT to be mapped in
+  extern void *BootGDT;
+  vmm_set_page(0, V2P(&BootGDT), V2P(&BootGDT), PAGE_PRESENT);
 
   debug_info("BOOT COMPLETE\n");
-  // Cause a divide-by-zero exception
-  int a = 5, b = 0;
-  int c = a/b;
-  (void)c;
-
   for(;;)asm("hlt");
 }

kernel/boot/multiboot.c

@@ -19,6 +19,12 @@ void parse_multiboot1()
     debug_ok("MBOOT1 - bootloader\n");
     mboot_data.bootloader = P2V(data->bootloader_name);
   }
+  if(data->flags & (1<<6))
+  {
+    debug_ok("MBOOT1 - memory map\n");
+    mboot_data.mmap_size = data->mmap_len/sizeof(mboot1_mmap_entry);
+    mboot_data.mmap = P2V(data->mmap_addr);
+  }
 }
 
 char *mboot_tags_type[] = {
@@ -58,6 +64,12 @@ void parse_multiboot2()
         mboot_data.bootloader = tag->data;
         debug_ok("MBOOT2 - handle ");
         break;
+      case MBOOT2_MMAP:
+        mboot_data.mmap_size = (tag->size - 8)/
+          ((mboot2_memory_map*)tag->data)->entry_size;
+        mboot_data.mmap = tag->data;
+        debug_ok("MBOOT2 - handle ");
+        break;
       default:
         debug_warning("MBOOT2 - ignore ");
         break;
@@ -92,3 +104,59 @@ void multiboot_init(uint64_t magic, void *data)
   if(mboot_data.bootloader)
     debug_info("MBOOT - Bootloader: %s\n", mboot_data.bootloader);
 }
+
+#define overlap(start1, end1, start2, end2) \
+  (!( \
+    ((uintptr_t)(start1) >= (uintptr_t)(end2)) \
+      || ((uintptr_t)(end1) <= (uintptr_t)(start2)) \
+  ))
+#define overlapsz(start1, size1, start2, size2) \
+  overlap((start1), (start1)+(size1), (start2), (start2)+(size2))
+
+int multiboot_page_used(uintptr_t addr)
+{
+  if(mboot_data.version == 1)
+  {
+    mboot1_info *data = mboot_data.data;
+    if(overlapsz(addr, PAGE_SIZE, V2P(data), sizeof(data)))
+      return 1;
+    if(data->flags & (1<<6))
+    {
+      if(overlapsz(addr, PAGE_SIZE, data->mmap_addr, data->mmap_len*sizeof(mboot1_mmap_entry)))
+        return 1;
+    }
+  } else {
+    mboot2_tags_head *data = mboot_data.data;
+    if(overlapsz(addr, PAGE_SIZE, V2P(data), data->total_size))
+      return 1;
+  }
+  return 0;
+}
+
+int multiboot_get_memory_area(uintptr_t *start, uintptr_t *end, uint32_t *type)
+{
+  static uint32_t entry = 0;
+  if(entry >= mboot_data.mmap_size)
+    return 0;
+  if(mboot_data.version == 1)
+  {
+    mboot1_mmap_entry *map = (void *)mboot_data.mmap;
+    *start = ((uintptr_t)map[entry].base_hi << 32) + map[entry].base_lo;
+    *end = *start + ((uintptr_t)map[entry].len_hi << 32) + map[entry].len_lo;
+    *type = map[entry].type;
+  } else {
+    mboot2_memory_map *mmap = mboot_data.mmap;
+    mboot2_mmap_entry *e = mmap->entries;
+    uint32_t i = entry;
+    while(i)
+    {
+      e = incptr(e, mmap->entry_size);
+      i--;
+    }
+    *start = (uintptr_t)e->base_addr;
+    *end = *start + (uintptr_t)e->length;
+    *type = e->type;
+  }
+  entry++;
+  return 1;
+}

kernel/include/mem.h

@@ -5,6 +5,7 @@
 #ifdef __ASSEMBLER__
   #define V2P(a) ((a) - KERNEL_OFFSET)
   #define P2V(a) ((a) + KERNEL_OFFSET)
+  #define PAGE_OFFSET(p) ((p) & 0xFFF)
   #define P1_OFFSET(p) (((p) >> 12) & 0x1FF)
   #define P2_OFFSET(p) (((p) >> 21) & 0x1FF)
   #define P3_OFFSET(p) (((p) >> 30) & 0x1FF)
@@ -15,8 +16,15 @@
   #define P2V(a) ((void *)((uintptr_t)(a) + KERNEL_OFFSET))
   // returns an address n bytes after the target of pointer p
   #define incptr(p, n) ((void *)(((uintptr_t)(p)) + (n)))
+  #define PAGE_OFFSET(p) (((uintptr_t)(p)) & 0xFFF)
+  #define P1_OFFSET(p) ((((uintptr_t)(p)) >> 12) & 0x1FF)
+  #define P2_OFFSET(p) ((((uintptr_t)(p)) >> 21) & 0x1FF)
+  #define P3_OFFSET(p) ((((uintptr_t)(p)) >> 30) & 0x1FF)
+  #define P4_OFFSET(p) ((((uintptr_t)(p)) >> 39) & 0x1FF)
 #endif
 
+#define PAGE_SIZE 0x1000
+#define PAGE_FLAGS_MASK 0xFFF
 // Paging
 #define   PAGE_PRESENT        0x001
 #define   PAGE_WRITE          0x002
@@ -27,3 +35,23 @@
 #define   PAGE_DIRTY          0x040
 #define   PAGE_HUGE           0x080
 #define   PAGE_GLOBAL         0x100
+
+#ifndef __ASSEMBLER__
+typedef void * page_table;
+extern page_table BootP4;
+
+extern int kernel_start, kernel_end;
+
+void pmm_init();
+uintptr_t pmm_alloc();
+void pmm_free(uintptr_t page);
+
+#define GET_P4() ((page_table)V2P(&BootP4))
+
+void vmm_init();
+page_table *vmm_new_P4();
+void vmm_set_P4(page_table *P4);
+void vmm_free_P4(page_table *P4);
+uintptr_t vmm_get_page(page_table *P4, uintptr_t addr);
+uintptr_t vmm_set_page(page_table *P4, uintptr_t addr, uintptr_t phys, uint32_t flags);
+#endif

kernel/include/multiboot.h

@@ -43,6 +43,15 @@
     uint32_t vbe_interface_len;
   } mboot1_info;
 
+typedef struct {
+  uint32_t size;
+  uint32_t base_lo;
+  uint32_t base_hi;
+  uint32_t len_lo;
+  uint32_t len_hi;
+  uint32_t type;
+} mboot1_mmap_entry;
+
 // MULTIBOOT 2
 
   typedef struct {
@@ -56,9 +65,21 @@
     char data[];
   } mboot2_tag_basic;
 
+  typedef struct {
+    uint64_t base_addr;
+    uint64_t length;
+    uint32_t type;
+    uint32_t reserved;
+  } mboot2_mmap_entry;
+  typedef struct {
+    uint32_t entry_size;
+    uint32_t entry_version;
+    mboot2_mmap_entry entries[];
+  } mboot2_memory_map;
 
   #define MBOOT2_CMDLINE 1
   #define MBOOT2_BOOTLOADER 2
+  #define MBOOT2_MMAP 6
 
   // Multiboot tags are padded to a multiple of 8 bytes
   #define next_tag(tag) ((void *)((((uintptr_t)tag) + tag->size + 7) & ~0x7))
@@ -69,9 +90,15 @@
     void *data;
     char *commandline;
     char *bootloader;
+    void *mmap;
+    uint32_t mmap_size;
   };
   extern struct mboot_data_st mboot_data;
 
+#define MBOOT_MMAP_FREE 1
+
   void multiboot_init(uint64_t magic, void *data);
+  int multiboot_page_used(uintptr_t addr);
+  int multiboot_get_memory_area(uintptr_t *start, uintptr_t *end, uint32_t *type);
 
 #endif

kernel/include/registers.h

@@ -4,4 +4,5 @@ void load_idt(void *);
 uint64_t read_cr0();
 uint64_t read_cr2();
 uint64_t read_cr3();
+void write_cr3(uint64_t);
 uint64_t read_cr4();

kernel/mem/pmm.c

@@ -0,0 +1,53 @@
+#include <mem.h>
+#include <multiboot.h>
+#include <debug.h>
+
+
+int pmm_running = 0;
+void *pmm_pos = &kernel_end;
+void **pmm_stack = 0;
+
+uintptr_t pmm_alloc()
+{
+  void **p;
+  if(!pmm_running)
+  {
+    p = (void **)pmm_pos;
+    pmm_pos = incptr(pmm_pos, PAGE_SIZE);
+  } else {
+    p = pmm_stack;
+    pmm_stack = *p;
+  }
+  return (uintptr_t)V2P(p);
+}
+
+void pmm_free(uintptr_t page)
+{
+  void **p  = (void **)P2V(page & ~PAGE_FLAGS_MASK);
+  *p = pmm_stack;
+  pmm_stack = p;
+  pmm_running = 1;
+}
+
+
+void pmm_init()
+{
+  uintptr_t start, end;
+  uint32_t type;
+
+  while(multiboot_get_memory_area(&start, &end, &type))
+  {
+    debug_info("PMM - %x->%x%s\n", start, end, (type==MBOOT_MMAP_FREE)?" (free)":" (reserved)");
+    for(uintptr_t p = start; (p+PAGE_SIZE) < end; p += PAGE_SIZE)
+    {
+      vmm_set_page(0, (uintptr_t)P2V(p), p, PAGE_PRESENT | PAGE_WRITE);
+      if((p >= (uintptr_t)V2P(&kernel_start)) && (p < (uintptr_t)V2P(pmm_pos)))
+        continue;
+      if(multiboot_page_used(p))
+        continue;
+      if(type == MBOOT_MMAP_FREE)
+        pmm_free(p);
+    }
+  }
+
+}

kernel/mem/vmm.c

@@ -0,0 +1,153 @@
+#include <mem.h>
+#include <string.h>
+#include <debug.h>
+#include <int.h>
+#include <registers.h>
+
+
+#define STRIP_FLAGS(addr) ((void *)(((uintptr_t)(addr)) & ~PAGE_FLAGS_MASK))
+#define ADD_FLAGS(addr, flags) ((void *)(((uintptr_t)(addr)) | ((flags) & PAGE_FLAGS_MASK)))
+
+
+extern page_table BootP4;
+
+page_table *vmm_new_P4()
+{
+  page_table *P4 = (page_table *)pmm_alloc();
+  memset(P2V(P4), 0, PAGE_SIZE);
+  ((page_table *)P2V(P4))[511] = ((page_table *)&BootP4)[511];
+  return P4;
+}
+
+void vmm_set_P4(page_table *P4)
+{
+  write_cr3((uint64_t)P4);
+}
+
+void vmm_free_P4(page_table *P4)
+{
+  P4 = P2V(P4);
+  for(int i4=0; i4 < (int)P4_OFFSET(KERNEL_OFFSET); i4++)
+  {
+    if(P4[i4])
+    {
+      page_table *P3 = P2V(STRIP_FLAGS(P4[i4]));
+      for(int i3=0; i3 < 0x200; i3++)
+      {
+        if(P3[i3])
+        {
+          page_table *P2 = P2V(STRIP_FLAGS(P3[i3]));
+          for(int i2=0; i2 < 0x200; i2++)
+          {
+            if(P2[i2])
+            {
+              page_table *P1 = P2V(STRIP_FLAGS(P2[i2]));
+              for(int i1=0; i1 < 0x200; i1++)
+              {
+                if(P1[i1])
+                {
+                  pmm_free((uintptr_t)STRIP_FLAGS(P1[i1]));
+                  P1[i1] = 0;
+                }
+              }
+              pmm_free((uintptr_t)STRIP_FLAGS(P2[i2]));
+              P2[i2] = 0;
+            }
+          }
+          pmm_free((uintptr_t)STRIP_FLAGS(P3[i3]));
+          P3[i3] = 0;
+        }
+      }
+      pmm_free((uintptr_t)STRIP_FLAGS(P4[i4]));
+      P4[i4] = 0;
+    }
+  }
+  pmm_free(V2P(P4));
+}
+
+void *vmm_find_page(page_table *P4, uintptr_t addr, uint32_t touch)
+{
+  addr &= ~PAGE_FLAGS_MASK;
+
+  page_table *P3, *P2, *P1;
+  P4 = P2V(P4);
+  if(!(P3 = P4[P4_OFFSET(addr)]))
+  {
+    if(!touch) return 0;
+    uintptr_t p = pmm_alloc();
+    memset(P2V(p), 0, PAGE_SIZE);
+    P4[P4_OFFSET(addr)] = P3 = ADD_FLAGS(p, PAGE_PRESENT | PAGE_WRITE | PAGE_USER);
+  }
+  P3 = P2V(STRIP_FLAGS(P3));
+
+  if(!(P2 = P3[P3_OFFSET(addr)]))
+  {
+    if(!touch) return 0;
+    uintptr_t p = pmm_alloc();
+    memset(P2V(p), 0, PAGE_SIZE);
+    P3[P3_OFFSET(addr)] = P2 = ADD_FLAGS(p, PAGE_PRESENT | PAGE_WRITE | PAGE_USER);
+  }
+  P2 = P2V(STRIP_FLAGS(P2));
+
+  if(!(P1 = P2[P2_OFFSET(addr)]))
+  {
+    if(!touch) return 0;
+    uintptr_t p = pmm_alloc();
+    memset(P2V(p), 0, PAGE_SIZE);
+    P2[P2_OFFSET(addr)] = P1 = ADD_FLAGS(p, PAGE_PRESENT | PAGE_WRITE | PAGE_USER);
+  }
+  return P2V(STRIP_FLAGS(P1));
+}
+
+uintptr_t vmm_get_page(page_table *P4, uintptr_t addr)
+{
+  page_table *P1;
+  if(!P4) P4=GET_P4();
+  if(!(P1 = vmm_find_page(P4, addr, 0))) return 0;
+  return (uintptr_t)(P1[P1_OFFSET(addr)]);
+}
+
+uintptr_t vmm_set_page(page_table *P4, uintptr_t addr, uintptr_t phys, uint32_t flags)
+{
+  page_table *P1;
+  if(!P4) P4=GET_P4();
+  if(!(P1 = vmm_find_page(P4, addr, 1))) return 0;
+  P1[P1_OFFSET(addr)] = ADD_FLAGS(phys, flags);
+  return (uintptr_t)P1[P1_OFFSET(addr)];
+}
+
+
+registers_t *page_fault_handler(registers_t *r)
+{
+  debug("\n ===== PAGE FAULT =====\n");
+  debug("Instruction at:%x tried to", r->rip);
+  if(r->err_code & 0x2)
+    debug(" read from");
+  else if(r->err_code & 0xE)
+    debug(" execute code from");
+  else
+    debug(" write to");
+  debug(" memory \nat:%x in",  read_cr2());
+  if(r->err_code & 0x8)
+    debug(" user mode");
+  else
+    debug(" kernel mode");
+  debug("\n");
+  debug("This caused a");
+  if(r->err_code & 0x1)
+    debug(" protection fault");
+  else
+    debug(" page miss");
+  debug(".\n ======================\n");
+  print_registers(r);
+
+#ifndef NDEBUG
+  asm("page_fault_breakpoint:");
+#endif
+  for(;;);
+}
+
+void vmm_init()
+{
+  register_int_handler(0xE, page_fault_handler);
+}