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