FDT fixes + driver handling

boot/limine/entry.c

@@ -1,6 +1,7 @@
 #include <zinnia/alloc.h>
 #include <zinnia/assert.h>
 #include <zinnia/cmdline.h>
+#include <zinnia/fdt.h>
 #include <zinnia/init.h>
 #include <zinnia/print.h>
 #include <zinnia/utils.h>
@@ -94,6 +95,12 @@ void kernel_entry() {
     info.virt_base = (uintptr_t)exec_res->virtual_base;
     info.hhdm_base = (uintptr_t)hhdm_res->offset;
 
+    const struct limine_dtb_response* dtb_res = dtb_request.response;
+    if (dtb_res && dtb_res->dtb_ptr) {
+        info.dtb = (phys_t)((uintptr_t)dtb_res->dtb_ptr - hhdm_res->offset);
+        info.dtb_size = fdt_read_u32((const uint8_t*)dtb_res->dtb_ptr + 4);
+    }
+
     const struct limine_framebuffer_response* fb_res = framebuffer_request.response;
     if (fb_res) {
         // Only use the first framebuffer.

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) {
+struct fdt_node fdt_next_node(struct fdt_node node) {
-    if (!tree || !tree->blob || !compatible)
-        return (struct fdt_node){};
     struct token token;
-    for (size_t offset = 0; read_token(tree, offset, &token); offset = token.next) {
+    if (!node.tree || !read_token(node.tree, node.offset, &token))
-        if (token.kind != FDT_BEGIN_NODE)
+        return (struct fdt_node){};
-            continue;
+    for (size_t offset = token.next; read_token(node.tree, offset, &token); offset = token.next) {
-        struct fdt_node node = {tree, offset};
+        if (token.kind == FDT_BEGIN_NODE)
-        struct fdt_prop property = fdt_property(node, "compatible");
+            return (struct fdt_node){node.tree, offset};
-        const char* data = property.data;
+        if (token.kind == FDT_END)
-        for (size_t i = 0; i < property.length;) {
+            break;
-            size_t length = strnlen(data + i, property.length - i);
+    }
-            if (length == property.length - i)
+    return (struct fdt_node){};
-                break;
+}
-            if (!strcmp(data + i, compatible))
+
-                return node;
+bool fdt_node_is_compatible(struct fdt_node node, const char* compatible) {
-            i += length + 1;
+    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',
+)

include/zinnia/fdt.h

@@ -1,5 +1,8 @@
 #pragma once
 
+#include <zinnia/errno.h>
+#include <zinnia/initgraph.h>
+#include <zinnia/types.h>
 #include <stddef.h>
 #include <stdint.h>
 
@@ -28,8 +31,6 @@ struct fdt_match {
     uintptr_t data;
 };
 
-#define FDT_MATCH_COMPATIBLE(compat) .compatible = (compat)
-
 struct fdt {
     const uint8_t* blob;
     size_t total_size;
@@ -65,3 +66,21 @@ bool fdt_property_u64(struct fdt_node node, const char* name, uint64_t* value);
 
 // Root of the device tree.
 struct fdt_node fdt_root(struct fdt* tree);
+
+struct fdt_node fdt_next_node(struct fdt_node node);
+bool fdt_node_is_compatible(struct fdt_node node, const char* compatible);
+bool fdt_node_reg(struct fdt_node node, size_t index, phys_t* addr, size_t* size);
+
+struct fdt_driver {
+    const char* name;
+
+    const struct fdt_match* match;
+    const size_t num_matches;
+
+    errno_t (*probe)(struct fdt_node node, const struct fdt_match* match);
+};
+
+struct fdt* fdt_get();
+errno_t fdt_driver_register(struct fdt_driver* driver);
+
+INIT_EXTERN(fdt_stage);

include/zinnia/init.h

@@ -39,7 +39,7 @@ struct boot_info {
     size_t num_files;
     struct boot_framebuffer fb;
     struct boot_file kernel_file;
-    void* dtb;
+    phys_t dtb;
     size_t dtb_size;
 #if CONFIG_ACPI
     phys_t rsdp;