7 files changed, 147 insertions, 6 deletions
diff --git a/sys/Makefile b/sys/Makefile
index 970809f..3376c79 100644
--- a/sys/Makefile
+++ b/sys/Makefile
@@ -12,6 +12,7 @@ SRC=\
     sys/init.c\
     sys/printf.c\
     sys/mm/kalloc.c\
+    sys/vm/vm.c\
     sys/smp/spinlock.c\
     dev/dev_init.c\
     
diff --git a/sys/dev/dev_init.c b/sys/dev/dev_init.c
index 9933c2a..bacf427 100644
--- a/sys/dev/dev_init.c
+++ b/sys/dev/dev_init.c
@@ -26,6 +26,19 @@ mem_init(struct fdt_header *fdt, struct devicetree *dt)
 	printf("[mem_init] found %d memory reserve map entries\n", n);
 }
 
+static void
+cpu_init(struct fdt_header *fdt, struct devicetree *dt)
+{
+
+	int cpus = fdt_path_offset(fdt, "/cpus");
+	if (cpus < 0)
+		printf("[dev_init] failed to find cpus node\n");
+
+	int node = 0;
+	fdt_for_each_subnode(node, fdt, cpus) {
+		
+	}
+}
 
 struct devicetree
 dev_init(struct fdt_header *fdt) 
@@ -39,6 +52,7 @@ dev_init(struct fdt_header *fdt)
 
 	printf("[dev_init] detecting memory...\n");
 	mem_init(fdt, &dt);
+	cpu_init(fdt, &dt);
 
 //	int offset = fdt_next_node(fdt, offset, 0);
 //	do {
diff --git a/sys/include/kalloc.h b/sys/include/kalloc.h
index 5553d94..d77eae5 100644
--- a/sys/include/kalloc.h
+++ b/sys/include/kalloc.h
@@ -1,13 +1,15 @@
 #ifndef _KALLOC_H
 #define _KALLOC_H
 
+#include <stdint.h>
+
 void *kalloc(void);
 
 void *kzalloc(void);
 
 void kfree(void *);
 
-void kalloc_init(size_t);
+void kalloc_init(uintptr_t end);
 
 void walkfree(void);
 
diff --git a/sys/include/sbi.h b/sys/include/sbi.h
index 86ca295..7430bc6 100644
--- a/sys/include/sbi.h
+++ b/sys/include/sbi.h
@@ -67,6 +67,12 @@ sbi_debug_console_write_byte(uint8_t byte)
 }
 
 static inline void
+sbi_remote_sfence_vma(unsigned long hart_mask, unsigned long hart_mask_base, unsigned long start_addr, unsigned long size)
+{
+	sbi_ecall(0x52464E43, 1, hart_mask, hart_mask_base, start_addr, size, 0, 0);
+}
+
+static inline void
 sbi_shutdown(void)
 {
 	sbi_ecall(8, 0, 0, 0, 0, 0, 0, 0);
diff --git a/sys/include/vm.h b/sys/include/vm.h
index 1883ea7..4378552 100644
--- a/sys/include/vm.h
+++ b/sys/include/vm.h
@@ -1,6 +1,70 @@
 #ifndef _VM_H
 #define _VM_H
 
-void vminit(void);
+#include <stdint.h>
+
+#define PAGEBITS_VALID    (1 << 0)
+#define PAGEBITS_R        (1 << 1)
+#define PAGEBITS_W        (1 << 2)
+#define PAGEBITS_X        (1 << 3)
+#define PAGEBITS_USER     (1 << 4)
+#define PAGEBITS_GLOBAL   (1 << 5)
+#define PAGEBITS_ACCESSED (1 << 6)
+#define PAGEBITS_DIRTY    (1 << 7)
+
+#define PAGEBITS_RO	PAGEBITS_R
+#define PAGEBITS_XO	PAGEBITS_X
+#define PAGEBITS_RW	(PAGEBITS_R | PAGEBITS_W)
+#define PAGEPITS_RX	(PAGEBITS_R | PAGEBITS_X)
+#define PAGEBITS_RWX	(PAGEBITS_R | PAGEBITS_W | PAGEBITS_X)
+
+#define MODE_SV39 	(8L << 60)
+
+typedef uint64_t pt_entry_t;
+typedef struct _pagetable {
+	pt_entry_t entries[512];
+} pagetable_t; 
+
+/* allocate kernel pagetable */ 
+void kptinit(void);
+void kptinithart(void);
+//void kptinitharts(uintptr_t mask);
+
+/* maps virtual address to physical address for @pt. */
+void vmap(pagetable_t *root, uintptr_t vaddr, uintptr_t paddr, int bits, int level);
+
+/* returns address of page table entry for @vaddr. if @alloc, next level page tables will be allocated */ 
+pt_entry_t *walkpt(pagetable_t *root, uintptr_t vaddr, int alloc);
+
+/* helper functions */
+static inline void write_satp(uintptr_t mode, uintptr_t ppn);
+static inline int pte_is_branch(pt_entry_t);
+static inline int pte_is_valid(pt_entry_t);
+
+#define PA2PTE(pa) ((uintptr_t)pa >> 2)
+#define PTE2PA(pa) ((uintptr_t)pa << 2)
+#define FLUSH_TLB() asm volatile("sfence.vma zero, zero")
+
+static inline void 
+write_satp(uintptr_t mode, uintptr_t ppn)
+{
+	FLUSH_TLB();
+	asm volatile("csrw satp, %0" : : "r"((ppn >> 12) | mode));
+	FLUSH_TLB();
+}
+
+static inline int
+pte_is_branch(pt_entry_t e)
+{
+	return ((e & PAGEBITS_RWX) == 0);
+}
+#define pte_is_leaf(e) (!pte_is_branch(e))
+
+static inline int
+pte_is_valid(pt_entry_t e)
+{
+	return (e & PAGEBITS_VALID);
+}
+
 
 #endif /* _VM_H */
diff --git a/sys/sys/init.c b/sys/sys/init.c
index 05e8d49..a2a2543 100644
--- a/sys/sys/init.c
+++ b/sys/sys/init.c
@@ -7,6 +7,7 @@
 #include <printf.h>
 #include <sbi.h>
 #include <stdint.h>
+#include <vm.h>
 
 extern uint64_t HEAP_START;
 
@@ -29,6 +30,9 @@ init(unsigned long hartid, struct fdt_header *fdt)
 	kalloc_init(dt.memory.origin + dt.memory.size);
 	printf("done!\n");
 
+	printf("allocating kernel pagetable...\n");
+	kptinit();
+	
 //	walkfree();
 
 	printf("bringing up other harts...\n");
diff --git a/sys/sys/vm/vm.c b/sys/sys/vm/vm.c
index a9173be..25c4142 100644
--- a/sys/sys/vm/vm.c
+++ b/sys/sys/vm/vm.c
@@ -1,10 +1,60 @@
 #include <vm.h>
+#include <stddef.h>
+#include <kalloc.h>
+#include <stdio.h>
 #include <asm.h>
 
-#define SV39 (8L << 60)
+pagetable_t *kpt;
 
-void
-vminit(void)
+#define VPN(vaddr, level) \
+	(((uintptr_t)vaddr >> (12 + (9 * level))) & 0x1ff)
+
+#define PPN(paddr, level) \
+	(((uintptr_t)paddr >> (12 + (9 * level))) & \
+		((level == 2) ? 0x3ffffff : 0x1ff))
+
+void 
+kptinit(void)
 {
-	csrw_satp(SATP, (uint64_t)pt >> 12);
+	kpt = kzalloc();
+}
+
+void 
+kptinithart(void)
+{
+	FLUSH_TLB();
+	write_satp(MODE_SV39, (uintptr_t)kpt); 
+	FLUSH_TLB();
+}
+
+void 
+vmap(pagetable_t *root, uintptr_t vaddr, uintptr_t paddr, int bits, int level)
+{
+	if (pte_is_branch(bits))
+		printf("error: pte_is_branch\n"); return;
+
+	pt_entry_t *e = walkpt(root, vaddr, 1);
+	if (pte_is_valid(*e))
+		printf("warning: mapping already exists for this address\n");
+
+	*e = PA2PTE(paddr) | PAGEBITS_VALID | bits;
+}
+
+pt_entry_t *
+walkpt(pagetable_t *root, uintptr_t vaddr, int alloc) {
+	pt_entry_t *pt = root->entries;
+	for (int level = 2; level > 0; level--) {
+		pt_entry_t *e = &pt[VPN(vaddr, level)];
+		if (!pte_is_valid(*e)) {
+			if (!alloc)
+				return NULL;
+			pt = kzalloc();
+			*e = PA2PTE(pt) | PAGEBITS_VALID;
+			continue;
+		}
+		pt = (pt_entry_t *)PTE2PA(*e);
+
+	}
+	/* now we should be at level 0, and e should point to our entry */ 
+	return &pt[VPN(vaddr, 0)];
 }