git.ozlabs.org Git - petitboot/blob - utils/hooks/30-add-offb.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
  13 #include <linux/fb.h>
  14
  15 #include <libfdt.h>
  16
  17 #include <file/file.h>
  18 #include <talloc/talloc.h>
  19
  20 static const char *fbdev_name = "fb0";
  21
  22 #define MAX_N_CELLS             4
  23 #define ADDRESS_PROP_SIZE       4096
  24
  25 struct offb_ctx {
  26         const char                      *dtb_name;
  27         void                            *dtb;
  28         int                             dtb_node;
  29         const char                      *path;
  30         struct fb_fix_screeninfo        fscreeninfo;
  31         struct fb_var_screeninfo        vscreeninfo;
  32 };
  33
  34 static int load_dtb(struct offb_ctx *ctx)
  35 {
  36         char *buf;
  37         int len;
  38         int rc;
  39
  40         rc = read_file(ctx, ctx->dtb_name, &buf, &len);
  41         if (rc) {
  42                 warn("error reading %s", ctx->dtb_name);
  43                 return rc;
  44         }
  45
  46         rc = fdt_check_header(buf);
  47         if (rc || (int)fdt_totalsize(buf) > len) {
  48                 warnx("invalid dtb: %s (rc %d)", ctx->dtb_name, rc);
  49                 return -1;
  50         }
  51
  52         len = fdt_totalsize(buf) + ADDRESS_PROP_SIZE;
  53
  54         ctx->dtb = talloc_array(ctx, char, len);
  55         if (!ctx->dtb) {
  56                 warn("Failed to allocate space for dtb\n");
  57                 return -1;
  58         }
  59         fdt_open_into(buf, ctx->dtb, len);
  60
  61         return 0;
  62 }
  63
  64 static int fbdev_sysfs_lookup(struct offb_ctx *ctx)
  65 {
  66         char *path, *linkpath, *nodepath;
  67         int fd, node;
  68         ssize_t rc __attribute__((unused));
  69
  70         path = talloc_asprintf(ctx, "/sys/class/graphics/%s", fbdev_name);
  71         if (!path) {
  72                 warn("Failed to allocate space for sysfs path\n");
  73                 return -1;
  74         }
  75
  76         fd = open(path, O_RDONLY | O_DIRECTORY);
  77         if (fd < 0) {
  78                 warn("Can't open device %s in sysfs", fbdev_name);
  79                 return -1;
  80         }
  81
  82         linkpath = talloc_zero_array(ctx, char, PATH_MAX + 1);
  83         if (!linkpath) {
  84                 warn("Failed to allocate space for link path\n");
  85                 return -1;
  86         }
  87
  88         rc = readlinkat(fd, "device/of_node", linkpath, PATH_MAX);
  89         if (rc < 0) {
  90                 warn("Can't read of_node link for device %s", fbdev_name);
  91                 return -1;
  92         }
  93
  94         /* readlinkat() returns a relative path such as:
  95          *
  96          *  ../../../../../../../firmware/devicetree/base/pciex@n/…/vga@0
  97          *
  98          * We only need the path component from the device tree itself; so
  99          * strip everything before /firmware/devicetree/base
 100          */
 101         nodepath = strstr(linkpath, "/firmware/devicetree/base/");
 102         if (!nodepath) {
 103                 warnx("Can't resolve device tree link for device %s",
 104                                 fbdev_name);
 105                 return -1;
 106         }
 107
 108         nodepath += strlen("/firmware/devicetree/base");
 109
 110         node = fdt_path_offset(ctx->dtb, nodepath);
 111         if (node < 0) {
 112                 warnx("Can't find node %s in device tree: %s",
 113                                 nodepath, fdt_strerror(node));
 114                 return -1;
 115         }
 116
 117         ctx->path = nodepath;
 118         ctx->dtb_node = node;
 119
 120         return 0;
 121 }
 122
 123 static int fbdev_device_query(struct offb_ctx *ctx)
 124 {
 125         int fd, rc = -1;
 126         char *path;
 127
 128         path = talloc_asprintf(ctx, "/dev/%s", fbdev_name);
 129         if (!path) {
 130                 warn("Failed to allocate space for device path\n");
 131                 return -1;
 132         }
 133
 134         fd = open(path, O_RDWR);
 135         if (fd < 0) {
 136                 warn("Can't open fb device %s", path);
 137                 return -1;
 138         }
 139
 140         rc = ioctl(fd, FBIOGET_VSCREENINFO, &ctx->vscreeninfo);
 141         if (rc) {
 142                 warn("ioctl(FBIOGET_VSCREENINFO) failed");
 143                 goto out;
 144         }
 145
 146         rc = ioctl(fd, FBIOGET_FSCREENINFO, &ctx->fscreeninfo);
 147         if (rc) {
 148                 warn("ioctl(FBIOGET_FSCREENINFO) failed");
 149                 goto out;
 150         }
 151
 152         fprintf(stderr, "Retrieved framebuffer details:\n");
 153         fprintf(stderr, "device %s:\n", fbdev_name);
 154         fprintf(stderr, "  addr: %lx\n", ctx->fscreeninfo.smem_start);
 155         fprintf(stderr, "   len: %" PRIu32 "\n", ctx->fscreeninfo.smem_len);
 156         fprintf(stderr, "  line: %d\n", ctx->fscreeninfo.line_length);
 157         fprintf(stderr, "   res:  %dx%d@%d\n", ctx->vscreeninfo.xres,
 158                         ctx->vscreeninfo.yres,
 159                         ctx->vscreeninfo.bits_per_pixel);
 160
 161         rc = 0;
 162
 163 out:
 164         close(fd);
 165         return rc;
 166 }
 167
 168 static char *next_dt_name(struct offb_ctx *ctx, const char **path)
 169 {
 170         const char *c, *p;
 171         char *name;
 172
 173         p = *path;
 174
 175         if (p[0] == '/')
 176                 p++;
 177
 178         if (p[0] == '\0')
 179                 return NULL;
 180
 181         c = strchrnul(p, '/');
 182
 183         name = talloc_strndup(ctx, p, c - p);
 184
 185         *path = c;
 186
 187         return name;
 188 }
 189
 190 static uint64_t of_read_number(const fdt32_t *data, int n)
 191 {
 192         uint64_t x;
 193
 194         x = fdt32_to_cpu(data[0]);
 195         if (n > 1) {
 196                 x <<= 32;
 197                 x |= fdt32_to_cpu(data[1]);
 198         }
 199         return x;
 200 }
 201
 202 /* Do a single translation across a PCI bridge. This results in either;
 203  * - Translating a 2-cell CPU address into a 3-cell PCI address, or
 204  * - Translating a 3-cell PCI address into a 3-cell PCI address with a
 205  *   different offset.
 206  *
 207  * To simplify translation we make some assumptions about addresses:
 208  * Addresses are either 3 or 2 cells wide
 209  * Size is always 2 cells wide
 210  * The first cell of a 3 cell address is the PCI memory type
 211  */
 212 static int do_translate(void *fdt, int node,
 213                 const fdt32_t *ranges, int range_size,
 214                 uint32_t *addr, uint32_t *size,
 215                 int *addr_cells, int *size_cells)
 216 {
 217         uint64_t addr_current_base, addr_child_base, addr_size;
 218         uint64_t addr_current, offset, new_addr;
 219         uint64_t current_pci_flags, child_pci_flags;
 220         int i, na, ns, cna, cns, prop_len;
 221         const fdt32_t *prop;
 222         const char *type;
 223         bool pci = false;
 224
 225         type = fdt_getprop(fdt, node, "device_type", NULL);
 226         pci = type && (!strcmp(type, "pci") || !strcmp(type, "pciex"));
 227
 228         /* We don't translate at vga@0, so we should always see a pci or
 229          * pciex device_type */
 230         if (!pci)
 231                 return -1;
 232
 233         if (range_size == 0) {
 234                 fprintf(stderr, "Empty ranges property, 1:1 translation\n");
 235                 return 0;
 236         }
 237
 238         /* Number of cells for address and size at current level */
 239         na = *addr_cells;
 240         ns = *size_cells;
 241
 242         /* Number of cells for address and size at child level */
 243         prop = fdt_getprop(fdt, node, "#address-cells", &prop_len);
 244         cna = prop ? fdt32_to_cpu(*prop) : 2;
 245         prop = fdt_getprop(fdt, node, "#size-cells", &prop_len);
 246         cns = prop ? fdt32_to_cpu(*prop) : 2;
 247
 248         /* We're translating back to a PCI address, so the size should grow */
 249         if (na > cna) {
 250                 fprintf(stderr, "na > cna, unexpected\n");
 251                 return -1;
 252         }
 253
 254         /* If the current address is a PCI address, its type should match the
 255          * type of every subsequent child address */
 256         current_pci_flags = na > 2 ? of_read_number(addr, 1) : 0;
 257         child_pci_flags = cna > 2 ? of_read_number(ranges, 1) : 0;
 258         if (current_pci_flags != 0 && current_pci_flags != child_pci_flags) {
 259                 fprintf(stderr, "Unexpected change in flags: %lx, %lx\n",
 260                         current_pci_flags, child_pci_flags);
 261                 return -1;
 262         }
 263
 264         if (ns != cns) {
 265                 fprintf(stderr, "Unexpected change in #size-cells: %d vs %d\n",
 266                         ns, cns);
 267                 return -1;
 268         }
 269
 270         /*
 271          * The ranges property is of the form
 272          *      < upstream addr base > < downstream addr base > < size >
 273          * The current address stored in addr is similarly of the form
 274          *      < current address > < size >
 275          * Where either address base and the current address can be a 2-cell
 276          * CPU address or a 3-cell PCI address.
 277          *
 278          * For PCI addresses ignore the type flag in the first cell and use the
 279          * 64-bit address in the remaining 2 cells.
 280          */
 281         if (na > 2) {
 282                 addr_current_base =  of_read_number(ranges + cna + 1, na - 1);
 283                 addr_current =  of_read_number(addr + 1, na - 1);
 284         } else {
 285                 addr_current_base =  of_read_number(ranges + cna, na);
 286                 addr_current =  of_read_number(addr, na);
 287         }
 288         if (cna > 2)
 289                 addr_child_base =  of_read_number(ranges + 1, cna - 1);
 290         else
 291                 addr_child_base =  of_read_number(ranges, cna);
 292
 293         /*
 294          * Perform the actual translation. Find the offset of the current
 295          * address from the upstream base, and add the offset to the
 296          * downstream base to find the new address.
 297          * The new address will be cna-cells wide, inheriting child_pci_flags
 298          * as the memory type.
 299          */
 300         addr_size = of_read_number(size, ns);
 301         offset = addr_current - addr_current_base;
 302         new_addr = addr_child_base + offset;
 303
 304         memset(addr, 0, *addr_cells);
 305         memset(size, 0, *size_cells);
 306         *addr_cells = cna;
 307         *size_cells = cns;
 308
 309         /* Update the current address in addr.
 310          * It's highly unlikely any translation will leave us with a 2-cell
 311          * CPU address, but for completeness only include PCI flags if the
 312          * child offset was definitely a PCI address */
 313         if (*addr_cells > 2)
 314                 addr[0] = cpu_to_fdt32(child_pci_flags);
 315         for (i = *addr_cells - 1; i >= *addr_cells - 2; i--) {
 316                 addr[i] = cpu_to_fdt32(new_addr & 0xffffffff);
 317                 new_addr >>= 32;
 318         }
 319         for (i = *size_cells - 1; i >= 0; i--) {
 320                 size[i] = cpu_to_fdt32(addr_size & 0xffffffff);
 321                 addr_size >>= 32;
 322         }
 323
 324         fprintf(stderr, "New address:\n\t");
 325         for (i = 0; i < *addr_cells; i++)
 326                 fprintf(stderr, " %lx ", of_read_number(&addr[i], 1));
 327         fprintf(stderr, "\n");
 328
 329         return 0;
 330 }
 331
 332 static int create_translated_addresses(struct offb_ctx *ctx,
 333                 int dev_node, const char *path,
 334                 uint64_t in_addr, uint64_t in_size,
 335                 fdt32_t *reg, int reg_cells)
 336 {
 337         uint32_t addr[MAX_N_CELLS], size[MAX_N_CELLS];
 338         int addr_cells, size_cells, node, prop_len, ranges_len, rc, i;
 339         const fdt32_t *ranges, *prop;
 340         char *name;
 341
 342         prop = fdt_getprop(ctx->dtb, 0, "#address-cells", &prop_len);
 343         addr_cells = prop ? fdt32_to_cpu(*prop) : 2;
 344
 345         prop = fdt_getprop(ctx->dtb, 0, "#size-cells", &prop_len);
 346         size_cells = prop ? fdt32_to_cpu(*prop) : 2;
 347
 348         memset(addr, 0, sizeof(uint32_t) * MAX_N_CELLS);
 349         for (i = addr_cells - 1; i >= 0; i--) {
 350                 addr[i] = cpu_to_fdt32(in_addr & 0xffffffff);
 351                 in_addr >>= 32;
 352         }
 353         memset(size, 0, sizeof(uint32_t) * MAX_N_CELLS);
 354         for (i = size_cells - 1; i >= 0; i--) {
 355                 size[i] = cpu_to_fdt32(in_size & 0xffffffff);
 356                 in_size >>= 32;
 357         }
 358
 359         node = 0;
 360         for (;;) {
 361                 /* get the name of the next child node to 'node' */
 362                 name = next_dt_name(ctx, &path);
 363                 if (!name)
 364                         return -1;
 365
 366                 node = fdt_subnode_offset(ctx->dtb, node, name);
 367                 if (node < 0)
 368                         return -1;
 369                 if (node == dev_node)
 370                         break;
 371
 372                 ranges = fdt_getprop(ctx->dtb, node, "ranges", &ranges_len);
 373                 if (!ranges)
 374                         return -1;
 375
 376                 rc = do_translate(ctx->dtb, node, ranges, ranges_len,
 377                              addr, size, &addr_cells, &size_cells);
 378                 if (rc)
 379                         return -1;
 380         }
 381
 382         fprintf(stderr, "Final address:\n\t");
 383         for (i = 0; i < addr_cells; i++)
 384                 fprintf(stderr, " %lx ", of_read_number(&addr[i], 1));
 385         fprintf(stderr, "\n");
 386
 387         if (addr_cells + size_cells > reg_cells) {
 388                 fprintf(stderr, "Error: na + ns larger than reg\n");
 389                 return -1;
 390         }
 391
 392         memcpy(reg, addr, sizeof(fdt32_t) * addr_cells);
 393         memcpy(reg + addr_cells, size, sizeof(fdt32_t) * size_cells);
 394
 395         return 0;
 396 }
 397
 398 #define fdt_set_check(dtb, node, fn, prop, ...) \
 399         do {                                                            \
 400                 int __x = fn(dtb, node, prop, __VA_ARGS__);             \
 401                 if (__x) {                                              \
 402                         warnx("failed to update device tree (%s): %s",  \
 403                                         prop, fdt_strerror(__x));       \
 404                         return -1;                                      \
 405                 }                                                       \
 406         } while (0);
 407
 408 static int populate_devicetree(struct offb_ctx *ctx)
 409 {
 410         fdt32_t reg[5];
 411         void *dtb = ctx->dtb;
 412         int rc, node = ctx->dtb_node;
 413
 414         memset(reg, 0, sizeof(reg));
 415         rc = create_translated_addresses(ctx, node, ctx->path,
 416                                 ctx->fscreeninfo.smem_start,
 417                                 ctx->fscreeninfo.smem_len,
 418                                 reg, 5);
 419
 420         if (rc) {
 421                 fprintf(stderr, "Failed to translate address\n");
 422                 return rc;
 423         }
 424
 425         fdt_set_check(dtb, node, fdt_setprop_string, "device_type", "display");
 426
 427         fdt_set_check(dtb, node, fdt_setprop, "assigned-addresses",
 428                         reg, sizeof(reg));
 429
 430         fdt_set_check(dtb, node, fdt_setprop_cell,
 431                         "width", ctx->vscreeninfo.xres);
 432         fdt_set_check(dtb, node, fdt_setprop_cell,
 433                         "height", ctx->vscreeninfo.yres);
 434         fdt_set_check(dtb, node, fdt_setprop_cell,
 435                         "depth", ctx->vscreeninfo.bits_per_pixel);
 436
 437         fdt_set_check(dtb, node, fdt_setprop, "little-endian", NULL, 0);
 438         fdt_set_check(dtb, node, fdt_setprop, "linux,opened", NULL, 0);
 439         fdt_set_check(dtb, node, fdt_setprop, "linux,boot-display", NULL, 0);
 440
 441         return 0;
 442 }
 443
 444 static int write_devicetree(struct offb_ctx *ctx)
 445 {
 446         int rc;
 447
 448         fdt_pack(ctx->dtb);
 449
 450         rc = replace_file(ctx->dtb_name, ctx->dtb, fdt_totalsize(ctx->dtb));
 451         if (rc)
 452                 warn("failed to write file %s", ctx->dtb_name);
 453
 454         return rc;
 455 }
 456
 457
 458 int main(void)
 459 {
 460         struct offb_ctx *ctx;
 461         int rc;
 462
 463         ctx = talloc_zero(NULL, struct offb_ctx);
 464
 465         ctx->dtb_name = getenv("boot_dtb");
 466         if (!ctx->dtb_name) {
 467                 talloc_free(ctx);
 468                 return EXIT_SUCCESS;
 469         }
 470
 471         rc = load_dtb(ctx);
 472         if (rc)
 473                 goto out;
 474
 475         rc = fbdev_sysfs_lookup(ctx);
 476         if (rc)
 477                 goto out;
 478
 479         rc = fbdev_device_query(ctx);
 480         if (rc)
 481                 goto out;
 482
 483         rc = populate_devicetree(ctx);
 484         if (rc)
 485                 goto out;
 486
 487         rc = write_devicetree(ctx);
 488
 489 out:
 490         talloc_free(ctx);
 491         return rc ? EXIT_FAILURE : EXIT_SUCCESS;
 492 }