machine: add FDT parser

4 files changed, 300 insertions, 7 deletions

diff --git a/machine/fdt.c b/machine/fdt.c
new file mode 100644
index 0000000..f297e52
--- /dev/null
+++ b/machine/fdt.c
@@ -0,0 +1,236 @@
+#include <stdint.h>
+#include <string.h>
+#include "fdt.h"
+#include "mtrap.h"
+
+static inline uint32_t bswap(uint32_t x)
+{
+  uint32_t y = (x & 0x00FF00FF) <<  8 | (x & 0xFF00FF00) >>  8;
+  uint32_t z = (y & 0x0000FFFF) << 16 | (y & 0xFFFF0000) >> 16;
+  return z;
+}
+
+static const uint32_t *fdt_scan_helper(
+  const uint32_t *lex,
+  const char *strings,
+  const char *name,
+  struct fdt_scan_node *parent,
+  fdt_cb cb,
+  void *extra)
+{
+  struct fdt_scan_node node;
+  struct fdt_scan_prop prop;
+
+  node.parent = parent;
+  node.base = lex;
+  node.name = name;
+  node.address_cells = 2;
+  node.size_cells = 1;
+  prop.node = &node;
+
+  while (1) {
+    switch (bswap(lex[0])) {
+      case FDT_BEGIN_NODE: {
+        const char *child_name = (const char *)(lex+1);
+        lex = fdt_scan_helper(
+          lex + 2 + strlen(child_name)/4,
+          strings, child_name, &node, cb, extra);
+        break;
+      }
+      case FDT_PROP: {
+        prop.name  = strings + bswap(lex[2]);
+        prop.len   = bswap(lex[1]);
+        prop.value = lex + 3;
+        if (!strcmp(prop.name, "#address-cells")) { node.address_cells = bswap(lex[3]); }
+        if (!strcmp(prop.name, "#size-cells"))    { node.size_cells    = bswap(lex[3]); }
+        lex += 3 + (prop.len+3)/4;
+        cb(&prop, extra);
+        break;
+      }
+      case FDT_END_NODE: return lex + 1;
+      case FDT_NOP:      lex += 1; break;
+      default:           return lex; // FDT_END
+    }
+  }
+}
+
+void fdt_scan(uintptr_t fdt, fdt_cb cb, void *extra)
+{
+  struct fdt_header *header = (struct fdt_header *)fdt;
+
+  // Only process FDT that we understand
+  if (bswap(header->magic) != FDT_MAGIC ||
+      bswap(header->last_comp_version) > FDT_VERSION) return;
+
+  const char *strings = (const char *)(fdt + bswap(header->off_dt_strings));
+  const uint32_t *lex = (const uint32_t *)(fdt + bswap(header->off_dt_struct));
+
+  fdt_scan_helper(lex, strings, "/", 0, cb, extra);
+}
+
+const uint32_t *fdt_get_address(const struct fdt_scan_node *node, const uint32_t *value, uintptr_t *result)
+{
+  *result = 0;
+  for (int cells = node->address_cells; cells > 0; --cells) *result += bswap(*value++);
+  return value;
+}
+
+const uint32_t *fdt_get_size(const struct fdt_scan_node *node, const uint32_t *value, uintptr_t *result)
+{
+  *result = 0;
+  for (int cells = node->size_cells; cells > 0; --cells) *result += bswap(*value++);
+  return value;
+}
+
+static void find_mem(const struct fdt_scan_prop *prop, void *extra)
+{
+  const uint32_t **base = (const uint32_t **)extra;
+  if (!strcmp(prop->name, "device_type") && !strcmp((const char*)prop->value, "memory")) {
+     *base = prop->node->base;
+  } else if (!strcmp(prop->name, "reg") && prop->node->base == *base) {
+    const uint32_t *value = prop->value;
+    const uint32_t *end = value + prop->len/4;
+    assert (prop->len % 4 == 0);
+    while (end - value > 0) {
+      uintptr_t base, size;
+      value = fdt_get_address(prop->node->parent, value, &base);
+      value = fdt_get_size   (prop->node->parent, value, &size);
+      if (base == DRAM_BASE) { mem_size = size; }
+    }
+    assert (end == value);
+  }
+}
+
+void query_mem(uintptr_t fdt)
+{
+  mem_size = 0;
+  const uint32_t *base = 0;
+  fdt_scan(fdt, &find_mem, &base);
+  assert (mem_size > 0);
+}
+
+static uint32_t hart_phandles[MAX_HARTS];
+
+struct hart_scan {
+  const uint32_t *base;
+  int cpu;
+  int controller;
+  int cells;
+  int hart;
+  uint32_t phandle;
+};
+
+static void init_hart(struct hart_scan *scan, const uint32_t *base)
+{
+  scan->base = base;
+  scan->cpu = 0;
+  scan->controller = 0;
+  scan->cells = -1;
+  scan->hart = -1;
+  scan->phandle = 0;
+}
+
+static void done_hart(struct hart_scan *scan)
+{
+  if (scan->cpu) {
+    assert (scan->controller == 1);
+    assert (scan->cells == 1);
+    assert (scan->hart >= 0);
+    if (scan->hart < MAX_HARTS) {
+      hart_phandles[scan->hart] = scan->phandle;
+      if (scan->hart >= num_harts) num_harts = scan->hart + 1;
+    }
+  }
+}
+
+static void count_harts(const struct fdt_scan_prop *prop, void *extra)
+{
+  struct hart_scan *scan = (struct hart_scan *)extra;
+  if (prop->node->base != scan->base) {
+    done_hart(scan);
+    init_hart(scan, prop->node->base);
+  }
+  if (!strcmp(prop->name, "device_type") && !strcmp((const char*)prop->value, "cpu")) {
+    scan->cpu = 1;
+  } else if (!strcmp(prop->name, "interrupt-controller")) {
+    scan->controller = 1;
+  } else if (!strcmp(prop->name, "#interrupt-cells")) {
+    scan->cells = bswap(prop->value[0]);
+  } else if (!strcmp(prop->name, "phandle")) {
+    scan->phandle = bswap(prop->value[0]);
+  } else if (!strcmp(prop->name, "reg")) {
+    uintptr_t reg;
+    fdt_get_address(prop->node->parent, prop->value, &reg);
+    scan->hart = reg;
+  }
+}
+
+void query_harts(uintptr_t fdt)
+{
+  struct hart_scan scan;
+  num_harts = 0;
+  init_hart(&scan, 0);
+  fdt_scan(fdt, &count_harts, &scan);
+  done_hart(&scan);
+  assert (num_harts > 0);
+}
+
+struct clint_scan
+{
+  const uint32_t *base;
+  uintptr_t address;
+};
+
+static void find_clint_base(const struct fdt_scan_prop *prop, void *extra)
+{
+  struct clint_scan *scan = (struct clint_scan *)extra;
+  if (!strcmp(prop->name, "compatible") && !strcmp((const char*)prop->value, "riscv,clint0"))
+     scan->base = prop->node->base;
+}
+
+static void find_clint_reg(const struct fdt_scan_prop *prop, void *extra)
+{
+  struct clint_scan *scan = (struct clint_scan *)extra;
+  if (!strcmp(prop->name, "reg") && scan->base == prop->node->base)
+    fdt_get_address(prop->node->parent, prop->value, &scan->address);
+}
+
+static void setup_clint(const struct fdt_scan_prop *prop, void *extra)
+{
+  struct clint_scan *scan = (struct clint_scan *)extra;
+  if (!strcmp(prop->name, "interrupts-extended") && scan->base == prop->node->base) {
+    const uint32_t *value = prop->value;
+    const uint32_t *end = value + prop->len/4;
+    assert (prop->len % 16 == 0);
+    for (int index = 0; end - value > 0; ++index) {
+      uint32_t phandle = bswap(value[0]);
+      int hart;
+      for (hart = 0; hart < MAX_HARTS; ++hart)
+        if (hart_phandles[hart] == phandle)
+          break;
+      if (hart < MAX_HARTS) {
+        hls_t *hls = hls_init(hart);
+        hls->ipi = (void*)(scan->address + index * 4);
+        hls->timecmp = (void*)(scan->address + 0x4000 + (index * 8));
+        *hls->ipi = 1; // wakeup the hart
+      }
+      value += 4;
+    }
+  }
+}
+
+void query_clint(uintptr_t fdt)
+{
+  struct clint_scan scan;
+  scan.base = 0;
+  scan.address = 0;
+
+  fdt_scan(fdt, &find_clint_base, &scan);
+  assert (scan.base != 0);
+
+  fdt_scan(fdt, &find_clint_reg,  &scan);
+  assert (scan.address != 0);
+
+  mtime = (void*)(scan.address + 0xbff8);
+  fdt_scan(fdt, &setup_clint, &scan);
+}
diff --git a/machine/fdt.h b/machine/fdt.h
new file mode 100644
index 0000000..71726ce
--- /dev/null
+++ b/machine/fdt.h
@@ -0,0 +1,54 @@
+#ifndef FDT_H
+#define FDT_H
+
+#define FDT_MAGIC	0xd00dfeed
+#define FDT_VERSION	17
+
+struct fdt_header {
+  uint32_t magic;
+  uint32_t totalsize;
+  uint32_t off_dt_struct;
+  uint32_t off_dt_strings;
+  uint32_t off_mem_rsvmap;
+  uint32_t version;
+  uint32_t last_comp_version; /* <= 17 */
+  uint32_t boot_cpuid_phys;
+  uint32_t size_dt_strings;
+  uint32_t size_dt_struct;
+};
+
+#define FDT_BEGIN_NODE	1
+#define FDT_END_NODE	2
+#define FDT_PROP	3
+#define FDT_NOP		4
+#define FDT_END		9
+
+struct fdt_scan_node {
+  const struct fdt_scan_node *parent;
+  const uint32_t *base; // token that began the node
+  const char *name;
+  int address_cells;
+  int size_cells;
+};
+
+struct fdt_scan_prop {
+  const struct fdt_scan_node *node;
+  const char *name;
+  const uint32_t *value;
+  int len; // in bytes of value
+};
+
+// Scan the contents of FDT
+typedef void (*fdt_cb)(const struct fdt_scan_prop *prop, void *extra);
+void fdt_scan(uintptr_t fdt, fdt_cb cb, void *extra);
+
+// Extract fields
+const uint32_t *fdt_get_address(const struct fdt_scan_node *node, const uint32_t *base, uintptr_t *value);
+const uint32_t *fdt_get_size(const struct fdt_scan_node *node, const uint32_t *base, uintptr_t *value);
+
+// Setup memory+clint+plic
+void query_mem(uintptr_t fdt);
+void query_harts(uintptr_t fdt);
+void query_clint(uintptr_t fdt);
+
+#endif
diff --git a/machine/machine.mk.in b/machine/machine.mk.in
index 6267f5f..10d85b4 100644
--- a/machine/machine.mk.in
+++ b/machine/machine.mk.in
@@ -4,6 +4,7 @@ machine_subproject_deps = \
 machine_hdrs = \
   atomic.h \
   bits.h \
+  fdt.h \
   emulation.h \
   encoding.h \
   fp_emulation.h \
@@ -14,6 +15,7 @@ machine_hdrs = \
   vm.h \
 
 machine_c_srcs = \
+  fdt.c \
   mtrap.c \
   minit.c \
   htif.c \
diff --git a/machine/minit.c b/machine/minit.c
index 76d9bae..2616f19 100644
--- a/machine/minit.c
+++ b/machine/minit.c
@@ -2,6 +2,7 @@
 #include "atomic.h"
 #include "vm.h"
 #include "fp_emulation.h"
+#include "fdt.h"
 #include <string.h>
 #include <limits.h>
 
@@ -71,11 +72,10 @@ hls_t* hls_init(uintptr_t id)
   return hls;
 }
 
+#define DTB 0x1014
+
 static void memory_init()
 {
-  // TODO FDT: mem_size
-  mem_size = 16 << 20;
-
   mem_size = mem_size / MEGAPAGE_SIZE * MEGAPAGE_SIZE;
 }
 
@@ -107,9 +107,6 @@ static void prci_test()
 
 static void hart_plic_init()
 {
-  // TODO FDT: mtime, ipi, timecmp
-  return;
-
   // clear pending interrupts
   *HLS()->ipi = 0;
   *HLS()->timecmp = -1ULL;
@@ -129,7 +126,11 @@ void init_first_hart()
 {
   hart_init();
   hls_init(0); // this might get called again from parse_config_string
-  //parse_config_string();
+
+  query_mem(DTB);
+  query_harts(DTB);
+  query_clint(DTB);
+
   plic_init();
   hart_plic_init();
   //prci_test();