git.ozlabs.org Git - petitboot/blob - utils/hooks/30-dtb-updates.c

   1
   2 #define _GNU_SOURCE
   3
   4 #include <stdlib.h>
   5 #include <err.h>
   6 #include <fcntl.h>
   7 #include <limits.h>
   8 #include <sys/ioctl.h>
   9 #include <string.h>
  10 #include <stdbool.h>
  11 #include <inttypes.h>
  12 #include <errno.h>
  13
  14 #include <linux/fb.h>
  15
  16 #include <libfdt.h>
  17
  18 #include <file/file.h>
  19 #include <talloc/talloc.h>
  20
  21 static const char *fbdev_name = "fb0";
  22
  23 #define MAX_N_CELLS             4
  24 #define ADDRESS_PROP_SIZE       4096
  25
  26 struct offb_ctx {
  27         const char                      *dtb_name;
  28         void                            *dtb;
  29         int                             dtb_node;
  30         const char                      *path;
  31         struct fb_fix_screeninfo        fscreeninfo;
  32         struct fb_var_screeninfo        vscreeninfo;
  33 };
  34
  35 static int load_dtb(struct offb_ctx *ctx)
  36 {
  37         char *buf;
  38         int len;
  39         int rc;
  40
  41         rc = read_file(ctx, ctx->dtb_name, &buf, &len);
  42         if (rc) {
  43                 warn("error reading %s", ctx->dtb_name);
  44                 return rc;
  45         }
  46
  47         rc = fdt_check_header(buf);
  48         if (rc || (int)fdt_totalsize(buf) > len) {
  49                 warnx("invalid dtb: %s (rc %d)", ctx->dtb_name, rc);
  50                 return -1;
  51         }
  52
  53         len = fdt_totalsize(buf) + ADDRESS_PROP_SIZE;
  54
  55         ctx->dtb = talloc_array(ctx, char, len);
  56         if (!ctx->dtb) {
  57                 warn("Failed to allocate space for dtb\n");
  58                 return -1;
  59         }
  60         fdt_open_into(buf, ctx->dtb, len);
  61
  62         return 0;
  63 }
  64
  65 static int fbdev_sysfs_lookup(struct offb_ctx *ctx)
  66 {
  67         char *path, *linkpath, *nodepath;
  68         int fd, node;
  69         ssize_t rc __attribute__((unused));
  70
  71         path = talloc_asprintf(ctx, "/sys/class/graphics/%s", fbdev_name);
  72         if (!path) {
  73                 warn("Failed to allocate space for sysfs path\n");
  74                 return -1;
  75         }
  76
  77         fd = open(path, O_RDONLY | O_DIRECTORY);
  78         if (fd < 0) {
  79                 warn("Can't open device %s in sysfs", fbdev_name);
  80                 return -1;
  81         }
  82
  83         linkpath = talloc_zero_array(ctx, char, PATH_MAX + 1);
  84         if (!linkpath) {
  85                 warn("Failed to allocate space for link path\n");
  86                 return -1;
  87         }
  88
  89         rc = readlinkat(fd, "device/of_node", linkpath, PATH_MAX);
  90         if (rc < 0) {
  91                 warn("Can't read of_node link for device %s", fbdev_name);
  92                 return -1;
  93         }
  94
  95         /* readlinkat() returns a relative path such as:
  96          *
  97          *  ../../../../../../../firmware/devicetree/base/pciex@n/…/vga@0
  98          *
  99          * We only need the path component from the device tree itself; so
 100          * strip everything before /firmware/devicetree/base
 101          */
 102         nodepath = strstr(linkpath, "/firmware/devicetree/base/");
 103         if (!nodepath) {
 104                 warnx("Can't resolve device tree link for device %s",
 105                                 fbdev_name);
 106                 return -1;
 107         }
 108
 109         nodepath += strlen("/firmware/devicetree/base");
 110
 111         node = fdt_path_offset(ctx->dtb, nodepath);
 112         if (node < 0) {
 113                 warnx("Can't find node %s in device tree: %s",
 114                                 nodepath, fdt_strerror(node));
 115                 return -1;
 116         }
 117
 118         ctx->path = nodepath;
 119         ctx->dtb_node = node;
 120
 121         return 0;
 122 }
 123
 124 static int fbdev_device_query(struct offb_ctx *ctx)
 125 {
 126         int fd, rc = -1;
 127         char *path;
 128
 129         path = talloc_asprintf(ctx, "/dev/%s", fbdev_name);
 130         if (!path) {
 131                 warn("Failed to allocate space for device path\n");
 132                 return -1;
 133         }
 134
 135         fd = open(path, O_RDWR);
 136         if (fd < 0) {
 137                 warn("Can't open fb device %s", path);
 138                 return -1;
 139         }
 140
 141         rc = ioctl(fd, FBIOGET_VSCREENINFO, &ctx->vscreeninfo);
 142         if (rc) {
 143                 warn("ioctl(FBIOGET_VSCREENINFO) failed");
 144                 goto out;
 145         }
 146
 147         rc = ioctl(fd, FBIOGET_FSCREENINFO, &ctx->fscreeninfo);
 148         if (rc) {
 149                 warn("ioctl(FBIOGET_FSCREENINFO) failed");
 150                 goto out;
 151         }
 152
 153         fprintf(stderr, "Retrieved framebuffer details:\n");
 154         fprintf(stderr, "device %s:\n", fbdev_name);
 155         fprintf(stderr, "  addr: %lx\n", ctx->fscreeninfo.smem_start);
 156         fprintf(stderr, "   len: %" PRIu32 "\n", ctx->fscreeninfo.smem_len);
 157         fprintf(stderr, "  line: %d\n", ctx->fscreeninfo.line_length);
 158         fprintf(stderr, "   res:  %dx%d@%d\n", ctx->vscreeninfo.xres,
 159                         ctx->vscreeninfo.yres,
 160                         ctx->vscreeninfo.bits_per_pixel);
 161
 162         rc = 0;
 163
 164 out:
 165         close(fd);
 166         return rc;
 167 }
 168
 169 static char *next_dt_name(struct offb_ctx *ctx, const char **path)
 170 {
 171         const char *c, *p;
 172         char *name;
 173
 174         p = *path;
 175
 176         if (p[0] == '/')
 177                 p++;
 178
 179         if (p[0] == '\0')
 180                 return NULL;
 181
 182         c = strchrnul(p, '/');
 183
 184         name = talloc_strndup(ctx, p, c - p);
 185
 186         *path = c;
 187
 188         return name;
 189 }
 190
 191 static uint64_t of_read_number(const fdt32_t *data, int n)
 192 {
 193         uint64_t x;
 194
 195         x = fdt32_to_cpu(data[0]);
 196         if (n > 1) {
 197                 x <<= 32;
 198                 x |= fdt32_to_cpu(data[1]);
 199         }
 200         return x;
 201 }
 202
 203 /* Do a single translation across a PCI bridge. This results in either;
 204  * - Translating a 2-cell CPU address into a 3-cell PCI address, or
 205  * - Translating a 3-cell PCI address into a 3-cell PCI address with a
 206  *   different offset.
 207  *
 208  * To simplify translation we make some assumptions about addresses:
 209  * Addresses are either 3 or 2 cells wide
 210  * Size is always 2 cells wide
 211  * The first cell of a 3 cell address is the PCI memory type
 212  */
 213 static int do_translate(void *fdt, int node,
 214                 const fdt32_t *ranges, int range_size,
 215                 uint32_t *addr, uint32_t *size,
 216                 int *addr_cells, int *size_cells)
 217 {
 218         uint64_t addr_current_base, addr_child_base, addr_size;
 219         uint64_t addr_current, offset, new_addr;
 220         uint64_t current_pci_flags, child_pci_flags;
 221         int i, na, ns, cna, cns, prop_len;
 222         const fdt32_t *prop;
 223         const char *type;
 224         bool pci = false;
 225
 226         type = fdt_getprop(fdt, node, "device_type", NULL);
 227         pci = type && (!strcmp(type, "pci") || !strcmp(type, "pciex"));
 228
 229         /* We don't translate at vga@0, so we should always see a pci or
 230          * pciex device_type */
 231         if (!pci)
 232                 return -1;
 233
 234         if (range_size == 0) {
 235                 fprintf(stderr, "Empty ranges property, 1:1 translation\n");
 236                 return 0;
 237         }
 238
 239         /* Number of cells for address and size at current level */
 240         na = *addr_cells;
 241         ns = *size_cells;
 242
 243         /* Number of cells for address and size at child level */
 244         prop = fdt_getprop(fdt, node, "#address-cells", &prop_len);
 245         cna = prop ? fdt32_to_cpu(*prop) : 2;
 246         prop = fdt_getprop(fdt, node, "#size-cells", &prop_len);
 247         cns = prop ? fdt32_to_cpu(*prop) : 2;
 248
 249         /* We're translating back to a PCI address, so the size should grow */
 250         if (na > cna) {
 251                 fprintf(stderr, "na > cna, unexpected\n");
 252                 return -1;
 253         }
 254
 255         /* If the current address is a PCI address, its type should match the
 256          * type of every subsequent child address */
 257         current_pci_flags = na > 2 ? of_read_number(addr, 1) : 0;
 258         child_pci_flags = cna > 2 ? of_read_number(ranges, 1) : 0;
 259         if (current_pci_flags != 0 && current_pci_flags != child_pci_flags) {
 260                 fprintf(stderr, "Unexpected change in flags: %" PRIu64 ", %" PRIu64 "\n",
 261                         current_pci_flags, child_pci_flags);
 262                 return -1;
 263         }
 264
 265         if (ns != cns) {
 266                 fprintf(stderr, "Unexpected change in #size-cells: %d vs %d\n",
 267                         ns, cns);
 268                 return -1;
 269         }
 270
 271         /*
 272          * The ranges property is of the form
 273          *      < upstream addr base > < downstream addr base > < size >
 274          * The current address stored in addr is similarly of the form
 275          *      < current address > < size >
 276          * Where either address base and the current address can be a 2-cell
 277          * CPU address or a 3-cell PCI address.
 278          *
 279          * For PCI addresses ignore the type flag in the first cell and use the
 280          * 64-bit address in the remaining 2 cells.
 281          */
 282         if (na > 2) {
 283                 addr_current_base =  of_read_number(ranges + cna + 1, na - 1);
 284                 addr_current =  of_read_number(addr + 1, na - 1);
 285         } else {
 286                 addr_current_base =  of_read_number(ranges + cna, na);
 287                 addr_current =  of_read_number(addr, na);
 288         }
 289         if (cna > 2)
 290                 addr_child_base =  of_read_number(ranges + 1, cna - 1);
 291         else
 292                 addr_child_base =  of_read_number(ranges, cna);
 293
 294         /*
 295          * Perform the actual translation. Find the offset of the current
 296          * address from the upstream base, and add the offset to the
 297          * downstream base to find the new address.
 298          * The new address will be cna-cells wide, inheriting child_pci_flags
 299          * as the memory type.
 300          */
 301         addr_size = of_read_number(size, ns);
 302         offset = addr_current - addr_current_base;
 303         new_addr = addr_child_base + offset;
 304
 305         memset(addr, 0, *addr_cells);
 306         memset(size, 0, *size_cells);
 307         *addr_cells = cna;
 308         *size_cells = cns;
 309
 310         /* Update the current address in addr.
 311          * It's highly unlikely any translation will leave us with a 2-cell
 312          * CPU address, but for completeness only include PCI flags if the
 313          * child offset was definitely a PCI address */
 314         if (*addr_cells > 2)
 315                 addr[0] = cpu_to_fdt32(child_pci_flags);
 316         for (i = *addr_cells - 1; i >= *addr_cells - 2; i--) {
 317                 addr[i] = cpu_to_fdt32(new_addr & 0xffffffff);
 318                 new_addr >>= 32;
 319         }
 320         for (i = *size_cells - 1; i >= 0; i--) {
 321                 size[i] = cpu_to_fdt32(addr_size & 0xffffffff);
 322                 addr_size >>= 32;
 323         }
 324
 325         fprintf(stderr, "New address:\n\t");
 326         for (i = 0; i < *addr_cells; i++)
 327                 fprintf(stderr, " %" PRIu64 " ", of_read_number(&addr[i], 1));
 328         fprintf(stderr, "\n");
 329
 330         return 0;
 331 }
 332
 333 static int create_translated_addresses(struct offb_ctx *ctx,
 334                 int dev_node, const char *path,
 335                 uint64_t in_addr, uint64_t in_size,
 336                 fdt32_t *reg, int reg_cells)
 337 {
 338         uint32_t addr[MAX_N_CELLS], size[MAX_N_CELLS];
 339         int addr_cells, size_cells, node, prop_len, ranges_len, rc, i;
 340         const fdt32_t *ranges, *prop;
 341         char *name;
 342
 343         prop = fdt_getprop(ctx->dtb, 0, "#address-cells", &prop_len);
 344         addr_cells = prop ? fdt32_to_cpu(*prop) : 2;
 345
 346         prop = fdt_getprop(ctx->dtb, 0, "#size-cells", &prop_len);
 347         size_cells = prop ? fdt32_to_cpu(*prop) : 2;
 348
 349         memset(addr, 0, sizeof(uint32_t) * MAX_N_CELLS);
 350         for (i = addr_cells - 1; i >= 0; i--) {
 351                 addr[i] = cpu_to_fdt32(in_addr & 0xffffffff);
 352                 in_addr >>= 32;
 353         }
 354         memset(size, 0, sizeof(uint32_t) * MAX_N_CELLS);
 355         for (i = size_cells - 1; i >= 0; i--) {
 356                 size[i] = cpu_to_fdt32(in_size & 0xffffffff);
 357                 in_size >>= 32;
 358         }
 359
 360         node = 0;
 361         for (;;) {
 362                 /* get the name of the next child node to 'node' */
 363                 name = next_dt_name(ctx, &path);
 364                 if (!name)
 365                         return -1;
 366
 367                 node = fdt_subnode_offset(ctx->dtb, node, name);
 368                 if (node < 0)
 369                         return -1;
 370                 if (node == dev_node)
 371                         break;
 372
 373                 ranges = fdt_getprop(ctx->dtb, node, "ranges", &ranges_len);
 374                 if (!ranges)
 375                         return -1;
 376
 377                 rc = do_translate(ctx->dtb, node, ranges, ranges_len,
 378                              addr, size, &addr_cells, &size_cells);
 379                 if (rc)
 380                         return -1;
 381         }
 382
 383         fprintf(stderr, "Final address:\n\t");
 384         for (i = 0; i < addr_cells; i++)
 385                 fprintf(stderr, " %" PRIu64 " ", of_read_number(&addr[i], 1));
 386         fprintf(stderr, "\n");
 387
 388         if (addr_cells + size_cells > reg_cells) {
 389                 fprintf(stderr, "Error: na + ns larger than reg\n");
 390                 return -1;
 391         }
 392
 393         memcpy(reg, addr, sizeof(fdt32_t) * addr_cells);
 394         memcpy(reg + addr_cells, size, sizeof(fdt32_t) * size_cells);
 395
 396         return 0;
 397 }
 398
 399 #define fdt_set_check(dtb, node, fn, prop, ...) \
 400         do {                                                            \
 401                 int __x = fn(dtb, node, prop, __VA_ARGS__);             \
 402                 if (__x) {                                              \
 403                         warnx("failed to update device tree (%s): %s",  \
 404                                         prop, fdt_strerror(__x));       \
 405                         return -1;                                      \
 406                 }                                                       \
 407         } while (0);
 408
 409 static int populate_devicetree(struct offb_ctx *ctx)
 410 {
 411         fdt32_t reg[5];
 412         void *dtb = ctx->dtb;
 413         int rc, node = ctx->dtb_node;
 414
 415         memset(reg, 0, sizeof(reg));
 416         rc = create_translated_addresses(ctx, node, ctx->path,
 417                                 ctx->fscreeninfo.smem_start,
 418                                 ctx->fscreeninfo.smem_len,
 419                                 reg, 5);
 420
 421         if (rc) {
 422                 fprintf(stderr, "Failed to translate address\n");
 423                 return rc;
 424         }
 425
 426         fdt_set_check(dtb, node, fdt_setprop_string, "device_type", "display");
 427
 428         fdt_set_check(dtb, node, fdt_setprop, "assigned-addresses",
 429                         reg, sizeof(reg));
 430
 431         fdt_set_check(dtb, node, fdt_setprop_cell,
 432                         "width", ctx->vscreeninfo.xres);
 433         fdt_set_check(dtb, node, fdt_setprop_cell,
 434                         "height", ctx->vscreeninfo.yres);
 435         fdt_set_check(dtb, node, fdt_setprop_cell,
 436                         "depth", ctx->vscreeninfo.bits_per_pixel);
 437
 438         fdt_set_check(dtb, node, fdt_setprop, "little-endian", NULL, 0);
 439         fdt_set_check(dtb, node, fdt_setprop, "linux,opened", NULL, 0);
 440         fdt_set_check(dtb, node, fdt_setprop, "linux,boot-display", NULL, 0);
 441
 442         return 0;
 443 }
 444
 445 /*
 446  * Find the device tree path assoicated with a hvc device.
 447  * On OPAL all hvc consoles have a 'serial@X' node under ibm,opal/consoles,
 448  * so we make a simplifying assumption that a hvcX is associated with a
 449  * serial@X node.
 450  */
 451 static char *get_hvc_path(struct offb_ctx *ctx, unsigned int termno)
 452 {
 453         char *serial;
 454         int node;
 455
 456         serial = talloc_asprintf(ctx, "serial@%u", termno);
 457         if (!serial)
 458                 return NULL;
 459
 460         node = fdt_subnode_offset(ctx->dtb, 0, "ibm,opal");
 461         if (node <= 0) {
 462                 fprintf(stderr, "Couldn't find ibm,opal\n");
 463                 return NULL;
 464         }
 465         node = fdt_subnode_offset(ctx->dtb, node, "consoles");
 466         if (node <= 0) {
 467                 fprintf(stderr, "Couldn't find ibm,opal/consoles\n");
 468                 return NULL;
 469         }
 470
 471         node = fdt_subnode_offset(ctx->dtb, node, serial);
 472         if (node <= 0) {
 473                 fprintf(stderr, "Could not locate hvc%u\n", termno);
 474                 return NULL;
 475         }
 476
 477         return talloc_asprintf(ctx, "/ibm,opal/consoles/%s", serial);
 478 }
 479
 480 /*
 481  * Find the device tree path of the vga device. On OPAL we assume there is only
 482  * one of these that represents any 'tty' console.
 483  */
 484 static char *get_vga_path(struct offb_ctx *ctx)
 485 {
 486         char *root, *vga_path;
 487
 488         root = strstr(ctx->path, "/pciex@");
 489         if (!root) {
 490                 fprintf(stderr, "Can't find root path for vga device in below:\n");
 491                 fprintf(stderr, "%s\n", ctx->path);
 492                 return NULL;
 493         }
 494
 495         vga_path = talloc_strdup(ctx, root);
 496         fprintf(stderr, "VGA target at '%s'\n", vga_path);
 497
 498         return vga_path;
 499 }
 500
 501 static int set_stdout(struct offb_ctx *ctx)
 502 {
 503         const char *boot_console, *ptr;
 504         long unsigned int termno;
 505         char *stdout_path;
 506         int node;
 507
 508         boot_console = getenv("boot_console");
 509         if (!boot_console) {
 510                 fprintf(stderr, "boot_console not set, using default stdout for boot\n");
 511                 return 0;
 512         }
 513
 514         if (strncmp(boot_console, "/dev/", strlen("/dev/")) != 0) {
 515                 /* We already have the full path */
 516                 stdout_path = talloc_strdup(ctx, boot_console);
 517                 /* Check for a tty* console but don't accidentally catch
 518                  * ttyS* consoles */
 519         } else if (strstr(boot_console, "tty") != NULL &&
 520                         strstr(boot_console, "ttyS") == NULL) {
 521                 fprintf(stderr, "TTY recognised: %s\n", boot_console);
 522                 stdout_path = get_vga_path(ctx);
 523         } else {
 524                 ptr = strstr(boot_console, "hvc");
 525                 if (!ptr || strlen(ptr) <= strlen("hvc")) {
 526                         fprintf(stderr, "Unrecognised console: %s\n",
 527                                         boot_console);
 528                         return 0;
 529                 }
 530                 ptr += strlen("hvc");
 531                 errno = 0;
 532                 termno = strtoul(ptr, NULL, 0);
 533                 if (errno) {
 534                         fprintf(stderr, "Couldn't parse termno from %s\n",
 535                                         boot_console);
 536                         return 0;
 537                 }
 538                 fprintf(stderr, "HVC recognised: %s\n", boot_console);
 539                 stdout_path = get_hvc_path(ctx, termno);
 540         }
 541
 542         if (!stdout_path) {
 543                 fprintf(stderr, "Couldn't parse %s into a path\n",
 544                                 boot_console);
 545                 return -1;
 546         }
 547
 548         fprintf(stderr, "stdout-path: %s\n", stdout_path);
 549
 550         node = fdt_subnode_offset(ctx->dtb, 0, "chosen");
 551         if (node <= 0) {
 552                 fprintf(stderr, "Failed to find chosen\n");
 553                 return -1;
 554         }
 555
 556         /*
 557          * linux,stdout-path is deprecated after v3.14 but we don't know
 558          * what the next kernel will be, so set both.
 559          */
 560         fdt_set_check(ctx->dtb, node, fdt_setprop_string, "stdout-path",
 561                         stdout_path);
 562         fdt_set_check(ctx->dtb, node, fdt_setprop_string, "linux,stdout-path",
 563                         stdout_path);
 564
 565         return 0;
 566 }
 567
 568 static int write_devicetree(struct offb_ctx *ctx)
 569 {
 570         int rc;
 571
 572         fdt_pack(ctx->dtb);
 573
 574         rc = replace_file(ctx->dtb_name, ctx->dtb, fdt_totalsize(ctx->dtb));
 575         if (rc)
 576                 warn("failed to write file %s", ctx->dtb_name);
 577
 578         return rc;
 579 }
 580
 581 static int set_offb(struct offb_ctx *ctx)
 582 {
 583         int rc;
 584
 585         rc = load_dtb(ctx);
 586         if (rc)
 587                 goto out;
 588
 589         rc = fbdev_sysfs_lookup(ctx);
 590         if (rc)
 591                 goto out;
 592
 593         rc = fbdev_device_query(ctx);
 594         if (rc)
 595                 goto out;
 596
 597         rc = populate_devicetree(ctx);
 598         if (rc)
 599                 goto out;
 600 out:
 601         return rc;
 602 }
 603
 604
 605 int main(void)
 606 {
 607         struct offb_ctx *ctx;
 608         int rc;
 609
 610         ctx = talloc_zero(NULL, struct offb_ctx);
 611
 612         ctx->dtb_name = getenv("boot_dtb");
 613         if (!ctx->dtb_name) {
 614                 talloc_free(ctx);
 615                 return EXIT_SUCCESS;
 616         }
 617
 618         if (set_offb(ctx)) {
 619                 warn("Failed offb setup step");
 620                 rc = -1;
 621         }
 622
 623         if (set_stdout(ctx)) {
 624                 warn("Failed stdout setup step\n");
 625                 rc = -1;
 626         }
 627
 628         if (write_devicetree(ctx)) {
 629                 warn("Failed to write back device tree\n");
 630                 rc = -1;
 631         }
 632
 633         talloc_free(ctx);
 634         return rc ? EXIT_FAILURE : EXIT_SUCCESS;
 635 }