Adds a basic DeviceTree reader.

4 files changed, 256 insertions, 2 deletions

diff --git a/kernel/Cargo.lock b/kernel/Cargo.lock
index c4e38bd..d831a4f 100644
--- a/kernel/Cargo.lock
+++ b/kernel/Cargo.lock
@@ -3,6 +3,15 @@
 version = 3
 
 [[package]]
+name = "bstr"
+version = "1.10.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "40723b8fb387abc38f4f4a37c09073622e41dd12327033091ef8950659e6dc0c"
+dependencies = [
+ "memchr",
+]
+
+[[package]]
 name = "contracts"
 version = "0.6.3"
 source = "registry+https://github.com/rust-lang/crates.io-index"
@@ -17,6 +26,7 @@ dependencies = [
 name = "kernel"
 version = "0.1.0"
 dependencies = [
+ "bstr",
  "contracts",
  "log",
  "spin",
@@ -29,6 +39,12 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "b5e6163cb8c49088c2c36f57875e58ccd8c87c7427f7fbd50ea6710b2f3f2e8f"
 
 [[package]]
+name = "memchr"
+version = "2.7.4"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "78ca9ab1a0babb1e7d5695e3530886289c18cf2f87ec19a575a0abdce112e3a3"
+
+[[package]]
 name = "proc-macro2"
 version = "1.0.78"
 source = "registry+https://github.com/rust-lang/crates.io-index"
diff --git a/kernel/Cargo.toml b/kernel/Cargo.toml
index d6449e8..ea81d5d 100644
--- a/kernel/Cargo.toml
+++ b/kernel/Cargo.toml
@@ -7,6 +7,7 @@ edition = "2021"
 crate-type = ["staticlib"]
 
 [dependencies]
+bstr = { version = "1.10.0", default-features = false }
 contracts = { version = "0.6.3", default-features = false }
 log = { version = "0.4.20", default-features = false }
 spin = { version = "0.9.8", default-features = false, features = ["mutex", "use_ticket_mutex"] }
diff --git a/kernel/src/device_tree.rs b/kernel/src/device_tree.rs
new file mode 100644
index 0000000..04f873c
--- /dev/null
+++ b/kernel/src/device_tree.rs
@@ -0,0 +1,224 @@
+//! Support for the DeviceTree format.
+
+use core::{iter, slice};
+
+use bstr::BStr;
+use contracts::requires;
+
+/// A reference to a flattened DeviceTree (DTB) in memory.
+#[derive(Debug)]
+pub struct FlattenedDeviceTree<'dt> {
+    header: &'dt FdtHeader,
+    struct_block: &'dt [u32],
+    strings_block: &'dt BStr,
+    memrsv_block: &'dt [FdtMemRsv],
+}
+
+impl<'dt> FlattenedDeviceTree<'dt> {
+    /// Looks for a DeviceTree at the given address, and returns it if it looks valid.
+    ///
+    /// # Safety
+    ///
+    /// - `ptr` must point to a spec-compliant flattened DeviceTree.
+    /// - The memory contained by the flattened DeviceTree must be valid for the duration of
+    ///   lifetime `'dt`.
+    /// - The memory contained by the flattened DeviceTree must not be mutated for the duration of
+    ///   lifetime `'dt`.
+    #[requires((ptr as *const FdtHeader).is_aligned())]
+    pub unsafe fn from_ptr(ptr: *const u8) -> Result<FlattenedDeviceTree<'dt>, DeviceTreeError> {
+        // Check that the header appears to point to a valid flattened DeviceTree.
+        let header: &'dt FdtHeader = &*(ptr as *const FdtHeader);
+        let magic = u32::from_be(header.magic);
+        if magic != 0xd00dfeed {
+            return Err(DeviceTreeError::BadMagic(magic));
+        }
+        let version = u32::from_be(header.version);
+        let last_comp_version = u32::from_be(header.last_comp_version);
+        if last_comp_version > 17 {
+            return Err(DeviceTreeError::IncompatibleVersion(
+                version,
+                last_comp_version,
+            ));
+        }
+
+        // Get pointers to each block.
+        let off_dt_struct = u32::from_be(header.off_dt_struct) as usize;
+        let size_dt_struct = u32::from_be(header.size_dt_struct) as usize;
+        let off_dt_strings = u32::from_be(header.off_dt_strings) as usize;
+        let size_dt_strings = u32::from_be(header.size_dt_strings) as usize;
+        let off_mem_rsvmap = u32::from_be(header.off_mem_rsvmap) as usize;
+
+        // Check that the structure block has an aligned size.
+        if (size_dt_struct & 0b11) != 0 {
+            return Err(DeviceTreeError::InvalidDeviceTree);
+        }
+
+        // Extract the structure and strings blocks.
+        let struct_block: &[u32] =
+            slice::from_raw_parts(ptr.add(off_dt_struct).cast(), size_dt_struct / 4);
+        let strings_block = BStr::new(slice::from_raw_parts(
+            ptr.add(off_dt_strings),
+            size_dt_strings,
+        ));
+
+        // Read memory reservations until the terminating one is found, then construct the block of
+        // the appropriate length.
+        let mut memrsv_count = 0;
+        let memrsv_ptr: *const FdtMemRsv = ptr.add(off_mem_rsvmap).cast();
+        let memrsv_block = loop {
+            let memrsv = *memrsv_ptr.add(memrsv_count);
+
+            // We can skip the endian conversion, since we're just testing against zero.
+            if memrsv == (FdtMemRsv { addr: 0, size: 0 }) {
+                break slice::from_raw_parts(memrsv_ptr, memrsv_count);
+            }
+
+            memrsv_count += 1;
+        };
+
+        Ok(FlattenedDeviceTree {
+            header,
+            struct_block,
+            strings_block,
+            memrsv_block,
+        })
+    }
+
+    /// Returns the string at the given offset of the strings block.
+    pub fn get_string(&self, offset: u32) -> Result<&BStr, DeviceTreeError> {
+        let out = &self.strings_block[offset as usize..];
+        let i = out
+            .iter()
+            .position(|&b| b == b'\0')
+            .ok_or(DeviceTreeError::StringMissingNulTerminator(offset))?;
+        Ok(&out[..i])
+    }
+
+    /// Returns an iterator over the events in the flattened DeviceTree's structure block.
+    pub fn struct_events(
+        &self,
+    ) -> impl '_ + Iterator<Item = Result<FdtStructEvent, DeviceTreeError>> {
+        let mut ptr = self.struct_block;
+        iter::from_fn(move || loop {
+            break match u32::from_be(ptr[0]) {
+                // FDT_BEGIN_NODE
+                0x00000001 => {
+                    // Save a pointer to the start of the extra data.
+                    let name_ptr = (&ptr[1]) as *const u32 as *const u8;
+
+                    // Look for a null terminator.
+                    //
+                    // SAFETY: This method can only be called when the FlattenedDeviceTree was
+                    // constructed from a valid flattened DeviceTree.
+                    let mut name_len = 0;
+                    while unsafe { *name_ptr.add(name_len) } != b'\0' {
+                        name_len += 1;
+                    }
+
+                    // Create the name as a BStr.
+                    //
+                    // SAFETY: Well, we already accessed this memory above when finding the null
+                    // terminator... But yes, this being valid is guaranteed by this being a
+                    // flattened DeviceTree.
+                    let name = BStr::new(unsafe { slice::from_raw_parts(name_ptr, name_len) });
+
+                    // Advance the pointer.
+                    let extra_data_count = (name_len + 4) >> 2;
+                    ptr = &ptr[1 + extra_data_count..];
+
+                    // Return the event.
+                    Some(Ok(FdtStructEvent::BeginNode(name)))
+                }
+                // FDT_END_NODE
+                0x00000002 => {
+                    // Advance the pointer.
+                    ptr = &ptr[1..];
+
+                    // Return the event.
+                    Some(Ok(FdtStructEvent::EndNode))
+                }
+                // FDT_PROP
+                0x00000003 => {
+                    // Get the length of the property data and the offset of the name in the
+                    // strings block.
+                    let len = u32::from_be(ptr[1]) as usize;
+                    let name_off = u32::from_be(ptr[2]);
+
+                    // Get the property data as a BStr.
+                    //
+                    // SAFETY: This is valid by the requirements of a flattened DeviceTree.
+                    let data = BStr::new(unsafe {
+                        slice::from_raw_parts(&ptr[3] as *const u32 as *const u8, len)
+                    });
+
+                    // Get the property name as a BStr.
+                    let name = match self.get_string(name_off) {
+                        Ok(name) => name,
+                        Err(err) => break Some(Err(err)),
+                    };
+
+                    // Advance the pointer.
+                    let data_count = (len + 3) >> 2;
+                    ptr = &ptr[3 + data_count..];
+
+                    // Return the event.
+                    Some(Ok(FdtStructEvent::Prop(name, data)))
+                }
+                // FDT_NOP
+                0x00000004 => {
+                    ptr = &ptr[1..];
+                    continue;
+                }
+                // FDT_END
+                0x00000009 => None,
+                token_type => Some(Err(DeviceTreeError::InvalidTokenType(token_type))),
+            };
+        })
+    }
+}
+
+/// An error encountered when reading a DeviceTree.
+#[derive(Debug)]
+pub enum DeviceTreeError {
+    BadMagic(u32),
+    IncompatibleVersion(u32, u32),
+    InvalidDeviceTree,
+    InvalidTokenType(u32),
+    StringMissingNulTerminator(u32),
+}
+
+/// The flattened DeviceTree header.
+///
+/// All fields are big-endian.
+#[derive(Debug)]
+#[repr(C)]
+struct FdtHeader {
+    magic: u32,
+    totalsize: u32,
+    off_dt_struct: u32,
+    off_dt_strings: u32,
+    off_mem_rsvmap: u32,
+    version: u32,
+    last_comp_version: u32,
+    boot_cpuid_phys: u32,
+    size_dt_strings: u32,
+    size_dt_struct: u32,
+}
+
+/// A memory reservation from the appropriate block of the DeviceTree.
+///
+/// All fields are big-endian.
+#[derive(Clone, Copy, Debug, Eq, PartialEq)]
+#[repr(C)]
+struct FdtMemRsv {
+    addr: u64,
+    size: u64,
+}
+
+/// An event returned from iterating over the structure block of a flattened DeviceTree.
+#[derive(Clone, Copy, Debug)]
+pub enum FdtStructEvent<'dt> {
+    BeginNode(&'dt BStr),
+    EndNode,
+    Prop(&'dt BStr, &'dt BStr),
+}
diff --git a/kernel/src/lib.rs b/kernel/src/lib.rs
index 842609f..0a724c6 100644
--- a/kernel/src/lib.rs
+++ b/kernel/src/lib.rs
@@ -1,6 +1,7 @@
 #![no_std]
 
 pub mod console;
+pub mod device_tree;
 pub mod util;
 
 #[cfg(not(test))]
@@ -8,12 +9,24 @@ mod panic;
 
 /// The entrypoint to the kernel. This should be executed by hart0 alone. It performs some early
 /// boot tasks, then wakes up any other harts.
+///
+/// # Safety
+///
+/// - The `device_tree` pointer must be a valid pointer into physical memory. See
+///   `device_tree::FlattenedDeviceTree::from_ptr` for the precise requirements.
+/// - This must be called in supervisor mode with paging and traps disabled, but with all traps
+///   delegated to supervisor mode.
 #[no_mangle]
-pub extern "C" fn hart0_boot(device_tree: *const u32) -> ! {
+pub unsafe extern "C" fn hart0_boot(device_tree: *const u8) -> ! {
     console::init();
 
     log::info!("device_tree = {device_tree:?}");
-    dbg!(42);
+    let flattened_device_tree = unsafe { device_tree::FlattenedDeviceTree::from_ptr(device_tree) }
+        .expect("invalid DeviceTree");
+    for event in flattened_device_tree.struct_events() {
+        let event = event.expect("invalid DeviceTree");
+        dbg!(event);
+    }
 
     todo!()
 }