From 401dfdd06df2fa1a4c82d828cf342b00a4f73c06 Mon Sep 17 00:00:00 2001
From: Berkus Decker <berkus@madfire.net>
Date: Fri, 23 Feb 2018 16:52:47 +0200
Subject: [PATCH] Implement table hierarchy traversing

---
 src/arch/aarch64/memory/paging/table.rs | 54 ++++++++++++++++++++++++-
 1 file changed, 53 insertions(+), 1 deletion(-)

diff --git a/src/arch/aarch64/memory/paging/table.rs b/src/arch/aarch64/memory/paging/table.rs
index 08c2ab7..7af50a6 100644
--- a/src/arch/aarch64/memory/paging/table.rs
+++ b/src/arch/aarch64/memory/paging/table.rs
@@ -4,7 +4,14 @@ use super::super::FrameAllocator;
 use super::ENTRY_COUNT;
 use arch::aarch64::memory::paging::entry::*;
 
-pub const L0: *mut Table<Level0> = 0xffff_ffff_ffff_f000 as *mut _;
+pub const L0: *mut Table<Level0> = 0xffff_ffff_ffff_f000 as *mut _; // L0 0o177777_777_777_777_777_0000
+                                                                    // L1 0o177777_777_777_777_XXX_0000, XXX is the L0 index
+                                                                    // L2 0o177777_777_777_XXX_YYY_0000, YYY is the L1 index
+                                                                    // L3 0o177777_777_XXX_YYY_ZZZ_0000, ZZZ is the L2 index
+
+// L1 = (L0 << 9) | (XXX << 12)
+// L2 = (L1 << 9) | (YYY << 12)
+// L3 = (L2 << 9) | (ZZZ << 12)
 
 pub struct Table<L: TableLevel> {
     entries: [Entry; ENTRY_COUNT],
@@ -22,6 +29,51 @@ where
     }
 }
 
+impl<L> Table<L>
+where
+    L: HierarchicalLevel,
+{
+    fn next_table_address(&self, index: usize) -> Option<usize> {
+        let entry_flags = self[index].flags();
+        if entry_flags.contains(EntryFlags::VALID | EntryFlags::TABLE) {
+            let table_address = self as *const _ as usize;
+            Some((table_address << 9) | (index << 12))
+        } else {
+            None
+        }
+    }
+
+    pub fn next_table(&self, index: usize) -> Option<&Table<L::NextLevel>> {
+        self.next_table_address(index)
+            .map(|address| unsafe { &*(address as *const _) })
+    }
+
+    pub fn next_table_mut(&mut self, index: usize) -> Option<&mut Table<L::NextLevel>> {
+        self.next_table_address(index)
+            .map(|address| unsafe { &mut *(address as *mut _) })
+    }
+
+    pub fn next_table_create<A>(
+        &mut self,
+        index: usize,
+        allocator: &mut A,
+    ) -> &mut Table<L::NextLevel>
+    where
+        A: FrameAllocator,
+    {
+        if self.next_table(index).is_none() {
+            assert!(
+                self.entries[index].flags().contains(EntryFlags::TABLE),
+                "mapping code does not support huge pages"
+            );
+            let frame = allocator.allocate_frame().expect("no frames available");
+            self.entries[index].set(frame, EntryFlags::VALID /*| WRITABLE*/);
+            self.next_table_mut(index).unwrap().zero();
+        }
+        self.next_table_mut(index).unwrap()
+    }
+}
+
 impl<L> Index<usize> for Table<L>
 where
     L: TableLevel,