alioth/src/firmware/dt/dtb.rs - PR #438

dtb.rs54.05%

// 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::{FromBytes, FromZeros, Immutable, IntoBytes};
 
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) {6x
    data.extend(s.as_bytes());6x
    let padding = align_up!(s.len() + 1, 2) - s.len();6x
    for _ in 0..padding {15x
        data.push(b'\0');15x
    }15x
}6x
 
fn pad_data(data: &mut Vec<u8>) {9x
    let padding = align_up!(data.len(), 2) - data.len();9x
    for _ in 0..padding {15x
        data.push(b'\0');15x
    }15x
}9x
 
impl PropVal {
    fn write_as_blob(&self, data: &mut Vec<u8>) {30x
        match self {30x
            PropVal::Empty => {}3x
            PropVal::U32(n) | PropVal::PHandle(n) => data.extend(n.to_be_bytes()),6x
            PropVal::U64(n) => data.extend(n.to_be_bytes()),3x
            PropVal::String(s) => push_string_align(data, s),3x
            PropVal::Str(s) => push_string_align(data, s),3x
            PropVal::Bytes(d) => {3x
                data.extend(d);3x
                pad_data(data);3x
            }3x
            PropVal::StringList(list) => {3x
                for s in list {6x
                    data.extend(s.as_bytes());6x
                    data.push(b'\0');6x
                }6x
                pad_data(data)3x
            }
            PropVal::U32List(reg) => {3x
                for v in reg {9x
                    data.extend(v.to_be_bytes())9x
                }
            }
            PropVal::U64List(reg) => {3x
                for v in reg {6x
                    data.extend(v.to_be_bytes())6x
                }
            }
        }
    }30x
}
 
#[repr(C)]
#[derive(IntoBytes, FromBytes, Immutable, 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(IntoBytes, FromBytes, Immutable, Debug)]
#[repr(C)]
pub struct FdtProp {
    len: Bu32,
    name_off: Bu32,
}
 
#[derive(IntoBytes, FromBytes, Immutable, 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 {3x
        StringBlock {3x
            total_size: 0,3x
            strings: HashMap::new(),3x
        }3x
    }3x
 
    fn add(&mut self, name: &'static str) -> usize {12x
        if let Some(offset) = self.strings.get(&name) {12x
            *offset3x
        } else {
            self.strings.insert(name, self.total_size);9x
            let ret = self.total_size;9x
            self.total_size += name.len() + 1;9x
            ret9x
        }
    }12x
 
    fn write_as_blob(self, data: &mut Vec<u8>) {3x
        let mut string_offset = self.strings.into_iter().collect::<Vec<_>>();3x
        string_offset.sort_by_key(|(_, offset)| *offset);3x
        for (s, _) in string_offset {9x
            data.extend(s.as_bytes());9x
            data.push(b'\0');9x
        }9x
        pad_data(data)3x
    }3x
}
 
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).unwrap();
        data
    }
}
 
#[cfg(test)]
#[path = "dtb_test.rs"]
mod tests;
 