ui/ncurses: Re-add autoboot selection to config screen
[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
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 }