Add the '--enable-print-device-tree' argument

I'm trying to debug some device tree problems while booting Linux and
figured it would be really nice to have access to the device tree while
trying to debug these problems.  I think this might be useful for lots
of people, so I went ahead and cleaned up the code enough that it should
actaully work in most cases.

8 files changed, 174 insertions, 7 deletions

diff --git a/bbl/bbl.c b/bbl/bbl.c
index b0a862d..7d1be2a 100644
--- a/bbl/bbl.c
+++ b/bbl/bbl.c
@@ -17,7 +17,7 @@ static uintptr_t dtb_output()
   return (end + MEGAPAGE_SIZE - 1) / MEGAPAGE_SIZE * MEGAPAGE_SIZE;
 }
 
-static void filter_dtb(uintptr_t source)
+static uintptr_t filter_dtb(uintptr_t source)
 {
   uintptr_t dest = dtb_output();
   uint32_t size = fdt_size(source);
@@ -28,6 +28,8 @@ static void filter_dtb(uintptr_t source)
   filter_plic(dest);
   filter_compat(dest, "riscv,clint0");
   filter_compat(dest, "riscv,debug-013");
+
+  return dest;
 }
 
 void boot_other_hart(uintptr_t dtb)
@@ -37,16 +39,19 @@ void boot_other_hart(uintptr_t dtb)
     entry = entry_point;
     mb();
   } while (!entry);
-  enter_supervisor_mode(entry, read_csr(mhartid), dtb_output());
+  enter_supervisor_mode(entry, read_csr(mhartid), dtb);
 }
 
 void boot_loader(uintptr_t dtb)
 {
   extern char _payload_start;
-  filter_dtb(dtb);
+  dtb = filter_dtb(dtb);
 #ifdef PK_ENABLE_LOGO
   print_logo();
 #endif
+#ifdef PK_PRINT_DEVICE_TREE
+  fdt_print(dtb);
+#endif
   mb();
   entry_point = &_payload_start;
   boot_other_hart(dtb);
diff --git a/config.h.in b/config.h.in
index 9557930..179cbf0 100644
--- a/config.h.in
+++ b/config.h.in
@@ -39,6 +39,9 @@
 /* Define if virtual memory support is enabled */
 #undef PK_ENABLE_VM
 
+/* Define if the DTS is to be displayed */
+#undef PK_PRINT_DEVICE_TREE
+
 /* Define if subproject MCPPBS_SPROJ_NORM is enabled */
 #undef PLATFORM_ENABLED
 
diff --git a/configure b/configure
index a9296e0..335fe2d 100755
--- a/configure
+++ b/configure
@@ -671,6 +671,7 @@ enable_option_checking
 enable_stow
 enable_32bit
 with_platform
+enable_print_device_tree
 enable_optional_subprojects
 enable_vm
 enable_logo
@@ -1318,6 +1319,8 @@ Optional Features:
   --enable-FEATURE[=ARG]  include FEATURE [ARG=yes]
   --enable-stow           Enable stow-based install
   --enable-32bit          Build a 32-bit pk
+  --enable-print-device-tree
+                          Print DTS when booting
   --enable-optional-subprojects
                           Enable all optional subprojects
   --disable-vm            Disable virtual memory
@@ -4103,6 +4106,19 @@ fi
 PLATFORM_NAME=$PLATFORM_NAME
 
 
+# Check whether --enable-print-device-tree was given.
+if test "${enable_print_device_tree+set}" = set; then :
+  enableval=$enable_print_device_tree;
+fi
+
+if test "x$enable_print_device_tree" == "xyes"; then :
+
+
+$as_echo "#define PK_PRINT_DEVICE_TREE /**/" >>confdefs.h
+
+
+fi
+
 
 
 LIBS="-lgcc"
diff --git a/configure.ac b/configure.ac
index 5813f3b..31b275f 100644
--- a/configure.ac
+++ b/configure.ac
@@ -105,6 +105,11 @@ AC_ARG_WITH([platform],
 	PLATFORM_NAME=spike)
 AC_SUBST(PLATFORM_NAME, $PLATFORM_NAME)
 
+AC_ARG_ENABLE([print-device-tree], AS_HELP_STRING([--enable-print-device-tree], [Print DTS when booting]))
+AS_IF([test "x$enable_print_device_tree" == "xyes"], [
+  AC_DEFINE([PK_PRINT_DEVICE_TREE],,[Define if the DTS is to be displayed])
+])
+
 AC_SUBST(CFLAGS)
 AC_SUBST(LDFLAGS)
 AC_SUBST([LIBS], ["-lgcc"])
diff --git a/machine/fdt.c b/machine/fdt.c
index 4f72f73..35681f8 100644
--- a/machine/fdt.c
+++ b/machine/fdt.c
@@ -1,5 +1,6 @@
 #include <stdint.h>
 #include <string.h>
+#include "config.h"
 #include "fdt.h"
 #include "mtrap.h"
 
@@ -10,6 +11,19 @@ static inline uint32_t bswap(uint32_t x)
   return z;
 }
 
+static inline int isstring(char c)
+{
+  if (c >= 'A' && c <= 'Z')
+    return 1;
+  if (c >= 'a' && c <= 'z')
+    return 1;
+  if (c >= '0' && c <= '9')
+    return 1;
+  if (c == '\0' || c == ' ' || c == ',' || c == '-')
+    return 1;
+  return 0;
+}
+
 static uint32_t *fdt_scan_helper(
   uint32_t *lex,
   const char *strings,
@@ -593,3 +607,116 @@ void filter_harts(uintptr_t fdt, unsigned long hart_mask)
   filter.mask = hart_mask;
   fdt_scan(fdt, &cb);
 }
+
+//////////////////////////////////////////// PRINT //////////////////////////////////////////////
+
+#ifdef PK_PRINT_DEVICE_TREE
+#define FDT_PRINT_MAX_DEPTH 32
+
+struct fdt_print_info {
+  int depth;
+  const struct fdt_scan_node *stack[FDT_PRINT_MAX_DEPTH];
+};
+
+void fdt_print_printm(struct fdt_print_info *info, const char *format, ...)
+{
+  va_list vl;
+
+  for (int i = 0; i < info->depth; ++i)
+    printm("  ");
+
+  va_start(vl, format);
+  vprintm(format, vl);
+  va_end(vl);
+}
+
+static void fdt_print_open(const struct fdt_scan_node *node, void *extra)
+{
+  struct fdt_print_info *info = (struct fdt_print_info *)extra;
+
+  while (node->parent != NULL && info->stack[info->depth-1] != node->parent) {
+    info->depth--;
+    fdt_print_printm(info, "}\r\n");
+  }
+
+  fdt_print_printm(info, "%s {\r\n", node->name);
+  info->stack[info->depth] = node;
+  info->depth++;
+}
+
+static void fdt_print_prop(const struct fdt_scan_prop *prop, void *extra)
+{
+  struct fdt_print_info *info = (struct fdt_print_info *)extra;
+  int asstring = 1;
+  char *char_data = (char *)(prop->value);
+
+  fdt_print_printm(info, "%s", prop->name);
+
+  if (prop->len == 0) {
+    printm(";\r\n");
+    return;
+  } else {
+    printm(" = ");
+  }
+
+  /* It appears that dtc uses a hueristic to detect strings so I'm using a
+   * similar one here. */
+  for (int i = 0; i < prop->len; ++i) {
+    if (!isstring(char_data[i]))
+      asstring = 0;
+    if (i > 0 && char_data[i] == '\0' && char_data[i-1] == '\0')
+      asstring = 0;
+  }
+
+  if (asstring) {
+    for (size_t i = 0; i < prop->len; i += strlen(char_data + i) + 1) {
+      if (i != 0)
+        printm(", ");
+      printm("\"%s\"", char_data + i);
+    }
+  } else {
+    printm("<");
+    for (size_t i = 0; i < prop->len/4; ++i) {
+      if (i != 0)
+        printm(" ");
+      printm("0x%08x", bswap(prop->value[i]));
+    }
+    printm(">");
+  }
+
+  printm(";\r\n");
+}
+
+static void fdt_print_done(const struct fdt_scan_node *node, void *extra)
+{
+  struct fdt_print_info *info = (struct fdt_print_info *)extra;
+}
+
+static int fdt_print_close(const struct fdt_scan_node *node, void *extra)
+{
+  struct fdt_print_info *info = (struct fdt_print_info *)extra;
+  return 0;
+}
+
+void fdt_print(uintptr_t fdt)
+{
+  struct fdt_print_info info;
+  struct fdt_cb cb;
+
+  info.depth = 0;
+
+  memset(&cb, 0, sizeof(cb));
+  cb.open = fdt_print_open;
+  cb.prop = fdt_print_prop;
+  cb.done = fdt_print_done;
+  cb.close = fdt_print_close;
+  cb.extra = &info;
+
+  fdt_scan(fdt, &cb);
+
+  while (info.depth > 0) {
+    info.depth--;
+    fdt_print_printm(&info, "}\r\n");
+  }
+}
+#endif
diff --git a/machine/fdt.h b/machine/fdt.h
index c12905e..5932e17 100644
--- a/machine/fdt.h
+++ b/machine/fdt.h
@@ -68,4 +68,9 @@ void filter_compat(uintptr_t fdt, const char *compat);
 // The hartids of available harts
 extern uint64_t hart_mask;
 
+#ifdef PK_PRINT_DEVICE_TREE
+// Prints the device tree to the console as a DTS
+void fdt_print(uintptr_t fdt);
+#endif
+
 #endif
diff --git a/machine/mtrap.c b/machine/mtrap.c
index 62137ff..a72b373 100644
--- a/machine/mtrap.c
+++ b/machine/mtrap.c
@@ -42,16 +42,20 @@ void putstring(const char* s)
     mcall_console_putchar(*s++);
 }
 
-void printm(const char* s, ...)
+void vprintm(const char* s, va_list vl)
 {
   char buf[256];
+  vsnprintf(buf, sizeof buf, s, vl);
+  putstring(buf);
+}
+
+void printm(const char* s, ...)
+{
   va_list vl;
 
   va_start(vl, s);
-  vsnprintf(buf, sizeof buf, s, vl);
+  vprintm(s, vl);
   va_end(vl);
-
-  putstring(buf);
 }
 
 static void send_ipi(uintptr_t recipient, int event)
diff --git a/machine/mtrap.h b/machine/mtrap.h
index 4ec0924..a15b265 100644
--- a/machine/mtrap.h
+++ b/machine/mtrap.h
@@ -13,6 +13,7 @@
 
 #include <stdint.h>
 #include <stddef.h>
+#include <stdarg.h>
 
 #define read_const_csr(reg) ({ unsigned long __tmp; \
   asm ("csrr %0, " #reg : "=r"(__tmp)); \
@@ -57,6 +58,7 @@ hls_t* hls_init(uintptr_t hart_id);
 void parse_config_string();
 void poweroff(void) __attribute((noreturn));
 void printm(const char* s, ...);
+void vprintm(const char *s, va_list args);
 void putstring(const char* s);
 #define assert(x) ({ if (!(x)) die("assertion failed: %s", #x); })
 #define die(str, ...) ({ printm("%s:%d: " str "\n", __FILE__, __LINE__, ##__VA_ARGS__); poweroff(); })