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