dt: Iterate through structure block

Author: sysheap

src/common/src/big_endian.rs

@@ -1,8 +1,8 @@
 use core::fmt::{Debug, Display};
 
-use crate::numbers::Number;
+use crate::{consumable_buffer::FromU8Buffer, numbers::Number};
 
-#[derive(PartialEq, Eq)]
+#[derive(PartialEq, Eq, Clone, Copy)]
 #[repr(transparent)]
 pub struct BigEndian<T: Number>(pub T);
 
@@ -12,6 +12,12 @@ impl<T: Number> BigEndian<T> {
     }
 }
 
+impl<T: Number> FromU8Buffer for BigEndian<T> {
+    fn from_u8_buffer(buffer: &[u8]) -> Self {
+        BigEndian(T::from_le_bytes(buffer))
+    }
+}
+
 impl<T: Number> Debug for BigEndian<T> {
     fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
         write!(f, "{}", self.get())

src/common/src/consumable_buffer.rs

@@ -0,0 +1,83 @@
+use core::slice;
+
+use crate::util::align_up;
+
+pub struct ConsumableBuffer<'a> {
+    buffer: &'a [u8],
+    position: usize,
+}
+
+impl<'a> ConsumableBuffer<'a> {
+    pub fn new(buffer: &'a [u8]) -> ConsumableBuffer<'a> {
+        ConsumableBuffer {
+            buffer,
+            position: 0,
+        }
+    }
+
+    pub fn consume_slice(&mut self, size: usize) -> Option<&'a [u8]> {
+        if self.position + size > self.buffer.len() {
+            return None;
+        }
+
+        if size == 0 {
+            return Some(&[]);
+        }
+
+        let result = &self.buffer[self.position..self.position + size];
+        self.position += size;
+        Some(result)
+    }
+
+    pub fn consume_sized_type<T: FromU8Buffer>(&mut self) -> Option<T> {
+        let size = core::mem::size_of::<T>();
+        let result = self.consume_slice(size)?;
+        Some(T::from_u8_buffer(result))
+    }
+
+    pub fn consume_alignment(&mut self, alignment: usize) -> Option<()> {
+        let aligned_value = align_up(self.position, alignment);
+        let diff = aligned_value - self.position;
+        self.consume_slice(diff)?;
+        Some(())
+    }
+
+    pub fn consume_str(&mut self) -> Option<&'a str> {
+        let mut length = 0;
+        while self.position + length < self.buffer.len() && self.buffer[self.position + length] != 0
+        {
+            length += 1;
+        }
+        // Check if we really found a null-terminated string
+        if self.buffer[self.position + length] != 0 {
+            return None;
+        }
+
+        let string = unsafe {
+            core::str::from_utf8(slice::from_raw_parts(&self.buffer[self.position], length)).ok()?
+        };
+
+        // Consume null byte
+        length += 1;
+
+        self.position += length;
+
+        Some(string)
+    }
+
+    pub fn empty(&self) -> bool {
+        self.position >= self.buffer.len()
+    }
+
+    pub fn size_left(&self) -> usize {
+        if self.position >= self.buffer.len() {
+            0
+        } else {
+            self.buffer.len() - self.position
+        }
+    }
+}
+
+pub trait FromU8Buffer: Copy {
+    fn from_u8_buffer(buffer: &[u8]) -> Self;
+}

src/common/src/lib.rs

@@ -6,9 +6,11 @@
 #![reexport_test_harness_main = "test_main"]
 
 pub mod big_endian;
+pub mod consumable_buffer;
 pub mod mutex;
 pub mod numbers;
 pub mod syscalls;
+pub mod util;
 
 // Inspired by https://os.phil-opp.com/testing/
 

src/common/src/numbers.rs

@@ -2,6 +2,7 @@ use core::fmt::{Debug, Display};
 
 pub trait Number: Debug + Display + Copy + Clone {
     fn from_be(value: Self) -> Self;
+    fn from_le_bytes(bytes: &[u8]) -> Self;
 }
 
 macro_rules! impl_number {
@@ -10,6 +11,9 @@ macro_rules! impl_number {
             fn from_be(value: Self) -> Self {
                 <$T>::from_be(value)
             }
+            fn from_le_bytes(bytes: &[u8]) -> Self {
+                <$T>::from_le_bytes(bytes.try_into().unwrap())
+            }
         }
     };
 }

src/common/src/util.rs

@@ -0,0 +1,8 @@
+pub const fn align_up(value: usize, alignment: usize) -> usize {
+    let remainder = value % alignment;
+    if remainder == 0 {
+        value
+    } else {
+        value + alignment - remainder
+    }
+}

src/kernel/src/device_tree.rs

@@ -1,9 +1,12 @@
-use common::big_endian::BigEndian;
+use common::{big_endian::BigEndian, consumable_buffer::ConsumableBuffer};
 use core::{
     fmt::{Debug, Display},
+    mem::size_of,
     slice,
 };
 
+use crate::info;
+
 const FDT_MAGIC: u32 = 0xd00dfeed;
 const FDT_VERSION: u32 = 17;
 
@@ -42,6 +45,29 @@ impl Header {
             slice::from_raw_parts(start, len)
         }
     }
+
+    pub fn get_structure_block(&self) -> StructureBlockIterator {
+        let offset = self.off_dt_struct.get();
+        let start = self.offset_from_header(offset as usize);
+        info!("Structure Block Start: {:p}", start);
+        StructureBlockIterator {
+            buffer: ConsumableBuffer::new(unsafe {
+                slice::from_raw_parts(start, self.size_dt_struct.get() as usize)
+            }),
+            header: self,
+        }
+    }
+
+    fn get_string(&self, offset: usize) -> Option<&str> {
+        let start: *const u8 = self.offset_from_header(self.off_dt_strings.get() as usize);
+        let size = self.size_dt_strings.get() as usize;
+        if offset >= size {
+            return None;
+        }
+        let strings_data = unsafe { slice::from_raw_parts(start, size) };
+        let mut consumable_buffer = ConsumableBuffer::new(&strings_data[offset..]);
+        consumable_buffer.consume_str()
+    }
 }
 
 impl Debug for Header {
@@ -109,6 +135,63 @@ impl Display for ReserveEntry {
     }
 }
 
+const FDT_BEGIN_NODE: u32 = 0x1;
+const FDT_END_NODE: u32 = 0x2;
+const FDT_PROP: u32 = 0x3;
+const FDT_NOP: u32 = 0x4;
+const FDT_END: u32 = 0x9;
+
+#[derive(Debug)]
+pub enum FdtToken<'a> {
+    BeginNode(&'a str),
+    EndNode,
+    Prop(&'a str, &'a [u8]),
+    Nop,
+    End,
+}
+
+pub struct StructureBlockIterator<'a> {
+    header: &'a Header,
+    buffer: ConsumableBuffer<'a>,
+}
+
+impl<'a> Iterator for StructureBlockIterator<'a> {
+    type Item = FdtToken<'a>;
+
+    fn next(&mut self) -> Option<Self::Item> {
+        if self.buffer.empty() {
+            return None;
+        }
+
+        let numeric_token_value = self.buffer.consume_sized_type::<BigEndian<u32>>()?;
+        let token = match numeric_token_value.get() {
+            FDT_BEGIN_NODE => {
+                let name = self.buffer.consume_str()?;
+                self.buffer.consume_alignment(size_of::<u32>());
+                FdtToken::BeginNode(name)
+            }
+            FDT_END_NODE => FdtToken::EndNode,
+            FDT_PROP => {
+                let len = self.buffer.consume_sized_type::<BigEndian<u32>>()?.get() as usize;
+                let string_offset =
+                    self.buffer.consume_sized_type::<BigEndian<u32>>()?.get() as usize;
+                let data = self.buffer.consume_slice(len)?;
+                self.buffer.consume_alignment(size_of::<u32>());
+                let string = self.header.get_string(string_offset)?;
+                FdtToken::Prop(string, data)
+            }
+            FDT_NOP => FdtToken::Nop,
+            FDT_END => {
+                assert!(self.buffer.empty());
+                FdtToken::End
+            }
+            _ => panic!("Unknown token: {:#x}", numeric_token_value.get()),
+        };
+
+        Some(token)
+    }
+}
+
 pub fn parse(device_tree_pointer: *const ()) -> &'static Header {
     let header = unsafe { &*(device_tree_pointer as *const Header) };
 

src/kernel/src/klibc/util.rs

@@ -1,15 +1,8 @@
 use core::ops::{BitAnd, BitAndAssign, BitOrAssign, Not, Shl, Shr};
 
-use crate::memory::PAGE_SIZE;
+use common::util::align_up;
 
-pub const fn align_up(value: usize, alignment: usize) -> usize {
-    let remainder = value % alignment;
-    if remainder == 0 {
-        value
-    } else {
-        value + alignment - remainder
-    }
-}
+use crate::memory::PAGE_SIZE;
 
 pub const fn minimum_amount_of_pages(value: usize) -> usize {
     align_up(value, PAGE_SIZE) / PAGE_SIZE

src/kernel/src/main.rs

@@ -70,6 +70,11 @@ extern "C" fn kernel_init(hart_id: usize, device_tree_pointer: *const ()) {
         "There should be no reserved memory regions"
     );
 
+    let structured_block = dtb.get_structure_block();
+    for node in structured_block {
+        debug!("{:?}", node);
+    }
+
     unsafe {
         info!("Initializing page allocator");
         info!(

src/kernel/src/memory/heap.rs

@@ -5,12 +5,9 @@ use core::{
     ptr::{null_mut, NonNull},
 };
 
-use common::mutex::Mutex;
+use common::{mutex::Mutex, util::align_up};
 
-use crate::{
-    assert::static_assert_size,
-    klibc::util::{align_up, minimum_amount_of_pages},
-};
+use crate::{assert::static_assert_size, klibc::util::minimum_amount_of_pages};
 
 use super::{page_allocator::PageAllocator, PAGE_SIZE};