hooks/add-offb: Add leading slash to HVC node paths
[petitboot] / 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 #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: %lx, %lx\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, " %lx ", 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, " %lx ", 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_tty, *ptr;
504         long unsigned int termno;
505         const fdt32_t *prop;
506         int node, prop_len;
507         char *stdout_path;
508
509         boot_tty = getenv("boot_tty");
510         if (!boot_tty) {
511                 fprintf(stderr, "boot_tty not set, using default stdout for boot\n");
512                 return 0;
513         }
514
515         if (strstr(boot_tty, "tty") != NULL) {
516                 fprintf(stderr, "TTY recognised: %s\n", boot_tty);
517                 stdout_path = get_vga_path(ctx);
518         } else {
519                 ptr = strstr(boot_tty, "hvc");
520                 if (!ptr || strlen(ptr) <= strlen("hvc")) {
521                         fprintf(stderr, "Unrecognised console: %s\n", boot_tty);
522                         return 0;
523                 }
524                 ptr += strlen("hvc");
525                 errno = 0;
526                 termno = strtoul(ptr, NULL, 0);
527                 if (errno) {
528                         fprintf(stderr, "Couldn't parse termno from %s\n", boot_tty);
529                         return 0;
530                 }
531                 fprintf(stderr, "HVC recognised: %s\n", boot_tty);
532                 stdout_path = get_hvc_path(ctx, termno);
533         }
534
535         if (!stdout_path) {
536                 fprintf(stderr, "Couldn't parse %s into a path\n", boot_tty);
537                 return -1;
538         }
539
540         fprintf(stderr, "stdout-path: %s\n", stdout_path);
541
542         node = fdt_subnode_offset(ctx->dtb, 0, "chosen");
543         if (node <= 0) {
544                 fprintf(stderr, "Failed to find chosen\n");
545                 return -1;
546         }
547
548         prop = fdt_getprop(ctx->dtb, node, "linux,stdout-path", &prop_len);
549         if (!prop) {
550                 fprintf(stderr, "Failed to find linux,stdout-path\n");
551                 return -1;
552         }
553
554         fdt_set_check(ctx->dtb, node, fdt_setprop_string, "linux,stdout-path",
555                         stdout_path);
556
557         return 0;
558 }
559
560 static int write_devicetree(struct offb_ctx *ctx)
561 {
562         int rc;
563
564         fdt_pack(ctx->dtb);
565
566         rc = replace_file(ctx->dtb_name, ctx->dtb, fdt_totalsize(ctx->dtb));
567         if (rc)
568                 warn("failed to write file %s", ctx->dtb_name);
569
570         return rc;
571 }
572
573
574 int main(void)
575 {
576         struct offb_ctx *ctx;
577         int rc;
578
579         ctx = talloc_zero(NULL, struct offb_ctx);
580
581         ctx->dtb_name = getenv("boot_dtb");
582         if (!ctx->dtb_name) {
583                 talloc_free(ctx);
584                 return EXIT_SUCCESS;
585         }
586
587         rc = load_dtb(ctx);
588         if (rc)
589                 goto out;
590
591         rc = fbdev_sysfs_lookup(ctx);
592         if (rc)
593                 goto out;
594
595         rc = fbdev_device_query(ctx);
596         if (rc)
597                 goto out;
598
599         rc = populate_devicetree(ctx);
600         if (rc)
601                 goto out;
602
603         rc = set_stdout(ctx);
604         if (rc)
605                 goto out;
606
607         rc = write_devicetree(ctx);
608
609 out:
610         talloc_free(ctx);
611         return rc ? EXIT_FAILURE : EXIT_SUCCESS;
612 }