diff options
Diffstat (limited to 'common')
| -rw-r--r-- | common/dtb_find.c | 178 | ||||
| -rw-r--r-- | common/main.c | 26 |
2 files changed, 204 insertions, 0 deletions
diff --git a/common/dtb_find.c b/common/dtb_find.c new file mode 100644 index 0000000..29a67f3 --- /dev/null +++ b/common/dtb_find.c @@ -0,0 +1,178 @@ +/* + * dtb_find.c - locate nodes and read reg by walking the flat + * devicetree. the machine tells the firmware where its devices + * live, a bootloader that hardcodes the gic address breaks on + * the first board with a different map. + * + * the walk is the standard token scan, FDT_BEGIN_NODE with a + * matching name at any depth, then the reg property inside, + * the first address/size pair decoded per the parent's cell + * counts, which the root carries in #address-cells and + * #size-cells. + * + * Copyright (C) 2026 Bradley Morgan <brads@mainlining.org> + */ + +#include <string.h> +#include <stdint.h> +#include <boot.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 int name_eq(const char *a, const char *b) +{ + while (*a && *a != '@') { + if (*a != *b) + return 0; + a++; + b++; + } + return *b == '\0' || *b == '@'; +} + +/* + * find the first node whose name matches, at any depth. returns + * the offset of its FDT_BEGIN_NODE token or 0 when absent. + */ +static uint32_t fdt_find_node(uintptr_t dtb, const char *name) +{ + uint8_t *basep = (uint8_t *)dtb; + uint32_t off_struct = be32(basep + 8); + uint32_t totalsize = be32(basep + 4); + uint8_t *p = basep + off_struct; + + if (be32(basep) != 0xd00dfeed) + return 0; + + 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; + + if (name_eq(n, name)) + return (uint32_t)(p - basep); + p += 4 + ((nlen + 3) & ~3); + } else if (token == FDT_PROP) { + uint32_t plen = be32(p + 4); + + p += 12 + ((plen + 3) & ~3); + } else if (token == FDT_END_NODE || + token == FDT_NOP) { + p += 4; + } else if (token == FDT_END) { + break; + } else { + return 0; + } + } + + return 0; +} + +/* + * read the first reg pair of a node at the given token offset, + * honoring the root cell counts. pairs of 2 or 4 cells are the + * ones machines carry, anything else fails. the caller reads + * more pairs off the returned cursor if it needs them. + */ +int tb_dtb_reg0(uintptr_t dtb, uint32_t node_off, uintptr_t *addr, + size_t *size) +{ + uint8_t *basep = (uint8_t *)dtb; + uint32_t off_strings = be32(basep + 12); + uint8_t *p = basep + node_off; + uint32_t totalsize = be32(basep + 4); + uint32_t ac = 2; + uint32_t sc = 2; + /* + * zero, the node's own FDT_BEGIN_NODE below brings it to + * one and the props inside sit at depth one. starting at + * one instead skips every prop in the node. + */ + int depth_open = 0; + + 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_open++; + p += 4 + ((nlen + 3) & ~3); + } else if (token == FDT_END_NODE) { + depth_open--; + if (!depth_open) + return -1; + p += 4; + } else if (token == FDT_PROP) { + uint32_t plen = be32(p + 4); + const char *pname = + (char *)basep + off_strings + be32(p + 8); + uint8_t *val = p + 12; + + if (depth_open == 1 && + strcmp(pname, "#address-cells") == 0) + ac = be32(val); + if (depth_open == 1 && + strcmp(pname, "#size-cells") == 0) + sc = be32(val); + if (depth_open == 1 && strcmp(pname, "reg") == 0) { + if (plen >= (ac + sc) * 4) { + uint64_t a = 0; + uint64_t s = 0; + + for (uint32_t i = 0; i < ac; i++) + a = (a << 32) | + be32(val + i * 4); + for (uint32_t i = 0; i < sc; i++) + s = (s << 32) | + be32(val + (ac + i) * 4); + *addr = (uintptr_t)a; + if (size) + *size = (size_t)s; + return 0; + } + return -1; + } + p += 12 + ((plen + 3) & ~3); + } else if (token == FDT_NOP) { + p += 4; + } else if (token == FDT_END) { + return -1; + } else { + return -1; + } + } + + return -1; +} + +/* + * the whole lookup in one call: find the node, read its first + * reg pair. + */ +int tb_dtb_find_reg0(uintptr_t dtb, const char *name, uintptr_t *addr, + size_t *size) +{ + uint32_t off = fdt_find_node(dtb, name); + + if (!off) + return -1; + + return tb_dtb_reg0(dtb, off, addr, size); +} diff --git a/common/main.c b/common/main.c index a8fdf58..a0237af 100644 --- a/common/main.c +++ b/common/main.c @@ -57,6 +57,7 @@ extern char __image_copy_end[]; 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_gic_init(uintptr_t gicd, uintptr_t gicc); extern int tb_dtb_add_chosen_prop(uintptr_t dtb, const char *name, const void *val, size_t len); static size_t initrd_size; @@ -165,6 +166,31 @@ void tashaboot_main(uintptr_t fw_arg) } fw_arg = newdtb; + + /* + * the interrupt controller the machine told us + * about, found by name, the reg pair read with the + * root cell counts. the gic goes into the defined + * off state before the kernel brings its own irq + * handling up. + */ + { + extern int tb_dtb_find_reg0(uintptr_t dtb, + const char *name, + uintptr_t *addr, + size_t *size); + uintptr_t gicd = 0; + uintptr_t gicc = 0; + size_t sz = 0; + + if (tb_dtb_find_reg0(fw_arg, "intc", &gicd, &sz) == 0 && + sz >= 0x10000) { + gicc = gicd + 0x10000; + tb_gic_init(gicd, gicc); + dprintf(ALWAYS, "gic: %lx off\n", + (unsigned long)gicd); + } + } } /* |