Initial commit.
diff --git a/src/fdt.c b/src/fdt.c
new file mode 100644
index 0000000..035ec64
--- /dev/null
+++ b/src/fdt.c
@@ -0,0 +1,327 @@
+#include "fdt.h"
+
+#include <stdint.h>
+
+#include "dlog.h"
+#include "std.h"
+
+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;
+ uint32_t boot_cpuid_phys;
+ uint32_t size_dt_strings;
+ uint32_t size_dt_struct;
+};
+
+struct fdt_reserve_entry {
+ uint64_t address;
+ uint64_t size;
+};
+
+enum fdt_token {
+ FDT_BEGIN_NODE = 1,
+ FDT_END_NODE = 2,
+ FDT_PROP = 3,
+ FDT_NOP = 4,
+ FDT_END = 9,
+};
+
+struct fdt_tokenizer {
+ const char *cur;
+ const char *end;
+ const char *strs;
+};
+
+#define FDT_VERSION 17
+#define FDT_MAGIC 0xd00dfeed
+
+static void fdt_tokenizer_init(struct fdt_tokenizer *t, const char *strs,
+ const char *begin, const char *end)
+{
+ t->strs = strs;
+ t->cur = begin;
+ t->end = end;
+}
+
+static void fdt_tokenizer_align(struct fdt_tokenizer *t)
+{
+ t->cur = (char *)(((size_t)t->cur + 3) & ~3);
+}
+
+static bool fdt_tokenizer_uint32(struct fdt_tokenizer *t, uint32_t *res)
+{
+ const char *next = t->cur + sizeof(*res);
+ if (next > t->end)
+ return false;
+
+ *res = ntohl(*(uint32_t *)t->cur);
+ t->cur = next;
+
+ return true;
+}
+
+static bool fdt_tokenizer_token(struct fdt_tokenizer *t, uint32_t *res)
+{
+ uint32_t v;
+
+ while (fdt_tokenizer_uint32(t, &v)) {
+ if (v != FDT_NOP) {
+ *res = v;
+ return true;
+ }
+ }
+ return false;
+}
+
+static bool fdt_tokenizer_bytes(struct fdt_tokenizer *t,
+ const char **res, size_t size)
+{
+ const char *next = t->cur + size;
+ if (next > t->end)
+ return false;
+
+ *res = t->cur;
+ t->cur = next;
+ fdt_tokenizer_align(t);
+
+ return true;
+}
+
+static bool fdt_tokenizer_str(struct fdt_tokenizer *t, const char **res)
+{
+ const char *p;
+ for (p = t->cur; p < t->end; p++) {
+ if (!*p) {
+ /* Found the end of the string. */
+ *res = t->cur;
+ t->cur = p + 1;
+ fdt_tokenizer_align(t);
+ return true;
+ }
+ }
+
+ return false;
+}
+
+void fdt_root_node(struct fdt_node *node, const struct fdt_header *hdr)
+{
+ uint32_t max_ver;
+ uint32_t min_ver;
+ uint32_t begin = ntohl(hdr->off_dt_struct);
+ uint32_t size = ntohl(hdr->size_dt_struct);
+
+ memset(node, 0, sizeof(*node));
+
+ /* Check the magic number before anything else. */
+ if (hdr->magic != ntohl(FDT_MAGIC))
+ return;
+
+ /* Check the version. */
+ max_ver = ntohl(hdr->version);
+ min_ver = ntohl(hdr->last_comp_version);
+ if (FDT_VERSION < min_ver || FDT_VERSION > max_ver)
+ return;
+
+ /* TODO: Verify that it is all within the fdt. */
+ node->begin = (const char *)hdr + begin;
+ node->end = node->begin + size;
+
+ /* TODO: Verify strings as well. */
+ node->strs = (char *)hdr + ntohl(hdr->off_dt_strings);
+}
+
+static bool fdt_next_property(struct fdt_tokenizer *t, const char **name,
+ const char **buf, uint32_t *size)
+{
+ uint32_t token;
+ uint32_t nameoff;
+
+ if (!fdt_tokenizer_token(t, &token))
+ return false;
+
+ if (token != FDT_PROP) {
+ /* Rewind so that caller will get the same token. */
+ t->cur -= sizeof(uint32_t);
+ return false;
+ }
+
+ if (!fdt_tokenizer_uint32(t, size) ||
+ !fdt_tokenizer_uint32(t, &nameoff) ||
+ !fdt_tokenizer_bytes(t, buf, *size)) {
+ /*
+ * Move cursor to the end so that caller won't get any new
+ * tokens.
+ */
+ t->cur = t->end;
+ return false;
+ }
+
+ /* TODO: Need to verify the strings. */
+ *name = t->strs + nameoff;
+
+ return true;
+}
+
+static bool fdt_next_subnode(struct fdt_tokenizer *t, const char **name)
+{
+ uint32_t token;
+
+ if (!fdt_tokenizer_token(t, &token))
+ return false;
+
+ if (token != FDT_BEGIN_NODE) {
+ /* Rewind so that caller will get the same token. */
+ t->cur -= sizeof(uint32_t);
+ return false;
+ }
+
+ if (!fdt_tokenizer_str(t, name)) {
+ /*
+ * Move cursor to the end so that caller won't get any new
+ * tokens.
+ */
+ t->cur = t->end;
+ return false;
+ }
+
+ return true;
+}
+
+static void fdt_skip_properties(struct fdt_tokenizer *t)
+{
+ const char *name;
+ const char *buf;
+ uint32_t size;
+ while (fdt_next_property(t, &name, &buf, &size));
+}
+
+static bool fdt_skip_node(struct fdt_tokenizer *t)
+{
+ const char *name;
+ uint32_t token;
+ size_t pending = 1;
+
+ fdt_skip_properties(t);
+
+ do {
+ while (fdt_next_subnode(t, &name)) {
+ fdt_skip_properties(t);
+ pending++;
+ }
+
+ if (!fdt_tokenizer_token(t, &token))
+ return false;
+
+ if (token != FDT_END_NODE) {
+ t->cur = t->end;
+ return false;
+ }
+
+ pending--;
+ } while (pending);
+
+ return true;
+}
+
+bool fdt_read_property(const struct fdt_node *node, const char *name,
+ const char **buf, uint32_t *size)
+{
+ struct fdt_tokenizer t;
+ const char *prop_name;
+
+ fdt_tokenizer_init(&t, node->strs, node->begin, node->end);
+
+ while (fdt_next_property(&t, &prop_name, buf, size)) {
+ if (!strcmp(prop_name, name))
+ return true;
+ }
+
+ return false;
+}
+
+bool fdt_find_child(struct fdt_node *node, const char *child)
+{
+ struct fdt_tokenizer t;
+ const char *name;
+
+ fdt_tokenizer_init(&t, node->strs, node->begin, node->end);
+
+ fdt_skip_properties(&t);
+
+ while (fdt_next_subnode(&t, &name)) {
+ if (!strcmp(name, child)) {
+ node->begin = t.cur;
+ return true;
+ }
+
+ fdt_skip_node(&t);
+ }
+
+ return false;
+}
+
+void fdt_dump(struct fdt_header *hdr)
+{
+ uint32_t token;
+ size_t depth = 0;
+ const char *name;
+ struct fdt_tokenizer t;
+ struct fdt_node node;
+
+ /* Traverse the whole thing. */
+ fdt_root_node(&node, hdr);
+
+ fdt_tokenizer_init(&t, node.strs, node.begin, node.end);
+
+ do {
+ while (fdt_next_subnode(&t, &name)) {
+ const char *buf;
+ uint32_t size;
+
+ dlog("%*sNew node: \"%s\"\n", 2 * depth, "", name);
+ depth++;
+ while (fdt_next_property(&t, &name, &buf, &size)) {
+ size_t i;
+ dlog("%*sproperty: \"%s\" (", 2 * depth, "", name);
+ for (i = 0; i < size; i++)
+ dlog("%s%02x", i == 0 ? "" : " ", buf[i]);
+ dlog(")\n");
+ }
+ }
+
+ if (!fdt_tokenizer_token(&t, &token))
+ return;
+
+ if (token != FDT_END_NODE)
+ return;
+
+ depth--;
+ } while (depth);
+
+ dlog("fdt: off_mem_rsvmap=%u\n", ntohl(hdr->off_mem_rsvmap));
+ {
+ struct fdt_reserve_entry *e = (struct fdt_reserve_entry *)((size_t)hdr + ntohl(hdr->off_mem_rsvmap));
+ while (e->address || e->size) {
+ dlog("Entry: %p (0x%x bytes)\n", ntohll(e->address), ntohll(e->size));
+ e++;
+ }
+ }
+}
+
+void fdt_add_mem_reservation(struct fdt_header *hdr, size_t addr, size_t len)
+{
+ /* TODO: Clean this up. */
+ char *begin = (char *)hdr + ntohl(hdr->off_mem_rsvmap);
+ struct fdt_reserve_entry *e = (struct fdt_reserve_entry *)begin;
+ hdr->totalsize = htonl(ntohl(hdr->totalsize) + sizeof(struct fdt_reserve_entry));
+ hdr->off_dt_struct = htonl(ntohl(hdr->off_dt_struct) + sizeof(struct fdt_reserve_entry));
+ hdr->off_dt_strings = htonl(ntohl(hdr->off_dt_strings) + sizeof(struct fdt_reserve_entry));
+ memmove(begin + sizeof(struct fdt_reserve_entry), begin, ntohl(hdr->totalsize) - ntohl(hdr->off_mem_rsvmap));
+ e->address = htonll(addr);
+ e->size = htonll(len);
+}