X-Git-Url: https://scm.lunaixsky.com/lunaix-os.git/blobdiff_plain/47c4e0c19ae8526b14ce4e0d7b243f7a4dc6fafd..bffa3430fbbaaad29bec0b5bee9c1f0bfc7fd068:/lunaix-os/hal/devtree.c

diff --git a/lunaix-os/hal/devtree.c b/lunaix-os/hal/devtree.c
new file mode 100644
index 0000000..81712ec
--- /dev/null
+++ b/lunaix-os/hal/devtree.c
@@ -0,0 +1,629 @@
+#include <lunaix/mm/valloc.h>
+#include <lunaix/syslog.h>
+
+#include <klibc/string.h>
+
+#include <hal/devtree.h>
+
+LOG_MODULE("dtb")
+
+static struct dt_context dtctx;
+
+void 
+fdt_itbegin(struct fdt_iter* fdti, struct fdt_header* fdt_hdr)
+{
+    unsigned int off_struct, off_str;
+    struct fdt_token* tok;
+    const char* str_blk;
+
+    off_str    = le(fdt_hdr->off_dt_strings);
+    off_struct = le(fdt_hdr->off_dt_struct);
+
+    tok = offset_t(fdt_hdr, struct fdt_token, off_struct);
+    str_blk = offset_t(fdt_hdr, const char, off_str);
+    
+    *fdti = (struct fdt_iter) {
+        .pos = tok,
+        .str_block = str_blk
+    };
+}
+
+void 
+fdt_itend(struct fdt_iter* fdti)
+{
+    fdti->pos = NULL;
+}
+
+bool
+fdt_itnext(struct fdt_iter* fdti)
+{
+    struct fdt_token *current;
+    struct fdt_prop  *prop;
+
+    current = fdti->pos;
+    if (!current) {
+        return false;
+    }
+
+    do
+    {
+        if (fdt_nope(current)) {
+            continue;
+        }
+
+        if (fdt_prop(current)) {
+            prop    = (struct fdt_prop*) current;
+            current = offset(current, prop->len);
+            continue;
+        }
+        
+        if (fdt_node_end(current)) {
+            fdti->depth--;
+            continue;
+        }
+        
+        // node begin
+
+        fdti->depth++;
+        if (fdti->depth == 1) {
+            // enter root node
+            break;
+        }
+
+        while (!fdt_prop(current) && !fdt_node_end(current)) {
+            current++;
+        }
+
+        if (fdt_prop(current)) {
+            break;
+        }
+        
+        current++;
+        
+    } while (fdt_nope(current) && fdti->depth > 0);
+
+    return fdti->depth > 0;
+}
+
+bool 
+fdt_itnext_at(struct fdt_iter* fdti, int level)
+{
+    while (fdti->depth != level && fdt_itnext(fdti));
+    
+    return fdti->depth == level;
+}
+
+void
+fdt_memrsvd_itbegin(struct fdt_memrsvd_iter* rsvdi, 
+                    struct fdt_header* fdt_hdr)
+{
+    size_t off = le(fdt_hdr->off_mem_rsvmap);
+    
+    rsvdi->block = 
+        offset_t(fdt_hdr, typeof(*rsvdi->block), off);
+    
+    rsvdi->block = &rsvdi->block[-1];
+}
+
+bool
+fdt_memrsvd_itnext(struct fdt_memrsvd_iter* rsvdi)
+{
+    struct fdt_memrsvd_ent* ent;
+
+    ent = rsvdi->block;
+    if (!ent) {
+        return false;
+    }
+
+    rsvdi->block++;
+
+    return ent->addr || ent->size;
+}
+
+void
+fdt_memrsvd_itend(struct fdt_memrsvd_iter* rsvdi)
+{
+    rsvdi->block = NULL;
+}
+
+static inline bool
+propeq(struct fdt_iter* it, const char* key)
+{
+    return streq(fdtit_prop_key(it), key);
+}
+
+static inline void
+__mkprop_val32(struct fdt_iter* it, struct dt_prop_val* val)
+{
+    val->u32_val = le(*(u32_t*)&it->prop[1]);
+    val->size = le(it->prop->len);
+}
+
+static inline void
+__mkprop_val64(struct fdt_iter* it, struct dt_prop_val* val)
+{
+    val->u64_val = le64(*(u64_t*)&it->prop[1]);
+    val->size = le(it->prop->len);
+}
+
+static inline void
+__mkprop_ptr(struct fdt_iter* it, struct dt_prop_val* val)
+{
+    val->ptr_val = __ptr(&it->prop[1]);
+    val->size = le(it->prop->len);
+}
+
+static inline u32_t
+__prop_getu32(struct fdt_iter* it)
+{
+    return le(*(u32_t*)&it->prop[1]);
+}
+
+static bool
+__parse_stdbase_prop(struct fdt_iter* it, struct dt_node_base* node)
+{
+    struct fdt_prop* prop;
+
+    prop = it->prop;
+
+    if (propeq(it, "compatible")) {
+        __mkprop_ptr(it, &node->compat);
+    } 
+    
+    else if (propeq(it, "model")) {
+        node->model = (const char*)&prop[1];
+    } 
+
+    else if (propeq(it, "phandle")) {
+        node->phandle = __prop_getu32(it);
+        hashtable_hash_in(dtctx.phnds_table, 
+                          &node->phnd_link, node->phandle);
+    }
+    
+    else if (propeq(it, "#address-cells")) {
+        node->addr_c = (char)__prop_getu32(it);
+    } 
+    
+    else if (propeq(it, "#size-cells")) {
+        node->sz_c = (char)__prop_getu32(it);
+    } 
+    
+    else if (propeq(it, "#interrupt-cells")) {
+        node->intr_c = (char)__prop_getu32(it);
+    } 
+    
+    else if (propeq(it, "status")) {
+        char peek = *(char*)&it->prop[1];
+        if (peek == 'o') {
+            node->status = STATUS_OK;
+        }
+        else if (peek == 'r') {
+            node->status = STATUS_RSVD;
+        }
+        else if (peek == 'd') {
+            node->status = STATUS_DISABLE;
+        }
+        else if (peek == 'f') {
+            node->status = STATUS_FAIL;
+        }
+    }
+
+    else {
+        return false;
+    }
+
+    return true;
+}
+
+static bool
+__parse_stdnode_prop(struct fdt_iter* it, struct dt_node* node)
+{
+    if (propeq(it, "reg")) {
+        __mkprop_ptr(it, &node->reg);
+    }
+
+    else if (propeq(it, "virtual-reg")) {
+        __mkprop_ptr(it, &node->vreg);
+    }
+
+    else if (propeq(it, "ranges")) {
+        __mkprop_ptr(it, &node->ranges);
+    }
+
+    else if (propeq(it, "dma-ranges")) {
+        __mkprop_ptr(it, &node->dma_ranges);
+    }
+
+    else {
+        return false;
+    }
+
+    return true;
+}
+
+static bool
+__parse_stdintr_prop(struct fdt_iter* it, struct dt_intr_node* node)
+{
+    if (propeq(it, "interrupt-map")) {
+        __mkprop_ptr(it, &node->intr_map);
+    }
+
+    else if (propeq(it, "interrupt-map-mask")) {
+        __mkprop_ptr(it, &node->intr_map_mask);
+    }
+
+    else if (propeq(it, "interrupt-parent")) {
+        node->parent_hnd = __prop_getu32(it);
+    }
+
+    else if (propeq(it, "interrupt-extended")) {
+        node->intr.extended = true;
+        __mkprop_ptr(it, &node->intr.arr);
+    }
+
+    else if (!node->intr.extended && propeq(it, "interrupts")) {
+        __mkprop_ptr(it, &node->intr.arr);
+    }
+
+    else {
+        return false;
+    }
+
+    return true;
+}
+
+static bool
+__parse_stdflags(struct fdt_iter* it, struct dt_node_base* node)
+{
+    if (propeq(it, "dma-coherent")) {
+        node->dma_coherent = true;
+    }
+
+    else if (propeq(it, "dma-noncoherent")) {
+        node->dma_ncoherent = true;
+    }
+
+    else if (propeq(it, "interrupt-controller")) {
+        node->intr_controll = true;
+    }
+
+    else {
+        return false;
+    }
+
+    return true;
+}
+
+static void
+__parse_other_prop(struct fdt_iter* it, struct dt_node_base* node)
+{
+    struct dt_prop* prop;
+    const char* key;
+    unsigned int hash;
+
+    prop = valloc(sizeof(*prop));
+    key  = fdtit_prop_key(it);
+
+    prop->key = HSTR(key, strlen(key));
+    __mkprop_ptr(it, &prop->val);
+
+    hstr_rehash(&prop->key, HSTR_FULL_HASH);
+    hash = prop->key.hash;
+
+    hashtable_hash_in(node->_op_bucket, &prop->ht, hash);
+}
+
+static void
+__fill_node(struct fdt_iter* it, struct dt_node* node)
+{
+    if (__parse_stdflags(it, &node->base)) {
+        return;
+    }
+
+    if (__parse_stdbase_prop(it, &node->base)) {
+        return;
+    }
+
+    if (__parse_stdnode_prop(it, node)) {
+        return;
+    }
+
+    if (__parse_stdintr_prop(it, &node->intr)) {
+        return;
+    }
+
+    __parse_other_prop(it, &node->base);
+}
+
+static void
+__fill_root(struct fdt_iter* it, struct dt_root* node)
+{
+    if (__parse_stdflags(it, &node->base)) {
+        return;
+    }
+    
+    if (__parse_stdbase_prop(it, &node->base)) {
+        return;
+    }
+
+    struct fdt_prop* prop;
+
+    prop = it->prop;
+    if (propeq(it, "serial-number")) {
+        node->serial = (const char*)&prop[1];
+    }
+
+    else if (propeq(it, "chassis-type")) {
+        node->chassis = (const char*)&prop[1];
+    }
+
+    __parse_other_prop(it, &node->base);
+}
+
+static inline void
+__init_node(struct dt_node_base* node)
+{
+    hashtable_init(node->_op_bucket);
+    llist_init_head(&node->children);
+}
+
+static inline void
+__init_node_regular(struct dt_node* node)
+{
+    __init_node(&node->base);
+    node->intr.parent_hnd = PHND_NULL;
+}
+
+static void
+__expand_extended_intr(struct dt_intr_node* intrupt)
+{
+    struct dt_prop_iter it;
+    struct dt_prop_val  arr;
+    struct dt_node *node;
+    struct dt_node *master;
+    struct dt_intr_prop* intr_prop;
+
+    if (!intrupt->intr.extended) {
+        return;
+    }
+
+    arr = intrupt->intr.arr;
+    node = DT_NODE(intrupt);
+
+    llist_init_head(&intrupt->intr.values);
+    
+    dt_decode(&it, &node->base, &arr, 1);
+    
+    dt_phnd_t phnd;
+    while(dtprop_next(&it)) {
+        phnd   = dtprop_to_u32(it.prop_loc);
+        master = dt_resolve_phandle(phnd);
+
+        if (!master) {
+            WARN("dtb: (intr_extended) malformed phandle: %d", phnd);
+            continue;
+        }
+
+        intr_prop = valloc(sizeof(*intr_prop));
+        
+        intr_prop->master = &master->intr;
+        intr_prop->val = (struct dt_prop_val) {
+            .encoded = it.prop_loc_next,
+            .size    = master->base.intr_c
+        };
+
+        llist_append(&intrupt->intr.values, &intr_prop->props);
+        dtprop_next_n(&it, intr_prop->val.size);
+    }
+}
+
+static void
+__resolve_phnd_references()
+{
+    struct dt_node_base *pos, *n;
+    struct dt_node *node, *parent, *default_parent;
+    struct dt_intr_node* intrupt;
+    dt_phnd_t phnd;
+    
+    llist_for_each(pos, n, &dtctx.nodes, nodes)
+    {
+        node = (struct dt_node*)pos;
+        intrupt = &node->intr;
+        if (!node->base.intr_c) {
+            continue;
+        }
+
+        phnd = intrupt->parent_hnd;
+        default_parent = (struct dt_node*)node->base.parent;
+        parent = default_parent;
+
+        if (phnd != PHND_NULL) {
+            parent = dt_resolve_phandle(phnd);
+        }
+
+        if (!parent) {
+            WARN("dtb: (phnd_resolve) malformed phandle: %d", phnd);
+            parent = default_parent;
+        }
+
+        intrupt->parent = &parent->intr;
+
+        __expand_extended_intr(intrupt);
+    }
+}
+
+bool
+dt_load(ptr_t dtb_dropoff)
+{
+    dtctx.reloacted_dtb = dtb_dropoff;
+
+    if (dtctx.fdt->magic != FDT_MAGIC) {
+        ERROR("invalid dtb, unexpected magic: 0x%x", dtctx.fdt->magic);
+        return false;
+    }
+
+    size_t str_off = le(dtctx.fdt->size_dt_strings);
+    dtctx.str_block = offset_t(dtb_dropoff, const char, str_off);
+
+    llist_init_head(&dtctx.nodes);
+    hashtable_init(dtctx.phnds_table);
+
+    struct fdt_iter it;
+    struct fdt_token* tok;
+    struct dt_node_base *node, *prev;
+    
+    struct dt_node_base* depth[16];
+    bool is_root_level, filled;
+
+    node = NULL;
+    depth[0] = NULL;
+    fdt_itbegin(&it, dtctx.fdt);
+    
+    while (fdt_itnext(&it)) {
+        is_root_level = it.depth == 1;
+
+        if (it.depth >= 16) {
+            // tree too deep
+            ERROR("strange dtb, too deep to dive.");
+            return false;
+        }
+
+        depth[it.depth] = NULL;
+        node = depth[it.depth - 1];
+
+        if (!node) {
+            // need new node
+            if (unlikely(is_root_level)) {
+                node = valloc(sizeof(struct dt_root));
+                __init_node(node);
+            }
+            else {
+                node = valloc(sizeof(struct dt_node));
+                prev = depth[it.depth - 2];
+
+                __init_node_regular((struct dt_node*)node);
+                llist_append(&prev->children, &node->siblings);
+                node->parent = prev;
+
+                llist_append(&dtctx.nodes, &node->nodes);
+            }
+
+            node->name = (const char*)&it.pos[1];
+        }
+
+        if (unlikely(is_root_level)) {
+            __fill_root(&it, (struct dt_root*)node);
+        }
+        else {
+            __fill_node(&it, (struct dt_node*)node);
+        }
+    }
+
+    fdt_itend(&it);
+
+    dtctx.root = (struct dt_root*)depth[0];
+
+    __resolve_phnd_references();
+
+    return true;
+}
+
+static bool
+__name_starts_with(struct dt_node_base* node, const char* name)
+{
+    int i = 0;
+    const char* be_matched = node->name;
+
+    while (be_matched[i] && name[i])
+    {
+        if (be_matched[i] != name[i]) {
+            return false;
+        }
+    }
+
+    return true;
+}
+
+struct dt_node*
+dt_resolve_phandle(dt_phnd_t phandle)
+{
+    struct dt_node_base *pos, *n;
+    hashtable_hash_foreach(dtctx.phnds_table, phandle, pos, n, phnd_link)
+    {
+        if (pos->phandle == phandle) {
+            return (struct dt_node*)pos;
+        }
+    }
+
+    return NULL;
+}
+
+void
+dt_begin_find(struct dt_node_iter* iter, 
+              struct dt_node* node, const char* name)
+{
+    node = node ? : (struct dt_node*)dtctx.root;
+
+    iter->head = &node->base;
+    iter->matched = NULL;
+    iter->name = name;
+
+    struct dt_node_base *pos, *n;
+    llist_for_each(pos, n, &node->base.children, siblings)
+    {
+        if (__name_starts_with(pos, name)) {
+            iter->matched = pos;
+            break;
+        }
+    }
+}
+
+bool
+dt_find_next(struct dt_node_iter* iter,
+             struct dt_node_base** matched)
+{
+    if (!dt_found_any(iter)) {
+        return false;
+    }
+
+    struct dt_node_base *pos, *head;
+
+    head = iter->head;
+    pos = iter->matched;
+    *matched = pos;
+
+    while (&pos->siblings != &head->children)
+    {
+        pos = list_next(pos, struct dt_node_base, siblings);
+
+        if (!__name_starts_with(pos, iter->name)) {
+            continue;
+        }
+
+        iter->matched = pos;
+        return true;
+    }
+
+    return false;
+}
+
+struct dt_prop_val*
+dt_getprop(struct dt_node* node, const char* name)
+{
+    struct hstr hashed_name;
+    struct dt_prop *pos, *n;
+    unsigned int hash;
+
+    hashed_name = HSTR(name, strlen(name));
+    hstr_rehash(&hashed_name, HSTR_FULL_HASH);
+    hash = hashed_name.hash;
+
+    hashtable_hash_foreach(node->base._op_bucket, hash, pos, n, ht)
+    {
+        if (HSTR_EQ(&pos->key, &hashed_name)) {
+            return &pos->val;
+        }
+    }
+
+    return NULL;
+}
\ No newline at end of file