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