rework parsing of interupt-map in interrupt node.

[lunaix-os.git] / lunaix-os / hal / devtree / dt_interrupt.c

diff --git a/lunaix-os/hal/devtree/dt_interrupt.c b/lunaix-os/hal/devtree/dt_interrupt.c
new file mode 100644 (file)
index 0000000..d2db72d
--- /dev/null
+++ b/lunaix-os/hal/devtree/dt_interrupt.c
@@ -0,0 +1,215 @@
+#include "devtree.h"
+
+#include <lunaix/mm/valloc.h>
+
+#include <klibc/string.h>
+
+static struct dt_intr_map*
+__get_map(struct dt_intr_node* node)
+{
+    if (!node->map) {
+        node->map = valloc(sizeof(struct dt_intr_map));
+        
+        node->map->resolved = false;
+        llist_init_head(&node->map->mapent);
+
+        INTR_TO_DTNODE(node)->base.intr_neuxs = true;
+    }
+
+    return node->map;
+}
+
+static void
+__prepare_key(struct dt_intr_mapkey* key, 
+              dt_enc_t key_raw, unsigned int keylen)
+{
+    *key = (struct dt_intr_mapkey) {
+        .val = valloc(keylen * sizeof(int)),
+        .size = keylen
+    };
+
+    memcpy(key->val, key_raw, keylen * sizeof(int));
+}
+
+static inline void
+__destory_key(struct dt_intr_mapkey* key)
+{
+    vfree(key->val);
+}
+
+static inline unsigned int
+__interrupt_keysize(struct dt_node_base* base)
+{
+    return base->addr_c + base->intr_c;
+}
+
+static void
+__mask_key(struct dt_intr_mapkey* k, struct dt_intr_mapkey* mask)
+{
+    for (int i = 0; i < k->size; i++)
+    {
+        k->val[i] &= mask->val[i];
+    }
+}
+
+static bool
+__compare_key(struct dt_intr_mapkey* k1, struct dt_intr_mapkey* k2)
+{
+    if (k1->size != k2->size) {
+        return false;
+    }
+
+    for (int i = 0; i < k1->size; i++)
+    {
+        if (k1->val[i] != k2->val[i]) {
+            return false;
+        }    
+    }
+    
+    return true;
+}
+
+static struct dt_node_base*
+__get_connected_nexus(struct dt_node_base* base)
+{
+    struct dt_node_base* current;
+
+    current = base;
+    while (current && !current->intr_neuxs) {
+        current = current->parent;
+    }
+
+    return current;
+}
+
+void
+resolve_interrupt_map(struct dt_node* node)
+{
+    struct dt_intr_node* inode;
+    struct dt_intr_map* imap;
+    struct dt_prop_iter iter;
+
+    struct dt_intr_mapent *ent;
+
+    unsigned int keysize, parent_keysize;
+    unsigned int advance;
+    dt_phnd_t parent_hnd;
+    
+    inode = &node->intr;
+    if (likely(!inode->map)) {
+        return;
+    }
+
+    imap = inode->map;
+    keysize = __interrupt_keysize(&node->base);
+    
+    __prepare_key(&imap->key_mask, imap->raw_mask.encoded, keysize);
+
+    dt_decode(&iter, &node->base, &imap->raw, 1);
+
+    advance = 0;
+    do 
+    {
+        advance = keysize;
+        ent = valloc(sizeof(*ent));
+
+        __prepare_key(&ent->key, iter.prop_loc, advance);
+        __mask_key(&ent->key, &imap->key_mask);
+
+        parent_hnd = dtprop_to_phnd(dtprop_extract(&iter, advance));
+        ent->parent = &dt_resolve_phandle(parent_hnd)->base;
+
+        advance++;
+        parent_keysize = __interrupt_keysize(ent->parent);
+
+        ent->parent_props.encoded = dtprop_extract(&iter, advance);
+        ent->parent_props.size = parent_keysize;
+
+        advance += parent_keysize;
+
+        llist_append(&imap->mapent, &ent->ents);
+        
+    } while (dtprop_next_n(&iter, advance));
+
+    imap->resolved = true;
+}
+
+struct dt_prop_val*
+resolve_interrupt(struct dt_node* node)
+{
+    struct dt_node_base* nexus;
+    struct dt_intr_node* i_nexus, *i_node;
+    struct dt_intr_mapkey key;
+    unsigned int keylen;
+
+    if (!node->intr.intr.valid) {
+        return NULL;
+    }
+
+    nexus = __get_connected_nexus(&node->base);
+    i_nexus = &BASE_TO_DTNODE(nexus)->intr;
+    i_node  = &node->intr;
+
+    if (!nexus) {
+        return &i_node->intr.arr;
+    }
+
+    keylen = nexus->addr_c + nexus->intr_c;
+    key = (struct dt_intr_mapkey) {
+        .val = valloc(keylen * sizeof(int)),
+        .size = keylen
+    };
+
+    memcpy( key.val, 
+            node->reg.encoded, nexus->addr_c * sizeof(int));
+    
+    memcpy(&key.val[nexus->addr_c],
+            i_node->intr.arr.encoded, nexus->intr_c * sizeof(int));
+
+    __mask_key(&key, &i_nexus->map->key_mask);
+
+    struct dt_intr_mapent *pos, *n;
+
+    llist_for_each(pos, n, &i_nexus->map->mapent, ents) {
+        if (__compare_key(&pos->key, &key))
+        {
+            return &pos->parent_props;
+        }
+    } 
+}
+
+bool
+parse_stdintr_prop(struct fdt_iter* it, struct dt_intr_node* node)
+{
+    struct dt_intr_map* map;
+
+    if (propeq(it, "interrupt-map")) {
+        map = __get_map(node);
+        __mkprop_ptr(it, &map->raw);
+    }
+
+    else if (propeq(it, "interrupt-map-mask")) {
+        map = __get_map(node);
+        __mkprop_ptr(it, &map->raw_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")) {
+        node->intr.valid = true;
+        __mkprop_ptr(it, &node->intr.arr);
+    }
+
+    else {
+        return false;
+    }
+
+    return true;
+}
\ No newline at end of file