sys/boot/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 <fdt.h>
  35 #include <libfdt.h>
  36 #include <sys/param.h>
  37 #include <sys/linker.h>
  38 #include <machine/elf.h>
  39
  40 #include "bootstrap.h"
  41 #include "glue.h"
  42
  43 #define DEBUG
  44
  45 #ifdef DEBUG
  46 #define debugf(fmt, args...) do { printf("%s(): ", __func__);   \
  47     printf(fmt,##args); } while (0)
  48 #else
  49 #define debugf(fmt, args...)
  50 #endif
  51
  52 #define FDT_CWD_LEN     256
  53 #define FDT_MAX_DEPTH   6
  54
  55 #define FDT_PROP_SEP    " = "
  56
  57 #define STR(number) #number
  58 #define STRINGIFY(number) STR(number)
  59
  60 #define COPYOUT(s,d,l)  archsw.arch_copyout((vm_offset_t)(s), d, l)
  61
  62 #define FDT_STATIC_DTB_SYMBOL   "fdt_static_dtb"
  63
  64 static struct fdt_header *fdtp = NULL;
  65
  66 static int fdt_cmd_nyi(int argc, char *argv[]);
  67
  68 static int fdt_cmd_mkprop(int argc, char *argv[]);
  69 static int fdt_cmd_cd(int argc, char *argv[]);
  70 static int fdt_cmd_hdr(int argc, char *argv[]);
  71 static int fdt_cmd_ls(int argc, char *argv[]);
  72 static int fdt_cmd_prop(int argc, char *argv[]);
  73 static int fdt_cmd_pwd(int argc, char *argv[]);
  74 static int fdt_cmd_rm(int argc, char *argv[]);
  75 static int fdt_cmd_mknode(int argc, char *argv[]);
  76
  77 typedef int cmdf_t(int, char *[]);
  78
  79 struct cmdtab {
  80         char    *name;
  81         cmdf_t  *handler;
  82 };
  83
  84 static const struct cmdtab commands[] = {
  85         { "alias", &fdt_cmd_nyi },
  86         { "cd", &fdt_cmd_cd },
  87         { "header", &fdt_cmd_hdr },
  88         { "ls", &fdt_cmd_ls },
  89         { "mknode", &fdt_cmd_mknode },
  90         { "mkprop", &fdt_cmd_mkprop },
  91         { "mres", &fdt_cmd_nyi },
  92         { "prop", &fdt_cmd_prop },
  93         { "pwd", &fdt_cmd_pwd },
  94         { "rm", &fdt_cmd_rm },
  95         { NULL, NULL }
  96 };
  97
  98 static char cwd[FDT_CWD_LEN] = "/";
  99
 100 static vm_offset_t
 101 fdt_find_static_dtb(void)
 102 {
 103         Elf_Sym sym;
 104         vm_offset_t dyntab, esym;
 105         uint64_t offs;
 106         struct preloaded_file *kfp;
 107         struct file_metadata *md;
 108         Elf_Sym *symtab;
 109         Elf_Dyn *dyn;
 110         char *strtab, *strp;
 111         int i, sym_count;
 112
 113         symtab = NULL;
 114         dyntab = esym = 0;
 115         strtab = strp = NULL;
 116
 117         offs = __elfN(relocation_offset);
 118
 119         kfp = file_findfile(NULL, NULL);
 120         if (kfp == NULL)
 121                 return (0);
 122
 123         md = file_findmetadata(kfp, MODINFOMD_ESYM);
 124         if (md == NULL)
 125                 return (0);
 126         COPYOUT(md->md_data, &esym, sizeof(esym));
 127
 128         md = file_findmetadata(kfp, MODINFOMD_DYNAMIC);
 129         if (md == NULL)
 130                 return (0);
 131         COPYOUT(md->md_data, &dyntab, sizeof(dyntab));
 132
 133         dyntab += offs;
 134
 135         /* Locate STRTAB and DYNTAB */
 136         for (dyn = (Elf_Dyn *)dyntab; dyn->d_tag != DT_NULL; dyn++) {
 137                 if (dyn->d_tag == DT_STRTAB) {
 138                         strtab = (char *)(uintptr_t)(dyn->d_un.d_ptr + offs);
 139                         continue;
 140                 } else if (dyn->d_tag == DT_SYMTAB) {
 141                         symtab = (Elf_Sym *)(uintptr_t)
 142                             (dyn->d_un.d_ptr + offs);
 143                         continue;
 144                 }
 145         }
 146
 147         if (symtab == NULL || strtab == NULL) {
 148                 /*
 149                  * No symtab? No strtab? That should not happen here,
 150                  * and should have been verified during __elfN(loadimage).
 151                  * This must be some kind of a bug.
 152                  */
 153                 return (0);
 154         }
 155
 156         sym_count = (int)((Elf_Sym *)esym - symtab) / sizeof(Elf_Sym);
 157
 158         /*
 159          * The most efficent 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         for (i = 0; i < sym_count; i++) {
 169                 COPYOUT(symtab + i, &sym, sizeof(sym));
 170                 if (ELF_ST_BIND(sym.st_info) != STB_GLOBAL ||
 171                     ELF_ST_TYPE(sym.st_info) != STT_NOTYPE)
 172                         continue;
 173
 174                 strp = strdupout((vm_offset_t)(strtab + sym.st_name));
 175                 if (strcmp(strp, FDT_STATIC_DTB_SYMBOL) == 0) {
 176                         /* Found a match ! */
 177                         free(strp);
 178                         return ((vm_offset_t)(sym.st_value + offs));
 179                 }
 180                 free(strp);
 181         }
 182         return (0);
 183 }
 184
 185 static int
 186 fdt_setup_fdtp()
 187 {
 188         struct preloaded_file *bfp;
 189         int err;
 190
 191         /*
 192          * Find the device tree blob.
 193          */
 194         bfp = file_findfile(NULL, "dtb");
 195         if (bfp == NULL) {
 196                 if ((fdtp = (struct fdt_header *)fdt_find_static_dtb()) == 0) {
 197                         command_errmsg = "no device tree blob found!";
 198                         return (CMD_ERROR);
 199                 }
 200         } else {
 201                 /* Dynamic blob has precedence over static. */
 202                 fdtp = (struct fdt_header *)bfp->f_addr;
 203         }
 204
 205         /*
 206          * Validate the blob.
 207          */
 208         err = fdt_check_header(fdtp);
 209         if (err < 0) {
 210                 if (err == -FDT_ERR_BADVERSION)
 211                         sprintf(command_errbuf,
 212                             "incompatible blob version: %d, should be: %d",
 213                             fdt_version(fdtp), FDT_LAST_SUPPORTED_VERSION);
 214
 215                 else
 216                         sprintf(command_errbuf, "error validating blob: %s",
 217                             fdt_strerror(err));
 218                 return (CMD_ERROR);
 219         }
 220         return (CMD_OK);
 221 }
 222
 223 #define fdt_strtovect(str, cellbuf, lim, cellsize) _fdt_strtovect((str), \
 224     (cellbuf), (lim), (cellsize), 0);
 225
 226 /* Force using base 16 */
 227 #define fdt_strtovectx(str, cellbuf, lim, cellsize) _fdt_strtovect((str), \
 228     (cellbuf), (lim), (cellsize), 16);
 229
 230 static int
 231 _fdt_strtovect(char *str, void *cellbuf, int lim, unsigned char cellsize,
 232     uint8_t base)
 233 {
 234         char *buf = str;
 235         char *end = str + strlen(str) - 2;
 236         uint32_t *u32buf = NULL;
 237         uint8_t *u8buf = NULL;
 238         int cnt = 0;
 239
 240         if (cellsize == sizeof(uint32_t))
 241                 u32buf = (uint32_t *)cellbuf;
 242         else
 243                 u8buf = (uint8_t *)cellbuf;
 244
 245         if (lim == 0)
 246                 return (0);
 247
 248         while (buf < end) {
 249
 250                 /* Skip white whitespace(s)/separators */
 251                 while (!isxdigit(*buf) && buf < end)
 252                         buf++;
 253
 254                 if (u32buf != NULL)
 255                         u32buf[cnt] =
 256                             cpu_to_fdt32((uint32_t)strtol(buf, NULL, base));
 257
 258                 else
 259                         u8buf[cnt] = (uint8_t)strtol(buf, NULL, base);
 260
 261                 if (cnt + 1 <= lim - 1)
 262                         cnt++;
 263                 else
 264                         break;
 265                 buf++;
 266                 /* Find another number */
 267                 while ((isxdigit(*buf) || *buf == 'x') && buf < end)
 268                         buf++;
 269         }
 270         return (cnt);
 271 }
 272
 273 #define TMP_MAX_ETH     8
 274
 275 void
 276 fixup_ethernet(const char *env, char *ethstr, int *eth_no, int len)
 277 {
 278         char *end, *str;
 279         uint8_t tmp_addr[6];
 280         int i, n;
 281
 282         /* Extract interface number */
 283         i = strtol(env + 3, &end, 10);
 284         if (end == (env + 3))
 285                 /* 'ethaddr' means interface 0 address */
 286                 n = 0;
 287         else
 288                 n = i;
 289
 290         if (n > TMP_MAX_ETH)
 291                 return;
 292
 293         str = ub_env_get(env);
 294
 295         /* Convert macaddr string into a vector of uints */
 296         fdt_strtovectx(str, &tmp_addr, 6, sizeof(uint8_t));
 297         if (n != 0) {
 298                 i = strlen(env) - 7;
 299                 strncpy(ethstr + 8, env + 3, i);
 300         }
 301         /* Set actual property to a value from vect */
 302         fdt_setprop(fdtp, fdt_path_offset(fdtp, ethstr),
 303             "local-mac-address", &tmp_addr, 6 * sizeof(uint8_t));
 304
 305         /* Clear ethernet..XXXX.. string */
 306         bzero(ethstr + 8, len - 8);
 307
 308         if (n + 1 > *eth_no)
 309                 *eth_no = n + 1;
 310 }
 311
 312 void
 313 fixup_cpubusfreqs(unsigned long cpufreq, unsigned long busfreq)
 314 {
 315         int lo, o = 0, o2, maxo = 0, depth;
 316         const uint32_t zero = 0;
 317
 318         /* We want to modify every subnode of /cpus */
 319         o = fdt_path_offset(fdtp, "/cpus");
 320
 321         /* maxo should contain offset of node next to /cpus */
 322         depth = 0;
 323         maxo = o;
 324         while (depth != -1)
 325                 maxo = fdt_next_node(fdtp, maxo, &depth);
 326
 327         /* Find CPU frequency properties */
 328         o = fdt_node_offset_by_prop_value(fdtp, o, "clock-frequency",
 329             &zero, sizeof(uint32_t));
 330
 331         o2 = fdt_node_offset_by_prop_value(fdtp, o, "bus-frequency", &zero,
 332             sizeof(uint32_t));
 333
 334         lo = MIN(o, o2);
 335
 336         while (o != -FDT_ERR_NOTFOUND && o2 != -FDT_ERR_NOTFOUND) {
 337
 338                 o = fdt_node_offset_by_prop_value(fdtp, lo,
 339                     "clock-frequency", &zero, sizeof(uint32_t));
 340
 341                 o2 = fdt_node_offset_by_prop_value(fdtp, lo, "bus-frequency",
 342                     &zero, sizeof(uint32_t));
 343
 344                 /* We're only interested in /cpus subnode(s) */
 345                 if (lo > maxo)
 346                         break;
 347
 348                 fdt_setprop_inplace_cell(fdtp, lo, "clock-frequency",
 349                     (uint32_t)cpufreq);
 350
 351                 fdt_setprop_inplace_cell(fdtp, lo, "bus-frequency",
 352                     (uint32_t)busfreq);
 353
 354                 lo = MIN(o, o2);
 355         }
 356 }
 357
 358 int
 359 fdt_reg_valid(uint32_t *reg, int len, int addr_cells, int size_cells)
 360 {
 361         int cells_in_tuple, i, tuples, tuple_size;
 362         uint32_t cur_start, cur_size;
 363
 364         cells_in_tuple = (addr_cells + size_cells);
 365         tuple_size = cells_in_tuple * sizeof(uint32_t);
 366         tuples = len / tuple_size;
 367         if (tuples == 0)
 368                 return (EINVAL);
 369
 370         for (i = 0; i < tuples; i++) {
 371                 if (addr_cells == 2)
 372                         cur_start = fdt64_to_cpu(reg[i * cells_in_tuple]);
 373                 else
 374                         cur_start = fdt32_to_cpu(reg[i * cells_in_tuple]);
 375
 376                 if (size_cells == 2)
 377                         cur_size = fdt64_to_cpu(reg[i * cells_in_tuple + 2]);
 378                 else
 379                         cur_size = fdt32_to_cpu(reg[i * cells_in_tuple + 1]);
 380
 381                 if (cur_size == 0)
 382                         return (EINVAL);
 383
 384                 debugf(" reg#%d (start: 0x%0x size: 0x%0x) valid!\n",
 385                     i, cur_start, cur_size);
 386         }
 387         return (0);
 388 }
 389
 390 void
 391 fixup_memory(struct sys_info *si)
 392 {
 393         struct mem_region *curmr;
 394         uint32_t addr_cells, size_cells;
 395         uint32_t *addr_cellsp, *reg,  *size_cellsp;
 396         int err, i, len, memory, realmrno, root;
 397         uint8_t *buf, *sb;
 398
 399         root = fdt_path_offset(fdtp, "/");
 400         if (root < 0) {
 401                 sprintf(command_errbuf, "Could not find root node !");
 402                 return;
 403         }
 404
 405         memory = fdt_path_offset(fdtp, "/memory");
 406         if (memory <= 0) {
 407                 /* Create proper '/memory' node. */
 408                 memory = fdt_add_subnode(fdtp, root, "memory");
 409                 if (memory <= 0) {
 410                         sprintf(command_errbuf, "Could not fixup '/memory' "
 411                             "node, error code : %d!\n", memory);
 412                         return;
 413                 }
 414
 415                 err = fdt_setprop(fdtp, memory, "device_type", "memory",
 416                     sizeof("memory"));
 417
 418                 if (err < 0)
 419                         return;
 420         }
 421
 422         addr_cellsp = (uint32_t *)fdt_getprop(fdtp, root, "#address-cells",
 423             NULL);
 424         size_cellsp = (uint32_t *)fdt_getprop(fdtp, root, "#size-cells", NULL);
 425
 426         if (addr_cellsp == NULL || size_cellsp == NULL) {
 427                 sprintf(command_errbuf, "Could not fixup '/memory' node : "
 428                     "%s %s property not found in root node!\n",
 429                     (!addr_cellsp) ? "#address-cells" : "",
 430                     (!size_cellsp) ? "#size-cells" : "");
 431                 return;
 432         }
 433
 434         addr_cells = fdt32_to_cpu(*addr_cellsp);
 435         size_cells = fdt32_to_cpu(*size_cellsp);
 436
 437         /* Count valid memory regions entries in sysinfo. */
 438         realmrno = si->mr_no;
 439         for (i = 0; i < si->mr_no; i++)
 440                 if (si->mr[i].start == 0 && si->mr[i].size == 0)
 441                         realmrno--;
 442
 443         if (realmrno == 0) {
 444                 sprintf(command_errbuf, "Could not fixup '/memory' node : "
 445                     "sysinfo doesn't contain valid memory regions info!\n");
 446                 return;
 447         }
 448
 449         if ((reg = (uint32_t *)fdt_getprop(fdtp, memory, "reg",
 450             &len)) != NULL) {
 451
 452                 if (fdt_reg_valid(reg, len, addr_cells, size_cells) == 0)
 453                         /*
 454                          * Do not apply fixup if existing 'reg' property
 455                          * seems to be valid.
 456                          */
 457                         return;
 458         }
 459
 460         len = (addr_cells + size_cells) * realmrno * sizeof(uint32_t);
 461         sb = buf = (uint8_t *)malloc(len);
 462         if (!buf)
 463                 return;
 464
 465         bzero(buf, len);
 466
 467         for (i = 0; i < si->mr_no; i++) {
 468                 curmr = &si->mr[i];
 469                 if (curmr->size != 0) {
 470                         /* Ensure endianess, and put cells into a buffer */
 471                         if (addr_cells == 2)
 472                                 *(uint64_t *)buf =
 473                                     cpu_to_fdt64(curmr->start);
 474                         else
 475                                 *(uint32_t *)buf =
 476                                     cpu_to_fdt32(curmr->start);
 477
 478                         buf += sizeof(uint32_t) * addr_cells;
 479                         if (size_cells == 2)
 480                                 *(uint64_t *)buf =
 481                                     cpu_to_fdt64(curmr->size);
 482                         else
 483                                 *(uint32_t *)buf =
 484                                     cpu_to_fdt32(curmr->size);
 485
 486                         buf += sizeof(uint32_t) * size_cells;
 487                 }
 488         }
 489
 490         /* Set property */
 491         if ((err = fdt_setprop(fdtp, memory, "reg", sb, len)) < 0)
 492                 sprintf(command_errbuf, "Could not fixup '/memory' node.\n");
 493 }
 494
 495 void
 496 fixup_stdout(const char *env)
 497 {
 498         const char *str;
 499         char *ptr;
 500         int serialno;
 501         int len, no, sero;
 502         const struct fdt_property *prop;
 503         char *tmp[10];
 504
 505         str = ub_env_get(env);
 506         ptr = (char *)str + strlen(str) - 1;
 507         while (ptr > str && isdigit(*(str - 1)))
 508                 str--;
 509
 510         if (ptr == str)
 511                 return;
 512
 513         serialno = (int)strtol(ptr, NULL, 0);
 514         no = fdt_path_offset(fdtp, "/chosen");
 515         if (no < 0)
 516                 return;
 517
 518         prop = fdt_get_property(fdtp, no, "stdout", &len);
 519
 520         /* If /chosen/stdout does not extist, create it */
 521         if (prop == NULL || (prop != NULL && len == 0)) {
 522
 523                 bzero(tmp, 10 * sizeof(char));
 524                 strcpy((char *)&tmp, "serial");
 525                 if (strlen(ptr) > 3)
 526                         /* Serial number too long */
 527                         return;
 528
 529                 strncpy((char *)tmp + 6, ptr, 3);
 530                 sero = fdt_path_offset(fdtp, (const char *)tmp);
 531                 if (sero < 0)
 532                         /*
 533                          * If serial device we're trying to assign
 534                          * stdout to doesn't exist in DT -- return.
 535                          */
 536                         return;
 537
 538                 fdt_setprop(fdtp, no, "stdout", &tmp,
 539                     strlen((char *)&tmp) + 1);
 540                 fdt_setprop(fdtp, no, "stdin", &tmp,
 541                     strlen((char *)&tmp) + 1);
 542         }
 543 }
 544
 545 /*
 546  * Locate the blob, fix it up and return its location.
 547  */
 548 void *
 549 fdt_fixup(void)
 550 {
 551         const char *env;
 552         char *ethstr;
 553         int chosen, err, eth_no, len;
 554         struct sys_info *si;
 555
 556         env = NULL;
 557         eth_no = 0;
 558         ethstr = NULL;
 559         len = 0;
 560
 561         err = fdt_setup_fdtp();
 562         if (err) {
 563                 sprintf(command_errbuf, "No valid device tree blob found!");
 564                 return (NULL);
 565         }
 566
 567         /* Create /chosen node (if not exists) */
 568         if ((chosen = fdt_subnode_offset(fdtp, 0, "chosen")) ==
 569             -FDT_ERR_NOTFOUND)
 570                 chosen = fdt_add_subnode(fdtp, 0, "chosen");
 571
 572         /* Value assigned to fixup-applied does not matter. */
 573         if (fdt_getprop(fdtp, chosen, "fixup-applied", NULL))
 574                 goto success;
 575
 576         /* Acquire sys_info */
 577         si = ub_get_sys_info();
 578
 579         while ((env = ub_env_enum(env)) != NULL) {
 580                 if (strncmp(env, "eth", 3) == 0 &&
 581                     strncmp(env + (strlen(env) - 4), "addr", 4) == 0) {
 582                         /*
 583                          * Handle Ethernet addrs: parse uboot env eth%daddr
 584                          */
 585
 586                         if (!eth_no) {
 587                                 /*
 588                                  * Check how many chars we will need to store
 589                                  * maximal eth iface number.
 590                                  */
 591                                 len = strlen(STRINGIFY(TMP_MAX_ETH)) +
 592                                     strlen("ethernet");
 593
 594                                 /*
 595                                  * Reserve mem for string "ethernet" and len
 596                                  * chars for iface no.
 597                                  */
 598                                 ethstr = (char *)malloc(len * sizeof(char));
 599                                 bzero(ethstr, len * sizeof(char));
 600                                 strcpy(ethstr, "ethernet0");
 601                         }
 602
 603                         /* Modify blob */
 604                         fixup_ethernet(env, ethstr, &eth_no, len);
 605
 606                 } else if (strcmp(env, "consoledev") == 0)
 607                         fixup_stdout(env);
 608         }
 609
 610         /* Modify cpu(s) and bus clock frequenties in /cpus node [Hz] */
 611         fixup_cpubusfreqs(si->clk_cpu, si->clk_bus);
 612
 613         /* Fixup memory regions */
 614         fixup_memory(si);
 615
 616         fdt_setprop(fdtp, chosen, "fixup-applied", NULL, 0);
 617
 618 success:
 619         return (fdtp);
 620 }
 621
 622 int
 623 command_fdt_internal(int argc, char *argv[])
 624 {
 625         cmdf_t *cmdh;
 626         char *cmd;
 627         int i, err;
 628
 629         if (argc < 2) {
 630                 command_errmsg = "usage is 'fdt <command> [<args>]";
 631                 return (CMD_ERROR);
 632         }
 633
 634         /*
 635          * Check if uboot env vars were parsed already. If not, do it now.
 636          */
 637         if (fdt_fixup() == NULL)
 638                 return (CMD_ERROR);
 639
 640         /*
 641          * Validate fdt <command>.
 642          */
 643         cmd = strdup(argv[1]);
 644         i = 0;
 645         cmdh = NULL;
 646         while (!(commands[i].name == NULL)) {
 647                 if (strcmp(cmd, commands[i].name) == 0) {
 648                         /* found it */
 649                         cmdh = commands[i].handler;
 650                         break;
 651                 }
 652                 i++;
 653         }
 654         if (cmdh == NULL) {
 655                 command_errmsg = "unknown command";
 656                 return (CMD_ERROR);
 657         }
 658
 659         /*
 660          * Call command handler.
 661          */
 662         err = (*cmdh)(argc, argv);
 663
 664         return (err);
 665 }
 666
 667 static int
 668 fdt_cmd_cd(int argc, char *argv[])
 669 {
 670         char *path;
 671         char tmp[FDT_CWD_LEN];
 672         int len, o;
 673
 674         path = (argc > 2) ? argv[2] : "/";
 675
 676         if (path[0] == '/') {
 677                 len = strlen(path);
 678                 if (len >= FDT_CWD_LEN)
 679                         goto fail;
 680         } else {
 681                 /* Handle path specification relative to cwd */
 682                 len = strlen(cwd) + strlen(path) + 1;
 683                 if (len >= FDT_CWD_LEN)
 684                         goto fail;
 685
 686                 strcpy(tmp, cwd);
 687                 strcat(tmp, "/");
 688                 strcat(tmp, path);
 689                 path = tmp;
 690         }
 691
 692         o = fdt_path_offset(fdtp, path);
 693         if (o < 0) {
 694                 sprintf(command_errbuf, "could not find node: '%s'", path);
 695                 return (CMD_ERROR);
 696         }
 697
 698         strcpy(cwd, path);
 699         return (CMD_OK);
 700
 701 fail:
 702         sprintf(command_errbuf, "path too long: %d, max allowed: %d",
 703             len, FDT_CWD_LEN - 1);
 704         return (CMD_ERROR);
 705 }
 706
 707 static int
 708 fdt_cmd_hdr(int argc __unused, char *argv[] __unused)
 709 {
 710         char line[80];
 711         int ver;
 712
 713         if (fdtp == NULL) {
 714                 command_errmsg = "no device tree blob pointer?!";
 715                 return (CMD_ERROR);
 716         }
 717
 718         ver = fdt_version(fdtp);
 719         pager_open();
 720         sprintf(line, "\nFlattened device tree header (%p):\n", fdtp);
 721         pager_output(line);
 722         sprintf(line, " magic                   = 0x%08x\n", fdt_magic(fdtp));
 723         pager_output(line);
 724         sprintf(line, " size                    = %d\n", fdt_totalsize(fdtp));
 725         pager_output(line);
 726         sprintf(line, " off_dt_struct           = 0x%08x\n",
 727             fdt_off_dt_struct(fdtp));
 728         pager_output(line);
 729         sprintf(line, " off_dt_strings          = 0x%08x\n",
 730             fdt_off_dt_strings(fdtp));
 731         pager_output(line);
 732         sprintf(line, " off_mem_rsvmap          = 0x%08x\n",
 733             fdt_off_mem_rsvmap(fdtp));
 734         pager_output(line);
 735         sprintf(line, " version                 = %d\n", ver);
 736         pager_output(line);
 737         sprintf(line, " last compatible version = %d\n",
 738             fdt_last_comp_version(fdtp));
 739         pager_output(line);
 740         if (ver >= 2) {
 741                 sprintf(line, " boot_cpuid              = %d\n",
 742                     fdt_boot_cpuid_phys(fdtp));
 743                 pager_output(line);
 744         }
 745         if (ver >= 3) {
 746                 sprintf(line, " size_dt_strings         = %d\n",
 747                     fdt_size_dt_strings(fdtp));
 748                 pager_output(line);
 749         }
 750         if (ver >= 17) {
 751                 sprintf(line, " size_dt_struct          = %d\n",
 752                     fdt_size_dt_struct(fdtp));
 753                 pager_output(line);
 754         }
 755         pager_close();
 756
 757         return (CMD_OK);
 758 }
 759
 760 static int
 761 fdt_cmd_ls(int argc, char *argv[])
 762 {
 763         const char *prevname[FDT_MAX_DEPTH] = { NULL };
 764         const char *name;
 765         char *path;
 766         int i, o, depth, len;
 767
 768         path = (argc > 2) ? argv[2] : NULL;
 769         if (path == NULL)
 770                 path = cwd;
 771
 772         o = fdt_path_offset(fdtp, path);
 773         if (o < 0) {
 774                 sprintf(command_errbuf, "could not find node: '%s'", path);
 775                 return (CMD_ERROR);
 776         }
 777
 778         for (depth = 0;
 779             (o >= 0) && (depth >= 0);
 780             o = fdt_next_node(fdtp, o, &depth)) {
 781
 782                 name = fdt_get_name(fdtp, o, &len);
 783
 784                 if (depth > FDT_MAX_DEPTH) {
 785                         printf("max depth exceeded: %d\n", depth);
 786                         continue;
 787                 }
 788
 789                 prevname[depth] = name;
 790
 791                 /* Skip root (i = 1) when printing devices */
 792                 for (i = 1; i <= depth; i++) {
 793                         if (prevname[i] == NULL)
 794                                 break;
 795
 796                         if (strcmp(cwd, "/") == 0)
 797                                 printf("/");
 798                         printf("%s", prevname[i]);
 799                 }
 800                 printf("\n");
 801         }
 802
 803         return (CMD_OK);
 804 }
 805
 806 static __inline int
 807 isprint(int c)
 808 {
 809
 810         return (c >= ' ' && c <= 0x7e);
 811 }
 812
 813 static int
 814 fdt_isprint(const void *data, int len, int *count)
 815 {
 816         const char *d;
 817         char ch;
 818         int yesno, i;
 819
 820         if (len == 0)
 821                 return (0);
 822
 823         d = (const char *)data;
 824         if (d[len - 1] != '\0')
 825                 return (0);
 826
 827         *count = 0;
 828         yesno = 1;
 829         for (i = 0; i < len; i++) {
 830                 ch = *(d + i);
 831                 if (isprint(ch) || (ch == '\0' && i > 0)) {
 832                         /* Count strings */
 833                         if (ch == '\0')
 834                                 (*count)++;
 835                         continue;
 836                 }
 837
 838                 yesno = 0;
 839                 break;
 840         }
 841
 842         return (yesno);
 843 }
 844
 845 static int
 846 fdt_data_str(const void *data, int len, int count, char **buf)
 847 {
 848         char *b, *tmp;
 849         const char *d;
 850         int buf_len, i, l;
 851
 852         /*
 853          * Calculate the length for the string and allocate memory.
 854          *
 855          * Note that 'len' already includes at least one terminator.
 856          */
 857         buf_len = len;
 858         if (count > 1) {
 859                 /*
 860                  * Each token had already a terminator buried in 'len', but we
 861                  * only need one eventually, don't count space for these.
 862                  */
 863                 buf_len -= count - 1;
 864
 865                 /* Each consecutive token requires a ", " separator. */
 866                 buf_len += count * 2;
 867         }
 868
 869         /* Add some space for surrounding double quotes. */
 870         buf_len += count * 2;
 871
 872         /* Note that string being put in 'tmp' may be as big as 'buf_len'. */
 873         b = (char *)malloc(buf_len);
 874         tmp = (char *)malloc(buf_len);
 875         if (b == NULL)
 876                 goto error;
 877
 878         if (tmp == NULL) {
 879                 free(b);
 880                 goto error;
 881         }
 882
 883         b[0] = '\0';
 884
 885         /*
 886          * Now that we have space, format the string.
 887          */
 888         i = 0;
 889         do {
 890                 d = (const char *)data + i;
 891                 l = strlen(d) + 1;
 892
 893                 sprintf(tmp, "\"%s\"%s", d,
 894                     (i + l) < len ?  ", " : "");
 895                 strcat(b, tmp);
 896
 897                 i += l;
 898
 899         } while (i < len);
 900         *buf = b;
 901
 902         free(tmp);
 903
 904         return (0);
 905 error:
 906         return (1);
 907 }
 908
 909 static int
 910 fdt_data_cell(const void *data, int len, char **buf)
 911 {
 912         char *b, *tmp;
 913         const uint32_t *c;
 914         int count, i, l;
 915
 916         /* Number of cells */
 917         count = len / 4;
 918
 919         /*
 920          * Calculate the length for the string and allocate memory.
 921          */
 922
 923         /* Each byte translates to 2 output characters */
 924         l = len * 2;
 925         if (count > 1) {
 926                 /* Each consecutive cell requires a " " separator. */
 927                 l += (count - 1) * 1;
 928         }
 929         /* Each cell will have a "0x" prefix */
 930         l += count * 2;
 931         /* Space for surrounding <> and terminator */
 932         l += 3;
 933
 934         b = (char *)malloc(l);
 935         tmp = (char *)malloc(l);
 936         if (b == NULL)
 937                 goto error;
 938
 939         if (tmp == NULL) {
 940                 free(b);
 941                 goto error;
 942         }
 943
 944         b[0] = '\0';
 945         strcat(b, "<");
 946
 947         for (i = 0; i < len; i += 4) {
 948                 c = (const uint32_t *)((const uint8_t *)data + i);
 949                 sprintf(tmp, "0x%08x%s", fdt32_to_cpu(*c),
 950                     i < (len - 4) ? " " : "");
 951                 strcat(b, tmp);
 952         }
 953         strcat(b, ">");
 954         *buf = b;
 955
 956         free(tmp);
 957
 958         return (0);
 959 error:
 960         return (1);
 961 }
 962
 963 static int
 964 fdt_data_bytes(const void *data, int len, char **buf)
 965 {
 966         char *b, *tmp;
 967         const char *d;
 968         int i, l;
 969
 970         /*
 971          * Calculate the length for the string and allocate memory.
 972          */
 973
 974         /* Each byte translates to 2 output characters */
 975         l = len * 2;
 976         if (len > 1)
 977                 /* Each consecutive byte requires a " " separator. */
 978                 l += (len - 1) * 1;
 979         /* Each byte will have a "0x" prefix */
 980         l += len * 2;
 981         /* Space for surrounding [] and terminator. */
 982         l += 3;
 983
 984         b = (char *)malloc(l);
 985         tmp = (char *)malloc(l);
 986         if (b == NULL)
 987                 goto error;
 988
 989         if (tmp == NULL) {
 990                 free(b);
 991                 goto error;
 992         }
 993
 994         b[0] = '\0';
 995         strcat(b, "[");
 996
 997         for (i = 0, d = data; i < len; i++) {
 998                 sprintf(tmp, "0x%02x%s", d[i], i < len - 1 ? " " : "");
 999                 strcat(b, tmp);
1000         }
1001         strcat(b, "]");
1002         *buf = b;
1003
1004         free(tmp);
1005
1006         return (0);
1007 error:
1008         return (1);
1009 }
1010
1011 static int
1012 fdt_data_fmt(const void *data, int len, char **buf)
1013 {
1014         int count;
1015
1016         if (len == 0) {
1017                 *buf = NULL;
1018                 return (1);
1019         }
1020
1021         if (fdt_isprint(data, len, &count))
1022                 return (fdt_data_str(data, len, count, buf));
1023
1024         else if ((len % 4) == 0)
1025                 return (fdt_data_cell(data, len, buf));
1026
1027         else
1028                 return (fdt_data_bytes(data, len, buf));
1029 }
1030
1031 static int
1032 fdt_prop(int offset)
1033 {
1034         char *line, *buf;
1035         const struct fdt_property *prop;
1036         const char *name;
1037         const void *data;
1038         int len, rv;
1039
1040         line = NULL;
1041         prop = fdt_offset_ptr(fdtp, offset, sizeof(*prop));
1042         if (prop == NULL)
1043                 return (1);
1044
1045         name = fdt_string(fdtp, fdt32_to_cpu(prop->nameoff));
1046         len = fdt32_to_cpu(prop->len);
1047
1048         rv = 0;
1049         buf = NULL;
1050         if (len == 0) {
1051                 /* Property without value */
1052                 line = (char *)malloc(strlen(name) + 2);
1053                 if (line == NULL) {
1054                         rv = 2;
1055                         goto out2;
1056                 }
1057                 sprintf(line, "%s\n", name);
1058                 goto out1;
1059         }
1060
1061         /*
1062          * Process property with value
1063          */
1064         data = prop->data;
1065
1066         if (fdt_data_fmt(data, len, &buf) != 0) {
1067                 rv = 3;
1068                 goto out2;
1069         }
1070
1071         line = (char *)malloc(strlen(name) + strlen(FDT_PROP_SEP) +
1072             strlen(buf) + 2);
1073         if (line == NULL) {
1074                 sprintf(command_errbuf, "could not allocate space for string");
1075                 rv = 4;
1076                 goto out2;
1077         }
1078
1079         sprintf(line, "%s" FDT_PROP_SEP "%s\n", name, buf);
1080
1081 out1:
1082         pager_open();
1083         pager_output(line);
1084         pager_close();
1085
1086 out2:
1087         if (buf)
1088                 free(buf);
1089
1090         if (line)
1091                 free(line);
1092
1093         return (rv);
1094 }
1095
1096 static int
1097 fdt_modprop(int nodeoff, char *propname, void *value, char mode)
1098 {
1099         uint32_t cells[100];
1100         char *buf;
1101         int len, rv;
1102         const struct fdt_property *p;
1103
1104         p = fdt_get_property(fdtp, nodeoff, propname, NULL);
1105
1106         if (p != NULL) {
1107                 if (mode == 1) {
1108                          /* Adding inexistant value in mode 1 is forbidden */
1109                         sprintf(command_errbuf, "property already exists!");
1110                         return (CMD_ERROR);
1111                 }
1112         } else if (mode == 0) {
1113                 sprintf(command_errbuf, "property does not exist!");
1114                 return (CMD_ERROR);
1115         }
1116         len = strlen(value);
1117         rv = 0;
1118         buf = (char *)value;
1119
1120         switch (*buf) {
1121         case '&':
1122                 /* phandles */
1123                 break;
1124         case '<':
1125                 /* Data cells */
1126                 len = fdt_strtovect(buf, (void *)&cells, 100,
1127                     sizeof(uint32_t));
1128
1129                 rv = fdt_setprop(fdtp, nodeoff, propname, &cells,
1130                     len * sizeof(uint32_t));
1131                 break;
1132         case '[':
1133                 /* Data bytes */
1134                 len = fdt_strtovect(buf, (void *)&cells, 100,
1135                     sizeof(uint8_t));
1136
1137                 rv = fdt_setprop(fdtp, nodeoff, propname, &cells,
1138                     len * sizeof(uint8_t));
1139                 break;
1140         case '"':
1141         default:
1142                 /* Default -- string */
1143                 rv = fdt_setprop_string(fdtp, nodeoff, propname, value);
1144                 break;
1145         }
1146
1147         if (rv != 0) {
1148                 if (rv == -FDT_ERR_NOSPACE)
1149                         sprintf(command_errbuf,
1150                             "Device tree blob is too small!\n");
1151                 else
1152                         sprintf(command_errbuf,
1153                             "Could not add/modify property!\n");
1154         }
1155         return (rv);
1156 }
1157
1158 /* Merge strings from argv into a single string */
1159 static int
1160 fdt_merge_strings(int argc, char *argv[], int start, char **buffer)
1161 {
1162         char *buf;
1163         int i, idx, sz;
1164
1165         *buffer = NULL;
1166         sz = 0;
1167
1168         for (i = start; i < argc; i++)
1169                 sz += strlen(argv[i]);
1170
1171         /* Additional bytes for whitespaces between args */
1172         sz += argc - start;
1173
1174         buf = (char *)malloc(sizeof(char) * sz);
1175         bzero(buf, sizeof(char) * sz);
1176
1177         if (buf == NULL) {
1178                 sprintf(command_errbuf, "could not allocate space "
1179                     "for string");
1180                 return (1);
1181         }
1182
1183         idx = 0;
1184         for (i = start, idx = 0; i < argc; i++) {
1185                 strcpy(buf + idx, argv[i]);
1186                 idx += strlen(argv[i]);
1187                 buf[idx] = ' ';
1188                 idx++;
1189         }
1190         buf[sz - 1] = '\0';
1191         *buffer = buf;
1192         return (0);
1193 }
1194
1195 /* Extract offset and name of node/property from a given path */
1196 static int
1197 fdt_extract_nameloc(char **pathp, char **namep, int *nodeoff)
1198 {
1199         int o;
1200         char *path = *pathp, *name = NULL, *subpath = NULL;
1201
1202         subpath = strrchr(path, '/');
1203         if (subpath == NULL) {
1204                 o = fdt_path_offset(fdtp, cwd);
1205                 name = path;
1206                 path = (char *)&cwd;
1207         } else {
1208                 *subpath = '\0';
1209                 if (strlen(path) == 0)
1210                         path = cwd;
1211
1212                 name = subpath + 1;
1213                 o = fdt_path_offset(fdtp, path);
1214         }
1215
1216         if (strlen(name) == 0) {
1217                 sprintf(command_errbuf, "name not specified");
1218                 return (1);
1219         }
1220         if (o < 0) {
1221                 sprintf(command_errbuf, "could not find node: '%s'", path);
1222                 return (1);
1223         }
1224         *namep = name;
1225         *nodeoff = o;
1226         *pathp = path;
1227         return (0);
1228 }
1229
1230 static int
1231 fdt_cmd_prop(int argc, char *argv[])
1232 {
1233         char *path, *propname, *value;
1234         int o, next, depth, rv;
1235         uint32_t tag;
1236
1237         path = (argc > 2) ? argv[2] : NULL;
1238
1239         value = NULL;
1240
1241         if (argc > 3) {
1242                 /* Merge property value strings into one */
1243                 if (fdt_merge_strings(argc, argv, 3, &value) != 0)
1244                         return (CMD_ERROR);
1245         } else
1246                 value = NULL;
1247
1248         if (path == NULL)
1249                 path = cwd;
1250
1251         rv = CMD_OK;
1252
1253         if (value) {
1254                 /* If value is specified -- try to modify prop. */
1255                 if (fdt_extract_nameloc(&path, &propname, &o) != 0)
1256                         return (CMD_ERROR);
1257
1258                 rv = fdt_modprop(o, propname, value, 0);
1259                 if (rv)
1260                         return (CMD_ERROR);
1261                 return (CMD_OK);
1262
1263         }
1264         /* User wants to display properties */
1265         o = fdt_path_offset(fdtp, path);
1266
1267         if (o < 0) {
1268                 sprintf(command_errbuf, "could not find node: '%s'", path);
1269                 rv = CMD_ERROR;
1270                 goto out;
1271         }
1272
1273         depth = 0;
1274         while (depth >= 0) {
1275                 tag = fdt_next_tag(fdtp, o, &next);
1276                 switch (tag) {
1277                 case FDT_NOP:
1278                         break;
1279                 case FDT_PROP:
1280                         if (depth > 1)
1281                                 /* Don't process properties of nested nodes */
1282                                 break;
1283
1284                         if (fdt_prop(o) != 0) {
1285                                 sprintf(command_errbuf, "could not process "
1286                                     "property");
1287                                 rv = CMD_ERROR;
1288                                 goto out;
1289                         }
1290                         break;
1291                 case FDT_BEGIN_NODE:
1292                         depth++;
1293                         if (depth > FDT_MAX_DEPTH) {
1294                                 printf("warning: nesting too deep: %d\n",
1295                                     depth);
1296                                 goto out;
1297                         }
1298                         break;
1299                 case FDT_END_NODE:
1300                         depth--;
1301                         if (depth == 0)
1302                                 /*
1303                                  * This is the end of our starting node, force
1304                                  * the loop finish.
1305                                  */
1306                                 depth--;
1307                         break;
1308                 }
1309                 o = next;
1310         }
1311 out:
1312         return (rv);
1313 }
1314
1315 static int
1316 fdt_cmd_mkprop(int argc, char *argv[])
1317 {
1318         int o;
1319         char *path, *propname, *value;
1320
1321         path = (argc > 2) ? argv[2] : NULL;
1322
1323         value = NULL;
1324
1325         if (argc > 3) {
1326                 /* Merge property value strings into one */
1327                 if (fdt_merge_strings(argc, argv, 3, &value) != 0)
1328                         return (CMD_ERROR);
1329         } else
1330                 value = NULL;
1331
1332         if (fdt_extract_nameloc(&path, &propname, &o) != 0)
1333                 return (CMD_ERROR);
1334
1335         if (fdt_modprop(o, propname, value, 1))
1336                 return (CMD_ERROR);
1337
1338         return (CMD_OK);
1339 }
1340
1341 static int
1342 fdt_cmd_rm(int argc, char *argv[])
1343 {
1344         int o, rv;
1345         char *path = NULL, *propname;
1346
1347         if (argc > 2)
1348                 path = argv[2];
1349         else {
1350                 sprintf(command_errbuf, "no node/property name specified");
1351                 return (CMD_ERROR);
1352         }
1353
1354         o = fdt_path_offset(fdtp, path);
1355         if (o < 0) {
1356                 /* If node not found -- try to find & delete property */
1357                 if (fdt_extract_nameloc(&path, &propname, &o) != 0)
1358                         return (CMD_ERROR);
1359
1360                 if ((rv = fdt_delprop(fdtp, o, propname)) != 0) {
1361                         sprintf(command_errbuf, "could not delete"
1362                             "%s\n", (rv == -FDT_ERR_NOTFOUND) ?
1363                             "(property/node does not exist)" : "");
1364                         return (CMD_ERROR);
1365
1366                 } else
1367                         return (CMD_OK);
1368         }
1369         /* If node exists -- remove node */
1370         rv = fdt_del_node(fdtp, o);
1371         if (rv) {
1372                 sprintf(command_errbuf, "could not delete node");
1373                 return (CMD_ERROR);
1374         }
1375         return (CMD_OK);
1376 }
1377
1378 static int
1379 fdt_cmd_mknode(int argc, char *argv[])
1380 {
1381         int o, rv;
1382         char *path = NULL, *nodename = NULL;
1383
1384         if (argc > 2)
1385                 path = argv[2];
1386         else {
1387                 sprintf(command_errbuf, "no node name specified");
1388                 return (CMD_ERROR);
1389         }
1390
1391         if (fdt_extract_nameloc(&path, &nodename, &o) != 0)
1392                 return (CMD_ERROR);
1393
1394         rv = fdt_add_subnode(fdtp, o, nodename);
1395
1396         if (rv < 0) {
1397                 if (rv == -FDT_ERR_NOSPACE)
1398                         sprintf(command_errbuf,
1399                             "Device tree blob is too small!\n");
1400                 else
1401                         sprintf(command_errbuf,
1402                             "Could not add node!\n");
1403                 return (CMD_ERROR);
1404         }
1405         return (CMD_OK);
1406 }
1407
1408 static int
1409 fdt_cmd_pwd(int argc, char *argv[])
1410 {
1411         char line[FDT_CWD_LEN];
1412
1413         pager_open();
1414         sprintf(line, "%s\n", cwd);
1415         pager_output(line);
1416         pager_close();
1417         return (CMD_OK);
1418 }
1419
1420 static int
1421 fdt_cmd_nyi(int argc, char *argv[])
1422 {
1423
1424         printf("command not yet implemented\n");
1425         return (CMD_ERROR);
1426 }