1/* 2 * This program is free software; you can redistribute it and/or modify 3 * it under the terms of the GNU General Public License as published by 4 * the Free Software Foundation; either version 2 of the License, or 5 * (at your option) any later version. 6 * 7 * This program is distributed in the hope that it will be useful, 8 * but WITHOUT ANY WARRANTY; without even the implied warranty of 9 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 10 * GNU General Public License for more details. 11 * 12 * You should have received a copy of the GNU General Public License 13 * along with this program; if not, write to the Free Software 14 * Foundation, 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. 15 * 16 * Copyright Pantelis Antoniou 2006 17 * Copyright (C) IBM Corporation 2006 18 * 19 * Authors: Pantelis Antoniou <pantelis@embeddedalley.com> 20 * Hollis Blanchard <hollisb@us.ibm.com> 21 * Mark A. Greer <mgreer@mvista.com> 22 * Paul Mackerras <paulus@samba.org> 23 */ 24 25#include <string.h> 26#include <stddef.h> 27#include "flatdevtree.h" 28#include "flatdevtree_env.h" 29 30#define _ALIGN(x, al) (((x) + (al) - 1) & ~((al) - 1)) 31 32static char *ft_root_node(struct ft_cxt *cxt) 33{ 34 return cxt->rgn[FT_STRUCT].start; 35} 36 37/* Routines for keeping node ptrs returned by ft_find_device current */ 38/* First entry not used b/c it would return 0 and be taken as NULL/error */ 39static void *ft_get_phandle(struct ft_cxt *cxt, char *node) 40{ 41 unsigned int i; 42 43 if (!node) 44 return NULL; 45 46 for (i = 1; i < cxt->nodes_used; i++) /* already there? */ 47 if (cxt->node_tbl[i] == node) 48 return (void *)i; 49 50 if (cxt->nodes_used < cxt->node_max) { 51 cxt->node_tbl[cxt->nodes_used] = node; 52 return (void *)cxt->nodes_used++; 53 } 54 55 return NULL; 56} 57 58static char *ft_node_ph2node(struct ft_cxt *cxt, const void *phandle) 59{ 60 unsigned int i = (unsigned int)phandle; 61 62 if (i < cxt->nodes_used) 63 return cxt->node_tbl[i]; 64 return NULL; 65} 66 67static void ft_node_update_before(struct ft_cxt *cxt, char *addr, int shift) 68{ 69 unsigned int i; 70 71 if (shift == 0) 72 return; 73 74 for (i = 1; i < cxt->nodes_used; i++) 75 if (cxt->node_tbl[i] < addr) 76 cxt->node_tbl[i] += shift; 77} 78 79static void ft_node_update_after(struct ft_cxt *cxt, char *addr, int shift) 80{ 81 unsigned int i; 82 83 if (shift == 0) 84 return; 85 86 for (i = 1; i < cxt->nodes_used; i++) 87 if (cxt->node_tbl[i] >= addr) 88 cxt->node_tbl[i] += shift; 89} 90 91/* Struct used to return info from ft_next() */ 92struct ft_atom { 93 u32 tag; 94 const char *name; 95 void *data; 96 u32 size; 97}; 98 99/* Set ptrs to current one's info; return addr of next one */ 100static char *ft_next(struct ft_cxt *cxt, char *p, struct ft_atom *ret) 101{ 102 u32 sz; 103 104 if (p >= cxt->rgn[FT_STRUCT].start + cxt->rgn[FT_STRUCT].size) 105 return NULL; 106 107 ret->tag = be32_to_cpu(*(u32 *) p); 108 p += 4; 109 110 switch (ret->tag) { /* Tag */ 111 case OF_DT_BEGIN_NODE: 112 ret->name = p; 113 ret->data = (void *)(p - 4); /* start of node */ 114 p += _ALIGN(strlen(p) + 1, 4); 115 break; 116 case OF_DT_PROP: 117 ret->size = sz = be32_to_cpu(*(u32 *) p); 118 ret->name = cxt->str_anchor + be32_to_cpu(*(u32 *) (p + 4)); 119 ret->data = (void *)(p + 8); 120 p += 8 + _ALIGN(sz, 4); 121 break; 122 case OF_DT_END_NODE: 123 case OF_DT_NOP: 124 break; 125 case OF_DT_END: 126 default: 127 p = NULL; 128 break; 129 } 130 131 return p; 132} 133 134#define HDR_SIZE _ALIGN(sizeof(struct boot_param_header), 8) 135#define EXPAND_INCR 1024 /* alloc this much extra when expanding */ 136 137/* See if the regions are in the standard order and non-overlapping */ 138static int ft_ordered(struct ft_cxt *cxt) 139{ 140 char *p = (char *)cxt->bph + HDR_SIZE; 141 enum ft_rgn_id r; 142 143 for (r = FT_RSVMAP; r <= FT_STRINGS; ++r) { 144 if (p > cxt->rgn[r].start) 145 return 0; 146 p = cxt->rgn[r].start + cxt->rgn[r].size; 147 } 148 return p <= (char *)cxt->bph + cxt->max_size; 149} 150 151/* Copy the tree to a newly-allocated region and put things in order */ 152static int ft_reorder(struct ft_cxt *cxt, int nextra) 153{ 154 unsigned long tot; 155 enum ft_rgn_id r; 156 char *p, *pend; 157 int stroff; 158 159 tot = HDR_SIZE + EXPAND_INCR; 160 for (r = FT_RSVMAP; r <= FT_STRINGS; ++r) 161 tot += cxt->rgn[r].size; 162 if (nextra > 0) 163 tot += nextra; 164 tot = _ALIGN(tot, 8); 165 166 if (!cxt->realloc) 167 return 0; 168 p = cxt->realloc(NULL, tot); 169 if (!p) 170 return 0; 171 172 memcpy(p, cxt->bph, sizeof(struct boot_param_header)); 173 /* offsets get fixed up later */ 174 175 cxt->bph = (struct boot_param_header *)p; 176 cxt->max_size = tot; 177 pend = p + tot; 178 p += HDR_SIZE; 179 180 memcpy(p, cxt->rgn[FT_RSVMAP].start, cxt->rgn[FT_RSVMAP].size); 181 cxt->rgn[FT_RSVMAP].start = p; 182 p += cxt->rgn[FT_RSVMAP].size; 183 184 memcpy(p, cxt->rgn[FT_STRUCT].start, cxt->rgn[FT_STRUCT].size); 185 ft_node_update_after(cxt, cxt->rgn[FT_STRUCT].start, 186 p - cxt->rgn[FT_STRUCT].start); 187 cxt->p += p - cxt->rgn[FT_STRUCT].start; 188 cxt->rgn[FT_STRUCT].start = p; 189 190 p = pend - cxt->rgn[FT_STRINGS].size; 191 memcpy(p, cxt->rgn[FT_STRINGS].start, cxt->rgn[FT_STRINGS].size); 192 stroff = cxt->str_anchor - cxt->rgn[FT_STRINGS].start; 193 cxt->rgn[FT_STRINGS].start = p; 194 cxt->str_anchor = p + stroff; 195 196 cxt->isordered = 1; 197 return 1; 198} 199 200static inline char *prev_end(struct ft_cxt *cxt, enum ft_rgn_id r) 201{ 202 if (r > FT_RSVMAP) 203 return cxt->rgn[r - 1].start + cxt->rgn[r - 1].size; 204 return (char *)cxt->bph + HDR_SIZE; 205} 206 207static inline char *next_start(struct ft_cxt *cxt, enum ft_rgn_id r) 208{ 209 if (r < FT_STRINGS) 210 return cxt->rgn[r + 1].start; 211 return (char *)cxt->bph + cxt->max_size; 212} 213 214/* 215 * See if we can expand region rgn by nextra bytes by using up 216 * free space after or before the region. 217 */ 218static int ft_shuffle(struct ft_cxt *cxt, char **pp, enum ft_rgn_id rgn, 219 int nextra) 220{ 221 char *p = *pp; 222 char *rgn_start, *rgn_end; 223 224 rgn_start = cxt->rgn[rgn].start; 225 rgn_end = rgn_start + cxt->rgn[rgn].size; 226 if (nextra <= 0 || rgn_end + nextra <= next_start(cxt, rgn)) { 227 /* move following stuff */ 228 if (p < rgn_end) { 229 if (nextra < 0) 230 memmove(p, p - nextra, rgn_end - p + nextra); 231 else 232 memmove(p + nextra, p, rgn_end - p); 233 if (rgn == FT_STRUCT) 234 ft_node_update_after(cxt, p, nextra); 235 } 236 cxt->rgn[rgn].size += nextra; 237 if (rgn == FT_STRINGS) 238 /* assumes strings only added at beginning */ 239 cxt->str_anchor += nextra; 240 return 1; 241 } 242 if (prev_end(cxt, rgn) <= rgn_start - nextra) { 243 /* move preceding stuff */ 244 if (p > rgn_start) { 245 memmove(rgn_start - nextra, rgn_start, p - rgn_start); 246 if (rgn == FT_STRUCT) 247 ft_node_update_before(cxt, p, -nextra); 248 } 249 *pp -= nextra; 250 cxt->rgn[rgn].start -= nextra; 251 cxt->rgn[rgn].size += nextra; 252 return 1; 253 } 254 return 0; 255} 256 257static int ft_make_space(struct ft_cxt *cxt, char **pp, enum ft_rgn_id rgn, 258 int nextra) 259{ 260 unsigned long size, ssize, tot; 261 char *str, *next; 262 enum ft_rgn_id r; 263 264 if (!cxt->isordered) { 265 unsigned long rgn_off = *pp - cxt->rgn[rgn].start; 266 267 if (!ft_reorder(cxt, nextra)) 268 return 0; 269 270 *pp = cxt->rgn[rgn].start + rgn_off; 271 } 272 if (ft_shuffle(cxt, pp, rgn, nextra)) 273 return 1; 274 275 /* See if there is space after the strings section */ 276 ssize = cxt->rgn[FT_STRINGS].size; 277 if (cxt->rgn[FT_STRINGS].start + ssize 278 < (char *)cxt->bph + cxt->max_size) { 279 /* move strings up as far as possible */ 280 str = (char *)cxt->bph + cxt->max_size - ssize; 281 cxt->str_anchor += str - cxt->rgn[FT_STRINGS].start; 282 memmove(str, cxt->rgn[FT_STRINGS].start, ssize); 283 cxt->rgn[FT_STRINGS].start = str; 284 /* enough space now? */ 285 if (rgn >= FT_STRUCT && ft_shuffle(cxt, pp, rgn, nextra)) 286 return 1; 287 } 288 289 /* how much total free space is there following this region? */ 290 tot = 0; 291 for (r = rgn; r < FT_STRINGS; ++r) { 292 char *r_end = cxt->rgn[r].start + cxt->rgn[r].size; 293 tot += next_start(cxt, rgn) - r_end; 294 } 295 296 /* cast is to shut gcc up; we know nextra >= 0 */ 297 if (tot < (unsigned int)nextra) { 298 /* have to reallocate */ 299 char *newp, *new_start; 300 int shift; 301 302 if (!cxt->realloc) 303 return 0; 304 size = _ALIGN(cxt->max_size + (nextra - tot) + EXPAND_INCR, 8); 305 newp = cxt->realloc(cxt->bph, size); 306 if (!newp) 307 return 0; 308 cxt->max_size = size; 309 shift = newp - (char *)cxt->bph; 310 311 if (shift) { /* realloc can return same addr */ 312 cxt->bph = (struct boot_param_header *)newp; 313 ft_node_update_after(cxt, cxt->rgn[FT_STRUCT].start, 314 shift); 315 for (r = FT_RSVMAP; r <= FT_STRINGS; ++r) { 316 new_start = cxt->rgn[r].start + shift; 317 cxt->rgn[r].start = new_start; 318 } 319 *pp += shift; 320 cxt->str_anchor += shift; 321 } 322 323 /* move strings up to the end */ 324 str = newp + size - ssize; 325 cxt->str_anchor += str - cxt->rgn[FT_STRINGS].start; 326 memmove(str, cxt->rgn[FT_STRINGS].start, ssize); 327 cxt->rgn[FT_STRINGS].start = str; 328 329 if (ft_shuffle(cxt, pp, rgn, nextra)) 330 return 1; 331 } 332 333 /* must be FT_RSVMAP and we need to move FT_STRUCT up */ 334 if (rgn == FT_RSVMAP) { 335 next = cxt->rgn[FT_RSVMAP].start + cxt->rgn[FT_RSVMAP].size 336 + nextra; 337 ssize = cxt->rgn[FT_STRUCT].size; 338 if (next + ssize >= cxt->rgn[FT_STRINGS].start) 339 return 0; /* "can't happen" */ 340 memmove(next, cxt->rgn[FT_STRUCT].start, ssize); 341 ft_node_update_after(cxt, cxt->rgn[FT_STRUCT].start, nextra); 342 cxt->rgn[FT_STRUCT].start = next; 343 344 if (ft_shuffle(cxt, pp, rgn, nextra)) 345 return 1; 346 } 347 348 return 0; /* "can't happen" */ 349} 350 351static void ft_put_word(struct ft_cxt *cxt, u32 v) 352{ 353 *(u32 *) cxt->p = cpu_to_be32(v); 354 cxt->p += 4; 355} 356 357static void ft_put_bin(struct ft_cxt *cxt, const void *data, unsigned int sz) 358{ 359 unsigned long sza = _ALIGN(sz, 4); 360 361 /* zero out the alignment gap if necessary */ 362 if (sz < sza) 363 *(u32 *) (cxt->p + sza - 4) = 0; 364 365 /* copy in the data */ 366 memcpy(cxt->p, data, sz); 367 368 cxt->p += sza; 369} 370 371int ft_begin_node(struct ft_cxt *cxt, const char *name) 372{ 373 unsigned long nlen = strlen(name) + 1; 374 unsigned long len = 8 + _ALIGN(nlen, 4); 375 376 if (!ft_make_space(cxt, &cxt->p, FT_STRUCT, len)) 377 return -1; 378 ft_put_word(cxt, OF_DT_BEGIN_NODE); 379 ft_put_bin(cxt, name, strlen(name) + 1); 380 return 0; 381} 382 383void ft_end_node(struct ft_cxt *cxt) 384{ 385 ft_put_word(cxt, OF_DT_END_NODE); 386} 387 388void ft_nop(struct ft_cxt *cxt) 389{ 390 if (ft_make_space(cxt, &cxt->p, FT_STRUCT, 4)) 391 ft_put_word(cxt, OF_DT_NOP); 392} 393 394#define NO_STRING 0x7fffffff 395 396static int lookup_string(struct ft_cxt *cxt, const char *name) 397{ 398 char *p, *end; 399 400 p = cxt->rgn[FT_STRINGS].start; 401 end = p + cxt->rgn[FT_STRINGS].size; 402 while (p < end) { 403 if (strcmp(p, (char *)name) == 0) 404 return p - cxt->str_anchor; 405 p += strlen(p) + 1; 406 } 407 408 return NO_STRING; 409} 410 411/* lookup string and insert if not found */ 412static int map_string(struct ft_cxt *cxt, const char *name) 413{ 414 int off; 415 char *p; 416 417 off = lookup_string(cxt, name); 418 if (off != NO_STRING) 419 return off; 420 p = cxt->rgn[FT_STRINGS].start; 421 if (!ft_make_space(cxt, &p, FT_STRINGS, strlen(name) + 1)) 422 return NO_STRING; 423 strcpy(p, name); 424 return p - cxt->str_anchor; 425} 426 427int ft_prop(struct ft_cxt *cxt, const char *name, const void *data, 428 unsigned int sz) 429{ 430 int off, len; 431 432 off = map_string(cxt, name); 433 if (off == NO_STRING) 434 return -1; 435 436 len = 12 + _ALIGN(sz, 4); 437 if (!ft_make_space(cxt, &cxt->p, FT_STRUCT, len)) 438 return -1; 439 440 ft_put_word(cxt, OF_DT_PROP); 441 ft_put_word(cxt, sz); 442 ft_put_word(cxt, off); 443 ft_put_bin(cxt, data, sz); 444 return 0; 445} 446 447int ft_prop_str(struct ft_cxt *cxt, const char *name, const char *str) 448{ 449 return ft_prop(cxt, name, str, strlen(str) + 1); 450} 451 452int ft_prop_int(struct ft_cxt *cxt, const char *name, unsigned int val) 453{ 454 u32 v = cpu_to_be32((u32) val); 455 456 return ft_prop(cxt, name, &v, 4); 457} 458 459/* Calculate the size of the reserved map */ 460static unsigned long rsvmap_size(struct ft_cxt *cxt) 461{ 462 struct ft_reserve *res; 463 464 res = (struct ft_reserve *)cxt->rgn[FT_RSVMAP].start; 465 while (res->start || res->len) 466 ++res; 467 return (char *)(res + 1) - cxt->rgn[FT_RSVMAP].start; 468} 469 470/* Calculate the size of the struct region by stepping through it */ 471static unsigned long struct_size(struct ft_cxt *cxt) 472{ 473 char *p = cxt->rgn[FT_STRUCT].start; 474 char *next; 475 struct ft_atom atom; 476 477 /* make check in ft_next happy */ 478 if (cxt->rgn[FT_STRUCT].size == 0) 479 cxt->rgn[FT_STRUCT].size = 0xfffffffful - (unsigned long)p; 480 481 while ((next = ft_next(cxt, p, &atom)) != NULL) 482 p = next; 483 return p + 4 - cxt->rgn[FT_STRUCT].start; 484} 485 486/* add `adj' on to all string offset values in the struct area */ 487static void adjust_string_offsets(struct ft_cxt *cxt, int adj) 488{ 489 char *p = cxt->rgn[FT_STRUCT].start; 490 char *next; 491 struct ft_atom atom; 492 int off; 493 494 while ((next = ft_next(cxt, p, &atom)) != NULL) { 495 if (atom.tag == OF_DT_PROP) { 496 off = be32_to_cpu(*(u32 *) (p + 8)); 497 *(u32 *) (p + 8) = cpu_to_be32(off + adj); 498 } 499 p = next; 500 } 501} 502 503/* start construction of the flat OF tree from scratch */ 504void ft_begin(struct ft_cxt *cxt, void *blob, unsigned int max_size, 505 void *(*realloc_fn) (void *, unsigned long)) 506{ 507 struct boot_param_header *bph = blob; 508 char *p; 509 struct ft_reserve *pres; 510 511 /* clear the cxt */ 512 memset(cxt, 0, sizeof(*cxt)); 513 514 cxt->bph = bph; 515 cxt->max_size = max_size; 516 cxt->realloc = realloc_fn; 517 cxt->isordered = 1; 518 519 /* zero everything in the header area */ 520 memset(bph, 0, sizeof(*bph)); 521 522 bph->magic = cpu_to_be32(OF_DT_HEADER); 523 bph->version = cpu_to_be32(0x10); 524 bph->last_comp_version = cpu_to_be32(0x10); 525 526 /* start pointers */ 527 cxt->rgn[FT_RSVMAP].start = p = blob + HDR_SIZE; 528 cxt->rgn[FT_RSVMAP].size = sizeof(struct ft_reserve); 529 pres = (struct ft_reserve *)p; 530 cxt->rgn[FT_STRUCT].start = p += sizeof(struct ft_reserve); 531 cxt->rgn[FT_STRUCT].size = 4; 532 cxt->rgn[FT_STRINGS].start = blob + max_size; 533 cxt->rgn[FT_STRINGS].size = 0; 534 535 /* init rsvmap and struct */ 536 pres->start = 0; 537 pres->len = 0; 538 *(u32 *) p = cpu_to_be32(OF_DT_END); 539 540 cxt->str_anchor = blob; 541} 542 543/* open up an existing blob to be examined or modified */ 544int ft_open(struct ft_cxt *cxt, void *blob, unsigned int max_size, 545 unsigned int max_find_device, 546 void *(*realloc_fn) (void *, unsigned long)) 547{ 548 struct boot_param_header *bph = blob; 549 550 /* can't cope with version < 16 */ 551 if (be32_to_cpu(bph->version) < 16) 552 return -1; 553 554 /* clear the cxt */ 555 memset(cxt, 0, sizeof(*cxt)); 556 557 /* alloc node_tbl to track node ptrs returned by ft_find_device */ 558 ++max_find_device; 559 cxt->node_tbl = realloc_fn(NULL, max_find_device * sizeof(char *)); 560 if (!cxt->node_tbl) 561 return -1; 562 memset(cxt->node_tbl, 0, max_find_device * sizeof(char *)); 563 cxt->node_max = max_find_device; 564 cxt->nodes_used = 1; /* don't use idx 0 b/c looks like NULL */ 565 566 cxt->bph = bph; 567 cxt->max_size = max_size; 568 cxt->realloc = realloc_fn; 569 570 cxt->rgn[FT_RSVMAP].start = blob + be32_to_cpu(bph->off_mem_rsvmap); 571 cxt->rgn[FT_RSVMAP].size = rsvmap_size(cxt); 572 cxt->rgn[FT_STRUCT].start = blob + be32_to_cpu(bph->off_dt_struct); 573 cxt->rgn[FT_STRUCT].size = struct_size(cxt); 574 cxt->rgn[FT_STRINGS].start = blob + be32_to_cpu(bph->off_dt_strings); 575 cxt->rgn[FT_STRINGS].size = be32_to_cpu(bph->dt_strings_size); 576 /* Leave as '0' to force first ft_make_space call to do a ft_reorder 577 * and move dt to an area allocated by realloc. 578 cxt->isordered = ft_ordered(cxt); 579 */ 580 581 cxt->p = cxt->rgn[FT_STRUCT].start; 582 cxt->str_anchor = cxt->rgn[FT_STRINGS].start; 583 584 return 0; 585} 586 587/* add a reserver physical area to the rsvmap */ 588int ft_add_rsvmap(struct ft_cxt *cxt, u64 physaddr, u64 size) 589{ 590 char *p; 591 struct ft_reserve *pres; 592 593 p = cxt->rgn[FT_RSVMAP].start + cxt->rgn[FT_RSVMAP].size 594 - sizeof(struct ft_reserve); 595 if (!ft_make_space(cxt, &p, FT_RSVMAP, sizeof(struct ft_reserve))) 596 return -1; 597 598 pres = (struct ft_reserve *)p; 599 pres->start = cpu_to_be64(physaddr); 600 pres->len = cpu_to_be64(size); 601 602 return 0; 603} 604 605void ft_begin_tree(struct ft_cxt *cxt) 606{ 607 cxt->p = ft_root_node(cxt); 608} 609 610void ft_end_tree(struct ft_cxt *cxt) 611{ 612 struct boot_param_header *bph = cxt->bph; 613 char *p, *oldstr, *str, *endp; 614 unsigned long ssize; 615 int adj; 616 617 if (!cxt->isordered) 618 return; /* we haven't touched anything */ 619 620 /* adjust string offsets */ 621 oldstr = cxt->rgn[FT_STRINGS].start; 622 adj = cxt->str_anchor - oldstr; 623 if (adj) 624 adjust_string_offsets(cxt, adj); 625 626 /* make strings end on 8-byte boundary */ 627 ssize = cxt->rgn[FT_STRINGS].size; 628 endp = (char *)_ALIGN((unsigned long)cxt->rgn[FT_STRUCT].start 629 + cxt->rgn[FT_STRUCT].size + ssize, 8); 630 str = endp - ssize; 631 632 /* move strings down to end of structs */ 633 memmove(str, oldstr, ssize); 634 cxt->str_anchor = str; 635 cxt->rgn[FT_STRINGS].start = str; 636 637 /* fill in header fields */ 638 p = (char *)bph; 639 bph->totalsize = cpu_to_be32(endp - p); 640 bph->off_mem_rsvmap = cpu_to_be32(cxt->rgn[FT_RSVMAP].start - p); 641 bph->off_dt_struct = cpu_to_be32(cxt->rgn[FT_STRUCT].start - p); 642 bph->off_dt_strings = cpu_to_be32(cxt->rgn[FT_STRINGS].start - p); 643 bph->dt_strings_size = cpu_to_be32(ssize); 644} 645 646void *ft_find_device(struct ft_cxt *cxt, const char *srch_path) 647{ 648 char *node; 649 650 /* require absolute path */ 651 if (srch_path[0] != '/') 652 return NULL; 653 node = ft_find_descendent(cxt, ft_root_node(cxt), srch_path); 654 return ft_get_phandle(cxt, node); 655} 656 657void *ft_find_device_rel(struct ft_cxt *cxt, const void *top, 658 const char *srch_path) 659{ 660 char *node; 661 662 node = ft_node_ph2node(cxt, top); 663 if (node == NULL) 664 return NULL; 665 666 node = ft_find_descendent(cxt, node, srch_path); 667 return ft_get_phandle(cxt, node); 668} 669 670void *ft_find_descendent(struct ft_cxt *cxt, void *top, const char *srch_path) 671{ 672 struct ft_atom atom; 673 char *p; 674 const char *cp, *q; 675 int cl; 676 int depth = -1; 677 int dmatch = 0; 678 const char *path_comp[FT_MAX_DEPTH]; 679 680 cp = srch_path; 681 cl = 0; 682 p = top; 683 684 while ((p = ft_next(cxt, p, &atom)) != NULL) { 685 switch (atom.tag) { 686 case OF_DT_BEGIN_NODE: 687 ++depth; 688 if (depth != dmatch) 689 break; 690 cxt->genealogy[depth] = atom.data; 691 cxt->genealogy[depth + 1] = NULL; 692 if (depth && !(strncmp(atom.name, cp, cl) == 0 693 && (atom.name[cl] == '/' 694 || atom.name[cl] == '\0' 695 || atom.name[cl] == '@'))) 696 break; 697 path_comp[dmatch] = cp; 698 /* it matches so far, advance to next path component */ 699 cp += cl; 700 /* skip slashes */ 701 while (*cp == '/') 702 ++cp; 703 /* we're done if this is the end of the string */ 704 if (*cp == 0) 705 return atom.data; 706 /* look for end of this component */ 707 q = strchr(cp, '/'); 708 if (q) 709 cl = q - cp; 710 else 711 cl = strlen(cp); 712 ++dmatch; 713 break; 714 case OF_DT_END_NODE: 715 if (depth == 0) 716 return NULL; 717 if (dmatch > depth) { 718 --dmatch; 719 cl = cp - path_comp[dmatch] - 1; 720 cp = path_comp[dmatch]; 721 while (cl > 0 && cp[cl - 1] == '/') 722 --cl; 723 } 724 --depth; 725 break; 726 } 727 } 728 return NULL; 729} 730 731void *__ft_get_parent(struct ft_cxt *cxt, void *node) 732{ 733 int d; 734 struct ft_atom atom; 735 char *p; 736 737 for (d = 0; cxt->genealogy[d] != NULL; ++d) 738 if (cxt->genealogy[d] == node) 739 return d > 0 ? cxt->genealogy[d - 1] : NULL; 740 741 /* have to do it the hard way... */ 742 p = ft_root_node(cxt); 743 d = 0; 744 while ((p = ft_next(cxt, p, &atom)) != NULL) { 745 switch (atom.tag) { 746 case OF_DT_BEGIN_NODE: 747 cxt->genealogy[d] = atom.data; 748 if (node == atom.data) { 749 /* found it */ 750 cxt->genealogy[d + 1] = NULL; 751 return d > 0 ? cxt->genealogy[d - 1] : NULL; 752 } 753 ++d; 754 break; 755 case OF_DT_END_NODE: 756 --d; 757 break; 758 } 759 } 760 return NULL; 761} 762 763void *ft_get_parent(struct ft_cxt *cxt, const void *phandle) 764{ 765 void *node = ft_node_ph2node(cxt, phandle); 766 if (node == NULL) 767 return NULL; 768 769 node = __ft_get_parent(cxt, node); 770 return ft_get_phandle(cxt, node); 771} 772 773static const void *__ft_get_prop(struct ft_cxt *cxt, void *node, 774 const char *propname, unsigned int *len) 775{ 776 struct ft_atom atom; 777 int depth = 0; 778 779 while ((node = ft_next(cxt, node, &atom)) != NULL) { 780 switch (atom.tag) { 781 case OF_DT_BEGIN_NODE: 782 ++depth; 783 break; 784 785 case OF_DT_PROP: 786 if (depth != 1 || strcmp(atom.name, propname)) 787 break; 788 789 if (len) 790 *len = atom.size; 791 792 return atom.data; 793 794 case OF_DT_END_NODE: 795 if (--depth <= 0) 796 return NULL; 797 } 798 } 799 800 return NULL; 801} 802 803int ft_get_prop(struct ft_cxt *cxt, const void *phandle, const char *propname, 804 void *buf, const unsigned int buflen) 805{ 806 const void *data; 807 unsigned int size; 808 809 void *node = ft_node_ph2node(cxt, phandle); 810 if (!node) 811 return -1; 812 813 data = __ft_get_prop(cxt, node, propname, &size); 814 if (data) { 815 unsigned int clipped_size = min(size, buflen); 816 memcpy(buf, data, clipped_size); 817 return size; 818 } 819 820 return -1; 821} 822 823void *__ft_find_node_by_prop_value(struct ft_cxt *cxt, void *prev, 824 const char *propname, const char *propval, 825 unsigned int proplen) 826{ 827 struct ft_atom atom; 828 char *p = ft_root_node(cxt); 829 char *next; 830 int past_prev = prev ? 0 : 1; 831 int depth = -1; 832 833 while ((next = ft_next(cxt, p, &atom)) != NULL) { 834 const void *data; 835 unsigned int size; 836 837 switch (atom.tag) { 838 case OF_DT_BEGIN_NODE: 839 depth++; 840 841 if (prev == p) { 842 past_prev = 1; 843 break; 844 } 845 846 if (!past_prev || depth < 1) 847 break; 848 849 data = __ft_get_prop(cxt, p, propname, &size); 850 if (!data || size != proplen) 851 break; 852 if (memcmp(data, propval, size)) 853 break; 854 855 return p; 856 857 case OF_DT_END_NODE: 858 if (depth-- == 0) 859 return NULL; 860 861 break; 862 } 863 864 p = next; 865 } 866 867 return NULL; 868} 869 870void *ft_find_node_by_prop_value(struct ft_cxt *cxt, const void *prev, 871 const char *propname, const char *propval, 872 int proplen) 873{ 874 void *node = NULL; 875 876 if (prev) { 877 node = ft_node_ph2node(cxt, prev); 878 879 if (!node) 880 return NULL; 881 } 882 883 node = __ft_find_node_by_prop_value(cxt, node, propname, 884 propval, proplen); 885 return ft_get_phandle(cxt, node); 886} 887 888int ft_set_prop(struct ft_cxt *cxt, const void *phandle, const char *propname, 889 const void *buf, const unsigned int buflen) 890{ 891 struct ft_atom atom; 892 void *node; 893 char *p, *next; 894 int nextra; 895 896 node = ft_node_ph2node(cxt, phandle); 897 if (node == NULL) 898 return -1; 899 900 next = ft_next(cxt, node, &atom); 901 if (atom.tag != OF_DT_BEGIN_NODE) 902 /* phandle didn't point to a node */ 903 return -1; 904 p = next; 905 906 while ((next = ft_next(cxt, p, &atom)) != NULL) { 907 switch (atom.tag) { 908 case OF_DT_BEGIN_NODE: /* properties must go before subnodes */ 909 case OF_DT_END_NODE: 910 /* haven't found the property, insert here */ 911 cxt->p = p; 912 return ft_prop(cxt, propname, buf, buflen); 913 case OF_DT_PROP: 914 if (strcmp(atom.name, propname)) 915 break; 916 /* found an existing property, overwrite it */ 917 nextra = _ALIGN(buflen, 4) - _ALIGN(atom.size, 4); 918 cxt->p = atom.data; 919 if (nextra && !ft_make_space(cxt, &cxt->p, FT_STRUCT, 920 nextra)) 921 return -1; 922 *(u32 *) (cxt->p - 8) = cpu_to_be32(buflen); 923 ft_put_bin(cxt, buf, buflen); 924 return 0; 925 } 926 p = next; 927 } 928 return -1; 929} 930 931int ft_del_prop(struct ft_cxt *cxt, const void *phandle, const char *propname) 932{ 933 struct ft_atom atom; 934 void *node; 935 char *p, *next; 936 int size; 937 938 node = ft_node_ph2node(cxt, phandle); 939 if (node == NULL) 940 return -1; 941 942 p = node; 943 while ((next = ft_next(cxt, p, &atom)) != NULL) { 944 switch (atom.tag) { 945 case OF_DT_BEGIN_NODE: 946 case OF_DT_END_NODE: 947 return -1; 948 case OF_DT_PROP: 949 if (strcmp(atom.name, propname)) 950 break; 951 /* found the property, remove it */ 952 size = 12 + -_ALIGN(atom.size, 4); 953 cxt->p = p; 954 if (!ft_make_space(cxt, &cxt->p, FT_STRUCT, -size)) 955 return -1; 956 return 0; 957 } 958 p = next; 959 } 960 return -1; 961} 962 963void *ft_create_node(struct ft_cxt *cxt, const void *parent, const char *name) 964{ 965 struct ft_atom atom; 966 char *p, *next; 967 int depth = 0; 968 969 if (parent) { 970 p = ft_node_ph2node(cxt, parent); 971 if (!p) 972 return NULL; 973 } else { 974 p = ft_root_node(cxt); 975 } 976 977 while ((next = ft_next(cxt, p, &atom)) != NULL) { 978 switch (atom.tag) { 979 case OF_DT_BEGIN_NODE: 980 ++depth; 981 if (depth == 1 && strcmp(atom.name, name) == 0) 982 /* duplicate node name, return error */ 983 return NULL; 984 break; 985 case OF_DT_END_NODE: 986 --depth; 987 if (depth > 0) 988 break; 989 /* end of node, insert here */ 990 cxt->p = p; 991 ft_begin_node(cxt, name); 992 ft_end_node(cxt); 993 return p; 994 } 995 p = next; 996 } 997 return NULL; 998} 999