arch/powerpc/boot/flatdevtree.c

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