fix: Level 3 translation fault

src/aarch64/mmu.rs

@@ -11,6 +11,7 @@ unsafe extern "C" {
 
 const BLOCK: u64 = 0b01;
 const TABLE: u64 = 0b11;
+const PAGE: u64 = 0b11;
 
 pub const EL0_ACCESSIBLE: u64 = 1 << 6;
 
@@ -91,6 +92,7 @@ pub fn allocate_memory(
 
     Ok(())
 }
+
 pub fn allocate_memory_explicit(
     mut virtual_address: usize,
     mut size: usize,
@@ -103,9 +105,9 @@ pub fn allocate_memory_explicit(
 
     let level1_blocks = size / LEVEL1_BLOCK_SIZE;
     size %= LEVEL1_BLOCK_SIZE;
-    let level2_blocks = size / LEVEL2_BLOCK_SIZE;
+    let mut level2_blocks = size / LEVEL2_BLOCK_SIZE;
     size %= LEVEL2_BLOCK_SIZE;
-    let level3_pages = size / GRANULARITY;
+    let mut level3_pages = size / GRANULARITY;
     if size % GRANULARITY != 0 {
         return Err(NovaError::InvalidGranularity);
     }
@@ -114,6 +116,31 @@ pub fn allocate_memory_explicit(
         todo!("Currently not supported");
     }
 
+    let l2_alignment = (physical_address % LEVEL2_BLOCK_SIZE) / GRANULARITY;
+    if l2_alignment != 0 {
+        let l3_diff = LEVEL2_BLOCK_SIZE / GRANULARITY - l2_alignment;
+        if l3_diff > level3_pages {
+            level2_blocks -= 1;
+            level3_pages += TABLE_ENTRY_COUNT;
+        }
+
+        level3_pages -= l3_diff;
+
+        for _ in 0..l3_diff {
+            unsafe {
+                alloc_page_explicit(
+                    virtual_address,
+                    physical_address,
+                    &mut TRANSLATIONTABLE_TTBR0,
+                    additional_flags,
+                )?;
+            }
+
+            virtual_address += GRANULARITY;
+            physical_address += GRANULARITY;
+        }
+    }
+
     for _ in 0..level2_blocks {
         unsafe {
             alloc_block_l2_explicit(
@@ -178,13 +205,15 @@ fn map_page(
 ) -> Result<(), NovaError> {
     let (l1_off, l2_off, l3_off) = virtual_address_to_table_offset(virtual_address);
 
-    let table = navigate_table(base_table, [l1_off, l2_off, 0], 2)?;
+    let offsets = [l1_off, l2_off];
+
+    let table = navigate_table(base_table, &offsets)?;
 
     if table.0[l3_off] & 0b11 > 0 {
         return Err(NovaError::Paging);
     }
 
-    table.0[l3_off] = create_block_descriptor_entry(physical_address, additional_flags);
+    table.0[l3_off] = create_page_descriptor_entry(physical_address, additional_flags);
 
     Ok(())
 }
@@ -203,6 +232,10 @@ pub fn alloc_block_l2_explicit(
     base_table: &mut PageTable,
     additional_flags: u64,
 ) -> Result<(), NovaError> {
+    if physical_address % LEVEL2_BLOCK_SIZE != 0 {
+        return Err(NovaError::Misalignment);
+    }
+
     reserve_block_explicit(physical_address)?;
     map_l2_block(virtual_addr, physical_address, base_table, additional_flags)
 }
@@ -214,8 +247,8 @@ pub fn map_l2_block(
     additional_flags: u64,
 ) -> Result<(), NovaError> {
     let (l1_off, l2_off, _) = virtual_address_to_table_offset(virtual_addr);
-
+    let offsets = [l1_off];
-    let table = navigate_table(base_table, [l1_off, 0, 0], 1)?;
+    let table = navigate_table(base_table, &offsets)?;
 
     // Verify virtual address is available.
     if table.0[l2_off] & 0b11 != 0 {
@@ -312,13 +345,21 @@ fn reserve_block_explicit(physical_address: usize) -> Result<(), NovaError> {
 }
 
 fn create_block_descriptor_entry(physical_address: usize, additional_flags: u64) -> u64 {
-    (physical_address as u64 & 0x0000_FFFF_FFE0_0000)
+    (physical_address as u64 & 0x0000_FFFF_FFFF_F000)
         | BLOCK
         | ACCESS_FLAG
         | INNER_SHAREABILITY
         | additional_flags
 }
 
+fn create_page_descriptor_entry(physical_address: usize, additional_flags: u64) -> u64 {
+    (physical_address as u64 & 0x0000_FFFF_FFFF_F000)
+        | PAGE
+        | ACCESS_FLAG
+        | INNER_SHAREABILITY
+        | additional_flags
+}
+
 fn create_table_descriptor_entry(addr: usize) -> u64 {
     0 | (addr as u64 & 0x0000_FFFF_FFFF_F000) | TABLE
 }
@@ -331,22 +372,38 @@ fn virtual_address_to_table_offset(virtual_addr: usize) -> (usize, usize, usize)
     (l1_off, l2_off, l3_off)
 }
 
-fn navigate_table(
+pub fn sim_l3_access(addr: usize) {
-    initial_table: &mut PageTable,
+    unsafe {
-    offsets: [usize; 3],
+        let entry1 = TRANSLATIONTABLE_TTBR0.0[addr / LEVEL1_BLOCK_SIZE];
-    offsets_size: usize,
+        let table2 = &mut *(get_table_entry_address(entry1) as *mut PageTable);
-) -> Result<&mut PageTable, NovaError> {
+        let entry2 = table2.0[(addr % LEVEL1_BLOCK_SIZE) / LEVEL2_BLOCK_SIZE];
+        let table3 = &mut *(get_table_entry_address(entry2) as *mut PageTable);
+        let entry3 = table3.0[(addr % LEVEL2_BLOCK_SIZE) / GRANULARITY];
+    }
+}
+
+fn navigate_table<'a>(
+    initial_table: &'a mut PageTable,
+    offsets: &'a [usize],
+) -> Result<&'a mut PageTable, NovaError> {
     let root_table_ptr = initial_table as *mut PageTable;
     let mut table = initial_table;
-    for i in 0..offsets_size {
+    for offset in offsets {
-        let offset = offsets[i];
+        table = next_table(table, *offset, root_table_ptr)?;
+    }
+    Ok(table)
+}
+
+fn next_table(
+    table: &mut PageTable,
+    offset: usize,
+    root_table_ptr: *mut PageTable,
+) -> Result<&mut PageTable, NovaError> {
     match table.0[offset] & 0b11 {
         0 => {
             let new_table_addr = reserve_page();
 
             table.0[offset] = create_table_descriptor_entry(new_table_addr);
-                table =
-                    unsafe { &mut *(get_table_entry_address(table.0[offset]) as *mut PageTable) };
 
             map_page(
                 new_table_addr,
@@ -354,16 +411,13 @@ fn navigate_table(
                 unsafe { &mut *root_table_ptr },
                 NORMAL_MEM | WRITABLE | PXN | UXN,
             )?;
+
+            Ok(unsafe { &mut *(get_table_entry_address(table.0[offset]) as *mut PageTable) })
         }
         1 => return Err(NovaError::Paging),
-            3 => {
+        3 => Ok(unsafe { &mut *(get_table_entry_address(table.0[offset]) as *mut PageTable) }),
-                table =
+        _ => unreachable!(),
-                    unsafe { &mut *(get_table_entry_address(table.0[offset]) as *mut PageTable) }
     }
-            _ => panic!(),
-        };
-    }
-    Ok(table)
 }
 
 fn find_unallocated_page() -> Option<usize> {

src/main.rs

@@ -13,8 +13,8 @@ extern crate alloc;
 use nova::{
     aarch64::{
         mmu::{
-            allocate_memory_explicit, EL0_ACCESSIBLE, LEVEL2_BLOCK_SIZE, NORMAL_MEM, PXN, UXN,
+            allocate_memory_explicit, sim_l3_access, EL0_ACCESSIBLE, NORMAL_MEM,
-            WRITABLE,
+            PXN, UXN, WRITABLE,
         },
         registers::{daif, read_id_aa64mmfr0_el1},
     },
@@ -77,11 +77,12 @@ pub extern "C" fn main() -> ! {
         // TODO: Investigate why the size is off
         allocate_memory_explicit(
             0x3c100000,
-            1080 * 1920 * 4 + LEVEL2_BLOCK_SIZE + LEVEL2_BLOCK_SIZE,
+            1080 * 1920 * 4,
             0x3c100000,
             NORMAL_MEM | PXN | UXN | WRITABLE | EL0_ACCESSIBLE,
         )
         .unwrap();
+        sim_l3_access(0x3c100000);
         configure_mmu_el1();
     };
 
@@ -108,7 +109,11 @@ pub extern "C" fn kernel_main() -> ! {
     nova::initialize_kernel();
     println!("Exception Level: {}", get_current_el());
     daif::unmask_all();
+    let fb = FrameBuffer::default();
 
+    for i in 0..1080 {
+        fb.draw_pixel(50, i, RED);
+    }
     unsafe {
         el1_to_el0();
     };
@@ -131,7 +136,7 @@ pub extern "C" fn el0() -> ! {
 
     let fb = FrameBuffer::default();
 
-    for i in 0..1080 {
+    for i in 600..1080 {
         fb.draw_pixel(50, i, RED);
     }
     fb.draw_square(500, 500, 600, 700, RED);