From a94fa77621f760e99af8606e2a1f7f20aba1d408 Mon Sep 17 00:00:00 2001
From: Michael Zhao <michael.zhao@arm.com>
Date: Wed, 16 Jun 2021 12:45:32 +0800
Subject: [PATCH] arch: Add logging for FDT debugging on AArch64

To debug the FDT (Flattened Device Tree), we usually need to modify
source code to save the generted DTB data to disk, and use 'dtc' command
to decode the binary file into a text file to analyze.

It would be ideal if the FDT content can be seen in log.

This commit makes it real by:
- Introducing 'fdt' crate for parsing FDT.
- Printing the content of the FDT in tree view.

The parsing and printing only happen when Debug level logging enabled.

Signed-off-by: Michael Zhao <michael.zhao@arm.com>
---
 arch/Cargo.toml         |  4 +-
 arch/src/aarch64/fdt.rs | 83 +++++++++++++++++++++++++++++++++++++++++
 arch/src/aarch64/mod.rs |  5 +++
 3 files changed, 91 insertions(+), 1 deletion(-)

diff --git a/arch/Cargo.toml b/arch/Cargo.toml
index 6b1b6377a..fa65bf3e4 100644
--- a/arch/Cargo.toml
+++ b/arch/Cargo.toml
@@ -21,7 +21,9 @@ serde = {version = ">=1.0.27", features = ["rc"] }
 thiserror = "1.0"
 versionize = "0.1.6"
 versionize_derive = "0.1.4"
-vm-fdt = { git = "https://github.com/rust-vmm/vm-fdt", branch = "master" }
 vm-memory = { version = "0.5.0", features = ["backend-mmap", "backend-bitmap"] }
 vm-migration = { path = "../vm-migration" }
 
+[target.'cfg(target_arch = "aarch64")'.dependencies]
+fdt_parser = { version = "0.1.3", package = 'fdt'}
+vm-fdt = { git = "https://github.com/rust-vmm/vm-fdt", branch = "master" }
diff --git a/arch/src/aarch64/fdt.rs b/arch/src/aarch64/fdt.rs
index ba8c7c39d..1b74f4e61 100644
--- a/arch/src/aarch64/fdt.rs
+++ b/arch/src/aarch64/fdt.rs
@@ -6,10 +6,13 @@
 // Use of this source code is governed by a BSD-style license that can be
 // found in the THIRD-PARTY file.
 
+use byteorder::{BigEndian, ByteOrder};
+use std::cmp;
 use std::collections::HashMap;
 use std::ffi::CStr;
 use std::fmt::Debug;
 use std::result;
+use std::str;
 
 use super::super::DeviceType;
 use super::super::GuestMemoryMmap;
@@ -478,3 +481,83 @@ fn create_pci_nodes(
 
     Ok(())
 }
+
+// Parse the DTB binary and print for debugging
+pub fn print_fdt(dtb: &[u8]) {
+    match fdt_parser::Fdt::new(dtb) {
+        Ok(fdt) => {
+            if let Some(root) = fdt.find_node("/") {
+                debug!("Printing the FDT:");
+                print_node(root, 0);
+            } else {
+                debug!("Failed to find root node in FDT for debugging.");
+            }
+        }
+        Err(_) => debug!("Failed to parse FDT for debugging."),
+    }
+}
+
+fn print_node(node: fdt_parser::node::FdtNode<'_, '_>, n_spaces: usize) {
+    debug!("{:indent$}{}/", "", node.name, indent = n_spaces);
+    for property in node.properties() {
+        let name = property.name;
+
+        // If the property is 'compatible', its value requires special handling.
+        // The u8 array could contain multiple null-terminated strings.
+        // We copy the original array and simply replace all 'null' characters with spaces.
+        let value = if name == "compatible" {
+            let mut compatible = vec![0u8; 256];
+            let handled_value = property
+                .value
+                .iter()
+                .map(|&c| if c == 0 { b' ' } else { c })
+                .collect::<Vec<_>>();
+            let len = cmp::min(255, handled_value.len());
+            compatible[..len].copy_from_slice(&handled_value[..len]);
+            compatible[..(len + 1)].to_vec()
+        } else {
+            property.value.to_vec()
+        };
+        let value = &value;
+
+        // Now the value can be either:
+        //   - A null-terminated C string, or
+        //   - Binary data
+        // We follow a very simple logic to present the value:
+        //   - At first, try to convert it to CStr and print,
+        //   - If failed, print it as u32 array.
+        let value_result = match CStr::from_bytes_with_nul(value) {
+            Ok(value_cstr) => match value_cstr.to_str() {
+                Ok(value_str) => Some(value_str),
+                Err(_e) => None,
+            },
+            Err(_e) => None,
+        };
+
+        if let Some(value_str) = value_result {
+            debug!(
+                "{:indent$}{} : {:#?}",
+                "",
+                name,
+                value_str,
+                indent = (n_spaces + 2)
+            );
+        } else {
+            let mut array = Vec::with_capacity(256);
+            array.resize(value.len() / 4, 0u32);
+            BigEndian::read_u32_into(value, &mut array);
+            debug!(
+                "{:indent$}{} : {:X?}",
+                "",
+                name,
+                array,
+                indent = (n_spaces + 2)
+            );
+        };
+    }
+
+    // Print children nodes if there is any
+    for child in node.children() {
+        print_node(child, n_spaces + 2);
+    }
+}
diff --git a/arch/src/aarch64/mod.rs b/arch/src/aarch64/mod.rs
index e57f0a471..01fe78ced 100644
--- a/arch/src/aarch64/mod.rs
+++ b/arch/src/aarch64/mod.rs
@@ -18,6 +18,7 @@ use crate::DeviceType;
 use crate::GuestMemoryMmap;
 use crate::RegionType;
 use gic::GicDevice;
+use log::{log_enabled, Level};
 use std::collections::HashMap;
 use std::convert::TryInto;
 use std::ffi::CStr;
@@ -154,6 +155,10 @@ pub fn configure_system<T: DeviceInfoForFdt + Clone + Debug, S: ::std::hash::Bui
     )
     .map_err(|_| Error::SetupFdt)?;
 
+    if log_enabled!(Level::Debug) {
+        fdt::print_fdt(&fdt_final);
+    }
+
     fdt::write_fdt_to_memory(fdt_final, guest_mem).map_err(Error::WriteFdtToMemory)?;
 
     Ok(())