FDT fixes + driver handling

dev/dt/driver.c

@@ -0,0 +1,96 @@
+#define pr_fmt(fmt) "fdt: " fmt
+
+#include <zinnia/alloc.h>
+#include <zinnia/fdt.h>
+#include <zinnia/init.h>
+#include <zinnia/print.h>
+#include <zinnia/spin.h>
+#include <zinnia/vec.h>
+#include <string.h>
+
+static struct fdt tree;
+static bool tree_valid = false;
+
+static struct spinlock driver_lock = {};
+static vec(struct fdt_driver*) drivers = {};
+
+struct fdt* fdt_get() {
+    return tree_valid ? &tree : nullptr;
+}
+
+static const struct fdt_match* fdt_match_table(
+    const struct fdt_match* table,
+    size_t num_matches,
+    struct fdt_node node
+) {
+    for (size_t i = 0; i < num_matches; i++) {
+        if (fdt_node_is_compatible(node, table[i].compatible))
+            return &table[i];
+    }
+    return nullptr;
+}
+
+errno_t fdt_driver_register(struct fdt_driver* driver) {
+    if (driver->match == nullptr || driver->num_matches == 0) {
+        log_warn("Driver \"%s\" does not define any matches, ignoring\n", driver->name);
+        return 0;
+    }
+
+    if (!tree_valid)
+        return ENODEV;
+
+    spin_lock(&driver_lock);
+    vec_foreach(&drivers, i) {
+        if (strcmp(vec_elem_get(&drivers, i)->name, driver->name) == 0) {
+            spin_unlock(&driver_lock);
+            log_warn("Driver \"%s\" is already registered\n", driver->name);
+            return EEXIST;
+        }
+    }
+    errno_t status = vec_push(&drivers, driver);
+    spin_unlock(&driver_lock);
+    if (status != 0)
+        return status;
+
+    log("Registered platform driver \"%s\" with %zu match%s\n",
+        driver->name,
+        driver->num_matches,
+        driver->num_matches == 1 ? "" : "es");
+    for (size_t i = 0; i < driver->num_matches; i++)
+        log("  [%zu] = \"%s\"\n", i, driver->match[i].compatible);
+
+    for (struct fdt_node node = fdt_root(&tree); node.tree; node = fdt_next_node(node)) {
+        const struct fdt_match* match = fdt_match_table(driver->match, driver->num_matches, node);
+        if (match == nullptr)
+            continue;
+
+        log("Matched driver \"%s\" on rule [%zu]\n", driver->name, (size_t)(match - driver->match));
+
+        status = driver->probe(node, match);
+        if (status != 0)
+            log_warn("%s failed to probe: %i\n", driver->name, status);
+    }
+
+    return 0;
+}
+
+[[__init]]
+static bool fdt_stage_init(struct boot_info* info) {
+    if (!info->dtb)
+        return false;
+
+    const void* blob = HHDM_PTR(info->dtb);
+    if (!fdt_init(&tree, blob, info->dtb_size)) {
+        log_warn("Unable to parse the device tree blob\n");
+        return false;
+    }
+    tree_valid = true;
+
+    struct fdt_prop model = fdt_property(fdt_root(&tree), "model");
+    if (model.tree && model.length)
+        log("Running on \"%.*s\"\n", (int)model.length, (const char*)model.data);
+
+    return true;
+}
+INIT_TASK(fdt_stage, "dev.dt", fdt_stage_init);
+INIT_DEPENDS(fdt_stage, mem_stage);

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){};
 }

dev/dt/meson.build

@@ -1 +1,4 @@
-kernel_sources += files('fdt.c')
+kernel_sources += files(
+    'driver.c',
+    'fdt.c',
+)