lunaix-os/hal/devtree.c

   1 #include <lunaix/mm/valloc.h>
   2 #include <lunaix/syslog.h>
   3
   4 #include <klibc/string.h>
   5
   6 #include <hal/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_stdintr_prop(struct fdt_iter* it, struct dt_intr_node* node)
 246 {
 247     if (propeq(it, "interrupt-map")) {
 248         __mkprop_ptr(it, &node->intr_map);
 249     }
 250
 251     else if (propeq(it, "interrupt-map-mask")) {
 252         __mkprop_ptr(it, &node->intr_map_mask);
 253     }
 254
 255     else if (propeq(it, "interrupt-parent")) {
 256         node->parent_hnd = __prop_getu32(it);
 257     }
 258
 259     else if (propeq(it, "interrupt-extended")) {
 260         node->intr.extended = true;
 261         __mkprop_ptr(it, &node->intr.arr);
 262     }
 263
 264     else if (!node->intr.extended && propeq(it, "interrupts")) {
 265         __mkprop_ptr(it, &node->intr.arr);
 266     }
 267
 268     else {
 269         return false;
 270     }
 271
 272     return true;
 273 }
 274
 275 static bool
 276 __parse_stdflags(struct fdt_iter* it, struct dt_node_base* node)
 277 {
 278     if (propeq(it, "dma-coherent")) {
 279         node->dma_coherent = true;
 280     }
 281
 282     else if (propeq(it, "dma-noncoherent")) {
 283         node->dma_ncoherent = true;
 284     }
 285
 286     else if (propeq(it, "interrupt-controller")) {
 287         node->intr_controll = true;
 288     }
 289
 290     else {
 291         return false;
 292     }
 293
 294     return true;
 295 }
 296
 297 static void
 298 __parse_other_prop(struct fdt_iter* it, struct dt_node_base* node)
 299 {
 300     struct dt_prop* prop;
 301     const char* key;
 302     unsigned int hash;
 303
 304     prop = valloc(sizeof(*prop));
 305     key  = fdtit_prop_key(it);
 306
 307     prop->key = HSTR(key, strlen(key));
 308     __mkprop_ptr(it, &prop->val);
 309
 310     hstr_rehash(&prop->key, HSTR_FULL_HASH);
 311     hash = prop->key.hash;
 312
 313     hashtable_hash_in(node->_op_bucket, &prop->ht, hash);
 314 }
 315
 316 static void
 317 __fill_node(struct fdt_iter* it, struct dt_node* node)
 318 {
 319     if (__parse_stdflags(it, &node->base)) {
 320         return;
 321     }
 322
 323     if (__parse_stdbase_prop(it, &node->base)) {
 324         return;
 325     }
 326
 327     if (__parse_stdnode_prop(it, node)) {
 328         return;
 329     }
 330
 331     if (__parse_stdintr_prop(it, &node->intr)) {
 332         return;
 333     }
 334
 335     __parse_other_prop(it, &node->base);
 336 }
 337
 338 static void
 339 __fill_root(struct fdt_iter* it, struct dt_root* node)
 340 {
 341     if (__parse_stdflags(it, &node->base)) {
 342         return;
 343     }
 344
 345     if (__parse_stdbase_prop(it, &node->base)) {
 346         return;
 347     }
 348
 349     struct fdt_prop* prop;
 350
 351     prop = it->prop;
 352     if (propeq(it, "serial-number")) {
 353         node->serial = (const char*)&prop[1];
 354     }
 355
 356     else if (propeq(it, "chassis-type")) {
 357         node->chassis = (const char*)&prop[1];
 358     }
 359
 360     __parse_other_prop(it, &node->base);
 361 }
 362
 363 static inline void
 364 __init_node(struct dt_node_base* node)
 365 {
 366     hashtable_init(node->_op_bucket);
 367     llist_init_head(&node->children);
 368 }
 369
 370 static inline void
 371 __init_node_regular(struct dt_node* node)
 372 {
 373     __init_node(&node->base);
 374     node->intr.parent_hnd = PHND_NULL;
 375 }
 376
 377 static void
 378 __expand_extended_intr(struct dt_intr_node* intrupt)
 379 {
 380     struct dt_prop_iter it;
 381     struct dt_prop_val  arr;
 382     struct dt_node *node;
 383     struct dt_node *master;
 384     struct dt_intr_prop* intr_prop;
 385
 386     if (!intrupt->intr.extended) {
 387         return;
 388     }
 389
 390     arr = intrupt->intr.arr;
 391     node = DT_NODE(intrupt);
 392
 393     llist_init_head(&intrupt->intr.values);
 394
 395     dt_decode(&it, &node->base, &arr, 1);
 396
 397     dt_phnd_t phnd;
 398     while(dtprop_next(&it)) {
 399         phnd   = dtprop_to_u32(it.prop_loc);
 400         master = dt_resolve_phandle(phnd);
 401
 402         if (!master) {
 403             WARN("dtb: (intr_extended) malformed phandle: %d", phnd);
 404             continue;
 405         }
 406
 407         intr_prop = valloc(sizeof(*intr_prop));
 408
 409         intr_prop->master = &master->intr;
 410         intr_prop->val = (struct dt_prop_val) {
 411             .encoded = it.prop_loc_next,
 412             .size    = master->base.intr_c
 413         };
 414
 415         llist_append(&intrupt->intr.values, &intr_prop->props);
 416         dtprop_next_n(&it, intr_prop->val.size);
 417     }
 418 }
 419
 420 static void
 421 __resolve_phnd_references()
 422 {
 423     struct dt_node_base *pos, *n;
 424     struct dt_node *node, *parent, *default_parent;
 425     struct dt_intr_node* intrupt;
 426     dt_phnd_t phnd;
 427
 428     llist_for_each(pos, n, &dtctx.nodes, nodes)
 429     {
 430         node = (struct dt_node*)pos;
 431         intrupt = &node->intr;
 432         if (!node->base.intr_c) {
 433             continue;
 434         }
 435
 436         phnd = intrupt->parent_hnd;
 437         default_parent = (struct dt_node*)node->base.parent;
 438         parent = default_parent;
 439
 440         if (phnd != PHND_NULL) {
 441             parent = dt_resolve_phandle(phnd);
 442         }
 443
 444         if (!parent) {
 445             WARN("dtb: (phnd_resolve) malformed phandle: %d", phnd);
 446             parent = default_parent;
 447         }
 448
 449         intrupt->parent = &parent->intr;
 450
 451         __expand_extended_intr(intrupt);
 452     }
 453 }
 454
 455 bool
 456 dt_load(ptr_t dtb_dropoff)
 457 {
 458     dtctx.reloacted_dtb = dtb_dropoff;
 459
 460     if (dtctx.fdt->magic != FDT_MAGIC) {
 461         ERROR("invalid dtb, unexpected magic: 0x%x", dtctx.fdt->magic);
 462         return false;
 463     }
 464
 465     size_t str_off = le(dtctx.fdt->size_dt_strings);
 466     dtctx.str_block = offset_t(dtb_dropoff, const char, str_off);
 467
 468     llist_init_head(&dtctx.nodes);
 469     hashtable_init(dtctx.phnds_table);
 470
 471     struct fdt_iter it;
 472     struct fdt_token* tok;
 473     struct dt_node_base *node, *prev;
 474
 475     struct dt_node_base* depth[16];
 476     bool is_root_level, filled;
 477
 478     node = NULL;
 479     depth[0] = NULL;
 480     fdt_itbegin(&it, dtctx.fdt);
 481
 482     while (fdt_itnext(&it)) {
 483         is_root_level = it.depth == 1;
 484
 485         if (it.depth >= 16) {
 486             // tree too deep
 487             ERROR("strange dtb, too deep to dive.");
 488             return false;
 489         }
 490
 491         depth[it.depth] = NULL;
 492         node = depth[it.depth - 1];
 493
 494         if (!node) {
 495             // need new node
 496             if (unlikely(is_root_level)) {
 497                 node = valloc(sizeof(struct dt_root));
 498                 __init_node(node);
 499             }
 500             else {
 501                 node = valloc(sizeof(struct dt_node));
 502                 prev = depth[it.depth - 2];
 503
 504                 __init_node_regular((struct dt_node*)node);
 505                 llist_append(&prev->children, &node->siblings);
 506                 node->parent = prev;
 507
 508                 llist_append(&dtctx.nodes, &node->nodes);
 509             }
 510
 511             node->name = (const char*)&it.pos[1];
 512         }
 513
 514         if (unlikely(is_root_level)) {
 515             __fill_root(&it, (struct dt_root*)node);
 516         }
 517         else {
 518             __fill_node(&it, (struct dt_node*)node);
 519         }
 520     }
 521
 522     fdt_itend(&it);
 523
 524     dtctx.root = (struct dt_root*)depth[0];
 525
 526     __resolve_phnd_references();
 527
 528     return true;
 529 }
 530
 531 static bool
 532 __name_starts_with(struct dt_node_base* node, const char* name)
 533 {
 534     int i = 0;
 535     const char* be_matched = node->name;
 536
 537     while (be_matched[i] && name[i])
 538     {
 539         if (be_matched[i] != name[i]) {
 540             return false;
 541         }
 542     }
 543
 544     return true;
 545 }
 546
 547 struct dt_node*
 548 dt_resolve_phandle(dt_phnd_t phandle)
 549 {
 550     struct dt_node_base *pos, *n;
 551     hashtable_hash_foreach(dtctx.phnds_table, phandle, pos, n, phnd_link)
 552     {
 553         if (pos->phandle == phandle) {
 554             return (struct dt_node*)pos;
 555         }
 556     }
 557
 558     return NULL;
 559 }
 560
 561 void
 562 dt_begin_find(struct dt_node_iter* iter,
 563               struct dt_node* node, const char* name)
 564 {
 565     node = node ? : (struct dt_node*)dtctx.root;
 566
 567     iter->head = &node->base;
 568     iter->matched = NULL;
 569     iter->name = name;
 570
 571     struct dt_node_base *pos, *n;
 572     llist_for_each(pos, n, &node->base.children, siblings)
 573     {
 574         if (__name_starts_with(pos, name)) {
 575             iter->matched = pos;
 576             break;
 577         }
 578     }
 579 }
 580
 581 bool
 582 dt_find_next(struct dt_node_iter* iter,
 583              struct dt_node_base** matched)
 584 {
 585     if (!dt_found_any(iter)) {
 586         return false;
 587     }
 588
 589     struct dt_node_base *pos, *head;
 590
 591     head = iter->head;
 592     pos = iter->matched;
 593     *matched = pos;
 594
 595     while (&pos->siblings != &head->children)
 596     {
 597         pos = list_next(pos, struct dt_node_base, siblings);
 598
 599         if (!__name_starts_with(pos, iter->name)) {
 600             continue;
 601         }
 602
 603         iter->matched = pos;
 604         return true;
 605     }
 606
 607     return false;
 608 }
 609
 610 struct dt_prop_val*
 611 dt_getprop(struct dt_node* node, const char* name)
 612 {
 613     struct hstr hashed_name;
 614     struct dt_prop *pos, *n;
 615     unsigned int hash;
 616
 617     hashed_name = HSTR(name, strlen(name));
 618     hstr_rehash(&hashed_name, HSTR_FULL_HASH);
 619     hash = hashed_name.hash;
 620
 621     hashtable_hash_foreach(node->base._op_bucket, hash, pos, n, ht)
 622     {
 623         if (HSTR_EQ(&pos->key, &hashed_name)) {
 624             return &pos->val;
 625         }
 626     }
 627
 628     return NULL;
 629 }