stand/fdt/fdt_loader_cmd.c

   1 /*-
   2  * Copyright (c) 2009-2010 The FreeBSD Foundation
   3  * All rights reserved.
   4  *
   5  * This software was developed by Semihalf under sponsorship from
   6  * the FreeBSD Foundation.
   7  *
   8  * Redistribution and use in source and binary forms, with or without
   9  * modification, are permitted provided that the following conditions
  10  * are met:
  11  * 1. Redistributions of source code must retain the above copyright
  12  *    notice, this list of conditions and the following disclaimer.
  13  * 2. Redistributions in binary form must reproduce the above copyright
  14  *    notice, this list of conditions and the following disclaimer in the
  15  *    documentation and/or other materials provided with the distribution.
  16  *
  17  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
  18  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  20  * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
  21  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  22  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  23  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  24  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  25  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  26  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  27  * SUCH DAMAGE.
  28  */
  29
  30 #include <sys/cdefs.h>
  31 __FBSDID("$FreeBSD$");
  32
  33 #include <stand.h>
  34 #include <libfdt.h>
  35 #include <fdt.h>
  36 #include <sys/param.h>
  37 #include <sys/linker.h>
  38 #include <machine/elf.h>
  39
  40 #include "bootstrap.h"
  41 #include "fdt_platform.h"
  42
  43 #ifdef DEBUG
  44 #define debugf(fmt, args...) do { printf("%s(): ", __func__);   \
  45     printf(fmt,##args); } while (0)
  46 #else
  47 #define debugf(fmt, args...)
  48 #endif
  49
  50 #define FDT_CWD_LEN     256
  51 #define FDT_MAX_DEPTH   12
  52
  53 #define FDT_PROP_SEP    " = "
  54
  55 #define COPYOUT(s,d,l)  archsw.arch_copyout(s, d, l)
  56 #define COPYIN(s,d,l)   archsw.arch_copyin(s, d, l)
  57
  58 #define FDT_STATIC_DTB_SYMBOL   "fdt_static_dtb"
  59
  60 #define CMD_REQUIRES_BLOB       0x01
  61
  62 /* Location of FDT yet to be loaded. */
  63 /* This may be in read-only memory, so can't be manipulated directly. */
  64 static struct fdt_header *fdt_to_load = NULL;
  65 /* Location of FDT on heap. */
  66 /* This is the copy we actually manipulate. */
  67 static struct fdt_header *fdtp = NULL;
  68 /* Size of FDT blob */
  69 static size_t fdtp_size = 0;
  70 /* Have we loaded all the needed overlays */
  71 static int fdt_overlays_applied = 0;
  72
  73 static int fdt_load_dtb(vm_offset_t va);
  74 static void fdt_print_overlay_load_error(int err, const char *filename);
  75 static int fdt_check_overlay_compatible(void *base_fdt, void *overlay_fdt);
  76
  77 static int fdt_cmd_nyi(int argc, char *argv[]);
  78 static int fdt_load_dtb_overlays_string(const char * filenames);
  79
  80 static int fdt_cmd_addr(int argc, char *argv[]);
  81 static int fdt_cmd_mkprop(int argc, char *argv[]);
  82 static int fdt_cmd_cd(int argc, char *argv[]);
  83 static int fdt_cmd_hdr(int argc, char *argv[]);
  84 static int fdt_cmd_ls(int argc, char *argv[]);
  85 static int fdt_cmd_prop(int argc, char *argv[]);
  86 static int fdt_cmd_pwd(int argc, char *argv[]);
  87 static int fdt_cmd_rm(int argc, char *argv[]);
  88 static int fdt_cmd_mknode(int argc, char *argv[]);
  89 static int fdt_cmd_mres(int argc, char *argv[]);
  90
  91 typedef int cmdf_t(int, char *[]);
  92
  93 struct cmdtab {
  94         const char      *name;
  95         cmdf_t          *handler;
  96         int             flags;
  97 };
  98
  99 static const struct cmdtab commands[] = {
 100         { "addr", &fdt_cmd_addr,        0 },
 101         { "alias", &fdt_cmd_nyi,        0 },
 102         { "cd", &fdt_cmd_cd,            CMD_REQUIRES_BLOB },
 103         { "header", &fdt_cmd_hdr,       CMD_REQUIRES_BLOB },
 104         { "ls", &fdt_cmd_ls,            CMD_REQUIRES_BLOB },
 105         { "mknode", &fdt_cmd_mknode,    CMD_REQUIRES_BLOB },
 106         { "mkprop", &fdt_cmd_mkprop,    CMD_REQUIRES_BLOB },
 107         { "mres", &fdt_cmd_mres,        CMD_REQUIRES_BLOB },
 108         { "prop", &fdt_cmd_prop,        CMD_REQUIRES_BLOB },
 109         { "pwd", &fdt_cmd_pwd,          CMD_REQUIRES_BLOB },
 110         { "rm", &fdt_cmd_rm,            CMD_REQUIRES_BLOB },
 111         { NULL, NULL }
 112 };
 113
 114 static char cwd[FDT_CWD_LEN] = "/";
 115
 116 static vm_offset_t
 117 fdt_find_static_dtb()
 118 {
 119         Elf_Ehdr *ehdr;
 120         Elf_Shdr *shdr;
 121         Elf_Sym sym;
 122         vm_offset_t strtab, symtab, fdt_start;
 123         uint64_t offs;
 124         struct preloaded_file *kfp;
 125         struct file_metadata *md;
 126         char *strp;
 127         int i, sym_count;
 128
 129         debugf("fdt_find_static_dtb()\n");
 130
 131         sym_count = symtab = strtab = 0;
 132         strp = NULL;
 133
 134         offs = __elfN(relocation_offset);
 135
 136         kfp = file_findfile(NULL, NULL);
 137         if (kfp == NULL)
 138                 return (0);
 139
 140         /* Locate the dynamic symbols and strtab. */
 141         md = file_findmetadata(kfp, MODINFOMD_ELFHDR);
 142         if (md == NULL)
 143                 return (0);
 144         ehdr = (Elf_Ehdr *)md->md_data;
 145
 146         md = file_findmetadata(kfp, MODINFOMD_SHDR);
 147         if (md == NULL)
 148                 return (0);
 149         shdr = (Elf_Shdr *)md->md_data;
 150
 151         for (i = 0; i < ehdr->e_shnum; ++i) {
 152                 if (shdr[i].sh_type == SHT_DYNSYM && symtab == 0) {
 153                         symtab = shdr[i].sh_addr + offs;
 154                         sym_count = shdr[i].sh_size / sizeof(Elf_Sym);
 155                 } else if (shdr[i].sh_type == SHT_STRTAB && strtab == 0) {
 156                         strtab = shdr[i].sh_addr + offs;
 157                 }
 158         }
 159
 160         /*
 161          * The most efficient way to find a symbol would be to calculate a
 162          * hash, find proper bucket and chain, and thus find a symbol.
 163          * However, that would involve code duplication (e.g. for hash
 164          * function). So we're using simpler and a bit slower way: we're
 165          * iterating through symbols, searching for the one which name is
 166          * 'equal' to 'fdt_static_dtb'. To speed up the process a little bit,
 167          * we are eliminating symbols type of which is not STT_NOTYPE, or(and)
 168          * those which binding attribute is not STB_GLOBAL.
 169          */
 170         fdt_start = 0;
 171         while (sym_count > 0 && fdt_start == 0) {
 172                 COPYOUT(symtab, &sym, sizeof(sym));
 173                 symtab += sizeof(sym);
 174                 --sym_count;
 175                 if (ELF_ST_BIND(sym.st_info) != STB_GLOBAL ||
 176                     ELF_ST_TYPE(sym.st_info) != STT_NOTYPE)
 177                         continue;
 178                 strp = strdupout(strtab + sym.st_name);
 179                 if (strcmp(strp, FDT_STATIC_DTB_SYMBOL) == 0)
 180                         fdt_start = (vm_offset_t)sym.st_value + offs;
 181                 free(strp);
 182         }
 183         return (fdt_start);
 184 }
 185
 186 static int
 187 fdt_load_dtb(vm_offset_t va)
 188 {
 189         struct fdt_header header;
 190         int err;
 191
 192         debugf("fdt_load_dtb(0x%08jx)\n", (uintmax_t)va);
 193
 194         COPYOUT(va, &header, sizeof(header));
 195         err = fdt_check_header(&header);
 196         if (err < 0) {
 197                 if (err == -FDT_ERR_BADVERSION) {
 198                         snprintf(command_errbuf, sizeof(command_errbuf),
 199                             "incompatible blob version: %d, should be: %d",
 200                             fdt_version(fdtp), FDT_LAST_SUPPORTED_VERSION);
 201                 } else {
 202                         snprintf(command_errbuf, sizeof(command_errbuf),
 203                             "error validating blob: %s", fdt_strerror(err));
 204                 }
 205                 return (1);
 206         }
 207
 208         /*
 209          * Release previous blob
 210          */
 211         if (fdtp)
 212                 free(fdtp);
 213
 214         fdtp_size = fdt_totalsize(&header);
 215         fdtp = malloc(fdtp_size);
 216
 217         if (fdtp == NULL) {
 218                 command_errmsg = "can't allocate memory for device tree copy";
 219                 return (1);
 220         }
 221
 222         COPYOUT(va, fdtp, fdtp_size);
 223         debugf("DTB blob found at 0x%jx, size: 0x%jx\n", (uintmax_t)va, (uintmax_t)fdtp_size);
 224
 225         return (0);
 226 }
 227
 228 int
 229 fdt_load_dtb_addr(struct fdt_header *header)
 230 {
 231         int err;
 232
 233         debugf("fdt_load_dtb_addr(%p)\n", header);
 234
 235         fdtp_size = fdt_totalsize(header);
 236         err = fdt_check_header(header);
 237         if (err < 0) {
 238                 snprintf(command_errbuf, sizeof(command_errbuf),
 239                     "error validating blob: %s", fdt_strerror(err));
 240                 return (err);
 241         }
 242         free(fdtp);
 243         if ((fdtp = malloc(fdtp_size)) == NULL) {
 244                 command_errmsg = "can't allocate memory for device tree copy";
 245                 return (1);
 246         }
 247
 248         bcopy(header, fdtp, fdtp_size);
 249         return (0);
 250 }
 251
 252 int
 253 fdt_load_dtb_file(const char * filename)
 254 {
 255         struct preloaded_file *bfp, *oldbfp;
 256         int err;
 257
 258         debugf("fdt_load_dtb_file(%s)\n", filename);
 259
 260         oldbfp = file_findfile(NULL, "dtb");
 261
 262         /* Attempt to load and validate a new dtb from a file. */
 263         if ((bfp = file_loadraw(filename, "dtb", 1)) == NULL) {
 264                 snprintf(command_errbuf, sizeof(command_errbuf),
 265                     "failed to load file '%s'", filename);
 266                 return (1);
 267         }
 268         if ((err = fdt_load_dtb(bfp->f_addr)) != 0) {
 269                 file_discard(bfp);
 270                 return (err);
 271         }
 272
 273         /* A new dtb was validated, discard any previous file. */
 274         if (oldbfp)
 275                 file_discard(oldbfp);
 276         return (0);
 277 }
 278
 279 static int
 280 fdt_load_dtb_overlay(const char * filename)
 281 {
 282         struct preloaded_file *bfp;
 283         struct fdt_header header;
 284         int err;
 285
 286         debugf("fdt_load_dtb_overlay(%s)\n", filename);
 287
 288         /* Attempt to load and validate a new dtb from a file. FDT_ERR_NOTFOUND
 289          * is normally a libfdt error code, but libfdt would actually return
 290          * -FDT_ERR_NOTFOUND. We re-purpose the error code here to convey a
 291          * similar meaning: the file itself was not found, which can still be
 292          * considered an error dealing with FDT pieces.
 293          */
 294         if ((bfp = file_loadraw(filename, "dtbo", 1)) == NULL)
 295                 return (FDT_ERR_NOTFOUND);
 296
 297         COPYOUT(bfp->f_addr, &header, sizeof(header));
 298         err = fdt_check_header(&header);
 299
 300         if (err < 0) {
 301                 file_discard(bfp);
 302                 return (err);
 303         }
 304
 305         return (0);
 306 }
 307
 308 static void
 309 fdt_print_overlay_load_error(int err, const char *filename)
 310 {
 311
 312         switch (err) {
 313                 case FDT_ERR_NOTFOUND:
 314                         printf("%s: failed to load file\n", filename);
 315                         break;
 316                 case -FDT_ERR_BADVERSION:
 317                         printf("%s: incompatible blob version: %d, should be: %d\n",
 318                             filename, fdt_version(fdtp),
 319                             FDT_LAST_SUPPORTED_VERSION);
 320                         break;
 321                 default:
 322                         /* libfdt errs are negative */
 323                         if (err < 0)
 324                                 printf("%s: error validating blob: %s\n",
 325                                     filename, fdt_strerror(err));
 326                         else
 327                                 printf("%s: unknown load error\n", filename);
 328                         break;
 329         }
 330 }
 331
 332 static int
 333 fdt_load_dtb_overlays_string(const char * filenames)
 334 {
 335         char *names;
 336         char *name, *name_ext;
 337         char *comaptr;
 338         int err, namesz;
 339
 340         debugf("fdt_load_dtb_overlays_string(%s)\n", filenames);
 341
 342         names = strdup(filenames);
 343         if (names == NULL)
 344                 return (1);
 345         name = names;
 346         do {
 347                 comaptr = strchr(name, ',');
 348                 if (comaptr)
 349                         *comaptr = '\0';
 350                 err = fdt_load_dtb_overlay(name);
 351                 if (err == FDT_ERR_NOTFOUND) {
 352                         /* Allocate enough to append ".dtbo" */
 353                         namesz = strlen(name) + 6;
 354                         name_ext = malloc(namesz);
 355                         if (name_ext == NULL) {
 356                                 fdt_print_overlay_load_error(err, name);
 357                                 name = comaptr + 1;
 358                                 continue;
 359                         }
 360                         snprintf(name_ext, namesz, "%s.dtbo", name);
 361                         err = fdt_load_dtb_overlay(name_ext);
 362                         free(name_ext);
 363                 }
 364                 /* Catch error with either initial load or fallback load */
 365                 if (err != 0)
 366                         fdt_print_overlay_load_error(err, name);
 367                 name = comaptr + 1;
 368         } while(comaptr);
 369
 370         free(names);
 371         return (0);
 372 }
 373
 374 /*
 375  * fdt_check_overlay_compatible - check that the overlay_fdt is compatible with
 376  * base_fdt before we attempt to apply it. It will need to re-calculate offsets
 377  * in the base every time, rather than trying to cache them earlier in the
 378  * process, because the overlay application process can/will invalidate a lot of
 379  * offsets.
 380  */
 381 static int
 382 fdt_check_overlay_compatible(void *base_fdt, void *overlay_fdt)
 383 {
 384         const char *compat;
 385         int compat_len, ocompat_len;
 386         int oroot_offset, root_offset;
 387         int slidx, sllen;
 388
 389         oroot_offset = fdt_path_offset(overlay_fdt, "/");
 390         if (oroot_offset < 0)
 391                 return (oroot_offset);
 392         /*
 393          * If /compatible in the overlay does not exist or if it is empty, then
 394          * we're automatically compatible. We do this for the sake of rapid
 395          * overlay development for overlays that aren't intended to be deployed.
 396          * The user assumes the risk of using an overlay without /compatible.
 397          */
 398         if (fdt_get_property(overlay_fdt, oroot_offset, "compatible",
 399             &ocompat_len) == NULL || ocompat_len == 0)
 400                 return (0);
 401         root_offset = fdt_path_offset(base_fdt, "/");
 402         if (root_offset < 0)
 403                 return (root_offset);
 404         /*
 405          * However, an empty or missing /compatible on the base is an error,
 406          * because allowing this offers no advantages.
 407          */
 408         if (fdt_get_property(base_fdt, root_offset, "compatible",
 409             &compat_len) == NULL)
 410                 return (compat_len);
 411         else if(compat_len == 0)
 412                 return (1);
 413
 414         slidx = 0;
 415         compat = fdt_stringlist_get(overlay_fdt, oroot_offset, "compatible",
 416             slidx, &sllen);
 417         while (compat != NULL) {
 418                 if (fdt_stringlist_search(base_fdt, root_offset, "compatible",
 419                     compat) >= 0)
 420                         return (0);
 421                 ++slidx;
 422                 compat = fdt_stringlist_get(overlay_fdt, oroot_offset,
 423                     "compatible", slidx, &sllen);
 424         };
 425
 426         /* We've exhausted the overlay's /compatible property... no match */
 427         return (1);
 428 }
 429
 430 /*
 431  * Returns the number of overlays successfully applied
 432  */
 433 int
 434 fdt_apply_overlays()
 435 {
 436         struct preloaded_file *fp;
 437         size_t max_overlay_size, next_fdtp_size;
 438         size_t current_fdtp_size;
 439         void *current_fdtp;
 440         void *next_fdtp;
 441         void *overlay;
 442         int overlays_applied, rv;
 443
 444         if ((fdtp == NULL) || (fdtp_size == 0))
 445                 return (0);
 446
 447         if (fdt_overlays_applied)
 448                 return (0);
 449
 450         max_overlay_size = 0;
 451         for (fp = file_findfile(NULL, "dtbo"); fp != NULL; fp = fp->f_next) {
 452                 if (max_overlay_size < fp->f_size)
 453                         max_overlay_size = fp->f_size;
 454         }
 455
 456         /* Nothing to apply */
 457         if (max_overlay_size == 0)
 458                 return (0);
 459
 460         overlay = malloc(max_overlay_size);
 461         if (overlay == NULL) {
 462                 printf("failed to allocate memory for DTB blob with overlays\n");
 463                 return (0);
 464         }
 465         current_fdtp = fdtp;
 466         current_fdtp_size = fdtp_size;
 467         overlays_applied = 0;
 468         for (fp = file_findfile(NULL, "dtbo"); fp != NULL; fp = fp->f_next) {
 469                 COPYOUT(fp->f_addr, overlay, fp->f_size);
 470                 /* Check compatible first to avoid unnecessary allocation */
 471                 rv = fdt_check_overlay_compatible(current_fdtp, overlay);
 472                 if (rv != 0) {
 473                         printf("DTB overlay '%s' not compatible\n", fp->f_name);
 474                         continue;
 475                 }
 476                 printf("applying DTB overlay '%s'\n", fp->f_name);
 477                 next_fdtp_size = current_fdtp_size + fp->f_size;
 478                 next_fdtp = malloc(next_fdtp_size);
 479                 if (next_fdtp == NULL) {
 480                         /*
 481                          * Output warning, then move on to applying other
 482                          * overlays in case this one is simply too large.
 483                          */
 484                         printf("failed to allocate memory for overlay base\n");
 485                         continue;
 486                 }
 487                 rv = fdt_open_into(current_fdtp, next_fdtp, next_fdtp_size);
 488                 if (rv != 0) {
 489                         free(next_fdtp);
 490                         printf("failed to open base dtb into overlay base\n");
 491                         continue;
 492                 }
 493                 /* Both overlay and next_fdtp may be modified in place */
 494                 rv = fdt_overlay_apply(next_fdtp, overlay);
 495                 if (rv == 0) {
 496                         /* Rotate next -> current */
 497                         if (current_fdtp != fdtp)
 498                                 free(current_fdtp);
 499                         current_fdtp = next_fdtp;
 500                         fdt_pack(current_fdtp);
 501                         current_fdtp_size = fdt_totalsize(current_fdtp);
 502                         overlays_applied++;
 503                 } else {
 504                         /*
 505                          * Assume here that the base we tried to apply on is
 506                          * either trashed or in an inconsistent state. Trying to
 507                          * load it might work, but it's better to discard it and
 508                          * play it safe. */
 509                         free(next_fdtp);
 510                         printf("failed to apply overlay: %s\n",
 511                             fdt_strerror(rv));
 512                 }
 513         }
 514         /* We could have failed to apply all overlays; then we do nothing */
 515         if (current_fdtp != fdtp) {
 516                 free(fdtp);
 517                 fdtp = current_fdtp;
 518                 fdtp_size = current_fdtp_size;
 519         }
 520         free(overlay);
 521         fdt_overlays_applied = 1;
 522         return (overlays_applied);
 523 }
 524
 525 int
 526 fdt_pad_dtb(size_t padding)
 527 {
 528         void *padded_fdtp;
 529         size_t padded_fdtp_size;
 530
 531         padded_fdtp_size = fdtp_size + padding;
 532         padded_fdtp = malloc(padded_fdtp_size);
 533         if (padded_fdtp == NULL)
 534                 return (1);
 535         if (fdt_open_into(fdtp, padded_fdtp, padded_fdtp_size) != 0) {
 536                 free(padded_fdtp);
 537                 return (1);
 538         }
 539         fdtp = padded_fdtp;
 540         fdtp_size = padded_fdtp_size;
 541         return (0);
 542 }
 543
 544 int
 545 fdt_is_setup(void)
 546 {
 547
 548         if (fdtp != NULL)
 549                 return (1);
 550
 551         return (0);
 552 }
 553
 554 int
 555 fdt_setup_fdtp()
 556 {
 557         struct preloaded_file *bfp;
 558         vm_offset_t va;
 559
 560         debugf("fdt_setup_fdtp()\n");
 561
 562         /* If we already loaded a file, use it. */
 563         if ((bfp = file_findfile(NULL, "dtb")) != NULL) {
 564                 if (fdt_load_dtb(bfp->f_addr) == 0) {
 565                         printf("Using DTB from loaded file '%s'.\n",
 566                             bfp->f_name);
 567                         fdt_platform_load_overlays();
 568                         return (0);
 569                 }
 570         }
 571
 572         /* If we were given the address of a valid blob in memory, use it. */
 573         if (fdt_to_load != NULL) {
 574                 if (fdt_load_dtb_addr(fdt_to_load) == 0) {
 575                         printf("Using DTB from memory address %p.\n",
 576                             fdt_to_load);
 577                         fdt_platform_load_overlays();
 578                         return (0);
 579                 }
 580         }
 581
 582         if (fdt_platform_load_dtb() == 0) {
 583                 fdt_platform_load_overlays();
 584                 return (0);
 585         }
 586
 587         /* If there is a dtb compiled into the kernel, use it. */
 588         if ((va = fdt_find_static_dtb()) != 0) {
 589                 if (fdt_load_dtb(va) == 0) {
 590                         printf("Using DTB compiled into kernel.\n");
 591                         return (0);
 592                 }
 593         }
 594
 595         command_errmsg = "No device tree blob found!\n";
 596         return (1);
 597 }
 598
 599 #define fdt_strtovect(str, cellbuf, lim, cellsize) _fdt_strtovect((str), \
 600     (cellbuf), (lim), (cellsize), 0);
 601
 602 /* Force using base 16 */
 603 #define fdt_strtovectx(str, cellbuf, lim, cellsize) _fdt_strtovect((str), \
 604     (cellbuf), (lim), (cellsize), 16);
 605
 606 static int
 607 _fdt_strtovect(const char *str, void *cellbuf, int lim, unsigned char cellsize,
 608     uint8_t base)
 609 {
 610         const char *buf = str;
 611         const char *end = str + strlen(str) - 2;
 612         uint32_t *u32buf = NULL;
 613         uint8_t *u8buf = NULL;
 614         int cnt = 0;
 615
 616         if (cellsize == sizeof(uint32_t))
 617                 u32buf = (uint32_t *)cellbuf;
 618         else
 619                 u8buf = (uint8_t *)cellbuf;
 620
 621         if (lim == 0)
 622                 return (0);
 623
 624         while (buf < end) {
 625
 626                 /* Skip white whitespace(s)/separators */
 627                 while (!isxdigit(*buf) && buf < end)
 628                         buf++;
 629
 630                 if (u32buf != NULL)
 631                         u32buf[cnt] =
 632                             cpu_to_fdt32((uint32_t)strtol(buf, NULL, base));
 633
 634                 else
 635                         u8buf[cnt] = (uint8_t)strtol(buf, NULL, base);
 636
 637                 if (cnt + 1 <= lim - 1)
 638                         cnt++;
 639                 else
 640                         break;
 641                 buf++;
 642                 /* Find another number */
 643                 while ((isxdigit(*buf) || *buf == 'x') && buf < end)
 644                         buf++;
 645         }
 646         return (cnt);
 647 }
 648
 649 void
 650 fdt_fixup_ethernet(const char *str, char *ethstr, int len)
 651 {
 652         uint8_t tmp_addr[6];
 653
 654         /* Convert macaddr string into a vector of uints */
 655         fdt_strtovectx(str, &tmp_addr, 6, sizeof(uint8_t));
 656         /* Set actual property to a value from vect */
 657         fdt_setprop(fdtp, fdt_path_offset(fdtp, ethstr),
 658             "local-mac-address", &tmp_addr, 6 * sizeof(uint8_t));
 659 }
 660
 661 void
 662 fdt_fixup_cpubusfreqs(unsigned long cpufreq, unsigned long busfreq)
 663 {
 664         int lo, o = 0, o2, maxo = 0, depth;
 665         const uint32_t zero = 0;
 666
 667         /* We want to modify every subnode of /cpus */
 668         o = fdt_path_offset(fdtp, "/cpus");
 669         if (o < 0)
 670                 return;
 671
 672         /* maxo should contain offset of node next to /cpus */
 673         depth = 0;
 674         maxo = o;
 675         while (depth != -1)
 676                 maxo = fdt_next_node(fdtp, maxo, &depth);
 677
 678         /* Find CPU frequency properties */
 679         o = fdt_node_offset_by_prop_value(fdtp, o, "clock-frequency",
 680             &zero, sizeof(uint32_t));
 681
 682         o2 = fdt_node_offset_by_prop_value(fdtp, o, "bus-frequency", &zero,
 683             sizeof(uint32_t));
 684
 685         lo = MIN(o, o2);
 686
 687         while (o != -FDT_ERR_NOTFOUND && o2 != -FDT_ERR_NOTFOUND) {
 688
 689                 o = fdt_node_offset_by_prop_value(fdtp, lo,
 690                     "clock-frequency", &zero, sizeof(uint32_t));
 691
 692                 o2 = fdt_node_offset_by_prop_value(fdtp, lo, "bus-frequency",
 693                     &zero, sizeof(uint32_t));
 694
 695                 /* We're only interested in /cpus subnode(s) */
 696                 if (lo > maxo)
 697                         break;
 698
 699                 fdt_setprop_inplace_cell(fdtp, lo, "clock-frequency",
 700                     (uint32_t)cpufreq);
 701
 702                 fdt_setprop_inplace_cell(fdtp, lo, "bus-frequency",
 703                     (uint32_t)busfreq);
 704
 705                 lo = MIN(o, o2);
 706         }
 707 }
 708
 709 #ifdef notyet
 710 static int
 711 fdt_reg_valid(uint32_t *reg, int len, int addr_cells, int size_cells)
 712 {
 713         int cells_in_tuple, i, tuples, tuple_size;
 714         uint32_t cur_start, cur_size;
 715
 716         cells_in_tuple = (addr_cells + size_cells);
 717         tuple_size = cells_in_tuple * sizeof(uint32_t);
 718         tuples = len / tuple_size;
 719         if (tuples == 0)
 720                 return (EINVAL);
 721
 722         for (i = 0; i < tuples; i++) {
 723                 if (addr_cells == 2)
 724                         cur_start = fdt64_to_cpu(reg[i * cells_in_tuple]);
 725                 else
 726                         cur_start = fdt32_to_cpu(reg[i * cells_in_tuple]);
 727
 728                 if (size_cells == 2)
 729                         cur_size = fdt64_to_cpu(reg[i * cells_in_tuple + 2]);
 730                 else
 731                         cur_size = fdt32_to_cpu(reg[i * cells_in_tuple + 1]);
 732
 733                 if (cur_size == 0)
 734                         return (EINVAL);
 735
 736                 debugf(" reg#%d (start: 0x%0x size: 0x%0x) valid!\n",
 737                     i, cur_start, cur_size);
 738         }
 739         return (0);
 740 }
 741 #endif
 742
 743 void
 744 fdt_fixup_memory(struct fdt_mem_region *region, size_t num)
 745 {
 746         struct fdt_mem_region *curmr;
 747         uint32_t addr_cells, size_cells;
 748         uint32_t *addr_cellsp, *size_cellsp;
 749         int err, i, len, memory, root;
 750         size_t realmrno;
 751         uint8_t *buf, *sb;
 752         uint64_t rstart, rsize;
 753         int reserved;
 754
 755         root = fdt_path_offset(fdtp, "/");
 756         if (root < 0) {
 757                 sprintf(command_errbuf, "Could not find root node !");
 758                 return;
 759         }
 760
 761         memory = fdt_path_offset(fdtp, "/memory");
 762         if (memory <= 0) {
 763                 /* Create proper '/memory' node. */
 764                 memory = fdt_add_subnode(fdtp, root, "memory");
 765                 if (memory <= 0) {
 766                         snprintf(command_errbuf, sizeof(command_errbuf),
 767                             "Could not fixup '/memory' "
 768                             "node, error code : %d!\n", memory);
 769                         return;
 770                 }
 771
 772                 err = fdt_setprop(fdtp, memory, "device_type", "memory",
 773                     sizeof("memory"));
 774
 775                 if (err < 0)
 776                         return;
 777         }
 778
 779         addr_cellsp = (uint32_t *)fdt_getprop(fdtp, root, "#address-cells",
 780             NULL);
 781         size_cellsp = (uint32_t *)fdt_getprop(fdtp, root, "#size-cells", NULL);
 782
 783         if (addr_cellsp == NULL || size_cellsp == NULL) {
 784                 snprintf(command_errbuf, sizeof(command_errbuf),
 785                     "Could not fixup '/memory' node : "
 786                     "%s %s property not found in root node!\n",
 787                     (!addr_cellsp) ? "#address-cells" : "",
 788                     (!size_cellsp) ? "#size-cells" : "");
 789                 return;
 790         }
 791
 792         addr_cells = fdt32_to_cpu(*addr_cellsp);
 793         size_cells = fdt32_to_cpu(*size_cellsp);
 794
 795         /*
 796          * Convert memreserve data to memreserve property
 797          * Check if property already exists
 798          */
 799         reserved = fdt_num_mem_rsv(fdtp);
 800         if (reserved &&
 801             (fdt_getprop(fdtp, root, "memreserve", NULL) == NULL)) {
 802                 len = (addr_cells + size_cells) * reserved * sizeof(uint32_t);
 803                 sb = buf = (uint8_t *)malloc(len);
 804                 if (!buf)
 805                         return;
 806
 807                 bzero(buf, len);
 808
 809                 for (i = 0; i < reserved; i++) {
 810                         if (fdt_get_mem_rsv(fdtp, i, &rstart, &rsize))
 811                                 break;
 812                         if (rsize) {
 813                                 /* Ensure endianness, and put cells into a buffer */
 814                                 if (addr_cells == 2)
 815                                         *(uint64_t *)buf =
 816                                             cpu_to_fdt64(rstart);
 817                                 else
 818                                         *(uint32_t *)buf =
 819                                             cpu_to_fdt32(rstart);
 820
 821                                 buf += sizeof(uint32_t) * addr_cells;
 822                                 if (size_cells == 2)
 823                                         *(uint64_t *)buf =
 824                                             cpu_to_fdt64(rsize);
 825                                 else
 826                                         *(uint32_t *)buf =
 827                                             cpu_to_fdt32(rsize);
 828
 829                                 buf += sizeof(uint32_t) * size_cells;
 830                         }
 831                 }
 832
 833                 /* Set property */
 834                 if ((err = fdt_setprop(fdtp, root, "memreserve", sb, len)) < 0)
 835                         printf("Could not fixup 'memreserve' property.\n");
 836
 837                 free(sb);
 838         }
 839
 840         /* Count valid memory regions entries in sysinfo. */
 841         realmrno = num;
 842         for (i = 0; i < num; i++)
 843                 if (region[i].start == 0 && region[i].size == 0)
 844                         realmrno--;
 845
 846         if (realmrno == 0) {
 847                 sprintf(command_errbuf, "Could not fixup '/memory' node : "
 848                     "sysinfo doesn't contain valid memory regions info!\n");
 849                 return;
 850         }
 851
 852         len = (addr_cells + size_cells) * realmrno * sizeof(uint32_t);
 853         sb = buf = (uint8_t *)malloc(len);
 854         if (!buf)
 855                 return;
 856
 857         bzero(buf, len);
 858
 859         for (i = 0; i < num; i++) {
 860                 curmr = &region[i];
 861                 if (curmr->size != 0) {
 862                         /* Ensure endianness, and put cells into a buffer */
 863                         if (addr_cells == 2)
 864                                 *(uint64_t *)buf =
 865                                     cpu_to_fdt64(curmr->start);
 866                         else
 867                                 *(uint32_t *)buf =
 868                                     cpu_to_fdt32(curmr->start);
 869
 870                         buf += sizeof(uint32_t) * addr_cells;
 871                         if (size_cells == 2)
 872                                 *(uint64_t *)buf =
 873                                     cpu_to_fdt64(curmr->size);
 874                         else
 875                                 *(uint32_t *)buf =
 876                                     cpu_to_fdt32(curmr->size);
 877
 878                         buf += sizeof(uint32_t) * size_cells;
 879                 }
 880         }
 881
 882         /* Set property */
 883         if ((err = fdt_setprop(fdtp, memory, "reg", sb, len)) < 0)
 884                 sprintf(command_errbuf, "Could not fixup '/memory' node.\n");
 885
 886         free(sb);
 887 }
 888
 889 void
 890 fdt_fixup_stdout(const char *str)
 891 {
 892         char *ptr;
 893         int len, no, sero;
 894         const struct fdt_property *prop;
 895         char *tmp[10];
 896
 897         ptr = (char *)str + strlen(str) - 1;
 898         while (ptr > str && isdigit(*(str - 1)))
 899                 str--;
 900
 901         if (ptr == str)
 902                 return;
 903
 904         no = fdt_path_offset(fdtp, "/chosen");
 905         if (no < 0)
 906                 return;
 907
 908         prop = fdt_get_property(fdtp, no, "stdout", &len);
 909
 910         /* If /chosen/stdout does not extist, create it */
 911         if (prop == NULL || (prop != NULL && len == 0)) {
 912
 913                 bzero(tmp, 10 * sizeof(char));
 914                 strcpy((char *)&tmp, "serial");
 915                 if (strlen(ptr) > 3)
 916                         /* Serial number too long */
 917                         return;
 918
 919                 strncpy((char *)tmp + 6, ptr, 3);
 920                 sero = fdt_path_offset(fdtp, (const char *)tmp);
 921                 if (sero < 0)
 922                         /*
 923                          * If serial device we're trying to assign
 924                          * stdout to doesn't exist in DT -- return.
 925                          */
 926                         return;
 927
 928                 fdt_setprop(fdtp, no, "stdout", &tmp,
 929                     strlen((char *)&tmp) + 1);
 930                 fdt_setprop(fdtp, no, "stdin", &tmp,
 931                     strlen((char *)&tmp) + 1);
 932         }
 933 }
 934
 935 void
 936 fdt_load_dtb_overlays(const char *extras)
 937 {
 938         const char *s;
 939
 940         /* Any extra overlays supplied by pre-loader environment */
 941         if (extras != NULL && *extras != '\0') {
 942                 printf("Loading DTB overlays: '%s'\n", extras);
 943                 fdt_load_dtb_overlays_string(extras);
 944         }
 945
 946         /* Any overlays supplied by loader environment */
 947         s = getenv("fdt_overlays");
 948         if (s != NULL && *s != '\0') {
 949                 printf("Loading DTB overlays: '%s'\n", s);
 950                 fdt_load_dtb_overlays_string(s);
 951         }
 952 }
 953
 954 /*
 955  * Locate the blob, fix it up and return its location.
 956  */
 957 static int
 958 fdt_fixup(void)
 959 {
 960         int chosen;
 961
 962         debugf("fdt_fixup()\n");
 963
 964         if (fdtp == NULL && fdt_setup_fdtp() != 0)
 965                 return (0);
 966
 967         /* Create /chosen node (if not exists) */
 968         if ((chosen = fdt_subnode_offset(fdtp, 0, "chosen")) ==
 969             -FDT_ERR_NOTFOUND)
 970                 chosen = fdt_add_subnode(fdtp, 0, "chosen");
 971
 972         /* Value assigned to fixup-applied does not matter. */
 973         if (fdt_getprop(fdtp, chosen, "fixup-applied", NULL))
 974                 return (1);
 975
 976         fdt_platform_fixups();
 977
 978         /*
 979          * Re-fetch the /chosen subnode; our fixups may apply overlays or add
 980          * nodes/properties that invalidate the offset we grabbed or created
 981          * above, so we can no longer trust it.
 982          */
 983         chosen = fdt_subnode_offset(fdtp, 0, "chosen");
 984         fdt_setprop(fdtp, chosen, "fixup-applied", NULL, 0);
 985         return (1);
 986 }
 987
 988 /*
 989  * Copy DTB blob to specified location and return size
 990  */
 991 int
 992 fdt_copy(vm_offset_t va)
 993 {
 994         int err;
 995         debugf("fdt_copy va 0x%08x\n", va);
 996         if (fdtp == NULL) {
 997                 err = fdt_setup_fdtp();
 998                 if (err) {
 999                         printf("No valid device tree blob found!\n");
1000                         return (0);
1001                 }
1002         }
1003
1004         if (fdt_fixup() == 0)
1005                 return (0);
1006
1007         COPYIN(fdtp, va, fdtp_size);
1008         return (fdtp_size);
1009 }
1010
1011
1012
1013 int
1014 command_fdt_internal(int argc, char *argv[])
1015 {
1016         cmdf_t *cmdh;
1017         int flags;
1018         int i, err;
1019
1020         if (argc < 2) {
1021                 command_errmsg = "usage is 'fdt <command> [<args>]";
1022                 return (CMD_ERROR);
1023         }
1024
1025         /*
1026          * Validate fdt <command>.
1027          */
1028         i = 0;
1029         cmdh = NULL;
1030         while (!(commands[i].name == NULL)) {
1031                 if (strcmp(argv[1], commands[i].name) == 0) {
1032                         /* found it */
1033                         cmdh = commands[i].handler;
1034                         flags = commands[i].flags;
1035                         break;
1036                 }
1037                 i++;
1038         }
1039         if (cmdh == NULL) {
1040                 command_errmsg = "unknown command";
1041                 return (CMD_ERROR);
1042         }
1043
1044         if (flags & CMD_REQUIRES_BLOB) {
1045                 /*
1046                  * Check if uboot env vars were parsed already. If not, do it now.
1047                  */
1048                 if (fdt_fixup() == 0)
1049                         return (CMD_ERROR);
1050         }
1051
1052         /*
1053          * Call command handler.
1054          */
1055         err = (*cmdh)(argc, argv);
1056
1057         return (err);
1058 }
1059
1060 static int
1061 fdt_cmd_addr(int argc, char *argv[])
1062 {
1063         struct preloaded_file *fp;
1064         struct fdt_header *hdr;
1065         const char *addr;
1066         char *cp;
1067
1068         fdt_to_load = NULL;
1069
1070         if (argc > 2)
1071                 addr = argv[2];
1072         else {
1073                 sprintf(command_errbuf, "no address specified");
1074                 return (CMD_ERROR);
1075         }
1076
1077         hdr = (struct fdt_header *)strtoul(addr, &cp, 16);
1078         if (cp == addr) {
1079                 snprintf(command_errbuf, sizeof(command_errbuf),
1080                     "Invalid address: %s", addr);
1081                 return (CMD_ERROR);
1082         }
1083
1084         while ((fp = file_findfile(NULL, "dtb")) != NULL) {
1085                 file_discard(fp);
1086         }
1087
1088         fdt_to_load = hdr;
1089         return (CMD_OK);
1090 }
1091
1092 static int
1093 fdt_cmd_cd(int argc, char *argv[])
1094 {
1095         char *path;
1096         char tmp[FDT_CWD_LEN];
1097         int len, o;
1098
1099         path = (argc > 2) ? argv[2] : "/";
1100
1101         if (path[0] == '/') {
1102                 len = strlen(path);
1103                 if (len >= FDT_CWD_LEN)
1104                         goto fail;
1105         } else {
1106                 /* Handle path specification relative to cwd */
1107                 len = strlen(cwd) + strlen(path) + 1;
1108                 if (len >= FDT_CWD_LEN)
1109                         goto fail;
1110
1111                 strcpy(tmp, cwd);
1112                 strcat(tmp, "/");
1113                 strcat(tmp, path);
1114                 path = tmp;
1115         }
1116
1117         o = fdt_path_offset(fdtp, path);
1118         if (o < 0) {
1119                 snprintf(command_errbuf, sizeof(command_errbuf),
1120                     "could not find node: '%s'", path);
1121                 return (CMD_ERROR);
1122         }
1123
1124         strcpy(cwd, path);
1125         return (CMD_OK);
1126
1127 fail:
1128         snprintf(command_errbuf, sizeof(command_errbuf),
1129             "path too long: %d, max allowed: %d", len, FDT_CWD_LEN - 1);
1130         return (CMD_ERROR);
1131 }
1132
1133 static int
1134 fdt_cmd_hdr(int argc __unused, char *argv[] __unused)
1135 {
1136         char line[80];
1137         int ver;
1138
1139         if (fdtp == NULL) {
1140                 command_errmsg = "no device tree blob pointer?!";
1141                 return (CMD_ERROR);
1142         }
1143
1144         ver = fdt_version(fdtp);
1145         pager_open();
1146         sprintf(line, "\nFlattened device tree header (%p):\n", fdtp);
1147         if (pager_output(line))
1148                 goto out;
1149         sprintf(line, " magic                   = 0x%08x\n", fdt_magic(fdtp));
1150         if (pager_output(line))
1151                 goto out;
1152         sprintf(line, " size                    = %d\n", fdt_totalsize(fdtp));
1153         if (pager_output(line))
1154                 goto out;
1155         sprintf(line, " off_dt_struct           = 0x%08x\n",
1156             fdt_off_dt_struct(fdtp));
1157         if (pager_output(line))
1158                 goto out;
1159         sprintf(line, " off_dt_strings          = 0x%08x\n",
1160             fdt_off_dt_strings(fdtp));
1161         if (pager_output(line))
1162                 goto out;
1163         sprintf(line, " off_mem_rsvmap          = 0x%08x\n",
1164             fdt_off_mem_rsvmap(fdtp));
1165         if (pager_output(line))
1166                 goto out;
1167         sprintf(line, " version                 = %d\n", ver);
1168         if (pager_output(line))
1169                 goto out;
1170         sprintf(line, " last compatible version = %d\n",
1171             fdt_last_comp_version(fdtp));
1172         if (pager_output(line))
1173                 goto out;
1174         if (ver >= 2) {
1175                 sprintf(line, " boot_cpuid              = %d\n",
1176                     fdt_boot_cpuid_phys(fdtp));
1177                 if (pager_output(line))
1178                         goto out;
1179         }
1180         if (ver >= 3) {
1181                 sprintf(line, " size_dt_strings         = %d\n",
1182                     fdt_size_dt_strings(fdtp));
1183                 if (pager_output(line))
1184                         goto out;
1185         }
1186         if (ver >= 17) {
1187                 sprintf(line, " size_dt_struct          = %d\n",
1188                     fdt_size_dt_struct(fdtp));
1189                 if (pager_output(line))
1190                         goto out;
1191         }
1192 out:
1193         pager_close();
1194
1195         return (CMD_OK);
1196 }
1197
1198 static int
1199 fdt_cmd_ls(int argc, char *argv[])
1200 {
1201         const char *prevname[FDT_MAX_DEPTH] = { NULL };
1202         const char *name;
1203         char *path;
1204         int i, o, depth;
1205
1206         path = (argc > 2) ? argv[2] : NULL;
1207         if (path == NULL)
1208                 path = cwd;
1209
1210         o = fdt_path_offset(fdtp, path);
1211         if (o < 0) {
1212                 snprintf(command_errbuf, sizeof(command_errbuf),
1213                     "could not find node: '%s'", path);
1214                 return (CMD_ERROR);
1215         }
1216
1217         for (depth = 0;
1218             (o >= 0) && (depth >= 0);
1219             o = fdt_next_node(fdtp, o, &depth)) {
1220
1221                 name = fdt_get_name(fdtp, o, NULL);
1222
1223                 if (depth > FDT_MAX_DEPTH) {
1224                         printf("max depth exceeded: %d\n", depth);
1225                         continue;
1226                 }
1227
1228                 prevname[depth] = name;
1229
1230                 /* Skip root (i = 1) when printing devices */
1231                 for (i = 1; i <= depth; i++) {
1232                         if (prevname[i] == NULL)
1233                                 break;
1234
1235                         if (strcmp(cwd, "/") == 0)
1236                                 printf("/");
1237                         printf("%s", prevname[i]);
1238                 }
1239                 printf("\n");
1240         }
1241
1242         return (CMD_OK);
1243 }
1244
1245 static __inline int
1246 isprint(int c)
1247 {
1248
1249         return (c >= ' ' && c <= 0x7e);
1250 }
1251
1252 static int
1253 fdt_isprint(const void *data, int len, int *count)
1254 {
1255         const char *d;
1256         char ch;
1257         int yesno, i;
1258
1259         if (len == 0)
1260                 return (0);
1261
1262         d = (const char *)data;
1263         if (d[len - 1] != '\0')
1264                 return (0);
1265
1266         *count = 0;
1267         yesno = 1;
1268         for (i = 0; i < len; i++) {
1269                 ch = *(d + i);
1270                 if (isprint(ch) || (ch == '\0' && i > 0)) {
1271                         /* Count strings */
1272                         if (ch == '\0')
1273                                 (*count)++;
1274                         continue;
1275                 }
1276
1277                 yesno = 0;
1278                 break;
1279         }
1280
1281         return (yesno);
1282 }
1283
1284 static int
1285 fdt_data_str(const void *data, int len, int count, char **buf)
1286 {
1287         char *b, *tmp;
1288         const char *d;
1289         int buf_len, i, l;
1290
1291         /*
1292          * Calculate the length for the string and allocate memory.
1293          *
1294          * Note that 'len' already includes at least one terminator.
1295          */
1296         buf_len = len;
1297         if (count > 1) {
1298                 /*
1299                  * Each token had already a terminator buried in 'len', but we
1300                  * only need one eventually, don't count space for these.
1301                  */
1302                 buf_len -= count - 1;
1303
1304                 /* Each consecutive token requires a ", " separator. */
1305                 buf_len += count * 2;
1306         }
1307
1308         /* Add some space for surrounding double quotes. */
1309         buf_len += count * 2;
1310
1311         /* Note that string being put in 'tmp' may be as big as 'buf_len'. */
1312         b = (char *)malloc(buf_len);
1313         tmp = (char *)malloc(buf_len);
1314         if (b == NULL)
1315                 goto error;
1316
1317         if (tmp == NULL) {
1318                 free(b);
1319                 goto error;
1320         }
1321
1322         b[0] = '\0';
1323
1324         /*
1325          * Now that we have space, format the string.
1326          */
1327         i = 0;
1328         do {
1329                 d = (const char *)data + i;
1330                 l = strlen(d) + 1;
1331
1332                 sprintf(tmp, "\"%s\"%s", d,
1333                     (i + l) < len ?  ", " : "");
1334                 strcat(b, tmp);
1335
1336                 i += l;
1337
1338         } while (i < len);
1339         *buf = b;
1340
1341         free(tmp);
1342
1343         return (0);
1344 error:
1345         return (1);
1346 }
1347
1348 static int
1349 fdt_data_cell(const void *data, int len, char **buf)
1350 {
1351         char *b, *tmp;
1352         const uint32_t *c;
1353         int count, i, l;
1354
1355         /* Number of cells */
1356         count = len / 4;
1357
1358         /*
1359          * Calculate the length for the string and allocate memory.
1360          */
1361
1362         /* Each byte translates to 2 output characters */
1363         l = len * 2;
1364         if (count > 1) {
1365                 /* Each consecutive cell requires a " " separator. */
1366                 l += (count - 1) * 1;
1367         }
1368         /* Each cell will have a "0x" prefix */
1369         l += count * 2;
1370         /* Space for surrounding <> and terminator */
1371         l += 3;
1372
1373         b = (char *)malloc(l);
1374         tmp = (char *)malloc(l);
1375         if (b == NULL)
1376                 goto error;
1377
1378         if (tmp == NULL) {
1379                 free(b);
1380                 goto error;
1381         }
1382
1383         b[0] = '\0';
1384         strcat(b, "<");
1385
1386         for (i = 0; i < len; i += 4) {
1387                 c = (const uint32_t *)((const uint8_t *)data + i);
1388                 sprintf(tmp, "0x%08x%s", fdt32_to_cpu(*c),
1389                     i < (len - 4) ? " " : "");
1390                 strcat(b, tmp);
1391         }
1392         strcat(b, ">");
1393         *buf = b;
1394
1395         free(tmp);
1396
1397         return (0);
1398 error:
1399         return (1);
1400 }
1401
1402 static int
1403 fdt_data_bytes(const void *data, int len, char **buf)
1404 {
1405         char *b, *tmp;
1406         const char *d;
1407         int i, l;
1408
1409         /*
1410          * Calculate the length for the string and allocate memory.
1411          */
1412
1413         /* Each byte translates to 2 output characters */
1414         l = len * 2;
1415         if (len > 1)
1416                 /* Each consecutive byte requires a " " separator. */
1417                 l += (len - 1) * 1;
1418         /* Each byte will have a "0x" prefix */
1419         l += len * 2;
1420         /* Space for surrounding [] and terminator. */
1421         l += 3;
1422
1423         b = (char *)malloc(l);
1424         tmp = (char *)malloc(l);
1425         if (b == NULL)
1426                 goto error;
1427
1428         if (tmp == NULL) {
1429                 free(b);
1430                 goto error;
1431         }
1432
1433         b[0] = '\0';
1434         strcat(b, "[");
1435
1436         for (i = 0, d = data; i < len; i++) {
1437                 sprintf(tmp, "0x%02x%s", d[i], i < len - 1 ? " " : "");
1438                 strcat(b, tmp);
1439         }
1440         strcat(b, "]");
1441         *buf = b;
1442
1443         free(tmp);
1444
1445         return (0);
1446 error:
1447         return (1);
1448 }
1449
1450 static int
1451 fdt_data_fmt(const void *data, int len, char **buf)
1452 {
1453         int count;
1454
1455         if (len == 0) {
1456                 *buf = NULL;
1457                 return (1);
1458         }
1459
1460         if (fdt_isprint(data, len, &count))
1461                 return (fdt_data_str(data, len, count, buf));
1462
1463         else if ((len % 4) == 0)
1464                 return (fdt_data_cell(data, len, buf));
1465
1466         else
1467                 return (fdt_data_bytes(data, len, buf));
1468 }
1469
1470 static int
1471 fdt_prop(int offset)
1472 {
1473         char *line, *buf;
1474         const struct fdt_property *prop;
1475         const char *name;
1476         const void *data;
1477         int len, rv;
1478
1479         line = NULL;
1480         prop = fdt_offset_ptr(fdtp, offset, sizeof(*prop));
1481         if (prop == NULL)
1482                 return (1);
1483
1484         name = fdt_string(fdtp, fdt32_to_cpu(prop->nameoff));
1485         len = fdt32_to_cpu(prop->len);
1486
1487         rv = 0;
1488         buf = NULL;
1489         if (len == 0) {
1490                 /* Property without value */
1491                 line = (char *)malloc(strlen(name) + 2);
1492                 if (line == NULL) {
1493                         rv = 2;
1494                         goto out2;
1495                 }
1496                 sprintf(line, "%s\n", name);
1497                 goto out1;
1498         }
1499
1500         /*
1501          * Process property with value
1502          */
1503         data = prop->data;
1504
1505         if (fdt_data_fmt(data, len, &buf) != 0) {
1506                 rv = 3;
1507                 goto out2;
1508         }
1509
1510         line = (char *)malloc(strlen(name) + strlen(FDT_PROP_SEP) +
1511             strlen(buf) + 2);
1512         if (line == NULL) {
1513                 sprintf(command_errbuf, "could not allocate space for string");
1514                 rv = 4;
1515                 goto out2;
1516         }
1517
1518         sprintf(line, "%s" FDT_PROP_SEP "%s\n", name, buf);
1519
1520 out1:
1521         pager_open();
1522         pager_output(line);
1523         pager_close();
1524
1525 out2:
1526         if (buf)
1527                 free(buf);
1528
1529         if (line)
1530                 free(line);
1531
1532         return (rv);
1533 }
1534
1535 static int
1536 fdt_modprop(int nodeoff, char *propname, void *value, char mode)
1537 {
1538         uint32_t cells[100];
1539         const char *buf;
1540         int len, rv;
1541         const struct fdt_property *p;
1542
1543         p = fdt_get_property(fdtp, nodeoff, propname, NULL);
1544
1545         if (p != NULL) {
1546                 if (mode == 1) {
1547                          /* Adding inexistant value in mode 1 is forbidden */
1548                         sprintf(command_errbuf, "property already exists!");
1549                         return (CMD_ERROR);
1550                 }
1551         } else if (mode == 0) {
1552                 sprintf(command_errbuf, "property does not exist!");
1553                 return (CMD_ERROR);
1554         }
1555         rv = 0;
1556         buf = value;
1557
1558         switch (*buf) {
1559         case '&':
1560                 /* phandles */
1561                 break;
1562         case '<':
1563                 /* Data cells */
1564                 len = fdt_strtovect(buf, (void *)&cells, 100,
1565                     sizeof(uint32_t));
1566
1567                 rv = fdt_setprop(fdtp, nodeoff, propname, &cells,
1568                     len * sizeof(uint32_t));
1569                 break;
1570         case '[':
1571                 /* Data bytes */
1572                 len = fdt_strtovect(buf, (void *)&cells, 100,
1573                     sizeof(uint8_t));
1574
1575                 rv = fdt_setprop(fdtp, nodeoff, propname, &cells,
1576                     len * sizeof(uint8_t));
1577                 break;
1578         case '"':
1579         default:
1580                 /* Default -- string */
1581                 rv = fdt_setprop_string(fdtp, nodeoff, propname, value);
1582                 break;
1583         }
1584
1585         if (rv != 0) {
1586                 if (rv == -FDT_ERR_NOSPACE)
1587                         sprintf(command_errbuf,
1588                             "Device tree blob is too small!\n");
1589                 else
1590                         sprintf(command_errbuf,
1591                             "Could not add/modify property!\n");
1592         }
1593         return (rv);
1594 }
1595
1596 /* Merge strings from argv into a single string */
1597 static int
1598 fdt_merge_strings(int argc, char *argv[], int start, char **buffer)
1599 {
1600         char *buf;
1601         int i, idx, sz;
1602
1603         *buffer = NULL;
1604         sz = 0;
1605
1606         for (i = start; i < argc; i++)
1607                 sz += strlen(argv[i]);
1608
1609         /* Additional bytes for whitespaces between args */
1610         sz += argc - start;
1611
1612         buf = (char *)malloc(sizeof(char) * sz);
1613         if (buf == NULL) {
1614                 sprintf(command_errbuf, "could not allocate space "
1615                     "for string");
1616                 return (1);
1617         }
1618         bzero(buf, sizeof(char) * sz);
1619
1620         idx = 0;
1621         for (i = start, idx = 0; i < argc; i++) {
1622                 strcpy(buf + idx, argv[i]);
1623                 idx += strlen(argv[i]);
1624                 buf[idx] = ' ';
1625                 idx++;
1626         }
1627         buf[sz - 1] = '\0';
1628         *buffer = buf;
1629         return (0);
1630 }
1631
1632 /* Extract offset and name of node/property from a given path */
1633 static int
1634 fdt_extract_nameloc(char **pathp, char **namep, int *nodeoff)
1635 {
1636         int o;
1637         char *path = *pathp, *name = NULL, *subpath = NULL;
1638
1639         subpath = strrchr(path, '/');
1640         if (subpath == NULL) {
1641                 o = fdt_path_offset(fdtp, cwd);
1642                 name = path;
1643                 path = (char *)&cwd;
1644         } else {
1645                 *subpath = '\0';
1646                 if (strlen(path) == 0)
1647                         path = cwd;
1648
1649                 name = subpath + 1;
1650                 o = fdt_path_offset(fdtp, path);
1651         }
1652
1653         if (strlen(name) == 0) {
1654                 sprintf(command_errbuf, "name not specified");
1655                 return (1);
1656         }
1657         if (o < 0) {
1658                 snprintf(command_errbuf, sizeof(command_errbuf),
1659                     "could not find node: '%s'", path);
1660                 return (1);
1661         }
1662         *namep = name;
1663         *nodeoff = o;
1664         *pathp = path;
1665         return (0);
1666 }
1667
1668 static int
1669 fdt_cmd_prop(int argc, char *argv[])
1670 {
1671         char *path, *propname, *value;
1672         int o, next, depth, rv;
1673         uint32_t tag;
1674
1675         path = (argc > 2) ? argv[2] : NULL;
1676
1677         value = NULL;
1678
1679         if (argc > 3) {
1680                 /* Merge property value strings into one */
1681                 if (fdt_merge_strings(argc, argv, 3, &value) != 0)
1682                         return (CMD_ERROR);
1683         } else
1684                 value = NULL;
1685
1686         if (path == NULL)
1687                 path = cwd;
1688
1689         rv = CMD_OK;
1690
1691         if (value) {
1692                 /* If value is specified -- try to modify prop. */
1693                 if (fdt_extract_nameloc(&path, &propname, &o) != 0)
1694                         return (CMD_ERROR);
1695
1696                 rv = fdt_modprop(o, propname, value, 0);
1697                 if (rv)
1698                         return (CMD_ERROR);
1699                 return (CMD_OK);
1700
1701         }
1702         /* User wants to display properties */
1703         o = fdt_path_offset(fdtp, path);
1704
1705         if (o < 0) {
1706                 snprintf(command_errbuf, sizeof(command_errbuf),
1707                     "could not find node: '%s'", path);
1708                 rv = CMD_ERROR;
1709                 goto out;
1710         }
1711
1712         depth = 0;
1713         while (depth >= 0) {
1714                 tag = fdt_next_tag(fdtp, o, &next);
1715                 switch (tag) {
1716                 case FDT_NOP:
1717                         break;
1718                 case FDT_PROP:
1719                         if (depth > 1)
1720                                 /* Don't process properties of nested nodes */
1721                                 break;
1722
1723                         if (fdt_prop(o) != 0) {
1724                                 sprintf(command_errbuf, "could not process "
1725                                     "property");
1726                                 rv = CMD_ERROR;
1727                                 goto out;
1728                         }
1729                         break;
1730                 case FDT_BEGIN_NODE:
1731                         depth++;
1732                         if (depth > FDT_MAX_DEPTH) {
1733                                 printf("warning: nesting too deep: %d\n",
1734                                     depth);
1735                                 goto out;
1736                         }
1737                         break;
1738                 case FDT_END_NODE:
1739                         depth--;
1740                         if (depth == 0)
1741                                 /*
1742                                  * This is the end of our starting node, force
1743                                  * the loop finish.
1744                                  */
1745                                 depth--;
1746                         break;
1747                 }
1748                 o = next;
1749         }
1750 out:
1751         return (rv);
1752 }
1753
1754 static int
1755 fdt_cmd_mkprop(int argc, char *argv[])
1756 {
1757         int o;
1758         char *path, *propname, *value;
1759
1760         path = (argc > 2) ? argv[2] : NULL;
1761
1762         value = NULL;
1763
1764         if (argc > 3) {
1765                 /* Merge property value strings into one */
1766                 if (fdt_merge_strings(argc, argv, 3, &value) != 0)
1767                         return (CMD_ERROR);
1768         } else
1769                 value = NULL;
1770
1771         if (fdt_extract_nameloc(&path, &propname, &o) != 0)
1772                 return (CMD_ERROR);
1773
1774         if (fdt_modprop(o, propname, value, 1))
1775                 return (CMD_ERROR);
1776
1777         return (CMD_OK);
1778 }
1779
1780 static int
1781 fdt_cmd_rm(int argc, char *argv[])
1782 {
1783         int o, rv;
1784         char *path = NULL, *propname;
1785
1786         if (argc > 2)
1787                 path = argv[2];
1788         else {
1789                 sprintf(command_errbuf, "no node/property name specified");
1790                 return (CMD_ERROR);
1791         }
1792
1793         o = fdt_path_offset(fdtp, path);
1794         if (o < 0) {
1795                 /* If node not found -- try to find & delete property */
1796                 if (fdt_extract_nameloc(&path, &propname, &o) != 0)
1797                         return (CMD_ERROR);
1798
1799                 if ((rv = fdt_delprop(fdtp, o, propname)) != 0) {
1800                         snprintf(command_errbuf, sizeof(command_errbuf),
1801                             "could not delete %s\n",
1802                             (rv == -FDT_ERR_NOTFOUND) ?
1803                             "(property/node does not exist)" : "");
1804                         return (CMD_ERROR);
1805
1806                 } else
1807                         return (CMD_OK);
1808         }
1809         /* If node exists -- remove node */
1810         rv = fdt_del_node(fdtp, o);
1811         if (rv) {
1812                 sprintf(command_errbuf, "could not delete node");
1813                 return (CMD_ERROR);
1814         }
1815         return (CMD_OK);
1816 }
1817
1818 static int
1819 fdt_cmd_mknode(int argc, char *argv[])
1820 {
1821         int o, rv;
1822         char *path = NULL, *nodename = NULL;
1823
1824         if (argc > 2)
1825                 path = argv[2];
1826         else {
1827                 sprintf(command_errbuf, "no node name specified");
1828                 return (CMD_ERROR);
1829         }
1830
1831         if (fdt_extract_nameloc(&path, &nodename, &o) != 0)
1832                 return (CMD_ERROR);
1833
1834         rv = fdt_add_subnode(fdtp, o, nodename);
1835
1836         if (rv < 0) {
1837                 if (rv == -FDT_ERR_NOSPACE)
1838                         sprintf(command_errbuf,
1839                             "Device tree blob is too small!\n");
1840                 else
1841                         sprintf(command_errbuf,
1842                             "Could not add node!\n");
1843                 return (CMD_ERROR);
1844         }
1845         return (CMD_OK);
1846 }
1847
1848 static int
1849 fdt_cmd_pwd(int argc, char *argv[])
1850 {
1851         char line[FDT_CWD_LEN];
1852
1853         pager_open();
1854         sprintf(line, "%s\n", cwd);
1855         pager_output(line);
1856         pager_close();
1857         return (CMD_OK);
1858 }
1859
1860 static int
1861 fdt_cmd_mres(int argc, char *argv[])
1862 {
1863         uint64_t start, size;
1864         int i, total;
1865         char line[80];
1866
1867         pager_open();
1868         total = fdt_num_mem_rsv(fdtp);
1869         if (total > 0) {
1870                 if (pager_output("Reserved memory regions:\n"))
1871                         goto out;
1872                 for (i = 0; i < total; i++) {
1873                         fdt_get_mem_rsv(fdtp, i, &start, &size);
1874                         sprintf(line, "reg#%d: (start: 0x%jx, size: 0x%jx)\n",
1875                             i, start, size);
1876                         if (pager_output(line))
1877                                 goto out;
1878                 }
1879         } else
1880                 pager_output("No reserved memory regions\n");
1881 out:
1882         pager_close();
1883
1884         return (CMD_OK);
1885 }
1886
1887 static int
1888 fdt_cmd_nyi(int argc, char *argv[])
1889 {
1890
1891         printf("command not yet implemented\n");
1892         return (CMD_ERROR);
1893 }
1894
1895 const char *
1896 fdt_devmatch_next(int *tag, int *compatlen)
1897 {
1898         const struct fdt_property *p;
1899         const struct fdt_property *status;
1900         int o, len = -1;
1901         static int depth = 0;
1902
1903         if (fdtp == NULL) {
1904                 fdt_setup_fdtp();
1905                 fdt_apply_overlays();
1906         }
1907
1908         if (*tag != 0) {
1909                 o = *tag;
1910                 /* We are at the end of the DTB */
1911                 if (o < 0)
1912                         return (NULL);
1913         } else {
1914                 o = fdt_path_offset(fdtp, "/");
1915                 if (o < 0) {
1916                         printf("Can't find dtb\n");
1917                         return (NULL);
1918                 }
1919                 depth = 0;
1920         }
1921
1922         /* Find the next node with a compatible property */
1923         while (1) {
1924                 p = NULL;
1925                 if (o >= 0 && depth >= 0) {
1926                         /* skip disabled nodes */
1927                         status = fdt_get_property(fdtp, o, "status", &len);
1928                         if (len > 0) {
1929                                 if (strcmp(status->data, "disabled") == 0) {
1930                                         o = fdt_next_node(fdtp, o, &depth);
1931                                         if (o < 0) /* End of tree */
1932                                                 return (NULL);
1933                                         continue;
1934                                 }
1935                         }
1936
1937                         p = fdt_get_property(fdtp, o, "compatible", &len);
1938                 }
1939                 if (p)
1940                         break;
1941                 o = fdt_next_node(fdtp, o, &depth);
1942                 if (o < 0) /* End of tree */
1943                         return (NULL);
1944         }
1945
1946         /* Prepare next node for next call */
1947         o = fdt_next_node(fdtp, o, &depth);
1948         *tag = o;
1949
1950         if (len >= 0) {
1951                 *compatlen = len;
1952                 return (p->data);
1953         }
1954         return (NULL);
1955 }