lunaix-os/hal/devtree/dt_interrupt.c

   1 #include "devtree.h"
   2
   3 #include <lunaix/mm/valloc.h>
   4
   5 #include <klibc/string.h>
   6
   7 static struct dt_intr_map*
   8 __get_map(struct dt_intr_node* node)
   9 {
  10     if (!node->map) {
  11         node->map = valloc(sizeof(struct dt_intr_map));
  12
  13         node->map->resolved = false;
  14         llist_init_head(&node->map->mapent);
  15
  16         INTR_TO_DTNODE(node)->base.intr_neuxs = true;
  17     }
  18
  19     return node->map;
  20 }
  21
  22 static void
  23 __prepare_key(struct dt_intr_mapkey* key,
  24               dt_enc_t key_raw, unsigned int keylen)
  25 {
  26     *key = (struct dt_intr_mapkey) {
  27         .val = valloc(keylen * sizeof(int)),
  28         .size = keylen
  29     };
  30
  31     memcpy(key->val, key_raw, keylen * sizeof(int));
  32 }
  33
  34 static inline void
  35 __destory_key(struct dt_intr_mapkey* key)
  36 {
  37     vfree(key->val);
  38 }
  39
  40 static inline unsigned int
  41 __interrupt_keysize(struct dt_node_base* base)
  42 {
  43     return dt_size_cells(base) + base->intr_c;
  44 }
  45
  46 static void
  47 __mask_key(struct dt_intr_mapkey* k, struct dt_intr_mapkey* mask)
  48 {
  49     for (int i = 0; i < k->size; i++)
  50     {
  51         k->val[i] &= mask->val[i];
  52     }
  53 }
  54
  55 static bool
  56 __compare_key(struct dt_intr_mapkey* k1, struct dt_intr_mapkey* k2)
  57 {
  58     if (k1->size != k2->size) {
  59         return false;
  60     }
  61
  62     for (int i = 0; i < k1->size; i++)
  63     {
  64         if (k1->val[i] != k2->val[i]) {
  65             return false;
  66         }
  67     }
  68
  69     return true;
  70 }
  71
  72 static struct dt_node_base*
  73 __get_connected_nexus(struct dt_node_base* base)
  74 {
  75     struct dt_node_base* current;
  76
  77     current = base;
  78     while (current && !current->intr_neuxs) {
  79         current = current->parent;
  80     }
  81
  82     return current;
  83 }
  84
  85 void
  86 dt_resolve_interrupt_map(struct dt_node* node)
  87 {
  88     struct dt_intr_node* inode;
  89     struct dt_intr_map* imap;
  90     struct dt_prop_iter iter;
  91
  92     struct dt_intr_mapent *ent;
  93
  94     unsigned int keysize, parent_keysize;
  95     unsigned int advance;
  96     dt_phnd_t parent_hnd;
  97
  98     inode = &node->intr;
  99     if (likely(!inode->map)) {
 100         return;
 101     }
 102
 103     imap = inode->map;
 104     keysize = __interrupt_keysize(&node->base);
 105
 106     __prepare_key(&imap->key_mask, imap->raw_mask.encoded, keysize);
 107
 108     dt_decode(&iter, &node->base, &imap->raw, 1);
 109
 110     advance = 0;
 111     do
 112     {
 113         advance = keysize;
 114         ent = valloc(sizeof(*ent));
 115
 116         __prepare_key(&ent->key, iter.prop_loc, advance);
 117         __mask_key(&ent->key, &imap->key_mask);
 118
 119         parent_hnd = dtprop_to_phnd(dtprop_extract(&iter, advance));
 120         ent->parent = &dt_resolve_phandle(parent_hnd)->base;
 121
 122         advance++;
 123         parent_keysize = __interrupt_keysize(ent->parent);
 124
 125         ent->parent_props.encoded = dtprop_extract(&iter, advance);
 126         ent->parent_props.size = parent_keysize;
 127
 128         advance += parent_keysize;
 129
 130         llist_append(&imap->mapent, &ent->ents);
 131
 132     } while (dtprop_next_n(&iter, advance));
 133
 134     imap->resolved = true;
 135 }
 136
 137 struct dt_prop_val*
 138 dt_resolve_interrupt(struct dt_node* node)
 139 {
 140     struct dt_node_base* nexus;
 141     struct dt_intr_node* i_nexus, *i_node;
 142     struct dt_intr_mapkey key;
 143     unsigned int keylen;
 144
 145     if (!node->intr.intr.valid) {
 146         return NULL;
 147     }
 148
 149     nexus = __get_connected_nexus(&node->base);
 150     i_nexus = &BASE_TO_DTNODE(nexus)->intr;
 151     i_node  = &node->intr;
 152
 153     if (!nexus) {
 154         return &i_node->intr.arr;
 155     }
 156
 157     keylen = __interrupt_keysize(nexus);
 158     key = (struct dt_intr_mapkey) {
 159         .val = valloc(keylen * sizeof(int)),
 160         .size = keylen
 161     };
 162
 163     memcpy( key.val,
 164             node->reg.encoded, dt_addr_cells(nexus) * sizeof(int));
 165
 166     memcpy(&key.val[dt_addr_cells(nexus)],
 167             i_node->intr.arr.encoded, nexus->intr_c * sizeof(int));
 168
 169     __mask_key(&key, &i_nexus->map->key_mask);
 170
 171     struct dt_intr_mapent *pos, *n;
 172
 173     llist_for_each(pos, n, &i_nexus->map->mapent, ents) {
 174         if (__compare_key(&pos->key, &key))
 175         {
 176             return &pos->parent_props;
 177         }
 178     }
 179
 180     __destory_key(&key);
 181 }
 182
 183 bool
 184 parse_stdintr_prop(struct fdt_iter* it, struct dt_intr_node* node)
 185 {
 186     struct dt_intr_map* map;
 187
 188     if (propeq(it, "interrupt-map")) {
 189         map = __get_map(node);
 190         __mkprop_ptr(it, &map->raw);
 191     }
 192
 193     else if (propeq(it, "interrupt-map-mask")) {
 194         map = __get_map(node);
 195         __mkprop_ptr(it, &map->raw_mask);
 196     }
 197
 198     else if (propeq(it, "interrupt-parent")) {
 199         node->parent_hnd = __prop_getu32(it);
 200     }
 201
 202     else if (propeq(it, "interrupt-extended")) {
 203         node->intr.extended = true;
 204         __mkprop_ptr(it, &node->intr.arr);
 205     }
 206
 207     else if (!node->intr.extended && propeq(it, "interrupts")) {
 208         node->intr.valid = true;
 209         __mkprop_ptr(it, &node->intr.arr);
 210     }
 211
 212     else {
 213         return false;
 214     }
 215
 216     return true;
 217 }