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 void
 431 fdt_apply_overlays()
 432 {
 433         struct preloaded_file *fp;
 434         size_t max_overlay_size, next_fdtp_size;
 435         size_t current_fdtp_size;
 436         void *current_fdtp;
 437         void *next_fdtp;
 438         void *overlay;
 439         int rv;
 440
 441         if ((fdtp == NULL) || (fdtp_size == 0))
 442                 return;
 443
 444         if (fdt_overlays_applied)
 445                 return;
 446
 447         max_overlay_size = 0;
 448         for (fp = file_findfile(NULL, "dtbo"); fp != NULL; fp = fp->f_next) {
 449                 if (max_overlay_size < fp->f_size)
 450                         max_overlay_size = fp->f_size;
 451         }
 452
 453         /* Nothing to apply */
 454         if (max_overlay_size == 0)
 455                 return;
 456
 457         overlay = malloc(max_overlay_size);
 458         if (overlay == NULL) {
 459                 printf("failed to allocate memory for DTB blob with overlays\n");
 460                 return;
 461         }
 462         current_fdtp = fdtp;
 463         current_fdtp_size = fdtp_size;
 464         for (fp = file_findfile(NULL, "dtbo"); fp != NULL; fp = fp->f_next) {
 465                 COPYOUT(fp->f_addr, overlay, fp->f_size);
 466                 /* Check compatible first to avoid unnecessary allocation */
 467                 rv = fdt_check_overlay_compatible(current_fdtp, overlay);
 468                 if (rv != 0) {
 469                         printf("DTB overlay '%s' not compatible\n", fp->f_name);
 470                         continue;
 471                 }
 472                 printf("applying DTB overlay '%s'\n", fp->f_name);
 473                 next_fdtp_size = current_fdtp_size + fp->f_size;
 474                 next_fdtp = malloc(next_fdtp_size);
 475                 if (next_fdtp == NULL) {
 476                         /*
 477                          * Output warning, then move on to applying other
 478                          * overlays in case this one is simply too large.
 479                          */
 480                         printf("failed to allocate memory for overlay base\n");
 481                         continue;
 482                 }
 483                 rv = fdt_open_into(current_fdtp, next_fdtp, next_fdtp_size);
 484                 if (rv != 0) {
 485                         free(next_fdtp);
 486                         printf("failed to open base dtb into overlay base\n");
 487                         continue;
 488                 }
 489                 /* Both overlay and next_fdtp may be modified in place */
 490                 rv = fdt_overlay_apply(next_fdtp, overlay);
 491                 if (rv == 0) {
 492                         /* Rotate next -> current */
 493                         if (current_fdtp != fdtp)
 494                                 free(current_fdtp);
 495                         current_fdtp = next_fdtp;
 496                         current_fdtp_size = next_fdtp_size;
 497                 } else {
 498                         /*
 499                          * Assume here that the base we tried to apply on is
 500                          * either trashed or in an inconsistent state. Trying to
 501                          * load it might work, but it's better to discard it and
 502                          * play it safe. */
 503                         free(next_fdtp);
 504                         printf("failed to apply overlay: %s\n",
 505                             fdt_strerror(rv));
 506                 }
 507         }
 508         /* We could have failed to apply all overlays; then we do nothing */
 509         if (current_fdtp != fdtp) {
 510                 free(fdtp);
 511                 fdtp = current_fdtp;
 512                 fdtp_size = current_fdtp_size;
 513         }
 514         free(overlay);
 515         fdt_overlays_applied = 1;
 516 }
 517
 518 int
 519 fdt_is_setup(void)
 520 {
 521
 522         if (fdtp != NULL)
 523                 return (1);
 524
 525         return (0);
 526 }
 527
 528 int
 529 fdt_setup_fdtp()
 530 {
 531         struct preloaded_file *bfp;
 532         vm_offset_t va;
 533
 534         debugf("fdt_setup_fdtp()\n");
 535
 536         /* If we already loaded a file, use it. */
 537         if ((bfp = file_findfile(NULL, "dtb")) != NULL) {
 538                 if (fdt_load_dtb(bfp->f_addr) == 0) {
 539                         printf("Using DTB from loaded file '%s'.\n",
 540                             bfp->f_name);
 541                         fdt_platform_load_overlays();
 542                         return (0);
 543                 }
 544         }
 545
 546         /* If we were given the address of a valid blob in memory, use it. */
 547         if (fdt_to_load != NULL) {
 548                 if (fdt_load_dtb_addr(fdt_to_load) == 0) {
 549                         printf("Using DTB from memory address %p.\n",
 550                             fdt_to_load);
 551                         fdt_platform_load_overlays();
 552                         return (0);
 553                 }
 554         }
 555
 556         if (fdt_platform_load_dtb() == 0) {
 557                 fdt_platform_load_overlays();
 558                 return (0);
 559         }
 560
 561         /* If there is a dtb compiled into the kernel, use it. */
 562         if ((va = fdt_find_static_dtb()) != 0) {
 563                 if (fdt_load_dtb(va) == 0) {
 564                         printf("Using DTB compiled into kernel.\n");
 565                         return (0);
 566                 }
 567         }
 568
 569         command_errmsg = "No device tree blob found!\n";
 570         return (1);
 571 }
 572
 573 #define fdt_strtovect(str, cellbuf, lim, cellsize) _fdt_strtovect((str), \
 574     (cellbuf), (lim), (cellsize), 0);
 575
 576 /* Force using base 16 */
 577 #define fdt_strtovectx(str, cellbuf, lim, cellsize) _fdt_strtovect((str), \
 578     (cellbuf), (lim), (cellsize), 16);
 579
 580 static int
 581 _fdt_strtovect(const char *str, void *cellbuf, int lim, unsigned char cellsize,
 582     uint8_t base)
 583 {
 584         const char *buf = str;
 585         const char *end = str + strlen(str) - 2;
 586         uint32_t *u32buf = NULL;
 587         uint8_t *u8buf = NULL;
 588         int cnt = 0;
 589
 590         if (cellsize == sizeof(uint32_t))
 591                 u32buf = (uint32_t *)cellbuf;
 592         else
 593                 u8buf = (uint8_t *)cellbuf;
 594
 595         if (lim == 0)
 596                 return (0);
 597
 598         while (buf < end) {
 599
 600                 /* Skip white whitespace(s)/separators */
 601                 while (!isxdigit(*buf) && buf < end)
 602                         buf++;
 603
 604                 if (u32buf != NULL)
 605                         u32buf[cnt] =
 606                             cpu_to_fdt32((uint32_t)strtol(buf, NULL, base));
 607
 608                 else
 609                         u8buf[cnt] = (uint8_t)strtol(buf, NULL, base);
 610
 611                 if (cnt + 1 <= lim - 1)
 612                         cnt++;
 613                 else
 614                         break;
 615                 buf++;
 616                 /* Find another number */
 617                 while ((isxdigit(*buf) || *buf == 'x') && buf < end)
 618                         buf++;
 619         }
 620         return (cnt);
 621 }
 622
 623 void
 624 fdt_fixup_ethernet(const char *str, char *ethstr, int len)
 625 {
 626         uint8_t tmp_addr[6];
 627
 628         /* Convert macaddr string into a vector of uints */
 629         fdt_strtovectx(str, &tmp_addr, 6, sizeof(uint8_t));
 630         /* Set actual property to a value from vect */
 631         fdt_setprop(fdtp, fdt_path_offset(fdtp, ethstr),
 632             "local-mac-address", &tmp_addr, 6 * sizeof(uint8_t));
 633 }
 634
 635 void
 636 fdt_fixup_cpubusfreqs(unsigned long cpufreq, unsigned long busfreq)
 637 {
 638         int lo, o = 0, o2, maxo = 0, depth;
 639         const uint32_t zero = 0;
 640
 641         /* We want to modify every subnode of /cpus */
 642         o = fdt_path_offset(fdtp, "/cpus");
 643         if (o < 0)
 644                 return;
 645
 646         /* maxo should contain offset of node next to /cpus */
 647         depth = 0;
 648         maxo = o;
 649         while (depth != -1)
 650                 maxo = fdt_next_node(fdtp, maxo, &depth);
 651
 652         /* Find CPU frequency properties */
 653         o = fdt_node_offset_by_prop_value(fdtp, o, "clock-frequency",
 654             &zero, sizeof(uint32_t));
 655
 656         o2 = fdt_node_offset_by_prop_value(fdtp, o, "bus-frequency", &zero,
 657             sizeof(uint32_t));
 658
 659         lo = MIN(o, o2);
 660
 661         while (o != -FDT_ERR_NOTFOUND && o2 != -FDT_ERR_NOTFOUND) {
 662
 663                 o = fdt_node_offset_by_prop_value(fdtp, lo,
 664                     "clock-frequency", &zero, sizeof(uint32_t));
 665
 666                 o2 = fdt_node_offset_by_prop_value(fdtp, lo, "bus-frequency",
 667                     &zero, sizeof(uint32_t));
 668
 669                 /* We're only interested in /cpus subnode(s) */
 670                 if (lo > maxo)
 671                         break;
 672
 673                 fdt_setprop_inplace_cell(fdtp, lo, "clock-frequency",
 674                     (uint32_t)cpufreq);
 675
 676                 fdt_setprop_inplace_cell(fdtp, lo, "bus-frequency",
 677                     (uint32_t)busfreq);
 678
 679                 lo = MIN(o, o2);
 680         }
 681 }
 682
 683 #ifdef notyet
 684 static int
 685 fdt_reg_valid(uint32_t *reg, int len, int addr_cells, int size_cells)
 686 {
 687         int cells_in_tuple, i, tuples, tuple_size;
 688         uint32_t cur_start, cur_size;
 689
 690         cells_in_tuple = (addr_cells + size_cells);
 691         tuple_size = cells_in_tuple * sizeof(uint32_t);
 692         tuples = len / tuple_size;
 693         if (tuples == 0)
 694                 return (EINVAL);
 695
 696         for (i = 0; i < tuples; i++) {
 697                 if (addr_cells == 2)
 698                         cur_start = fdt64_to_cpu(reg[i * cells_in_tuple]);
 699                 else
 700                         cur_start = fdt32_to_cpu(reg[i * cells_in_tuple]);
 701
 702                 if (size_cells == 2)
 703                         cur_size = fdt64_to_cpu(reg[i * cells_in_tuple + 2]);
 704                 else
 705                         cur_size = fdt32_to_cpu(reg[i * cells_in_tuple + 1]);
 706
 707                 if (cur_size == 0)
 708                         return (EINVAL);
 709
 710                 debugf(" reg#%d (start: 0x%0x size: 0x%0x) valid!\n",
 711                     i, cur_start, cur_size);
 712         }
 713         return (0);
 714 }
 715 #endif
 716
 717 void
 718 fdt_fixup_memory(struct fdt_mem_region *region, size_t num)
 719 {
 720         struct fdt_mem_region *curmr;
 721         uint32_t addr_cells, size_cells;
 722         uint32_t *addr_cellsp, *size_cellsp;
 723         int err, i, len, memory, root;
 724         size_t realmrno;
 725         uint8_t *buf, *sb;
 726         uint64_t rstart, rsize;
 727         int reserved;
 728
 729         root = fdt_path_offset(fdtp, "/");
 730         if (root < 0) {
 731                 sprintf(command_errbuf, "Could not find root node !");
 732                 return;
 733         }
 734
 735         memory = fdt_path_offset(fdtp, "/memory");
 736         if (memory <= 0) {
 737                 /* Create proper '/memory' node. */
 738                 memory = fdt_add_subnode(fdtp, root, "memory");
 739                 if (memory <= 0) {
 740                         snprintf(command_errbuf, sizeof(command_errbuf),
 741                             "Could not fixup '/memory' "
 742                             "node, error code : %d!\n", memory);
 743                         return;
 744                 }
 745
 746                 err = fdt_setprop(fdtp, memory, "device_type", "memory",
 747                     sizeof("memory"));
 748
 749                 if (err < 0)
 750                         return;
 751         }
 752
 753         addr_cellsp = (uint32_t *)fdt_getprop(fdtp, root, "#address-cells",
 754             NULL);
 755         size_cellsp = (uint32_t *)fdt_getprop(fdtp, root, "#size-cells", NULL);
 756
 757         if (addr_cellsp == NULL || size_cellsp == NULL) {
 758                 snprintf(command_errbuf, sizeof(command_errbuf),
 759                     "Could not fixup '/memory' node : "
 760                     "%s %s property not found in root node!\n",
 761                     (!addr_cellsp) ? "#address-cells" : "",
 762                     (!size_cellsp) ? "#size-cells" : "");
 763                 return;
 764         }
 765
 766         addr_cells = fdt32_to_cpu(*addr_cellsp);
 767         size_cells = fdt32_to_cpu(*size_cellsp);
 768
 769         /*
 770          * Convert memreserve data to memreserve property
 771          * Check if property already exists
 772          */
 773         reserved = fdt_num_mem_rsv(fdtp);
 774         if (reserved &&
 775             (fdt_getprop(fdtp, root, "memreserve", NULL) == NULL)) {
 776                 len = (addr_cells + size_cells) * reserved * sizeof(uint32_t);
 777                 sb = buf = (uint8_t *)malloc(len);
 778                 if (!buf)
 779                         return;
 780
 781                 bzero(buf, len);
 782
 783                 for (i = 0; i < reserved; i++) {
 784                         if (fdt_get_mem_rsv(fdtp, i, &rstart, &rsize))
 785                                 break;
 786                         if (rsize) {
 787                                 /* Ensure endianness, and put cells into a buffer */
 788                                 if (addr_cells == 2)
 789                                         *(uint64_t *)buf =
 790                                             cpu_to_fdt64(rstart);
 791                                 else
 792                                         *(uint32_t *)buf =
 793                                             cpu_to_fdt32(rstart);
 794
 795                                 buf += sizeof(uint32_t) * addr_cells;
 796                                 if (size_cells == 2)
 797                                         *(uint64_t *)buf =
 798                                             cpu_to_fdt64(rsize);
 799                                 else
 800                                         *(uint32_t *)buf =
 801                                             cpu_to_fdt32(rsize);
 802
 803                                 buf += sizeof(uint32_t) * size_cells;
 804                         }
 805                 }
 806
 807                 /* Set property */
 808                 if ((err = fdt_setprop(fdtp, root, "memreserve", sb, len)) < 0)
 809                         printf("Could not fixup 'memreserve' property.\n");
 810
 811                 free(sb);
 812         }
 813
 814         /* Count valid memory regions entries in sysinfo. */
 815         realmrno = num;
 816         for (i = 0; i < num; i++)
 817                 if (region[i].start == 0 && region[i].size == 0)
 818                         realmrno--;
 819
 820         if (realmrno == 0) {
 821                 sprintf(command_errbuf, "Could not fixup '/memory' node : "
 822                     "sysinfo doesn't contain valid memory regions info!\n");
 823                 return;
 824         }
 825
 826         len = (addr_cells + size_cells) * realmrno * sizeof(uint32_t);
 827         sb = buf = (uint8_t *)malloc(len);
 828         if (!buf)
 829                 return;
 830
 831         bzero(buf, len);
 832
 833         for (i = 0; i < num; i++) {
 834                 curmr = &region[i];
 835                 if (curmr->size != 0) {
 836                         /* Ensure endianness, and put cells into a buffer */
 837                         if (addr_cells == 2)
 838                                 *(uint64_t *)buf =
 839                                     cpu_to_fdt64(curmr->start);
 840                         else
 841                                 *(uint32_t *)buf =
 842                                     cpu_to_fdt32(curmr->start);
 843
 844                         buf += sizeof(uint32_t) * addr_cells;
 845                         if (size_cells == 2)
 846                                 *(uint64_t *)buf =
 847                                     cpu_to_fdt64(curmr->size);
 848                         else
 849                                 *(uint32_t *)buf =
 850                                     cpu_to_fdt32(curmr->size);
 851
 852                         buf += sizeof(uint32_t) * size_cells;
 853                 }
 854         }
 855
 856         /* Set property */
 857         if ((err = fdt_setprop(fdtp, memory, "reg", sb, len)) < 0)
 858                 sprintf(command_errbuf, "Could not fixup '/memory' node.\n");
 859
 860         free(sb);
 861 }
 862
 863 void
 864 fdt_fixup_stdout(const char *str)
 865 {
 866         char *ptr;
 867         int len, no, sero;
 868         const struct fdt_property *prop;
 869         char *tmp[10];
 870
 871         ptr = (char *)str + strlen(str) - 1;
 872         while (ptr > str && isdigit(*(str - 1)))
 873                 str--;
 874
 875         if (ptr == str)
 876                 return;
 877
 878         no = fdt_path_offset(fdtp, "/chosen");
 879         if (no < 0)
 880                 return;
 881
 882         prop = fdt_get_property(fdtp, no, "stdout", &len);
 883
 884         /* If /chosen/stdout does not extist, create it */
 885         if (prop == NULL || (prop != NULL && len == 0)) {
 886
 887                 bzero(tmp, 10 * sizeof(char));
 888                 strcpy((char *)&tmp, "serial");
 889                 if (strlen(ptr) > 3)
 890                         /* Serial number too long */
 891                         return;
 892
 893                 strncpy((char *)tmp + 6, ptr, 3);
 894                 sero = fdt_path_offset(fdtp, (const char *)tmp);
 895                 if (sero < 0)
 896                         /*
 897                          * If serial device we're trying to assign
 898                          * stdout to doesn't exist in DT -- return.
 899                          */
 900                         return;
 901
 902                 fdt_setprop(fdtp, no, "stdout", &tmp,
 903                     strlen((char *)&tmp) + 1);
 904                 fdt_setprop(fdtp, no, "stdin", &tmp,
 905                     strlen((char *)&tmp) + 1);
 906         }
 907 }
 908
 909 void
 910 fdt_load_dtb_overlays(const char *extras)
 911 {
 912         const char *s;
 913
 914         /* Any extra overlays supplied by pre-loader environment */
 915         if (extras != NULL && *extras != '\0') {
 916                 printf("Loading DTB overlays: '%s'\n", extras);
 917                 fdt_load_dtb_overlays_string(extras);
 918         }
 919
 920         /* Any overlays supplied by loader environment */
 921         s = getenv("fdt_overlays");
 922         if (s != NULL && *s != '\0') {
 923                 printf("Loading DTB overlays: '%s'\n", s);
 924                 fdt_load_dtb_overlays_string(s);
 925         }
 926 }
 927
 928 /*
 929  * Locate the blob, fix it up and return its location.
 930  */
 931 static int
 932 fdt_fixup(void)
 933 {
 934         int chosen;
 935
 936         debugf("fdt_fixup()\n");
 937
 938         if (fdtp == NULL && fdt_setup_fdtp() != 0)
 939                 return (0);
 940
 941         /* Create /chosen node (if not exists) */
 942         if ((chosen = fdt_subnode_offset(fdtp, 0, "chosen")) ==
 943             -FDT_ERR_NOTFOUND)
 944                 chosen = fdt_add_subnode(fdtp, 0, "chosen");
 945
 946         /* Value assigned to fixup-applied does not matter. */
 947         if (fdt_getprop(fdtp, chosen, "fixup-applied", NULL))
 948                 return (1);
 949
 950         fdt_platform_fixups();
 951
 952         /*
 953          * Re-fetch the /chosen subnode; our fixups may apply overlays or add
 954          * nodes/properties that invalidate the offset we grabbed or created
 955          * above, so we can no longer trust it.
 956          */
 957         chosen = fdt_subnode_offset(fdtp, 0, "chosen");
 958         fdt_setprop(fdtp, chosen, "fixup-applied", NULL, 0);
 959         return (1);
 960 }
 961
 962 /*
 963  * Copy DTB blob to specified location and return size
 964  */
 965 int
 966 fdt_copy(vm_offset_t va)
 967 {
 968         int err;
 969         debugf("fdt_copy va 0x%08x\n", va);
 970         if (fdtp == NULL) {
 971                 err = fdt_setup_fdtp();
 972                 if (err) {
 973                         printf("No valid device tree blob found!\n");
 974                         return (0);
 975                 }
 976         }
 977
 978         if (fdt_fixup() == 0)
 979                 return (0);
 980
 981         COPYIN(fdtp, va, fdtp_size);
 982         return (fdtp_size);
 983 }
 984
 985
 986
 987 int
 988 command_fdt_internal(int argc, char *argv[])
 989 {
 990         cmdf_t *cmdh;
 991         int flags;
 992         int i, err;
 993
 994         if (argc < 2) {
 995                 command_errmsg = "usage is 'fdt <command> [<args>]";
 996                 return (CMD_ERROR);
 997         }
 998
 999         /*
1000          * Validate fdt <command>.
1001          */
1002         i = 0;
1003         cmdh = NULL;
1004         while (!(commands[i].name == NULL)) {
1005                 if (strcmp(argv[1], commands[i].name) == 0) {
1006                         /* found it */
1007                         cmdh = commands[i].handler;
1008                         flags = commands[i].flags;
1009                         break;
1010                 }
1011                 i++;
1012         }
1013         if (cmdh == NULL) {
1014                 command_errmsg = "unknown command";
1015                 return (CMD_ERROR);
1016         }
1017
1018         if (flags & CMD_REQUIRES_BLOB) {
1019                 /*
1020                  * Check if uboot env vars were parsed already. If not, do it now.
1021                  */
1022                 if (fdt_fixup() == 0)
1023                         return (CMD_ERROR);
1024         }
1025
1026         /*
1027          * Call command handler.
1028          */
1029         err = (*cmdh)(argc, argv);
1030
1031         return (err);
1032 }
1033
1034 static int
1035 fdt_cmd_addr(int argc, char *argv[])
1036 {
1037         struct preloaded_file *fp;
1038         struct fdt_header *hdr;
1039         const char *addr;
1040         char *cp;
1041
1042         fdt_to_load = NULL;
1043
1044         if (argc > 2)
1045                 addr = argv[2];
1046         else {
1047                 sprintf(command_errbuf, "no address specified");
1048                 return (CMD_ERROR);
1049         }
1050
1051         hdr = (struct fdt_header *)strtoul(addr, &cp, 16);
1052         if (cp == addr) {
1053                 snprintf(command_errbuf, sizeof(command_errbuf),
1054                     "Invalid address: %s", addr);
1055                 return (CMD_ERROR);
1056         }
1057
1058         while ((fp = file_findfile(NULL, "dtb")) != NULL) {
1059                 file_discard(fp);
1060         }
1061
1062         fdt_to_load = hdr;
1063         return (CMD_OK);
1064 }
1065
1066 static int
1067 fdt_cmd_cd(int argc, char *argv[])
1068 {
1069         char *path;
1070         char tmp[FDT_CWD_LEN];
1071         int len, o;
1072
1073         path = (argc > 2) ? argv[2] : "/";
1074
1075         if (path[0] == '/') {
1076                 len = strlen(path);
1077                 if (len >= FDT_CWD_LEN)
1078                         goto fail;
1079         } else {
1080                 /* Handle path specification relative to cwd */
1081                 len = strlen(cwd) + strlen(path) + 1;
1082                 if (len >= FDT_CWD_LEN)
1083                         goto fail;
1084
1085                 strcpy(tmp, cwd);
1086                 strcat(tmp, "/");
1087                 strcat(tmp, path);
1088                 path = tmp;
1089         }
1090
1091         o = fdt_path_offset(fdtp, path);
1092         if (o < 0) {
1093                 snprintf(command_errbuf, sizeof(command_errbuf),
1094                     "could not find node: '%s'", path);
1095                 return (CMD_ERROR);
1096         }
1097
1098         strcpy(cwd, path);
1099         return (CMD_OK);
1100
1101 fail:
1102         snprintf(command_errbuf, sizeof(command_errbuf),
1103             "path too long: %d, max allowed: %d", len, FDT_CWD_LEN - 1);
1104         return (CMD_ERROR);
1105 }
1106
1107 static int
1108 fdt_cmd_hdr(int argc __unused, char *argv[] __unused)
1109 {
1110         char line[80];
1111         int ver;
1112
1113         if (fdtp == NULL) {
1114                 command_errmsg = "no device tree blob pointer?!";
1115                 return (CMD_ERROR);
1116         }
1117
1118         ver = fdt_version(fdtp);
1119         pager_open();
1120         sprintf(line, "\nFlattened device tree header (%p):\n", fdtp);
1121         if (pager_output(line))
1122                 goto out;
1123         sprintf(line, " magic                   = 0x%08x\n", fdt_magic(fdtp));
1124         if (pager_output(line))
1125                 goto out;
1126         sprintf(line, " size                    = %d\n", fdt_totalsize(fdtp));
1127         if (pager_output(line))
1128                 goto out;
1129         sprintf(line, " off_dt_struct           = 0x%08x\n",
1130             fdt_off_dt_struct(fdtp));
1131         if (pager_output(line))
1132                 goto out;
1133         sprintf(line, " off_dt_strings          = 0x%08x\n",
1134             fdt_off_dt_strings(fdtp));
1135         if (pager_output(line))
1136                 goto out;
1137         sprintf(line, " off_mem_rsvmap          = 0x%08x\n",
1138             fdt_off_mem_rsvmap(fdtp));
1139         if (pager_output(line))
1140                 goto out;
1141         sprintf(line, " version                 = %d\n", ver);
1142         if (pager_output(line))
1143                 goto out;
1144         sprintf(line, " last compatible version = %d\n",
1145             fdt_last_comp_version(fdtp));
1146         if (pager_output(line))
1147                 goto out;
1148         if (ver >= 2) {
1149                 sprintf(line, " boot_cpuid              = %d\n",
1150                     fdt_boot_cpuid_phys(fdtp));
1151                 if (pager_output(line))
1152                         goto out;
1153         }
1154         if (ver >= 3) {
1155                 sprintf(line, " size_dt_strings         = %d\n",
1156                     fdt_size_dt_strings(fdtp));
1157                 if (pager_output(line))
1158                         goto out;
1159         }
1160         if (ver >= 17) {
1161                 sprintf(line, " size_dt_struct          = %d\n",
1162                     fdt_size_dt_struct(fdtp));
1163                 if (pager_output(line))
1164                         goto out;
1165         }
1166 out:
1167         pager_close();
1168
1169         return (CMD_OK);
1170 }
1171
1172 static int
1173 fdt_cmd_ls(int argc, char *argv[])
1174 {
1175         const char *prevname[FDT_MAX_DEPTH] = { NULL };
1176         const char *name;
1177         char *path;
1178         int i, o, depth;
1179
1180         path = (argc > 2) ? argv[2] : NULL;
1181         if (path == NULL)
1182                 path = cwd;
1183
1184         o = fdt_path_offset(fdtp, path);
1185         if (o < 0) {
1186                 snprintf(command_errbuf, sizeof(command_errbuf),
1187                     "could not find node: '%s'", path);
1188                 return (CMD_ERROR);
1189         }
1190
1191         for (depth = 0;
1192             (o >= 0) && (depth >= 0);
1193             o = fdt_next_node(fdtp, o, &depth)) {
1194
1195                 name = fdt_get_name(fdtp, o, NULL);
1196
1197                 if (depth > FDT_MAX_DEPTH) {
1198                         printf("max depth exceeded: %d\n", depth);
1199                         continue;
1200                 }
1201
1202                 prevname[depth] = name;
1203
1204                 /* Skip root (i = 1) when printing devices */
1205                 for (i = 1; i <= depth; i++) {
1206                         if (prevname[i] == NULL)
1207                                 break;
1208
1209                         if (strcmp(cwd, "/") == 0)
1210                                 printf("/");
1211                         printf("%s", prevname[i]);
1212                 }
1213                 printf("\n");
1214         }
1215
1216         return (CMD_OK);
1217 }
1218
1219 static __inline int
1220 isprint(int c)
1221 {
1222
1223         return (c >= ' ' && c <= 0x7e);
1224 }
1225
1226 static int
1227 fdt_isprint(const void *data, int len, int *count)
1228 {
1229         const char *d;
1230         char ch;
1231         int yesno, i;
1232
1233         if (len == 0)
1234                 return (0);
1235
1236         d = (const char *)data;
1237         if (d[len - 1] != '\0')
1238                 return (0);
1239
1240         *count = 0;
1241         yesno = 1;
1242         for (i = 0; i < len; i++) {
1243                 ch = *(d + i);
1244                 if (isprint(ch) || (ch == '\0' && i > 0)) {
1245                         /* Count strings */
1246                         if (ch == '\0')
1247                                 (*count)++;
1248                         continue;
1249                 }
1250
1251                 yesno = 0;
1252                 break;
1253         }
1254
1255         return (yesno);
1256 }
1257
1258 static int
1259 fdt_data_str(const void *data, int len, int count, char **buf)
1260 {
1261         char *b, *tmp;
1262         const char *d;
1263         int buf_len, i, l;
1264
1265         /*
1266          * Calculate the length for the string and allocate memory.
1267          *
1268          * Note that 'len' already includes at least one terminator.
1269          */
1270         buf_len = len;
1271         if (count > 1) {
1272                 /*
1273                  * Each token had already a terminator buried in 'len', but we
1274                  * only need one eventually, don't count space for these.
1275                  */
1276                 buf_len -= count - 1;
1277
1278                 /* Each consecutive token requires a ", " separator. */
1279                 buf_len += count * 2;
1280         }
1281
1282         /* Add some space for surrounding double quotes. */
1283         buf_len += count * 2;
1284
1285         /* Note that string being put in 'tmp' may be as big as 'buf_len'. */
1286         b = (char *)malloc(buf_len);
1287         tmp = (char *)malloc(buf_len);
1288         if (b == NULL)
1289                 goto error;
1290
1291         if (tmp == NULL) {
1292                 free(b);
1293                 goto error;
1294         }
1295
1296         b[0] = '\0';
1297
1298         /*
1299          * Now that we have space, format the string.
1300          */
1301         i = 0;
1302         do {
1303                 d = (const char *)data + i;
1304                 l = strlen(d) + 1;
1305
1306                 sprintf(tmp, "\"%s\"%s", d,
1307                     (i + l) < len ?  ", " : "");
1308                 strcat(b, tmp);
1309
1310                 i += l;
1311
1312         } while (i < len);
1313         *buf = b;
1314
1315         free(tmp);
1316
1317         return (0);
1318 error:
1319         return (1);
1320 }
1321
1322 static int
1323 fdt_data_cell(const void *data, int len, char **buf)
1324 {
1325         char *b, *tmp;
1326         const uint32_t *c;
1327         int count, i, l;
1328
1329         /* Number of cells */
1330         count = len / 4;
1331
1332         /*
1333          * Calculate the length for the string and allocate memory.
1334          */
1335
1336         /* Each byte translates to 2 output characters */
1337         l = len * 2;
1338         if (count > 1) {
1339                 /* Each consecutive cell requires a " " separator. */
1340                 l += (count - 1) * 1;
1341         }
1342         /* Each cell will have a "0x" prefix */
1343         l += count * 2;
1344         /* Space for surrounding <> and terminator */
1345         l += 3;
1346
1347         b = (char *)malloc(l);
1348         tmp = (char *)malloc(l);
1349         if (b == NULL)
1350                 goto error;
1351
1352         if (tmp == NULL) {
1353                 free(b);
1354                 goto error;
1355         }
1356
1357         b[0] = '\0';
1358         strcat(b, "<");
1359
1360         for (i = 0; i < len; i += 4) {
1361                 c = (const uint32_t *)((const uint8_t *)data + i);
1362                 sprintf(tmp, "0x%08x%s", fdt32_to_cpu(*c),
1363                     i < (len - 4) ? " " : "");
1364                 strcat(b, tmp);
1365         }
1366         strcat(b, ">");
1367         *buf = b;
1368
1369         free(tmp);
1370
1371         return (0);
1372 error:
1373         return (1);
1374 }
1375
1376 static int
1377 fdt_data_bytes(const void *data, int len, char **buf)
1378 {
1379         char *b, *tmp;
1380         const char *d;
1381         int i, l;
1382
1383         /*
1384          * Calculate the length for the string and allocate memory.
1385          */
1386
1387         /* Each byte translates to 2 output characters */
1388         l = len * 2;
1389         if (len > 1)
1390                 /* Each consecutive byte requires a " " separator. */
1391                 l += (len - 1) * 1;
1392         /* Each byte will have a "0x" prefix */
1393         l += len * 2;
1394         /* Space for surrounding [] and terminator. */
1395         l += 3;
1396
1397         b = (char *)malloc(l);
1398         tmp = (char *)malloc(l);
1399         if (b == NULL)
1400                 goto error;
1401
1402         if (tmp == NULL) {
1403                 free(b);
1404                 goto error;
1405         }
1406
1407         b[0] = '\0';
1408         strcat(b, "[");
1409
1410         for (i = 0, d = data; i < len; i++) {
1411                 sprintf(tmp, "0x%02x%s", d[i], i < len - 1 ? " " : "");
1412                 strcat(b, tmp);
1413         }
1414         strcat(b, "]");
1415         *buf = b;
1416
1417         free(tmp);
1418
1419         return (0);
1420 error:
1421         return (1);
1422 }
1423
1424 static int
1425 fdt_data_fmt(const void *data, int len, char **buf)
1426 {
1427         int count;
1428
1429         if (len == 0) {
1430                 *buf = NULL;
1431                 return (1);
1432         }
1433
1434         if (fdt_isprint(data, len, &count))
1435                 return (fdt_data_str(data, len, count, buf));
1436
1437         else if ((len % 4) == 0)
1438                 return (fdt_data_cell(data, len, buf));
1439
1440         else
1441                 return (fdt_data_bytes(data, len, buf));
1442 }
1443
1444 static int
1445 fdt_prop(int offset)
1446 {
1447         char *line, *buf;
1448         const struct fdt_property *prop;
1449         const char *name;
1450         const void *data;
1451         int len, rv;
1452
1453         line = NULL;
1454         prop = fdt_offset_ptr(fdtp, offset, sizeof(*prop));
1455         if (prop == NULL)
1456                 return (1);
1457
1458         name = fdt_string(fdtp, fdt32_to_cpu(prop->nameoff));
1459         len = fdt32_to_cpu(prop->len);
1460
1461         rv = 0;
1462         buf = NULL;
1463         if (len == 0) {
1464                 /* Property without value */
1465                 line = (char *)malloc(strlen(name) + 2);
1466                 if (line == NULL) {
1467                         rv = 2;
1468                         goto out2;
1469                 }
1470                 sprintf(line, "%s\n", name);
1471                 goto out1;
1472         }
1473
1474         /*
1475          * Process property with value
1476          */
1477         data = prop->data;
1478
1479         if (fdt_data_fmt(data, len, &buf) != 0) {
1480                 rv = 3;
1481                 goto out2;
1482         }
1483
1484         line = (char *)malloc(strlen(name) + strlen(FDT_PROP_SEP) +
1485             strlen(buf) + 2);
1486         if (line == NULL) {
1487                 sprintf(command_errbuf, "could not allocate space for string");
1488                 rv = 4;
1489                 goto out2;
1490         }
1491
1492         sprintf(line, "%s" FDT_PROP_SEP "%s\n", name, buf);
1493
1494 out1:
1495         pager_open();
1496         pager_output(line);
1497         pager_close();
1498
1499 out2:
1500         if (buf)
1501                 free(buf);
1502
1503         if (line)
1504                 free(line);
1505
1506         return (rv);
1507 }
1508
1509 static int
1510 fdt_modprop(int nodeoff, char *propname, void *value, char mode)
1511 {
1512         uint32_t cells[100];
1513         const char *buf;
1514         int len, rv;
1515         const struct fdt_property *p;
1516
1517         p = fdt_get_property(fdtp, nodeoff, propname, NULL);
1518
1519         if (p != NULL) {
1520                 if (mode == 1) {
1521                          /* Adding inexistant value in mode 1 is forbidden */
1522                         sprintf(command_errbuf, "property already exists!");
1523                         return (CMD_ERROR);
1524                 }
1525         } else if (mode == 0) {
1526                 sprintf(command_errbuf, "property does not exist!");
1527                 return (CMD_ERROR);
1528         }
1529         rv = 0;
1530         buf = value;
1531
1532         switch (*buf) {
1533         case '&':
1534                 /* phandles */
1535                 break;
1536         case '<':
1537                 /* Data cells */
1538                 len = fdt_strtovect(buf, (void *)&cells, 100,
1539                     sizeof(uint32_t));
1540
1541                 rv = fdt_setprop(fdtp, nodeoff, propname, &cells,
1542                     len * sizeof(uint32_t));
1543                 break;
1544         case '[':
1545                 /* Data bytes */
1546                 len = fdt_strtovect(buf, (void *)&cells, 100,
1547                     sizeof(uint8_t));
1548
1549                 rv = fdt_setprop(fdtp, nodeoff, propname, &cells,
1550                     len * sizeof(uint8_t));
1551                 break;
1552         case '"':
1553         default:
1554                 /* Default -- string */
1555                 rv = fdt_setprop_string(fdtp, nodeoff, propname, value);
1556                 break;
1557         }
1558
1559         if (rv != 0) {
1560                 if (rv == -FDT_ERR_NOSPACE)
1561                         sprintf(command_errbuf,
1562                             "Device tree blob is too small!\n");
1563                 else
1564                         sprintf(command_errbuf,
1565                             "Could not add/modify property!\n");
1566         }
1567         return (rv);
1568 }
1569
1570 /* Merge strings from argv into a single string */
1571 static int
1572 fdt_merge_strings(int argc, char *argv[], int start, char **buffer)
1573 {
1574         char *buf;
1575         int i, idx, sz;
1576
1577         *buffer = NULL;
1578         sz = 0;
1579
1580         for (i = start; i < argc; i++)
1581                 sz += strlen(argv[i]);
1582
1583         /* Additional bytes for whitespaces between args */
1584         sz += argc - start;
1585
1586         buf = (char *)malloc(sizeof(char) * sz);
1587         if (buf == NULL) {
1588                 sprintf(command_errbuf, "could not allocate space "
1589                     "for string");
1590                 return (1);
1591         }
1592         bzero(buf, sizeof(char) * sz);
1593
1594         idx = 0;
1595         for (i = start, idx = 0; i < argc; i++) {
1596                 strcpy(buf + idx, argv[i]);
1597                 idx += strlen(argv[i]);
1598                 buf[idx] = ' ';
1599                 idx++;
1600         }
1601         buf[sz - 1] = '\0';
1602         *buffer = buf;
1603         return (0);
1604 }
1605
1606 /* Extract offset and name of node/property from a given path */
1607 static int
1608 fdt_extract_nameloc(char **pathp, char **namep, int *nodeoff)
1609 {
1610         int o;
1611         char *path = *pathp, *name = NULL, *subpath = NULL;
1612
1613         subpath = strrchr(path, '/');
1614         if (subpath == NULL) {
1615                 o = fdt_path_offset(fdtp, cwd);
1616                 name = path;
1617                 path = (char *)&cwd;
1618         } else {
1619                 *subpath = '\0';
1620                 if (strlen(path) == 0)
1621                         path = cwd;
1622
1623                 name = subpath + 1;
1624                 o = fdt_path_offset(fdtp, path);
1625         }
1626
1627         if (strlen(name) == 0) {
1628                 sprintf(command_errbuf, "name not specified");
1629                 return (1);
1630         }
1631         if (o < 0) {
1632                 snprintf(command_errbuf, sizeof(command_errbuf),
1633                     "could not find node: '%s'", path);
1634                 return (1);
1635         }
1636         *namep = name;
1637         *nodeoff = o;
1638         *pathp = path;
1639         return (0);
1640 }
1641
1642 static int
1643 fdt_cmd_prop(int argc, char *argv[])
1644 {
1645         char *path, *propname, *value;
1646         int o, next, depth, rv;
1647         uint32_t tag;
1648
1649         path = (argc > 2) ? argv[2] : NULL;
1650
1651         value = NULL;
1652
1653         if (argc > 3) {
1654                 /* Merge property value strings into one */
1655                 if (fdt_merge_strings(argc, argv, 3, &value) != 0)
1656                         return (CMD_ERROR);
1657         } else
1658                 value = NULL;
1659
1660         if (path == NULL)
1661                 path = cwd;
1662
1663         rv = CMD_OK;
1664
1665         if (value) {
1666                 /* If value is specified -- try to modify prop. */
1667                 if (fdt_extract_nameloc(&path, &propname, &o) != 0)
1668                         return (CMD_ERROR);
1669
1670                 rv = fdt_modprop(o, propname, value, 0);
1671                 if (rv)
1672                         return (CMD_ERROR);
1673                 return (CMD_OK);
1674
1675         }
1676         /* User wants to display properties */
1677         o = fdt_path_offset(fdtp, path);
1678
1679         if (o < 0) {
1680                 snprintf(command_errbuf, sizeof(command_errbuf),
1681                     "could not find node: '%s'", path);
1682                 rv = CMD_ERROR;
1683                 goto out;
1684         }
1685
1686         depth = 0;
1687         while (depth >= 0) {
1688                 tag = fdt_next_tag(fdtp, o, &next);
1689                 switch (tag) {
1690                 case FDT_NOP:
1691                         break;
1692                 case FDT_PROP:
1693                         if (depth > 1)
1694                                 /* Don't process properties of nested nodes */
1695                                 break;
1696
1697                         if (fdt_prop(o) != 0) {
1698                                 sprintf(command_errbuf, "could not process "
1699                                     "property");
1700                                 rv = CMD_ERROR;
1701                                 goto out;
1702                         }
1703                         break;
1704                 case FDT_BEGIN_NODE:
1705                         depth++;
1706                         if (depth > FDT_MAX_DEPTH) {
1707                                 printf("warning: nesting too deep: %d\n",
1708                                     depth);
1709                                 goto out;
1710                         }
1711                         break;
1712                 case FDT_END_NODE:
1713                         depth--;
1714                         if (depth == 0)
1715                                 /*
1716                                  * This is the end of our starting node, force
1717                                  * the loop finish.
1718                                  */
1719                                 depth--;
1720                         break;
1721                 }
1722                 o = next;
1723         }
1724 out:
1725         return (rv);
1726 }
1727
1728 static int
1729 fdt_cmd_mkprop(int argc, char *argv[])
1730 {
1731         int o;
1732         char *path, *propname, *value;
1733
1734         path = (argc > 2) ? argv[2] : NULL;
1735
1736         value = NULL;
1737
1738         if (argc > 3) {
1739                 /* Merge property value strings into one */
1740                 if (fdt_merge_strings(argc, argv, 3, &value) != 0)
1741                         return (CMD_ERROR);
1742         } else
1743                 value = NULL;
1744
1745         if (fdt_extract_nameloc(&path, &propname, &o) != 0)
1746                 return (CMD_ERROR);
1747
1748         if (fdt_modprop(o, propname, value, 1))
1749                 return (CMD_ERROR);
1750
1751         return (CMD_OK);
1752 }
1753
1754 static int
1755 fdt_cmd_rm(int argc, char *argv[])
1756 {
1757         int o, rv;
1758         char *path = NULL, *propname;
1759
1760         if (argc > 2)
1761                 path = argv[2];
1762         else {
1763                 sprintf(command_errbuf, "no node/property name specified");
1764                 return (CMD_ERROR);
1765         }
1766
1767         o = fdt_path_offset(fdtp, path);
1768         if (o < 0) {
1769                 /* If node not found -- try to find & delete property */
1770                 if (fdt_extract_nameloc(&path, &propname, &o) != 0)
1771                         return (CMD_ERROR);
1772
1773                 if ((rv = fdt_delprop(fdtp, o, propname)) != 0) {
1774                         snprintf(command_errbuf, sizeof(command_errbuf),
1775                             "could not delete %s\n",
1776                             (rv == -FDT_ERR_NOTFOUND) ?
1777                             "(property/node does not exist)" : "");
1778                         return (CMD_ERROR);
1779
1780                 } else
1781                         return (CMD_OK);
1782         }
1783         /* If node exists -- remove node */
1784         rv = fdt_del_node(fdtp, o);
1785         if (rv) {
1786                 sprintf(command_errbuf, "could not delete node");
1787                 return (CMD_ERROR);
1788         }
1789         return (CMD_OK);
1790 }
1791
1792 static int
1793 fdt_cmd_mknode(int argc, char *argv[])
1794 {
1795         int o, rv;
1796         char *path = NULL, *nodename = NULL;
1797
1798         if (argc > 2)
1799                 path = argv[2];
1800         else {
1801                 sprintf(command_errbuf, "no node name specified");
1802                 return (CMD_ERROR);
1803         }
1804
1805         if (fdt_extract_nameloc(&path, &nodename, &o) != 0)
1806                 return (CMD_ERROR);
1807
1808         rv = fdt_add_subnode(fdtp, o, nodename);
1809
1810         if (rv < 0) {
1811                 if (rv == -FDT_ERR_NOSPACE)
1812                         sprintf(command_errbuf,
1813                             "Device tree blob is too small!\n");
1814                 else
1815                         sprintf(command_errbuf,
1816                             "Could not add node!\n");
1817                 return (CMD_ERROR);
1818         }
1819         return (CMD_OK);
1820 }
1821
1822 static int
1823 fdt_cmd_pwd(int argc, char *argv[])
1824 {
1825         char line[FDT_CWD_LEN];
1826
1827         pager_open();
1828         sprintf(line, "%s\n", cwd);
1829         pager_output(line);
1830         pager_close();
1831         return (CMD_OK);
1832 }
1833
1834 static int
1835 fdt_cmd_mres(int argc, char *argv[])
1836 {
1837         uint64_t start, size;
1838         int i, total;
1839         char line[80];
1840
1841         pager_open();
1842         total = fdt_num_mem_rsv(fdtp);
1843         if (total > 0) {
1844                 if (pager_output("Reserved memory regions:\n"))
1845                         goto out;
1846                 for (i = 0; i < total; i++) {
1847                         fdt_get_mem_rsv(fdtp, i, &start, &size);
1848                         sprintf(line, "reg#%d: (start: 0x%jx, size: 0x%jx)\n",
1849                             i, start, size);
1850                         if (pager_output(line))
1851                                 goto out;
1852                 }
1853         } else
1854                 pager_output("No reserved memory regions\n");
1855 out:
1856         pager_close();
1857
1858         return (CMD_OK);
1859 }
1860
1861 static int
1862 fdt_cmd_nyi(int argc, char *argv[])
1863 {
1864
1865         printf("command not yet implemented\n");
1866         return (CMD_ERROR);
1867 }
1868
1869 const char *
1870 fdt_devmatch_next(int *tag, int *compatlen)
1871 {
1872         const struct fdt_property *p;
1873         const struct fdt_property *status;
1874         int o, len = -1;
1875         static int depth = 0;
1876
1877         if (fdtp == NULL) {
1878                 fdt_setup_fdtp();
1879                 fdt_apply_overlays();
1880         }
1881
1882         if (*tag != 0) {
1883                 o = *tag;
1884                 /* We are at the end of the DTB */
1885                 if (o < 0)
1886                         return (NULL);
1887         } else {
1888                 o = fdt_path_offset(fdtp, "/");
1889                 if (o < 0) {
1890                         printf("Can't find dtb\n");
1891                         return (NULL);
1892                 }
1893                 depth = 0;
1894         }
1895
1896         /* Find the next node with a compatible property */
1897         while (1) {
1898                 p = NULL;
1899                 if (o >= 0 && depth >= 0) {
1900                         /* skip disabled nodes */
1901                         status = fdt_get_property(fdtp, o, "status", &len);
1902                         if (len > 0) {
1903                                 if (strcmp(status->data, "disabled") == 0) {
1904                                         o = fdt_next_node(fdtp, o, &depth);
1905                                         if (o < 0) /* End of tree */
1906                                                 return (NULL);
1907                                         continue;
1908                                 }
1909                         }
1910
1911                         p = fdt_get_property(fdtp, o, "compatible", &len);
1912                 }
1913                 if (p)
1914                         break;
1915                 o = fdt_next_node(fdtp, o, &depth);
1916                 if (o < 0) /* End of tree */
1917                         return (NULL);
1918         }
1919
1920         /* Prepare next node for next call */
1921         o = fdt_next_node(fdtp, o, &depth);
1922         *tag = o;
1923
1924         if (len >= 0) {
1925                 *compatlen = len;
1926                 return (p->data);
1927         }
1928         return (NULL);
1929 }