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.

4 files changed, 142 insertions, 4 deletions

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(); })