Feat: lab 4

kernel/lib/dtb.c

@@ -0,0 +1,110 @@
+#include <logger.h>
+#include <errcode.h>
+#include <dtb.h>
+#include <utils.h>
+#include <string.h>
+#include <kmalloc.h>
+#include <vector.h>
+
+void *dtb_start;
+void *dtb_end;
+void *dtb_memory_start;
+void *dtb_memory_end;
+vector_t *dtb_reserved_entries;
+
+fdt_reserve_entry_t *fdt_memory_cur;
+uint32_t *fdt_struct_cur;
+
+static inline
+void _fdt_struct_callback(const char *path, const vector_t *cbs, const vector_t *props)
+{
+  for (int i = 0; i < (int)cbs->size; ++i)
+    if (!strcmp(path, VEC_AT(fdt_callback_t, cbs, i)->name))
+      VEC_AT(fdt_callback_t, cbs, i)->func(props);
+}
+
+static inline
+void _memory_traverse()
+{
+  dtb_reserved_entries = make_vector(0);
+  while (fdt_memory_cur->address != 0 || fdt_memory_cur->size != 0) {
+    DEBUG(ntoh64(fdt_memory_cur->address));
+    DEBUG(ntoh64(fdt_memory_cur->size));
+
+    VEC_PUSH(dtb_reserved_entries, fdt_memory_cur++);
+  }
+}
+
+#define nextnode(ptr) (void *)((((uint64_t)(ptr) >> 2) + 1) << 2)
+static inline
+void _struct_traverse(const char *path, const vector_t *cbs)
+{
+  vector_t *props = make_vector(0);
+
+  fdt_prop_t *prop;
+  const fdt_prop_header_t *prop_header;
+  const char *fdt_node_path = 0x0;
+
+  for (;;) {
+    fdt_struct_cur = ALIGN4(fdt_struct_cur);
+
+    switch (ntoh32(*fdt_struct_cur)) {
+    case FDT_BEGIN_NODE:
+      ++fdt_struct_cur;
+      fdt_node_path = (void *)fdt_struct_cur;
+      do {
+        BUMP(uint32_t, char, fdt_struct_cur);
+      } while (*(char *)fdt_struct_cur != '\0');
+      BUMP(uint32_t, char, fdt_struct_cur); // '\0'
+
+      fdt_struct_cur = ALIGN4(fdt_struct_cur);
+      _struct_traverse(fdt_node_path, cbs);
+      break;
+    case FDT_END_NODE:
+      fdt_struct_cur = nextnode(fdt_struct_cur);
+      goto traverse_struct_callback;
+    case FDT_PROP:
+      ++fdt_struct_cur;
+      prop_header = BUMP(uint32_t, fdt_prop_header_t, fdt_struct_cur);
+
+      prop = kmalloc(sizeof(fdt_prop_t));
+      prop->len = ntoh32(prop_header->len);
+      prop->name = dtb_start + 
+                   ntoh32(((fdt_header_t *)dtb_start)->off_dt_strings) +
+                   ntoh32(prop_header->nameoff);
+      prop->value = (void *)fdt_struct_cur;
+      VEC_PUSH(props, prop);
+
+      fdt_struct_cur = (void *)((uint64_t)fdt_struct_cur + 
+                                ntoh32(prop_header->len));
+      fdt_struct_cur = ALIGN4(fdt_struct_cur);
+      break;
+    case FDT_NOP:
+      fdt_struct_cur = nextnode(fdt_struct_cur);
+      break;
+    case FDT_END:
+      return;
+    default:
+      exit(ERR_UNREACHABLE);
+    }
+  }
+traverse_struct_callback:
+  _fdt_struct_callback(path, cbs, props);
+}
+
+void fdt_traverse(const vector_t *struct_cbs)
+{
+  fdt_memory_cur = dtb_start + 
+    ntoh32(((fdt_header_t *)dtb_start)->off_mem_rsvmap);
+  fdt_struct_cur = dtb_start + 
+    ntoh32(((fdt_header_t *)dtb_start)->off_dt_struct);
+  dtb_end = dtb_start +
+    ntoh32(((fdt_header_t *)dtb_start)->totalsize);
+
+  DEBUG(fdt_memory_cur);
+  DEBUG(fdt_struct_cur);
+  _memory_traverse();
+  DEBUG_DTB("memory traverse done");
+  _struct_traverse("", struct_cbs);
+  DEBUG_DTB("struct traverse done");
+}