lunaix-os/hal/devtree/dt.c

   1 #include <lunaix/mm/valloc.h>
   2 #include <lunaix/syslog.h>
   3
   4 #include <klibc/string.h>
   5
   6 #include "devtree.h"
   7
   8 LOG_MODULE("dtb")
   9
  10 static struct dt_context dtctx;
  11
  12 void
  13 fdt_itbegin(struct fdt_iter* fdti, struct fdt_header* fdt_hdr)
  14 {
  15     unsigned int off_struct, off_str;
  16     struct fdt_token* tok;
  17     const char* str_blk;
  18
  19     off_str    = le(fdt_hdr->off_dt_strings);
  20     off_struct = le(fdt_hdr->off_dt_struct);
  21
  22     tok = offset_t(fdt_hdr, struct fdt_token, off_struct);
  23     str_blk = offset_t(fdt_hdr, const char, off_str);
  24
  25     *fdti = (struct fdt_iter) {
  26         .pos = tok,
  27         .str_block = str_blk
  28     };
  29 }
  30
  31 void
  32 fdt_itend(struct fdt_iter* fdti)
  33 {
  34     fdti->pos = NULL;
  35 }
  36
  37 bool
  38 fdt_itnext(struct fdt_iter* fdti)
  39 {
  40     struct fdt_token *current;
  41     struct fdt_prop  *prop;
  42
  43     current = fdti->pos;
  44     if (!current) {
  45         return false;
  46     }
  47
  48     do
  49     {
  50         if (fdt_nope(current)) {
  51             continue;
  52         }
  53
  54         if (fdt_prop(current)) {
  55             prop    = (struct fdt_prop*) current;
  56             current = offset(current, prop->len);
  57             continue;
  58         }
  59
  60         if (fdt_node_end(current)) {
  61             fdti->depth--;
  62             continue;
  63         }
  64
  65         // node begin
  66
  67         fdti->depth++;
  68         if (fdti->depth == 1) {
  69             // enter root node
  70             break;
  71         }
  72
  73         while (!fdt_prop(current) && !fdt_node_end(current)) {
  74             current++;
  75         }
  76
  77         if (fdt_prop(current)) {
  78             break;
  79         }
  80
  81         current++;
  82
  83     } while (fdt_nope(current) && fdti->depth > 0);
  84
  85     return fdti->depth > 0;
  86 }
  87
  88 bool
  89 fdt_itnext_at(struct fdt_iter* fdti, int level)
  90 {
  91     while (fdti->depth != level && fdt_itnext(fdti));
  92
  93     return fdti->depth == level;
  94 }
  95
  96 void
  97 fdt_memrsvd_itbegin(struct fdt_memrsvd_iter* rsvdi,
  98                     struct fdt_header* fdt_hdr)
  99 {
 100     size_t off = le(fdt_hdr->off_mem_rsvmap);
 101
 102     rsvdi->block =
 103         offset_t(fdt_hdr, typeof(*rsvdi->block), off);
 104
 105     rsvdi->block = &rsvdi->block[-1];
 106 }
 107
 108 bool
 109 fdt_memrsvd_itnext(struct fdt_memrsvd_iter* rsvdi)
 110 {
 111     struct fdt_memrsvd_ent* ent;
 112
 113     ent = rsvdi->block;
 114     if (!ent) {
 115         return false;
 116     }
 117
 118     rsvdi->block++;
 119
 120     return ent->addr || ent->size;
 121 }
 122
 123 void
 124 fdt_memrsvd_itend(struct fdt_memrsvd_iter* rsvdi)
 125 {
 126     rsvdi->block = NULL;
 127 }
 128
 129 static inline bool
 130 propeq(struct fdt_iter* it, const char* key)
 131 {
 132     return streq(fdtit_prop_key(it), key);
 133 }
 134
 135 static inline void
 136 __mkprop_val32(struct fdt_iter* it, struct dt_prop_val* val)
 137 {
 138     val->u32_val = le(*(u32_t*)&it->prop[1]);
 139     val->size = le(it->prop->len);
 140 }
 141
 142 static inline void
 143 __mkprop_val64(struct fdt_iter* it, struct dt_prop_val* val)
 144 {
 145     val->u64_val = le64(*(u64_t*)&it->prop[1]);
 146     val->size = le(it->prop->len);
 147 }
 148
 149 static inline void
 150 __mkprop_ptr(struct fdt_iter* it, struct dt_prop_val* val)
 151 {
 152     val->ptr_val = __ptr(&it->prop[1]);
 153     val->size = le(it->prop->len);
 154 }
 155
 156 static inline u32_t
 157 __prop_getu32(struct fdt_iter* it)
 158 {
 159     return le(*(u32_t*)&it->prop[1]);
 160 }
 161
 162 static bool
 163 __parse_stdbase_prop(struct fdt_iter* it, struct dt_node_base* node)
 164 {
 165     struct fdt_prop* prop;
 166
 167     prop = it->prop;
 168
 169     if (propeq(it, "compatible")) {
 170         __mkprop_ptr(it, &node->compat);
 171     }
 172
 173     else if (propeq(it, "model")) {
 174         node->model = (const char*)&prop[1];
 175     }
 176
 177     else if (propeq(it, "phandle")) {
 178         node->phandle = __prop_getu32(it);
 179         hashtable_hash_in(dtctx.phnds_table,
 180                           &node->phnd_link, node->phandle);
 181     }
 182
 183     else if (propeq(it, "#address-cells")) {
 184         node->addr_c = (char)__prop_getu32(it);
 185     }
 186
 187     else if (propeq(it, "#size-cells")) {
 188         node->sz_c = (char)__prop_getu32(it);
 189     }
 190
 191     else if (propeq(it, "#interrupt-cells")) {
 192         node->intr_c = (char)__prop_getu32(it);
 193     }
 194
 195     else if (propeq(it, "status")) {
 196         char peek = *(char*)&it->prop[1];
 197         if (peek == 'o') {
 198             node->status = STATUS_OK;
 199         }
 200         else if (peek == 'r') {
 201             node->status = STATUS_RSVD;
 202         }
 203         else if (peek == 'd') {
 204             node->status = STATUS_DISABLE;
 205         }
 206         else if (peek == 'f') {
 207             node->status = STATUS_FAIL;
 208         }
 209     }
 210
 211     else {
 212         return false;
 213     }
 214
 215     return true;
 216 }
 217
 218 static bool
 219 __parse_stdnode_prop(struct fdt_iter* it, struct dt_node* node)
 220 {
 221     if (propeq(it, "reg")) {
 222         __mkprop_ptr(it, &node->reg);
 223     }
 224
 225     else if (propeq(it, "virtual-reg")) {
 226         __mkprop_ptr(it, &node->vreg);
 227     }
 228
 229     else if (propeq(it, "ranges")) {
 230         __mkprop_ptr(it, &node->ranges);
 231     }
 232
 233     else if (propeq(it, "dma-ranges")) {
 234         __mkprop_ptr(it, &node->dma_ranges);
 235     }
 236
 237     else {
 238         return false;
 239     }
 240
 241     return true;
 242 }
 243
 244 static bool
 245 __parse_stdflags(struct fdt_iter* it, struct dt_node_base* node)
 246 {
 247     if (propeq(it, "dma-coherent")) {
 248         node->dma_coherent = true;
 249     }
 250
 251     else if (propeq(it, "dma-noncoherent")) {
 252         node->dma_ncoherent = true;
 253     }
 254
 255     else if (propeq(it, "interrupt-controller")) {
 256         node->intr_controll = true;
 257     }
 258
 259     else {
 260         return false;
 261     }
 262
 263     return true;
 264 }
 265
 266 static void
 267 __parse_other_prop(struct fdt_iter* it, struct dt_node_base* node)
 268 {
 269     struct dt_prop* prop;
 270     const char* key;
 271     unsigned int hash;
 272
 273     prop = valloc(sizeof(*prop));
 274     key  = fdtit_prop_key(it);
 275
 276     prop->key = HSTR(key, strlen(key));
 277     __mkprop_ptr(it, &prop->val);
 278
 279     hstr_rehash(&prop->key, HSTR_FULL_HASH);
 280     hash = prop->key.hash;
 281
 282     hashtable_hash_in(node->_op_bucket, &prop->ht, hash);
 283 }
 284
 285 static void
 286 __fill_node(struct fdt_iter* it, struct dt_node* node)
 287 {
 288     if (__parse_stdflags(it, &node->base)) {
 289         return;
 290     }
 291
 292     if (__parse_stdbase_prop(it, &node->base)) {
 293         return;
 294     }
 295
 296     if (__parse_stdnode_prop(it, node)) {
 297         return;
 298     }
 299
 300     if (__parse_stdintr_prop(it, &node->intr)) {
 301         return;
 302     }
 303
 304     __parse_other_prop(it, &node->base);
 305 }
 306
 307 static void
 308 __fill_root(struct fdt_iter* it, struct dt_root* node)
 309 {
 310     if (__parse_stdflags(it, &node->base)) {
 311         return;
 312     }
 313
 314     if (__parse_stdbase_prop(it, &node->base)) {
 315         return;
 316     }
 317
 318     struct fdt_prop* prop;
 319
 320     prop = it->prop;
 321     if (propeq(it, "serial-number")) {
 322         node->serial = (const char*)&prop[1];
 323     }
 324
 325     else if (propeq(it, "chassis-type")) {
 326         node->chassis = (const char*)&prop[1];
 327     }
 328
 329     __parse_other_prop(it, &node->base);
 330 }
 331
 332 static inline void
 333 __init_node(struct dt_node_base* node)
 334 {
 335     hashtable_init(node->_op_bucket);
 336     llist_init_head(&node->children);
 337
 338     if (node->parent)
 339         node->_std = node->parent->_std;
 340 }
 341
 342 static inline void
 343 __init_node_regular(struct dt_node* node)
 344 {
 345     __init_node(&node->base);
 346     node->intr.parent_hnd = PHND_NULL;
 347 }
 348
 349 static void
 350 __expand_extended_intr(struct dt_intr_node* intrupt)
 351 {
 352     struct dt_prop_iter it;
 353     struct dt_prop_val  arr;
 354     struct dt_node *node;
 355     struct dt_node *master;
 356     struct dt_intr_prop* intr_prop;
 357
 358     if (!intrupt->intr.extended) {
 359         return;
 360     }
 361
 362     arr = intrupt->intr.arr;
 363     node = INTR_TO_DTNODE(intrupt);
 364
 365     llist_init_head(&intrupt->intr.values);
 366
 367     dt_decode(&it, &node->base, &arr, 1);
 368
 369     dt_phnd_t phnd;
 370     do {
 371         phnd   = dtprop_to_u32(it.prop_loc);
 372         master = dt_resolve_phandle(phnd);
 373
 374         if (!master) {
 375             WARN("dtb: (intr_extended) malformed phandle: %d", phnd);
 376             continue;
 377         }
 378
 379         intr_prop = valloc(sizeof(*intr_prop));
 380
 381         intr_prop->master = &master->intr;
 382         intr_prop->val = (struct dt_prop_val) {
 383             .encoded = it.prop_loc_next,
 384             .size    = master->base.intr_c
 385         };
 386
 387         llist_append(&intrupt->intr.values, &intr_prop->props);
 388         dtprop_next_n(&it, intr_prop->val.size);
 389
 390     } while(dtprop_next(&it));
 391 }
 392
 393 static void
 394 __resolve_phnd_references()
 395 {
 396     struct dt_node_base *pos, *n;
 397     struct dt_node *node, *parent, *default_parent;
 398     struct dt_intr_node* intrupt;
 399     dt_phnd_t phnd;
 400
 401     llist_for_each(pos, n, &dtctx.nodes, nodes)
 402     {
 403         node = BASE_TO_DTNODE(pos);
 404         intrupt = &node->intr;
 405
 406         if (!node->base.intr_c) {
 407             continue;
 408         }
 409
 410         phnd = intrupt->parent_hnd;
 411         default_parent = (struct dt_node*)node->base.parent;
 412         parent = default_parent;
 413
 414         if (phnd != PHND_NULL) {
 415             parent = dt_resolve_phandle(phnd);
 416         }
 417
 418         if (!parent) {
 419             WARN("dtb: (phnd_resolve) malformed phandle: %d", phnd);
 420             parent = default_parent;
 421         }
 422
 423         intrupt->parent = &parent->intr;
 424
 425         __expand_extended_intr(intrupt);
 426     }
 427 }
 428
 429 static void
 430 __resolve_inter_map()
 431 {
 432     struct dt_node_base *pos, *n;
 433
 434     llist_for_each(pos, n, &dtctx.nodes, nodes)
 435     {
 436         dt_resolve_interrupt_map(BASE_TO_DTNODE(pos));
 437     }
 438 }
 439
 440 bool
 441 dt_load(ptr_t dtb_dropoff)
 442 {
 443     dtctx.reloacted_dtb = dtb_dropoff;
 444
 445     if (dtctx.fdt->magic != FDT_MAGIC) {
 446         ERROR("invalid dtb, unexpected magic: 0x%x", dtctx.fdt->magic);
 447         return false;
 448     }
 449
 450     size_t str_off = le(dtctx.fdt->size_dt_strings);
 451     dtctx.str_block = offset_t(dtb_dropoff, const char, str_off);
 452
 453     llist_init_head(&dtctx.nodes);
 454     hashtable_init(dtctx.phnds_table);
 455
 456     struct fdt_iter it;
 457     struct fdt_token* tok;
 458     struct dt_node_base *node, *prev;
 459
 460     struct dt_node_base* depth[16];
 461     bool is_root_level, filled;
 462
 463     node = NULL;
 464     depth[0] = NULL;
 465     fdt_itbegin(&it, dtctx.fdt);
 466
 467     while (fdt_itnext(&it)) {
 468         is_root_level = it.depth == 1;
 469
 470         if (it.depth >= 16) {
 471             // tree too deep
 472             ERROR("strange dtb, too deep to dive.");
 473             return false;
 474         }
 475
 476         depth[it.depth] = NULL;
 477         node = depth[it.depth - 1];
 478
 479         if (!node) {
 480             // need new node
 481             if (unlikely(is_root_level)) {
 482                 node = vzalloc(sizeof(struct dt_root));
 483                 __init_node(node);
 484             }
 485             else {
 486                 node = vzalloc(sizeof(struct dt_node));
 487                 prev = depth[it.depth - 2];
 488                 node->parent = prev;
 489
 490                 __init_node_regular((struct dt_node*)node);
 491                 llist_append(&prev->children, &node->siblings);
 492
 493                 llist_append(&dtctx.nodes, &node->nodes);
 494             }
 495
 496             node->name = (const char*)&it.pos[1];
 497         }
 498
 499         if (unlikely(is_root_level)) {
 500             __fill_root(&it, (struct dt_root*)node);
 501         }
 502         else {
 503             __fill_node(&it, (struct dt_node*)node);
 504         }
 505     }
 506
 507     fdt_itend(&it);
 508
 509     dtctx.root = (struct dt_root*)depth[0];
 510
 511     __resolve_phnd_references();
 512     __resolve_inter_map();
 513
 514     INFO("device tree loaded");
 515
 516     return true;
 517 }
 518
 519 struct dt_node*
 520 dt_resolve_phandle(dt_phnd_t phandle)
 521 {
 522     struct dt_node_base *pos, *n;
 523     hashtable_hash_foreach(dtctx.phnds_table, phandle, pos, n, phnd_link)
 524     {
 525         if (pos->phandle == phandle) {
 526             return (struct dt_node*)pos;
 527         }
 528     }
 529
 530     return NULL;
 531 }
 532
 533 static bool
 534 __byname_predicate(struct dt_node_iter* iter, struct dt_node_base* node)
 535 {
 536     int i = 0;
 537     const char* be_matched = node->name;
 538     const char* name = (const char*)iter->closure;
 539
 540     while (be_matched[i] && name[i])
 541     {
 542         if (be_matched[i] != name[i]) {
 543             return false;
 544         }
 545
 546         i++;
 547     }
 548
 549     return true;
 550 }
 551
 552 void
 553 dt_begin_find_byname(struct dt_node_iter* iter,
 554               struct dt_node* node, const char* name)
 555 {
 556     dt_begin_find(iter, node, __byname_predicate, name);
 557 }
 558
 559 void
 560 dt_begin_find(struct dt_node_iter* iter, struct dt_node* node,
 561               node_predicate_t pred, void* closure)
 562 {
 563     node = node ? : (struct dt_node*)dtctx.root;
 564
 565     iter->head = &node->base;
 566     iter->matched = NULL;
 567     iter->closure = closure;
 568     iter->pred = pred;
 569
 570     struct dt_node_base *pos, *n;
 571     llist_for_each(pos, n, &node->base.children, siblings)
 572     {
 573         if (pred(iter, pos)) {
 574             iter->matched = pos;
 575             break;
 576         }
 577     }
 578 }
 579
 580 bool
 581 dt_find_next(struct dt_node_iter* iter,
 582              struct dt_node_base** matched)
 583 {
 584     if (!dt_found_any(iter)) {
 585         return false;
 586     }
 587
 588     struct dt_node_base *pos, *head;
 589
 590     head = iter->head;
 591     pos = iter->matched;
 592     *matched = pos;
 593
 594     while (&pos->siblings != &head->children)
 595     {
 596         pos = list_next(pos, struct dt_node_base, siblings);
 597
 598         if (!iter->pred(iter, pos)) {
 599             continue;
 600         }
 601
 602         iter->matched = pos;
 603         return true;
 604     }
 605
 606     return false;
 607 }
 608
 609 struct dt_prop_val*
 610 dt_getprop(struct dt_node_base* base, const char* name)
 611 {
 612     struct hstr hashed_name;
 613     struct dt_prop *pos, *n;
 614     unsigned int hash;
 615
 616     hashed_name = HSTR(name, strlen(name));
 617     hstr_rehash(&hashed_name, HSTR_FULL_HASH);
 618     hash = hashed_name.hash;
 619
 620     hashtable_hash_foreach(base->_op_bucket, hash, pos, n, ht)
 621     {
 622         if (HSTR_EQ(&pos->key, &hashed_name)) {
 623             return &pos->val;
 624         }
 625     }
 626
 627     return NULL;
 628 }