stand/fdt/fdt_loader_cmd.c

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