From e84344f59ae36cb11782f5d8157d2346ad2dfd94 Mon Sep 17 00:00:00 2001
From: Philipp Oppermann <dev@phil-opp.com>
Date: Wed, 9 Dec 2015 00:34:19 +0100
Subject: [PATCH] Add function to translate virtual to physical address

---
 src/memory/paging/mod.rs | 74 ++++++++++++++++++++++++++++++++++++++++
 1 file changed, 74 insertions(+)

diff --git a/src/memory/paging/mod.rs b/src/memory/paging/mod.rs
index 53ad489b..9c2cc37b 100644
--- a/src/memory/paging/mod.rs
+++ b/src/memory/paging/mod.rs
@@ -1,3 +1,5 @@
+use memory::{PAGE_SIZE, Frame};
+
 mod entry;
 mod table;
 
@@ -9,3 +11,75 @@ pub type VirtualAddress = usize;
 pub struct Page {
     number: usize,
 }
+
+impl Page {
+    fn containing_address(address: VirtualAddress) -> Page {
+        assert!(address < 0x0000_8000_0000_0000 || address >= 0xffff_8000_0000_0000,
+                "invalid address: 0x{:x}",
+                address);
+        Page { number: address / PAGE_SIZE }
+    }
+
+    fn start_address(&self) -> usize {
+        self.number * PAGE_SIZE
+    }
+
+    fn p4_index(&self) -> usize {
+        (self.number >> 27) & 0o777
+    }
+    fn p3_index(&self) -> usize {
+        (self.number >> 18) & 0o777
+    }
+    fn p2_index(&self) -> usize {
+        (self.number >> 9) & 0o777
+    }
+    fn p1_index(&self) -> usize {
+        (self.number >> 0) & 0o777
+    }
+}
+
+pub fn translate(virtual_address: VirtualAddress) -> Option<PhysicalAddress> {
+    let offset = virtual_address % PAGE_SIZE;
+    translate_page(Page::containing_address(virtual_address))
+        .map(|frame| frame.number * PAGE_SIZE + offset)
+}
+
+fn translate_page(page: Page) -> Option<Frame> {
+    use self::entry::HUGE_PAGE;
+
+    let p3 = unsafe { &*table::P4 }.next_table(page.p4_index());
+
+    let huge_page = || {
+        p3.and_then(|p3| {
+            let p3_entry = &p3[page.p3_index()];
+            // 1GiB page?
+            if let Some(start_frame) = p3_entry.pointed_frame() {
+                if p3_entry.flags().contains(HUGE_PAGE) {
+                    // address must be 1GiB aligned
+                    assert!(start_frame.number % (ENTRY_COUNT * ENTRY_COUNT) == 0);
+                    return Some(Frame {
+                        number: start_frame.number + page.p2_index() * ENTRY_COUNT +
+                                page.p1_index(),
+                    });
+                }
+            }
+            if let Some(p2) = p3.next_table(page.p3_index()) {
+                let p2_entry = &p2[page.p2_index()];
+                // 2MiB page?
+                if let Some(start_frame) = p2_entry.pointed_frame() {
+                    if p2_entry.flags().contains(HUGE_PAGE) {
+                        // address must be 2MiB aligned
+                        assert!(start_frame.number % ENTRY_COUNT == 0);
+                        return Some(Frame { number: start_frame.number + page.p1_index() });
+                    }
+                }
+            }
+            None
+        })
+    };
+
+    p3.and_then(|p3| p3.next_table(page.p3_index()))
+      .and_then(|p2| p2.next_table(page.p2_index()))
+      .and_then(|p1| p1[page.p1_index()].pointed_frame())
+      .or_else(huge_page)
+}