2 files changed, 78 insertions, 9 deletions

diff --git a/src/arch/riscv/include/vm.h b/src/arch/riscv/include/vm.h
index 5bf03c796e..cc26b13d21 100644
--- a/src/arch/riscv/include/vm.h
+++ b/src/arch/riscv/include/vm.h
@@ -63,7 +63,7 @@ size_t pte_ppn(pte_t pte);
 pte_t ptd_create(uintptr_t ppn);
 pte_t pte_create(uintptr_t ppn, int prot, int user);
 
-void walk_page_table(void);
+void print_page_table(void);
 
 void init_vm(uintptr_t virtMemStart, uintptr_t physMemStart, uintptr_t pageTableStart);
 void mstatus_init(void); // need to setup mstatus so we know we have virtual memory
diff --git a/src/arch/riscv/virtual_memory.c b/src/arch/riscv/virtual_memory.c
index bbbba7a690..bea552de86 100644
--- a/src/arch/riscv/virtual_memory.c
+++ b/src/arch/riscv/virtual_memory.c
@@ -23,11 +23,77 @@
 
 pte_t* root_page_table;
 
-void walk_page_table(void) {
-	// TODO: implement a full walk to make sure memory was set up
-	//const size_t pte_per_page = RISCV_PGSIZE/sizeof(void*);
-	pte_t* t = root_page_table;
-	printk(BIOS_DEBUG, "root_page_table: %p\n", t);
+/* Indent the following text by 2*level spaces */
+static void indent(int level)
+{
+	int i;
+	for (i = 0; i < level; i++)
+		printk(BIOS_DEBUG, "  ");
+}
+
+/*
+ * Convert a page table index at a given page table level to a virtual address
+ * offset
+ */
+static uintptr_t index_to_virt_addr(int index, int level)
+{
+	/*
+	 * Index is at most RISCV_PGLEVEL_BITS bits wide (not considering the
+	 * leading zeroes. If level==0, the below expression thus shifts index
+	 * into the highest bits of a 64-bit number, and then shifts it down
+	 * with sign extension.
+	 *
+	 * If level>0, then the expression should work as expected, without any
+	 * magic.
+	 */
+	return ((intptr_t)index)
+		<< (64 - RISCV_PGLEVEL_BITS - level * RISCV_PGLEVEL_BITS)
+		>> (64 - VA_BITS);
+}
+
+/* Dump the page table structures to the console -- helper function */
+static void print_page_table_at(pte_t *pt, intptr_t virt_addr, int level)
+{
+	int i;
+
+	indent(level);
+	printk(BIOS_DEBUG, "Level %d page table at 0x%p\n", level, pt);
+
+	for (i = 0; i < RISCV_PGSIZE / sizeof(pte_t); i++) {
+		char urwx[8];
+		uintptr_t pointer;
+		intptr_t next_virt_addr;
+
+		if (!(pt[i] & PTE_V))
+			continue;
+
+		urwx[0] = (pt[i] & PTE_U)? 'u' : '-';
+		urwx[1] = (pt[i] & PTE_R)? 'r' : '-';
+		urwx[2] = (pt[i] & PTE_W)? 'w' : '-';
+		urwx[3] = (pt[i] & PTE_X)? 'x' : '-';
+		urwx[4] = '\0';
+
+		next_virt_addr = virt_addr + index_to_virt_addr(i, level);
+
+		pointer = ((uintptr_t)pt[i] >> 10) << RISCV_PGSHIFT;
+
+		indent(level + 1);
+		printk(BIOS_DEBUG, "Valid PTE at index %d (0x%016zx -> 0x%zx), ",
+				i, (size_t) next_virt_addr, (size_t) pointer);
+		if (PTE_TABLE(pt[i]))
+			printk(BIOS_DEBUG, "page table\n");
+		else
+			printk(BIOS_DEBUG, "protections %s\n", urwx);
+
+		if (PTE_TABLE(pt[i])) {
+			print_page_table_at((pte_t *)pointer, next_virt_addr, level + 1);
+		}
+	}
+}
+
+/* Print the page table structures to the console */
+void print_page_table(void) {
+	print_page_table_at(root_page_table, 0, 0);
 }
 
 void flush_tlb(void)
@@ -124,14 +190,17 @@ void initVirtualMemory(void) {
 	uintptr_t pageTableStart = 0x1400000;
 	init_vm(virtualStart, physicalStart, pageTableStart);
 	mb();
+
+#if IS_ENABLED(CONFIG_DEBUG_PRINT_PAGE_TABLES)
 	printk(BIOS_DEBUG, "Finished initializing virtual memory, starting walk...\n");
-	walk_page_table();
+	print_page_table();
+#else
+	printk(BIOS_DEBUG, "Finished initializing virtual memory\n");
+#endif
 }
 
 void mstatus_init(void)
 {
-	// supervisor support is required
-
 	uintptr_t ms = 0;
 	ms = INSERT_FIELD(ms, MSTATUS_FS, 3);
 	ms = INSERT_FIELD(ms, MSTATUS_XS, 3);