feat: add lower kernel identity-mapping

kernel/src/arch/aarch64/boot/entry.S

@@ -8,6 +8,9 @@
 .section .text._entry
 .global _entry
 _entry:
+    // Preserve FDT base address
+    mov x8, x0
+
     mrs x1, mpidr_el1
     and x1, x1, #3
     beq 2f
@@ -30,6 +33,7 @@ _entry:
     ADR_REL x0, bsp_stack_top
     mov sp, x0
 
+    mov x0, x8
     b __aa64_bsp_main
 
 .section .bss

kernel/src/arch/aarch64/boot/mod.rs

@@ -1,13 +1,14 @@
 //! aarch64 common boot logic
 
 use crate::arch::{aarch64::asm::CPACR_EL1, machine};
-use crate::dev::Device;
+use crate::dev::{Device, fdt::DeviceTree};
+use crate::mem::virt;
 use cortex_a::asm::barrier::{self, dsb, isb};
 use cortex_a::registers::{DAIF, SCTLR_EL1, VBAR_EL1};
 use tock_registers::interfaces::{ReadWriteable, Writeable};
 
 #[no_mangle]
-fn __aa64_bsp_main() {
+fn __aa64_bsp_main(fdt_base: usize) {
     // Disable FP instruction trapping
     CPACR_EL1.modify(CPACR_EL1::FPEN::TrapNone);
 
@@ -28,7 +29,14 @@ fn __aa64_bsp_main() {
         isb(barrier::SY);
     }
 
+    // Enable MMU
+    virt::enable().expect("Failed to initialize virtual memory");
+
     machine::init_board().unwrap();
+
+    let fdt = DeviceTree::from_phys(fdt_base).expect("Failed to obtain a device tree");
+    fdt.dump();
+
     unsafe {
         machine::local_timer().enable().unwrap();
     }

kernel/src/dev/fdt.rs

@@ -0,0 +1,88 @@
+use error::Errno;
+use fdt_rs::prelude::*;
+use fdt_rs::{
+    base::DevTree,
+    index::{DevTreeIndex, DevTreeIndexNode},
+};
+
+#[repr(align(16))]
+struct Wrap {
+    data: [u8; 16384],
+}
+
+static mut INDEX_BUFFER: Wrap = Wrap { data: [0; 16384] };
+
+type INode<'a> = DevTreeIndexNode<'a, 'a, 'a>;
+
+pub struct DeviceTree {
+    tree: DevTree<'static>,
+    index: DevTreeIndex<'static, 'static>,
+}
+
+fn tab(depth: usize) {
+    for _ in 0..depth {
+        debug!("\t");
+    }
+}
+
+fn dump_node(node: &INode, depth: usize) {
+    if node.name().unwrap().starts_with("virtio_mmio@") {
+        return;
+    }
+
+    tab(depth);
+    debugln!("{:?} {{", node.name().unwrap());
+
+    for prop in node.props() {
+        tab(depth + 1);
+        let name = prop.name().unwrap();
+        debug!("{:?} = ", name);
+
+        match name {
+            "compatible" => debug!("{:?}", prop.str().unwrap()),
+            "#address-cells" | "#size-cells" => debug!("{}", prop.u32(0).unwrap()),
+            "reg" => {
+                debug!("<");
+                let len = prop.length() / 4;
+                for i in 0..len {
+                    debug!("{:#010x}", prop.u32(i).unwrap());
+                    if i < len - 1 {
+                        debug!(", ");
+                    }
+                }
+                debug!(">");
+            },
+            _ => debug!("..."),
+        }
+        debugln!(";");
+    }
+
+    if node.children().next().is_some() {
+        debugln!("");
+    }
+
+    for child in node.children() {
+        dump_node(&child, depth + 1);
+    }
+
+    tab(depth);
+    debugln!("}}");
+}
+
+impl DeviceTree {
+    pub fn dump(&self) {
+        dump_node(&self.index.root(), 0);
+    }
+
+    pub fn from_phys(base: usize) -> Result<DeviceTree, Errno> {
+        // TODO virtualize address
+        let tree = unsafe { DevTree::from_raw_pointer(base as *const _) }.unwrap();
+        let layout = DevTreeIndex::get_layout(&tree).unwrap();
+        let index = DevTreeIndex::new(tree, unsafe {
+            &mut INDEX_BUFFER.data[0..layout.size() + layout.align()]
+        })
+        .unwrap();
+
+        Ok(DeviceTree { tree, index })
+    }
+}

kernel/src/dev/mod.rs

@@ -3,6 +3,8 @@
 use error::Errno;
 
 // Device classes
+#[allow(missing_docs)]
+pub mod fdt;
 pub mod gpio;
 pub mod irq;
 pub mod pci;

kernel/src/mem/mod.rs

@@ -1,5 +1,7 @@
 //! Memory management and functions module
 
+pub mod virt;
+
 /// See memcpy(3p).
 ///
 /// # Safety

kernel/src/mem/virt/mod.rs

@@ -0,0 +1,69 @@
+#![allow(missing_docs)]
+
+use cortex_a::asm::barrier::{self, dsb, isb};
+use cortex_a::registers::{ID_AA64MMFR0_EL1, MAIR_EL1, SCTLR_EL1, TCR_EL1, TTBR0_EL1, TTBR1_EL1};
+use error::Errno;
+use tock_registers::interfaces::{ReadWriteable, Readable, Writeable};
+
+const PTE_BLOCK_AF: u64 = 1 << 10;
+const PTE_BLOCK_ISH: u64 = 3 << 8;
+const PTE_BLOCK_OSH: u64 = 2 << 8;
+const PTE_PRESENT: u64 = 1 << 0;
+
+#[no_mangle]
+static mut KERNEL_TTBR0: [u64; 512] = {
+    let mut table = [0; 512];
+    // TODO fine-grained mapping
+    table[0] = (0 << 30) | PTE_BLOCK_AF | PTE_BLOCK_ISH | PTE_PRESENT;
+    table[1] = (1 << 30) | PTE_BLOCK_AF | PTE_BLOCK_ISH | PTE_PRESENT;
+
+    table
+};
+
+pub struct DeviceMemory {
+    base: usize,
+    count: usize,
+}
+
+impl DeviceMemory {
+    pub fn map(phys: usize, count: usize) -> Result<Self, Errno> {
+        todo!()
+    }
+}
+
+impl Drop for DeviceMemory {
+    fn drop(&mut self) {
+        todo!()
+    }
+}
+
+pub fn enable() -> Result<(), Errno> {
+    MAIR_EL1.write(
+        MAIR_EL1::Attr0_Normal_Outer::NonCacheable + MAIR_EL1::Attr0_Normal_Inner::NonCacheable,
+    );
+    TTBR0_EL1.set(unsafe { &mut KERNEL_TTBR0 as *mut _ as u64 });
+
+    if ID_AA64MMFR0_EL1.matches_all(ID_AA64MMFR0_EL1::TGran4::NotSupported) {
+        return Err(Errno::InvalidArgument);
+    }
+    let parange = ID_AA64MMFR0_EL1.read(ID_AA64MMFR0_EL1::PARange);
+
+    unsafe {
+        dsb(barrier::ISH);
+        isb(barrier::SY);
+    }
+
+    TCR_EL1.write(
+        TCR_EL1::IPS.val(parange)
+            + TCR_EL1::T0SZ.val(25)
+            + TCR_EL1::TG0::KiB_4
+            + TCR_EL1::SH0::Outer
+            + TCR_EL1::IRGN0::NonCacheable
+            + TCR_EL1::ORGN0::NonCacheable
+            + TCR_EL1::EPD1::SET
+    );
+
+    SCTLR_EL1.modify(SCTLR_EL1::M::SET);
+
+    Ok(())
+}