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