FDT fixes + driver handling

dev/dt/fdt.c

@@ -212,24 +212,70 @@ bool fdt_property_u64(struct fdt_node node, const char* name, uint64_t* value) {
     return true;
 }
 
-struct fdt_node fdt_find_compatible(struct fdt* tree, const char* compatible) {
-    if (!tree || !tree->blob || !compatible)
-        return (struct fdt_node){};
+struct fdt_node fdt_next_node(struct fdt_node node) {
     struct token token;
-    for (size_t offset = 0; read_token(tree, offset, &token); offset = token.next) {
-        if (token.kind != FDT_BEGIN_NODE)
-            continue;
-        struct fdt_node node = {tree, offset};
-        struct fdt_prop property = fdt_property(node, "compatible");
-        const char* data = property.data;
-        for (size_t i = 0; i < property.length;) {
-            size_t length = strnlen(data + i, property.length - i);
-            if (length == property.length - i)
-                break;
-            if (!strcmp(data + i, compatible))
-                return node;
-            i += length + 1;
-        }
+    if (!node.tree || !read_token(node.tree, node.offset, &token))
+        return (struct fdt_node){};
+    for (size_t offset = token.next; read_token(node.tree, offset, &token); offset = token.next) {
+        if (token.kind == FDT_BEGIN_NODE)
+            return (struct fdt_node){node.tree, offset};
+        if (token.kind == FDT_END)
+            break;
+    }
+    return (struct fdt_node){};
+}
+
+bool fdt_node_is_compatible(struct fdt_node node, const char* compatible) {
+    struct fdt_prop property = fdt_property(node, "compatible");
+    const char* data = property.data;
+    for (size_t i = 0; i < property.length;) {
+        size_t length = strnlen(data + i, property.length - i);
+        if (length == property.length - i)
+            break;
+        if (!strcmp(data + i, compatible))
+            return true;
+        i += length + 1;
+    }
+    return false;
+}
+
+static uint64_t read_cells(const uint8_t* data, uint32_t cells) {
+    uint64_t value = 0;
+    for (uint32_t i = 0; i < cells; i++)
+        value = value << 32 | fdt_read_u32(data + i * 4);
+    return value;
+}
+
+bool fdt_node_reg(struct fdt_node node, size_t index, phys_t* addr, size_t* size) {
+    if (!node.tree)
+        return false;
+    struct fdt_node root = fdt_root(node.tree);
+    uint32_t addr_cells = 2;
+    uint32_t size_cells = 2;
+    fdt_property_u32(root, "#address-cells", &addr_cells);
+    fdt_property_u32(root, "#size-cells", &size_cells);
+    if (addr_cells > 2 || size_cells > 2)
+        return false;
+
+    struct fdt_prop reg = fdt_property(node, "reg");
+    const size_t stride = (addr_cells + size_cells) * 4;
+    if (!reg.tree || index >= reg.length / stride)
+        return false;
+
+    const uint8_t* entry = (const uint8_t*)reg.data + index * stride;
+    if (addr)
+        *addr = read_cells(entry, addr_cells);
+    if (size)
+        *size = read_cells(entry + addr_cells * 4, size_cells);
+    return true;
+}
+
+struct fdt_node fdt_find_compatible(struct fdt* tree, const char* compatible) {
+    if (!tree || !tree->blob || !compatible)
+        return (struct fdt_node){};
+    for (struct fdt_node node = fdt_root(tree); node.tree; node = fdt_next_node(node)) {
+        if (fdt_node_is_compatible(node, compatible))
+            return node;
     }
     return (struct fdt_node){};
 }