feat(dt): add a module for creating devicetree

Signed-off-by: Changyuan Lyu <changyuanl@google.com>

alioth/src/firmware/dt/dt.rs

@@ -0,0 +1,89 @@
+// Copyright 2024 Google LLC
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     https://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+pub mod dtb;
+
+use std::collections::HashMap;
+use std::mem::size_of_val;
+
+#[derive(Debug, Clone)]
+pub enum PropVal {
+    Empty,
+    U32(u32),
+    U64(u64),
+    String(String),
+    Str(&'static str),
+    PHandle(u32),
+    StringList(Vec<String>),
+    U32List(Vec<u32>),
+    U64List(Vec<u64>),
+    PropSpec(Vec<u8>),
+}
+
+impl PropVal {
+    pub fn size(&self) -> usize {
+        match self {
+            PropVal::Empty => 0,
+            PropVal::U32(_) | PropVal::PHandle(_) => 4,
+            PropVal::U64(_) => 8,
+            PropVal::String(s) => s.len() + 1,
+            PropVal::Str(s) => s.len() + 1,
+            PropVal::PropSpec(d) => d.len(),
+            PropVal::U32List(r) => size_of_val(r.as_slice()),
+            PropVal::U64List(r) => size_of_val(r.as_slice()),
+            PropVal::StringList(l) => l.iter().map(|s| s.len() + 1).sum(),
+        }
+    }
+}
+
+#[derive(Debug, Clone, Default)]
+pub struct Node {
+    pub props: HashMap<&'static str, PropVal>,
+    pub nodes: HashMap<String, Node>,
+}
+
+#[derive(Debug, Clone, Default)]
+pub struct DeviceTree {
+    pub root: Node,
+    pub reserved_mem: Vec<(usize, usize)>,
+    pub boot_cpuid_phys: u32,
+}
+
+impl DeviceTree {
+    pub fn new() -> Self {
+        DeviceTree::default()
+    }
+}
+
+#[cfg(test)]
+mod test {
+    use crate::firmware::dt::PropVal;
+
+    #[test]
+    fn test_val_size() {
+        assert_eq!(PropVal::Empty.size(), 0);
+        assert_eq!(PropVal::U32(1).size(), 4);
+        assert_eq!(PropVal::U64(1).size(), 8);
+        assert_eq!(PropVal::String("s".to_owned()).size(), 2);
+        assert_eq!(PropVal::Str("s").size(), 2);
+        assert_eq!(PropVal::PHandle(1).size(), 4);
+        assert_eq!(
+            PropVal::StringList(vec!["s1".to_owned(), "s12".to_owned()]).size(),
+            7
+        );
+        assert_eq!(PropVal::U32List(vec![1, 2]).size(), 8);
+        assert_eq!(PropVal::U64List(vec![1, 3]).size(), 16);
+        assert_eq!(PropVal::PropSpec(vec![1, 2, 3, 4]).size(), 4);
+    }
+}

alioth/src/firmware/dt/dtb.rs

@@ -0,0 +1,222 @@
+// Copyright 2024 Google LLC
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     https://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+use std::collections::HashMap;
+use std::mem::size_of;
+
+use zerocopy::{AsBytes, FromBytes, FromZeroes};
+
+use crate::align_up;
+use crate::firmware::dt::{DeviceTree, Node, PropVal};
+use crate::utils::endian::{Bu32, Bu64};
+
+pub const FDT_HEADER_MAGIC: [u8; 4] = [0xd0, 0x0d, 0xfe, 0xed];
+pub const FDT_HEADER_VERSION: u32 = 0x11;
+pub const FDT_HEADER_LAST_COMP_VERSION: u32 = 0x10;
+
+pub const FDT_BEGIN_NODE: [u8; 4] = [0x00, 0x00, 0x00, 0x01];
+pub const FDT_END_NODE: [u8; 4] = [0x00, 0x00, 0x00, 0x02];
+pub const FDT_PROP: [u8; 4] = [0x00, 0x00, 0x00, 0x03];
+pub const FDT_NOP: [u8; 4] = [0x00, 0x00, 0x00, 0x04];
+pub const FDT_END: [u8; 4] = [0x00, 0x00, 0x00, 0x09];
+
+fn push_string_align(data: &mut Vec<u8>, s: &str) {
+    data.extend(s.as_bytes());
+    let padding = align_up!(s.len() + 1, 4) - s.len();
+    for _ in 0..padding {
+        data.push(b'\0');
+    }
+}
+
+fn pad_data(data: &mut Vec<u8>) {
+    let padding = align_up!(data.len(), 4) - data.len();
+    for _ in 0..padding {
+        data.push(b'\0');
+    }
+}
+
+impl PropVal {
+    fn write_as_blob(&self, data: &mut Vec<u8>) {
+        match self {
+            PropVal::Empty => {}
+            PropVal::U32(n) | PropVal::PHandle(n) => data.extend(n.to_be_bytes()),
+            PropVal::U64(n) => data.extend(n.to_be_bytes()),
+            PropVal::String(s) => push_string_align(data, s),
+            PropVal::Str(s) => push_string_align(data, s),
+            PropVal::PropSpec(d) => data.extend(d),
+            PropVal::StringList(list) => {
+                for s in list {
+                    data.extend(s.as_bytes());
+                    data.push(b'\0');
+                }
+                pad_data(data)
+            }
+            PropVal::U32List(reg) => {
+                for v in reg {
+                    data.extend(v.to_be_bytes())
+                }
+            }
+            PropVal::U64List(reg) => {
+                for v in reg {
+                    data.extend(v.to_be_bytes())
+                }
+            }
+        }
+    }
+}
+
+#[repr(C)]
+#[derive(AsBytes, FromBytes, FromZeroes, Debug)]
+pub struct FdtHeader {
+    magic: Bu32,
+    total_size: Bu32,
+    off_dt_struct: Bu32,
+    off_dt_strings: Bu32,
+    off_mem_resvmap: Bu32,
+    version: Bu32,
+    last_comp_version: Bu32,
+    boot_cpuid_phys: Bu32,
+    size_dt_strings: Bu32,
+    size_dt_struct: Bu32,
+}
+
+#[derive(AsBytes, FromBytes, FromZeroes, Debug)]
+#[repr(C)]
+pub struct FdtProp {
+    len: Bu32,
+    name_off: Bu32,
+}
+
+#[derive(AsBytes, FromBytes, FromZeroes, Debug)]
+#[repr(C)]
+struct FdtReserveEntry {
+    address: Bu64,
+    size: Bu64,
+}
+
+#[derive(Debug)]
+pub struct StringBlock {
+    total_size: usize,
+    strings: HashMap<&'static str, usize>,
+}
+
+impl StringBlock {
+    fn new() -> Self {
+        StringBlock {
+            total_size: 0,
+            strings: HashMap::new(),
+        }
+    }
+
+    fn add(&mut self, name: &'static str) -> usize {
+        if let Some(offset) = self.strings.get(&name) {
+            *offset
+        } else {
+            self.strings.insert(name, self.total_size);
+            let ret = self.total_size;
+            self.total_size += name.len() + 1;
+            ret
+        }
+    }
+
+    fn write_as_blob(self, data: &mut Vec<u8>) {
+        let mut string_offset = self.strings.into_iter().collect::<Vec<_>>();
+        string_offset.sort_by_key(|(_, offset)| *offset);
+        for (s, _) in string_offset {
+            data.extend(s.as_bytes());
+            data.push(b'\0');
+        }
+        pad_data(data)
+    }
+}
+
+impl Node {
+    fn write_as_blob(&self, name: &str, string_block: &mut StringBlock, data: &mut Vec<u8>) {
+        data.extend(&FDT_BEGIN_NODE);
+        push_string_align(data, name);
+        for (prop_name, prop) in self.props.iter() {
+            data.extend(&FDT_PROP);
+            let fdt_prop = FdtProp {
+                len: Bu32::from(prop.size() as u32),
+                name_off: Bu32::from(string_block.add(prop_name) as u32),
+            };
+            data.extend(fdt_prop.as_bytes());
+            prop.write_as_blob(data);
+        }
+        for (node_name, node) in self.nodes.iter() {
+            node.write_as_blob(node_name, string_block, data)
+        }
+        data.extend(&FDT_END_NODE);
+    }
+}
+
+impl DeviceTree {
+    pub fn to_blob(&self) -> Vec<u8> {
+        let mut data = vec![0u8; size_of::<FdtHeader>()];
+
+        let off_mem_resvmap = data.len();
+        for (addr, size) in &self.reserved_mem {
+            let entry = FdtReserveEntry {
+                address: Bu64::from(*addr as u64),
+                size: Bu64::from(*size as u64),
+            };
+            data.extend(entry.as_bytes());
+        }
+        data.extend(FdtReserveEntry::new_zeroed().as_bytes());
+
+        let off_dt_struct = data.len();
+        let mut string_block = StringBlock::new();
+        self.root.write_as_blob("", &mut string_block, &mut data);
+        data.extend(&FDT_END);
+        let size_dt_struct = data.len() - off_dt_struct;
+
+        let off_dt_strings = data.len();
+        string_block.write_as_blob(&mut data);
+        let size_dt_strings = data.len() - off_dt_strings;
+
+        let total_size = data.len();
+
+        let header = FdtHeader {
+            magic: Bu32::from(FDT_HEADER_MAGIC),
+            total_size: Bu32::from(total_size as u32),
+            off_dt_struct: Bu32::from(off_dt_struct as u32),
+            off_dt_strings: Bu32::from(off_dt_strings as u32),
+            off_mem_resvmap: Bu32::from(off_mem_resvmap as u32),
+            version: Bu32::from(FDT_HEADER_VERSION),
+            last_comp_version: Bu32::from(FDT_HEADER_LAST_COMP_VERSION),
+            boot_cpuid_phys: Bu32::from(self.boot_cpuid_phys),
+            size_dt_strings: Bu32::from(size_dt_strings as u32),
+            size_dt_struct: Bu32::from(size_dt_struct as u32),
+        };
+        header.write_to_prefix(&mut data);
+        data
+    }
+}
+
+#[cfg(test)]
+mod test {
+    use super::StringBlock;
+
+    #[test]
+    fn test_string_block() {
+        let mut block = StringBlock::new();
+        assert_eq!(block.add("name1"), 0);
+        assert_eq!(block.add("name2"), 6);
+        assert_eq!(block.add("name1"), 0);
+        assert_eq!(block.add("name3"), 12);
+        let mut blob = vec![];
+        block.write_as_blob(&mut blob);
+        assert_eq!(blob, b"name1\0name2\0name3\0\0\0");
+    }
+}

alioth/src/firmware/firmware.rs

@@ -15,3 +15,5 @@
 #[path = "acpi/acpi.rs"]
 #[cfg(target_arch = "x86_64")]
 pub mod acpi;
+#[path = "dt/dt.rs"]
+pub mod dt;

alioth/src/utils/endian.rs

@@ -0,0 +1,106 @@
+// Copyright 2024 Google LLC
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     https://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+macro_rules! endian_impl {
+    ($ne_type:ident, $ed_type:ident, $endian:expr, $opposite:expr) => {
+        #[repr(transparent)]
+        #[derive(
+            ::zerocopy::AsBytes, ::zerocopy::FromBytes, ::zerocopy::FromZeroes, Copy, Clone,
+        )]
+        pub struct $ed_type {
+            v: $ne_type,
+        }
+
+        impl $ed_type {
+            pub fn to_ne(self) -> $ne_type {
+                #[cfg(target_endian = $endian)]
+                {
+                    self.v
+                }
+                #[cfg(target_endian = $opposite)]
+                {
+                    self.v.swap_bytes()
+                }
+            }
+        }
+
+        impl From<$ne_type> for $ed_type {
+            fn from(value: $ne_type) -> Self {
+                #[cfg(target_endian = $endian)]
+                {
+                    Self { v: value }
+                }
+                #[cfg(target_endian = $opposite)]
+                {
+                    Self {
+                        v: value.swap_bytes(),
+                    }
+                }
+            }
+        }
+
+        impl From<$ed_type> for $ne_type {
+            fn from(value: $ed_type) -> Self {
+                #[cfg(target_endian = $endian)]
+                {
+                    value.v
+                }
+                #[cfg(target_endian = $opposite)]
+                {
+                    value.v.swap_bytes()
+                }
+            }
+        }
+
+        impl ::core::fmt::Display for $ed_type {
+            fn fmt(&self, f: &mut ::core::fmt::Formatter<'_>) -> ::core::fmt::Result {
+                #[cfg(target_endian = $endian)]
+                {
+                    ::core::fmt::Display::fmt(&self.v, f)
+                }
+                #[cfg(target_endian = $opposite)]
+                {
+                    ::core::fmt::Display::fmt(&self.v.swap_bytes(), f)
+                }
+            }
+        }
+
+        impl ::core::fmt::Debug for $ed_type {
+            fn fmt(&self, f: &mut ::core::fmt::Formatter<'_>) -> ::core::fmt::Result {
+                f.write_str(stringify!($ed_type))?;
+                ::core::fmt::Write::write_char(f, '(')?;
+                ::core::fmt::Debug::fmt(&self.to_ne(), f)?;
+                ::core::fmt::Write::write_char(f, ')')
+            }
+        }
+
+        impl From<[u8; ::core::mem::size_of::<$ne_type>()]> for $ed_type {
+            fn from(value: [u8; ::core::mem::size_of::<$ne_type>()]) -> Self {
+                Self {
+                    v: $ne_type::from_ne_bytes(value),
+                }
+            }
+        }
+    };
+}
+
+macro_rules! endian_type {
+    ($ne_type:ident, $le_type:ident, $be_type:ident) => {
+        endian_impl!($ne_type, $le_type, "little", "big");
+        endian_impl!($ne_type, $be_type, "big", "little");
+    };
+}
+
+endian_type!(u32, Lu32, Bu32);
+endian_type!(u64, Lu64, Bu64);

alioth/src/utils/utils.rs

@@ -14,6 +14,7 @@
 
 use std::sync::atomic::{AtomicU64, Ordering};
 
+pub mod endian;
 #[cfg(target_os = "linux")]
 pub mod ioctls;