1 files changed, 167 insertions, 102 deletions

diff --git a/machine/fdt.c b/machine/fdt.c
index a040e87..5257f76 100644
--- a/machine/fdt.c
+++ b/machine/fdt.c
@@ -13,48 +13,59 @@ static inline uint32_t bswap(uint32_t x)
 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,
+  const struct fdt_cb *cb)
 {
-  struct fdt_scan_node node;
+  struct fdt_scan_node child;
   struct fdt_scan_prop prop;
+  int last = 0;
 
-  node.parent = parent;
-  node.base = lex;
-  node.name = name;
-  node.address_cells = 2;
-  node.size_cells = 1;
-  prop.node = &node;
+  child.parent = node;
+  child.address_cells = 2;
+  child.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);
+      case FDT_NOP: {
+        lex += 1;
         break;
       }
       case FDT_PROP: {
+        assert (!last);
         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]); }
+        if (node && !strcmp(prop.name, "#address-cells")) { node->address_cells = bswap(lex[3]); }
+        if (node && !strcmp(prop.name, "#size-cells"))    { node->size_cells    = bswap(lex[3]); }
         lex += 3 + (prop.len+3)/4;
-        cb(&prop, extra);
+        cb->prop(&prop, cb->extra);
         break;
       }
-      case FDT_END_NODE: return lex + 1;
-      case FDT_NOP:      lex += 1; break;
-      default:           return lex; // FDT_END
+      case FDT_BEGIN_NODE: {
+        if (!last && node && cb->done) cb->done(node, cb->extra);
+        last = 1;
+        child.name = (const char *)(lex+1);
+        if (cb->open) cb->open(&child, cb->extra);
+        lex = fdt_scan_helper(
+          lex + 2 + strlen(child.name)/4,
+          strings, &child, cb);
+        if (cb->close) cb->close(&child, cb->extra);
+        break;
+      }
+      case FDT_END_NODE: {
+        if (!last && node && cb->done) cb->done(node, cb->extra);
+        return lex + 1;
+      }
+      default: { // FDT_END
+        if (!last && node && cb->done) cb->done(node, cb->extra);
+        return lex;
+      }
     }
   }
 }
 
-void fdt_scan(uintptr_t fdt, fdt_cb cb, void *extra)
+void fdt_scan(uintptr_t fdt, const struct fdt_cb *cb)
 {
   struct fdt_header *header = (struct fdt_header *)fdt;
 
@@ -65,7 +76,7 @@ void fdt_scan(uintptr_t fdt, fdt_cb cb, void *extra)
   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);
+  fdt_scan_helper(lex, strings, 0, cb);
 }
 
 uint32_t fdt_size(uintptr_t fdt)
@@ -92,37 +103,71 @@ const uint32_t *fdt_get_size(const struct fdt_scan_node *node, const uint32_t *v
   return value;
 }
 
-static void find_mem(const struct fdt_scan_prop *prop, void *extra)
+//////////////////////////////////////////// MEMORY SCAN /////////////////////////////////////////
+
+struct mem_scan {
+  int memory;
+  const uint32_t *reg_value;
+  int reg_len;
+};
+
+static void mem_open(const struct fdt_scan_node *node, void *extra)
 {
-  const uint32_t **base = (const uint32_t **)extra;
+  struct mem_scan *scan = (struct mem_scan *)extra;
+  memset(scan, 0, sizeof(*scan));
+}
+
+static void mem_prop(const struct fdt_scan_prop *prop, void *extra)
+{
+  struct mem_scan *scan = (struct mem_scan *)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);
+    scan->memory = 1;
+  } else if (!strcmp(prop->name, "reg")) {
+    scan->reg_value = prop->value;
+    scan->reg_len = prop->len;
+  }
+}
+
+static void mem_done(const struct fdt_scan_node *node, void *extra)
+{
+  struct mem_scan *scan = (struct mem_scan *)extra;
+  const uint32_t *value = scan->reg_value;
+  const uint32_t *end = value + scan->reg_len/4;
+  uintptr_t self = (uintptr_t)mem_done;
+
+  if (!scan->memory) return;
+  assert (scan->reg_value && scan->reg_len % 4 == 0);
+
+  while (end - value > 0) {
+    uintptr_t base, size;
+    value = fdt_get_address(node->parent, value, &base);
+    value = fdt_get_size   (node->parent, value, &size);
+    if (base <= self && self <= base + size) { mem_size = size; }
   }
+  assert (end == value);
 }
 
 void query_mem(uintptr_t fdt)
 {
+  struct fdt_cb cb;
+  struct mem_scan scan;
+
+  memset(&cb, 0, sizeof(cb));
+  cb.open = mem_open;
+  cb.prop = mem_prop;
+  cb.done = mem_done;
+  cb.extra = &scan;
+
   mem_size = 0;
-  const uint32_t *base = 0;
-  fdt_scan(fdt, &find_mem, &base);
+  fdt_scan(fdt, &cb);
   assert (mem_size > 0);
 }
 
+///////////////////////////////////////////// HART SCAN //////////////////////////////////////////
+
 static uint32_t hart_phandles[MAX_HARTS];
 
 struct hart_scan {
-  const uint32_t *base;
   int cpu;
   int controller;
   int cells;
@@ -130,36 +175,17 @@ struct hart_scan {
   uint32_t phandle;
 };
 
-static void init_hart(struct hart_scan *scan, const uint32_t *base)
+static void hart_open(const struct fdt_scan_node *node, void *extra)
 {
-  scan->base = base;
-  scan->cpu = 0;
-  scan->controller = 0;
+  struct hart_scan *scan = (struct hart_scan *)extra;
+  memset(scan, 0, sizeof(*scan));
   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)
+static void hart_prop(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")) {
@@ -175,72 +201,111 @@ static void count_harts(const struct fdt_scan_prop *prop, void *extra)
   }
 }
 
+static void hart_done(const struct fdt_scan_node *node, void *extra)
+{
+  struct hart_scan *scan = (struct hart_scan *)extra;
+
+  if (!scan->cpu) return;
+  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;
+  }
+}
+
 void query_harts(uintptr_t fdt)
 {
+  struct fdt_cb cb;
   struct hart_scan scan;
+
+  memset(&cb, 0, sizeof(cb));
+  cb.open = hart_open;
+  cb.prop = hart_prop;
+  cb.done = hart_done;
+  cb.extra = &scan;
+
   num_harts = 0;
-  init_hart(&scan, 0);
-  fdt_scan(fdt, &count_harts, &scan);
-  done_hart(&scan);
+  fdt_scan(fdt, &cb);
   assert (num_harts > 0);
 }
 
+///////////////////////////////////////////// CLINT SCAN /////////////////////////////////////////
+
 struct clint_scan
 {
-  const uint32_t *base;
-  uintptr_t address;
+  int compat;
+  uintptr_t reg;
+  const uint32_t *int_value;
+  int int_len;
+  int done;
 };
 
-static void find_clint_base(const struct fdt_scan_prop *prop, void *extra)
+static void clint_open(const struct fdt_scan_node *node, 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;
+  scan->compat = 0;
+  scan->reg = 0;
+  scan->int_value = 0;
 }
 
-static void find_clint_reg(const struct fdt_scan_prop *prop, void *extra)
+static void clint_prop(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);
+  if (!strcmp(prop->name, "compatible") && !strcmp((const char*)prop->value, "riscv,clint0")) {
+    scan->compat = 1;
+  } else if (!strcmp(prop->name, "reg")) {
+    fdt_get_address(prop->node->parent, prop->value, &scan->reg);
+  } else if (!strcmp(prop->name, "interrupts-extended")) {
+    scan->int_value = prop->value;
+    scan->int_len = prop->len;
+  }
 }
 
-static void setup_clint(const struct fdt_scan_prop *prop, void *extra)
+static void clint_done(const struct fdt_scan_node *node, 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;
+  const uint32_t *value = scan->int_value;
+  const uint32_t *end = value + scan->int_len/4;
+
+  if (!scan->compat) return;
+  assert (scan->reg != 0);
+  assert (scan->int_value && scan->int_len % 16 == 0);
+  assert (!scan->done); // only one clint
+
+  scan->done = 1;
+  mtime = (void*)(scan->reg + 0xbff8);
+
+  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->reg + index * 4);
+      hls->timecmp = (void*)(scan->reg + 0x4000 + (index * 8));
+      *hls->ipi = 1; // wakeup the hart
     }
+    value += 4;
   }
 }
 
 void query_clint(uintptr_t fdt)
 {
+  struct fdt_cb cb;
   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);
+  memset(&cb, 0, sizeof(cb));
+  cb.open = clint_open;
+  cb.prop = clint_prop;
+  cb.done = clint_done;
+  cb.extra = &scan;
 
-  mtime = (void*)(scan.address + 0xbff8);
-  fdt_scan(fdt, &setup_clint, &scan);
+  scan.done = 0;
+  fdt_scan(fdt, &cb);
+  assert (scan.done);
 }