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