diff options
Diffstat (limited to 'common')
| -rw-r--r-- | common/dtb_grow.c | 177 | ||||
| -rw-r--r-- | common/dtb_reloc.c | 69 | ||||
| -rw-r--r-- | common/load.c | 4 | ||||
| -rw-r--r-- | common/main.c | 69 |
4 files changed, 309 insertions, 10 deletions
diff --git a/common/dtb_grow.c b/common/dtb_grow.c new file mode 100644 index 0000000..309a43c --- /dev/null +++ b/common/dtb_grow.c @@ -0,0 +1,177 @@ +/* + * dtb_grow.c - add properties to a node in a devicetree that has + * room, the relocated copy from dtb_reloc.c. the insert point is + * the node's FDT_END_NODE token, everything after it moves up by + * the inserted size, the header totalsize tracks it. + * + * the insert is safe when the node sits at the end of the struct + * block, which is the common shape, /chosen is created last by + * firmware and the tail behind it is two end tokens and the + * block end. the strings block sits after the grow room and + * never moves. + * + * Copyright (C) 2026 Bradley Morgan <brads@mainlining.org> + */ + +#include <string.h> +#include <endian.h> +#include <boot.h> +#include <dtb_patch.h> + +#define FDT_BEGIN_NODE 1 +#define FDT_END_NODE 2 +#define FDT_PROP 3 +#define FDT_NOP 4 +#define FDT_END 9 + +static uint32_t be32(const void *p) +{ + const uint8_t *b = p; + + return ((uint32_t)b[0] << 24) | ((uint32_t)b[1] << 16) | + ((uint32_t)b[2] << 8) | (uint32_t)b[3]; +} + +static void put_be32(void *p, uint32_t v) +{ + uint8_t *b = p; + + b[0] = (uint8_t)(v >> 24); + b[1] = (uint8_t)(v >> 16); + b[2] = (uint8_t)(v >> 8); + b[3] = (uint8_t)v; +} + +static int name_eq(const char *a, const char *b) +{ + while (*a && *a != '@') { + if (*a != *b) + return 0; + a++; + b++; + } + return *b == '\0' || *b == '@'; +} + +/* + * insert one property into /chosen before its end token. value is + * copied as raw cells, len the byte count. name lands in the free + * space after the strings block. returns 0 or -1. + */ +int tb_dtb_add_chosen_prop(uintptr_t dtb, const char *name, + const void *val, size_t len) +{ + uint8_t *basep = (uint8_t *)dtb; + uint32_t off_struct = be32(basep + 8); + uint32_t off_strings = be32(basep + 12); + uint32_t totalsize = be32(basep + 4); + uint8_t *p = basep + off_struct; + uint8_t *ins; + size_t name_len = strlen(name) + 1; + size_t prop_size; + int depth = 0; + int in_chosen = 0; + + if (be32(basep) != 0xd00dfeed) + return -1; + + /* find the chosen node's end token, one level under the root */ + while (p < basep + totalsize) { + uint32_t token = be32(p); + + if (token == FDT_BEGIN_NODE) { + char *n = (char *)(p + 4); + size_t nlen = strlen(n) + 1; + + depth++; + if (depth == 2 && name_eq(n, "chosen")) + in_chosen = 1; + p += 4 + ((nlen + 3) & ~3); + } else if (token == FDT_END_NODE) { + if (in_chosen && depth == 2) { + ins = p; + break; + } + depth--; + p += 4; + } else if (token == FDT_PROP) { + uint32_t plen = be32(p + 4); + + p += 12 + ((plen + 3) & ~3); + } else if (token == FDT_NOP) { + p += 4; + } else if (token == FDT_END) { + break; + } else { + return -1; + } + } + + if (!ins) + return -2; + + ins = p; + + /* + * the insert: the strings block moves up by prop_size so the + * struct block can grow into its old place, the struct tail + * after chosen moves up by prop_size, the new name lands at + * the end of the moved strings block, and totalsize covers + * both. prop name offsets are strings relative so they keep + * resolving after the move. + */ + { + prop_size = 12 + ((len + 3) & ~3); + size_t strings_len = (size_t)be32(basep + 32); + + /* strings block up by prop_size */ + for (size_t i = strings_len; i > 0; i--) + basep[off_strings + prop_size + i - 1] = + basep[off_strings + i - 1]; + + /* struct tail after the insert point up by prop_size */ + { + size_t tail = (size_t)(basep + off_strings - ins); + + for (size_t i = tail; i > 0; i--) + ins[i + prop_size - 1] = ins[i - 1]; + } + + /* the prop token, name offset = old strings length */ + put_be32(ins, FDT_PROP); + put_be32(ins + 4, (uint32_t)len); + put_be32(ins + 8, (uint32_t)strings_len); + for (size_t i = 0; i < len; i++) + ins[12 + i] = ((const uint8_t *)val)[i]; + for (size_t i = len; i < ((len + 3) & ~3); i++) + ins[12 + i] = 0; + + /* the name at the end of the moved strings block */ + for (size_t i = 0; i < name_len; i++) + basep[off_strings + prop_size + strings_len + i] = + name[i]; + + /* + * size_dt_struct bounds the token walk, libfdt + * rejects anything past it as BADSTRUCTURE. it grows + * by the prop size here, the strings size by the name + * length, totalsize by both. + */ + put_be32(basep + 4, totalsize + (uint32_t)prop_size + + (uint32_t)name_len); + put_be32(basep + 12, off_strings + (uint32_t)prop_size); + put_be32(basep + 36, be32(basep + 36) + (uint32_t)prop_size); + /* + * size_dt_strings must grow too, libfdt validates + * name offsets against it and rejects the whole tree + * when the new names sit past the declared end. the + * kernel's early parser is the same libfdt, a stale + * field there means no memory node and a page table + * panic before the first print. + */ + put_be32(basep + 32, (uint32_t)strings_len + + (uint32_t)name_len); + } + + return 0; +} diff --git a/common/dtb_reloc.c b/common/dtb_reloc.c new file mode 100644 index 0000000..c3e0fe5 --- /dev/null +++ b/common/dtb_reloc.c @@ -0,0 +1,69 @@ +/* + * dtb_reloc.c - grow the devicetree the way libfdt does, in a + * buffer with room to spare. firmware cannot edit a packed fdt + * in place, new properties shift everything behind them, so the + * blob is copied into scratch verbatim, the free space after + * totalsize is the room the insert code shifts into, then the + * walkers patch the copy and the kernel gets its address. + * + * The layout follows the devicetree specification: header, + * struct block, strings block, free space. The rebuild copies + * header, struct, strings, fixes the offsets in the new header, + * and leaves the gap between struct and strings as the room new + * properties will consume. + * + * Copyright (C) 2026 Bradley Morgan <brads@mainlining.org> + */ + +#include <string.h> +#include <endian.h> +#include <boot.h> +#include <dtb_patch.h> + +#define FDT_BEGIN_NODE 1 +#define FDT_END_NODE 2 +#define FDT_PROP 3 +#define FDT_NOP 4 +#define FDT_END 9 + +static uint32_t be32(const void *p) +{ + const uint8_t *b = p; + + return ((uint32_t)b[0] << 24) | ((uint32_t)b[1] << 16) | + ((uint32_t)b[2] << 8) | (uint32_t)b[3]; +} + +/* + * copy the blob into the scratch, grow bytes of headroom after + * the end. returns the new blob address or 0 on a short buffer. + */ +uintptr_t tb_dtb_relocate(uintptr_t dtb, void *scratch, size_t scratch_size, + size_t grow) +{ + uint8_t *in = (uint8_t *)dtb; + uint8_t *out = scratch; + uint32_t totalsize; + + if (be32(in) != 0xd00dfeed) + return 0; + + totalsize = be32(in + 4); + + if (scratch_size < (size_t)totalsize + grow) + return 0; + + /* + * verbatim copy, byte for byte. the grow room is the free + * scratch after totalsize, the insert code shifts the + * strings block into it. an interior gap between the + * struct and strings blocks only invites the walkers to + * count it as tree. + */ + for (uint32_t i = 0; i < totalsize; i++) + out[i] = in[i]; + + (void)grow; + + return (uintptr_t)out; +} diff --git a/common/load.c b/common/load.c index 56fc96a..3a0f8b6 100644 --- a/common/load.c +++ b/common/load.c @@ -60,7 +60,7 @@ int tb_load_semihosting(const char *fname, uintptr_t load_addr, * the initrd path, no header, no placement math, bytes to the * address the dtb /chosen already names. */ -int tb_load_raw(const char *fname, uintptr_t load_addr) +int tb_load_raw(const char *fname, uintptr_t load_addr, size_t *sizep) { long fd, len, ret; @@ -80,5 +80,7 @@ int tb_load_raw(const char *fname, uintptr_t load_addr) if (ret != len) return -6; + if (sizep) + *sizep = (size_t)len; return 0; } diff --git a/common/main.c b/common/main.c index cd234bb..a8fdf58 100644 --- a/common/main.c +++ b/common/main.c @@ -43,10 +43,6 @@ extern int tb_console_init(void); /* - * fixed load address, the osdev way. past the bootloader at the - * bottom of RAM, the image header decides its final resting place. - */ -/* * the payload goes 16MB clear of wherever this bootloader is * actually running, ADR knows the runtime base and qemu is free * to place us anywhere. hardcoding 0x40200000 smashed our own @@ -59,6 +55,11 @@ extern char __image_copy_end[]; #define TB_BOOTFILE "Image" extern void __NO_RETURN tb_boot_linux(uintptr_t ep, uintptr_t fw_arg); +extern uintptr_t tb_dtb_relocate(uintptr_t dtb, void *scratch, + size_t scratch_size, size_t grow); +extern int tb_dtb_add_chosen_prop(uintptr_t dtb, const char *name, + const void *val, size_t len); +static size_t initrd_size; void tashaboot_main(uintptr_t fw_arg) { @@ -141,22 +142,72 @@ void tashaboot_main(uintptr_t fw_arg) } /* + * firmware owns the devicetree it hands the kernel. ours + * relocates into scratch with grow room, then the chosen + * properties are added there and the kernel gets the new + * address, the same flow libfdt firmware uses. + */ + { + /* + * the scratch lives at a fixed free address, clear of + * our image, the payload, and the kernel relocation + * zone. a bss array would sit inside 0x40080000+ and + * the kernel overwrites it while copying itself. + */ + uint8_t *dtb_scratch = (uint8_t *)0x45000000ULL; + uintptr_t newdtb; + + newdtb = tb_dtb_relocate(fw_arg, dtb_scratch, + 0x10000, 0x200); + if (!newdtb) { + dprintf(ALWAYS, "dtb: relocate failed\n"); + platform_halt(); + } + + fw_arg = newdtb; + } + + /* * the initrd rides after the kernel, the dtb /chosen carries * linux,initrd-start and -end, both already patched in place * with this layout. */ { - extern int tb_load_raw(const char *fname, - uintptr_t load_addr); - int r = tb_load_raw(TB_INITRD_FILE, TB_INITRD_ADDR); + int r = tb_load_raw(TB_INITRD_FILE, TB_INITRD_ADDR, + &initrd_size); if (r == 0) - dprintf(ALWAYS, "initrd at %lx\n", - (unsigned long)TB_INITRD_ADDR); + dprintf(ALWAYS, "initrd at %lx, %lx bytes\n", + (unsigned long)TB_INITRD_ADDR, + (unsigned long)initrd_size); else dprintf(ALWAYS, "no initrd (%d)\n", r); } + /* + * the chosen properties, written now that the initrd size + * is known. the cells are big endian, the fdt is a big + * endian format end to end. + */ + { + uint8_t start_cells[8], end_cells[8]; + uint64_t start = TB_INITRD_ADDR; + uint64_t end = TB_INITRD_ADDR + initrd_size; + int a, b; + + for (int i = 0; i < 8; i++) { + start_cells[i] = (uint8_t)(start >> (56 - 8 * i)); + end_cells[i] = (uint8_t)(end >> (56 - 8 * i)); + } + a = tb_dtb_add_chosen_prop(fw_arg, "linux,initrd-start", + start_cells, 8); + b = tb_dtb_add_chosen_prop(fw_arg, "linux,initrd-end", + end_cells, 8); + dprintf(ALWAYS, "dtb: initrd props %d %d\n", a, b); + } + + + dprintf(ALWAYS, "loaded %llu bytes at %lx, entry %lx\n", (unsigned long long)img.size, img.load, img.ep); dprintf(ALWAYS, "jumping\n"); |