/* $OpenBSD: fdt.c,v 1.10 2023/02/13 16:16:03 kettenis Exp $ */ /* * Copyright (c) 2009 Dariusz Swiderski * Copyright (c) 2009, 2016 Mark Kettenis * * Permission to use, copy, modify, and distribute this software for any * purpose with or without fee is hereby granted, provided that the above * copyright notice and this permission notice appear in all copies. * * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. */ #include #include #include #include "fdt.h" unsigned int fdt_check_head(void *); char *fdt_get_str(uint32_t); void *skip_property(uint32_t *); void *skip_props(uint32_t *); void *skip_node_name(uint32_t *); void *skip_node(void *); void *skip_nops(uint32_t *); void *fdt_parent_node_recurse(void *, void *); static int tree_inited = 0; static struct fdt tree; unsigned int fdt_check_head(void *fdt) { struct fdt_head *fh; uint32_t *ptr, *tok; fh = fdt; ptr = (uint32_t *)fdt; if (betoh32(fh->fh_magic) != FDT_MAGIC) return 0; if (betoh32(fh->fh_version) > FDT_CODE_VERSION) return 0; tok = skip_nops(ptr + (betoh32(fh->fh_struct_off) / 4)); if (betoh32(*tok) != FDT_NODE_BEGIN) return 0; /* check for end signature on version 17 blob */ if ((betoh32(fh->fh_version) >= 17) && (betoh32(*(ptr + (betoh32(fh->fh_struct_off) / 4) + (betoh32(fh->fh_struct_size) / 4) - 1)) != FDT_END)) return 0; return betoh32(fh->fh_version); } /* * Initializes internal structures of module. * Has to be called once. */ int fdt_init(void *fdt) { int version; memset(&tree, 0, sizeof(struct fdt)); tree_inited = 0; if (!fdt) return 0; if (!(version = fdt_check_head(fdt))) return 0; tree.header = (struct fdt_head *)fdt; tree.tree = (char *)fdt + betoh32(tree.header->fh_struct_off); tree.strings = (char *)fdt + betoh32(tree.header->fh_strings_off); tree.memory = (char *)fdt + betoh32(tree.header->fh_reserve_off); tree.end = (char *)fdt + betoh32(tree.header->fh_size); tree.version = version; tree.strings_size = betoh32(tree.header->fh_strings_size); if (tree.version >= 17) tree.struct_size = betoh32(tree.header->fh_struct_size); tree_inited = 1; return version; } void fdt_finalize(void) { char *start = (char *)tree.header; tree.header->fh_size = htobe32(tree.end - start); tree.header->fh_struct_off = htobe32(tree.tree - start); tree.header->fh_strings_off = htobe32(tree.strings - start); tree.header->fh_reserve_off = htobe32(tree.memory - start); tree.header->fh_strings_size = htobe32(tree.strings_size); if (tree.version >= 17) tree.header->fh_struct_size = htobe32(tree.struct_size); } /* * Return the size of the FDT. */ size_t fdt_get_size(void *fdt) { if (!fdt) return 0; if (!fdt_check_head(fdt)) return 0; return betoh32(((struct fdt_head *)fdt)->fh_size); } /* * Retrieve string pointer from strings table. */ char * fdt_get_str(uint32_t num) { if (num > tree.strings_size) return NULL; return (tree.strings) ? (tree.strings + num) : NULL; } int fdt_add_str(char *name) { size_t len = roundup(strlen(name) + 1, sizeof(uint32_t)); char *end = tree.strings + tree.strings_size; if (end + len > tree.end) panic("FDT overflow"); tree.strings_size += len; memset(end, 0, len); memcpy(end, name, strlen(name)); return (end - tree.strings); } /* * Utility functions for skipping parts of tree. */ void * skip_nops(uint32_t *ptr) { while (betoh32(*ptr) == FDT_NOP) ptr++; return ptr; } void * skip_property(uint32_t *ptr) { uint32_t size; size = betoh32(*(ptr + 1)); /* move forward by magic + size + nameid + rounded up property size */ ptr += 3 + roundup(size, sizeof(uint32_t)) / sizeof(uint32_t); return skip_nops(ptr); } void * skip_props(uint32_t *ptr) { while (betoh32(*ptr) == FDT_PROPERTY) { ptr = skip_property(ptr); } return ptr; } void * skip_node_name(uint32_t *ptr) { /* skip name, aligned to 4 bytes, this is NULL term., so must add 1 */ ptr += roundup(strlen((char *)ptr) + 1, sizeof(uint32_t)) / sizeof(uint32_t); return skip_nops(ptr); } /* * Retrieves node property, the returned pointer is inside the fdt tree, * so we should not modify content pointed by it directly. */ int fdt_node_property(void *node, char *name, char **out) { uint32_t *ptr; uint32_t nameid; char *tmp; if (!tree_inited) return 0; ptr = (uint32_t *)node; if (betoh32(*ptr) != FDT_NODE_BEGIN) return 0; ptr = skip_node_name(ptr + 1); while (betoh32(*ptr) == FDT_PROPERTY) { nameid = betoh32(*(ptr + 2)); /* id of name in strings table */ tmp = fdt_get_str(nameid); if (!strcmp(name, tmp)) { *out = (char *)(ptr + 3); /* beginning of the value */ return betoh32(*(ptr + 1)); /* size of value */ } ptr = skip_property(ptr); } return 0; } int fdt_node_set_property(void *node, char *name, void *data, int len) { char *end = tree.strings + tree.strings_size; uint32_t *ptr, *next; uint32_t nameid; uint32_t curlen; size_t delta; char *tmp; if (!tree_inited) return 0; ptr = (uint32_t *)node; if (betoh32(*ptr) != FDT_NODE_BEGIN) return 0; ptr = skip_node_name(ptr + 1); while (betoh32(*ptr) == FDT_PROPERTY) { nameid = betoh32(*(ptr + 2)); /* id of name in strings table */ tmp = fdt_get_str(nameid); next = skip_property(ptr); if (!strcmp(name, tmp)) { curlen = betoh32(*(ptr + 1)); delta = roundup(len, sizeof(uint32_t)) - roundup(curlen, sizeof(uint32_t)); if (end + delta > tree.end) panic("FDT overflow"); memmove((char *)next + delta, next, end - (char *)next); tree.struct_size += delta; tree.strings += delta; *(ptr + 1) = htobe32(len); memcpy(ptr + 3, data, len); return 1; } ptr = next; } return 0; } int fdt_node_add_property(void *node, char *name, void *data, int len) { char *end = tree.strings + tree.strings_size; char *dummy; if (!tree_inited) return 0; if (!fdt_node_property(node, name, &dummy)) { uint32_t *ptr = (uint32_t *)node; if (betoh32(*ptr) != FDT_NODE_BEGIN) return 0; if (end + 3 * sizeof(uint32_t) > tree.end) panic("FDT overflow"); ptr = skip_node_name(ptr + 1); memmove(ptr + 3, ptr, end - (char *)ptr); tree.struct_size += 3 * sizeof(uint32_t); tree.strings += 3 * sizeof(uint32_t); *ptr++ = htobe32(FDT_PROPERTY); *ptr++ = htobe32(0); *ptr++ = htobe32(fdt_add_str(name)); } return fdt_node_set_property(node, name, data, len); } int fdt_node_add_node(void *node, char *name, void **child) { size_t len = roundup(strlen(name) + 1, sizeof(uint32_t)) + 8; char *end = tree.strings + tree.strings_size; uint32_t *ptr = (uint32_t *)node; if (!tree_inited) return 0; if (betoh32(*ptr) != FDT_NODE_BEGIN) return 0; if (end + len > tree.end) panic("FDT overflow"); ptr = skip_node_name(ptr + 1); ptr = skip_props(ptr); /* skip children */ while (betoh32(*ptr) == FDT_NODE_BEGIN) ptr = skip_node(ptr); memmove((char *)ptr + len, ptr, end - (char *)ptr); tree.struct_size += len; tree.strings += len; *child = ptr; *ptr++ = htobe32(FDT_NODE_BEGIN); memset(ptr, 0, len - 8); memcpy(ptr, name, strlen(name)); ptr += (len - 8) / sizeof(uint32_t); *ptr++ = htobe32(FDT_NODE_END); return 1; } /* * Retrieves next node, skipping all the children nodes of the pointed node, * returns pointer to next node, no matter if it exists or not. */ void * skip_node(void *node) { uint32_t *ptr = node; ptr++; ptr = skip_node_name(ptr); ptr = skip_props(ptr); /* skip children */ while (betoh32(*ptr) == FDT_NODE_BEGIN) ptr = skip_node(ptr); return skip_nops(ptr + 1); } /* * Retrieves next node, skipping all the children nodes of the pointed node, * returns pointer to next node if exists, otherwise returns NULL. * If passed 0 will return first node of the tree (root). */ void * fdt_next_node(void *node) { uint32_t *ptr; if (!tree_inited) return NULL; ptr = node; if (node == NULL) { ptr = skip_nops((uint32_t *)tree.tree); return (betoh32(*ptr) == FDT_NODE_BEGIN) ? ptr : NULL; } if (betoh32(*ptr) != FDT_NODE_BEGIN) return NULL; ptr++; ptr = skip_node_name(ptr); ptr = skip_props(ptr); /* skip children */ while (betoh32(*ptr) == FDT_NODE_BEGIN) ptr = skip_node(ptr); if (betoh32(*ptr) != FDT_NODE_END) return NULL; ptr = skip_nops(ptr + 1); if (betoh32(*ptr) != FDT_NODE_BEGIN) return NULL; return ptr; } /* * Retrieves node property as integers and puts them in the given * integer array. */ int fdt_node_property_ints(void *node, char *name, int *out, int outlen) { int *data; int i, inlen; inlen = fdt_node_property(node, name, (char **)&data) / sizeof(int); if (inlen <= 0) return -1; for (i = 0; i < inlen && i < outlen; i++) out[i] = betoh32(data[i]); return i; } /* * Retrieves node property as an integer. */ int fdt_node_property_int(void *node, char *name, int *out) { return fdt_node_property_ints(node, name, out, 1); } /* * Retrieves next node, skipping all the children nodes of the pointed node */ void * fdt_child_node(void *node) { uint32_t *ptr; if (!tree_inited) return NULL; ptr = node; if (betoh32(*ptr) != FDT_NODE_BEGIN) return NULL; ptr++; ptr = skip_node_name(ptr); ptr = skip_props(ptr); /* check if there is a child node */ return (betoh32(*ptr) == FDT_NODE_BEGIN) ? (ptr) : NULL; } /* * Retrieves node name. */ char * fdt_node_name(void *node) { uint32_t *ptr; if (!tree_inited) return NULL; ptr = node; if (betoh32(*ptr) != FDT_NODE_BEGIN) return NULL; return (char *)(ptr + 1); } void * fdt_find_node(char *name) { void *node = fdt_next_node(0); const char *p = name; if (!tree_inited) return NULL; if (*p != '/') return NULL; while (*p) { void *child; const char *q; while (*p == '/') p++; if (*p == 0) return node; q = strchr(p, '/'); if (q == NULL) q = p + strlen(p); for (child = fdt_child_node(node); child; child = fdt_next_node(child)) { if (strncmp(p, fdt_node_name(child), q - p) == 0) { node = child; break; } } if (child == NULL) return NULL; /* No match found. */ p = q; } return node; } void * fdt_parent_node_recurse(void *pnode, void *child) { void *node = fdt_child_node(pnode); void *tmp; while (node && (node != child)) { if ((tmp = fdt_parent_node_recurse(node, child))) return tmp; node = fdt_next_node(node); } return (node) ? pnode : NULL; } void * fdt_parent_node(void *node) { void *pnode = fdt_next_node(0); if (!tree_inited) return NULL; if (node == pnode) return NULL; return fdt_parent_node_recurse(pnode, node); } int fdt_node_is_compatible(void *node, const char *name) { char *data; int len; len = fdt_node_property(node, "compatible", &data); while (len > 0) { if (strcmp(data, name) == 0) return 1; len -= strlen(data) + 1; data += strlen(data) + 1; } return 0; } #ifdef DEBUG /* * Debug methods for printing whole tree, particular nodes and properties */ void * fdt_print_property(void *node, int level) { uint32_t *ptr; char *tmp, *value; int cnt; uint32_t nameid, size; ptr = (uint32_t *)node; if (!tree_inited) return NULL; if (betoh32(*ptr) != FDT_PROPERTY) return ptr; /* should never happen */ /* extract property name_id and size */ size = betoh32(*++ptr); nameid = betoh32(*++ptr); for (cnt = 0; cnt < level; cnt++) printf("\t"); tmp = fdt_get_str(nameid); printf("\t%s : ", tmp ? tmp : "NO_NAME"); ptr++; value = (char *)ptr; if (!strcmp(tmp, "device_type") || !strcmp(tmp, "compatible") || !strcmp(tmp, "model") || !strcmp(tmp, "bootargs") || !strcmp(tmp, "linux,stdout-path")) { printf("%s", value); } else if (!strcmp(tmp, "clock-frequency") || !strcmp(tmp, "timebase-frequency")) { printf("%d", betoh32(*((unsigned int *)value))); } else { for (cnt = 0; cnt < size; cnt++) { if ((cnt % sizeof(uint32_t)) == 0) printf(" "); printf("%x%x", value[cnt] >> 4, value[cnt] & 0xf); } } ptr += roundup(size, sizeof(uint32_t)) / sizeof(uint32_t); printf("\n"); return ptr; } void fdt_print_node(void *node, int level) { uint32_t *ptr; int cnt; ptr = (uint32_t *)node; if (betoh32(*ptr) != FDT_NODE_BEGIN) return; ptr++; for (cnt = 0; cnt < level; cnt++) printf("\t"); printf("%s :\n", fdt_node_name(node)); ptr = skip_node_name(ptr); while (betoh32(*ptr) == FDT_PROPERTY) ptr = fdt_print_property(ptr, level); } void fdt_print_node_recurse(void *node, int level) { void *child; fdt_print_node(node, level); for (child = fdt_child_node(node); child; child = fdt_next_node(child)) fdt_print_node_recurse(child, level + 1); } void fdt_print_tree(void) { fdt_print_node_recurse(fdt_next_node(0), 0); } #endif