123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682 |
- #include "ufdt_overlay.h"
- #include "libufdt.h"
- #include "ufdt_node_pool.h"
- #include "ufdt_overlay_internal.h"
- static void fdt_increase_u32(void *pos, uint32_t offset) {
- uint32_t val;
- dto_memcpy(&val, pos, sizeof(val));
- val = cpu_to_fdt32(fdt32_to_cpu(val) + offset);
- dto_memcpy(pos, &val, sizeof(val));
- }
- uint32_t ufdt_get_max_phandle(struct ufdt *tree) {
- struct ufdt_static_phandle_table sorted_table = tree->phandle_table;
- if (sorted_table.len > 0)
- return sorted_table.data[sorted_table.len - 1].phandle;
- else
- return 0;
- }
- static void ufdt_node_try_increase_phandle(struct ufdt_node *node,
- uint32_t offset) {
- int len = 0;
- char *prop_data = ufdt_node_get_fdt_prop_data_by_name(node, "phandle", &len);
- if (prop_data != NULL && len == sizeof(fdt32_t)) {
- fdt_increase_u32(prop_data, offset);
- }
- prop_data = ufdt_node_get_fdt_prop_data_by_name(node, "linux,phandle", &len);
- if (prop_data != NULL && len == sizeof(fdt32_t)) {
- fdt_increase_u32(prop_data, offset);
- }
- }
- void ufdt_try_increase_phandle(struct ufdt *tree, uint32_t offset) {
- struct ufdt_static_phandle_table sorted_table = tree->phandle_table;
- int i;
- for (i = 0; i < sorted_table.len; i++) {
- struct ufdt_node *target_node = sorted_table.data[i].node;
- ufdt_node_try_increase_phandle(target_node, offset);
- }
- }
- void *ufdt_get_fixup_location(struct ufdt *tree, const char *fixup) {
- char *path, *prop_ptr, *offset_ptr, *end_ptr;
- int prop_offset, prop_len;
- const char *prop_data;
- char path_buf[1024];
- char *path_mem = NULL;
- size_t fixup_len = strlen(fixup) + 1;
- if (fixup_len > sizeof(path_buf)) {
- path_mem = dto_malloc(fixup_len);
- path = path_mem;
- } else {
- path = path_buf;
- }
- dto_memcpy(path, fixup, fixup_len);
- prop_ptr = dto_strchr(path, ':');
- if (prop_ptr == NULL) {
- dto_error("Missing property part in '%s'\n", path);
- goto fail;
- }
- *prop_ptr = '\0';
- prop_ptr++;
- offset_ptr = dto_strchr(prop_ptr, ':');
- if (offset_ptr == NULL) {
- dto_error("Missing offset part in '%s'\n", path);
- goto fail;
- }
- *offset_ptr = '\0';
- offset_ptr++;
- prop_offset = dto_strtoul(offset_ptr, &end_ptr, 10 );
- if (*end_ptr != '\0') {
- dto_error("'%s' is not valid number\n", offset_ptr);
- goto fail;
- }
- struct ufdt_node *target_node;
- target_node = ufdt_get_node_by_path(tree, path);
- if (target_node == NULL) {
- dto_error("Path '%s' not found\n", path);
- goto fail;
- }
- prop_data =
- ufdt_node_get_fdt_prop_data_by_name(target_node, prop_ptr, &prop_len);
- if (prop_data == NULL) {
- dto_error("Property '%s' not found in '%s' node\n", prop_ptr, path);
- goto fail;
- }
-
- if (prop_len < prop_offset + (int)sizeof(uint32_t)) {
- dto_error("%s: property length is too small for fixup\n", path);
- goto fail;
- }
- if (path_mem) dto_free(path_mem);
- return (char *)prop_data + prop_offset;
- fail:
- if (path_mem) dto_free(path_mem);
- return NULL;
- }
- int ufdt_do_one_fixup(struct ufdt *tree, const char *fixups, int fixups_len,
- int phandle) {
- void *fixup_pos;
- uint32_t val;
- val = cpu_to_fdt32(phandle);
- while (fixups_len > 0) {
- fixup_pos = ufdt_get_fixup_location(tree, fixups);
- if (fixup_pos != NULL) {
- dto_memcpy(fixup_pos, &val, sizeof(val));
- } else {
- return -1;
- }
- fixups_len -= dto_strlen(fixups) + 1;
- fixups += dto_strlen(fixups) + 1;
- }
- return 0;
- }
- int ufdt_overlay_do_fixups(struct ufdt *main_tree, struct ufdt *overlay_tree) {
- int len = 0;
- struct ufdt_node *overlay_fixups_node =
- ufdt_get_node_by_path(overlay_tree, "/__fixups__");
- if (!overlay_fixups_node) {
-
- return 0;
- }
- struct ufdt_node *main_symbols_node =
- ufdt_get_node_by_path(main_tree, "/__symbols__");
- struct ufdt_node **it;
- for_each_prop(it, overlay_fixups_node) {
-
-
- if (!main_symbols_node) {
- dto_error("No node __symbols__ in main dtb.\n");
- return -1;
- }
- break;
- }
- for_each_prop(it, overlay_fixups_node) {
-
- struct ufdt_node *fixups = *it;
- char *symbol_path = ufdt_node_get_fdt_prop_data_by_name(
- main_symbols_node, ufdt_node_name(fixups), &len);
- if (!symbol_path) {
- dto_error("Couldn't find '%s' symbol in main dtb\n",
- ufdt_node_name(fixups));
- return -1;
- }
- struct ufdt_node *symbol_node;
- symbol_node = ufdt_get_node_by_path(main_tree, symbol_path);
- if (!symbol_node) {
- dto_error("Couldn't find '%s' path in main dtb\n", symbol_path);
- return -1;
- }
- uint32_t phandle = ufdt_node_get_phandle(symbol_node);
- const char *fixups_paths = ufdt_node_get_fdt_prop_data(fixups, &len);
- if (ufdt_do_one_fixup(overlay_tree, fixups_paths, len, phandle) < 0) {
- dto_error("Failed one fixup in ufdt_do_one_fixup\n");
- return -1;
- }
- }
- return 0;
- }
- static int ufdt_overlay_node(struct ufdt_node *target_node,
- struct ufdt_node *overlay_node,
- struct ufdt_node_pool *pool) {
- return ufdt_node_merge_into(target_node, overlay_node, pool);
- }
- enum overlay_result ufdt_overlay_get_target(struct ufdt *tree,
- struct ufdt_node *frag_node,
- struct ufdt_node **target_node) {
- uint32_t target;
- const char *target_path;
- const void *val;
- *target_node = NULL;
- val = ufdt_node_get_fdt_prop_data_by_name(frag_node, "target", NULL);
- if (val) {
- dto_memcpy(&target, val, sizeof(target));
- target = fdt32_to_cpu(target);
- *target_node = ufdt_get_node_by_phandle(tree, target);
- if (*target_node == NULL) {
- dto_error("failed to find target %04x\n", target);
- return OVERLAY_RESULT_TARGET_INVALID;
- }
- }
- if (*target_node == NULL) {
- target_path =
- ufdt_node_get_fdt_prop_data_by_name(frag_node, "target-path", NULL);
- if (target_path == NULL) {
- return OVERLAY_RESULT_MISSING_TARGET;
- }
- *target_node = ufdt_get_node_by_path(tree, target_path);
- if (*target_node == NULL) {
- dto_error("failed to find target-path %s\n", target_path);
- return OVERLAY_RESULT_TARGET_PATH_INVALID;
- }
- }
- return OVERLAY_RESULT_OK;
- }
- static enum overlay_result ufdt_apply_fragment(struct ufdt *tree,
- struct ufdt_node *frag_node,
- struct ufdt_node_pool *pool) {
- struct ufdt_node *target_node = NULL;
- struct ufdt_node *overlay_node = NULL;
- enum overlay_result result =
- ufdt_overlay_get_target(tree, frag_node, &target_node);
- if (target_node == NULL) {
- return result;
- }
- overlay_node = ufdt_node_get_node_by_path(frag_node, "__overlay__");
- if (overlay_node == NULL) {
- dto_error("missing __overlay__ sub-node\n");
- return OVERLAY_RESULT_MISSING_OVERLAY;
- }
- int err = ufdt_overlay_node(target_node, overlay_node, pool);
- if (err < 0) {
- dto_error("failed to overlay node %s to target %s\n",
- ufdt_node_name(overlay_node), ufdt_node_name(target_node));
- return OVERLAY_RESULT_MERGE_FAIL;
- }
- return OVERLAY_RESULT_OK;
- }
- static int ufdt_overlay_apply_fragments(struct ufdt *main_tree,
- struct ufdt *overlay_tree,
- struct ufdt_node_pool *pool) {
- enum overlay_result err;
- struct ufdt_node **it;
-
- for_each_node(it, overlay_tree->root) {
- err = ufdt_apply_fragment(main_tree, *it, pool);
- if (err == OVERLAY_RESULT_MERGE_FAIL) {
- return -1;
- }
- }
- return 0;
- }
- static int ufdt_local_fixup_prop(struct ufdt_node *target_prop_node,
- struct ufdt_node *local_fixup_prop_node,
- uint32_t phandle_offset) {
-
- char *prop_offsets_ptr;
- int len = 0;
- prop_offsets_ptr = ufdt_node_get_fdt_prop_data(local_fixup_prop_node, &len);
- char *prop_data;
- int target_length = 0;
- prop_data = ufdt_node_get_fdt_prop_data(target_prop_node, &target_length);
- if (prop_offsets_ptr == NULL || prop_data == NULL) return -1;
- int i;
- for (i = 0; i < len; i += sizeof(fdt32_t)) {
- int offset = fdt32_to_cpu(*(fdt32_t *)(prop_offsets_ptr + i));
- if (offset + sizeof(fdt32_t) > (size_t)target_length) return -1;
- fdt_increase_u32((prop_data + offset), phandle_offset);
- }
- return 0;
- }
- static int ufdt_local_fixup_node(struct ufdt_node *target_node,
- struct ufdt_node *local_fixups_node,
- uint32_t phandle_offset) {
- if (local_fixups_node == NULL) return 0;
- struct ufdt_node **it_local_fixups;
- struct ufdt_node *sub_target_node;
- for_each_prop(it_local_fixups, local_fixups_node) {
- sub_target_node = ufdt_node_get_property_by_name(
- target_node, ufdt_node_name(*it_local_fixups));
- if (sub_target_node != NULL) {
- int err = ufdt_local_fixup_prop(sub_target_node, *it_local_fixups,
- phandle_offset);
- if (err < 0) return -1;
- } else {
- return -1;
- }
- }
- for_each_node(it_local_fixups, local_fixups_node) {
- sub_target_node = ufdt_node_get_node_by_path(
- target_node, ufdt_node_name(*it_local_fixups));
- if (sub_target_node != NULL) {
- int err = ufdt_local_fixup_node(sub_target_node, *it_local_fixups,
- phandle_offset);
- if (err < 0) return -1;
- } else {
- return -1;
- }
- }
- return 0;
- }
- int ufdt_overlay_do_local_fixups(struct ufdt *tree, uint32_t phandle_offset) {
- struct ufdt_node *overlay_node = ufdt_get_node_by_path(tree, "/");
- struct ufdt_node *local_fixups_node =
- ufdt_get_node_by_path(tree, "/__local_fixups__");
- int err =
- ufdt_local_fixup_node(overlay_node, local_fixups_node, phandle_offset);
- if (err < 0) return -1;
- return 0;
- }
- static int ufdt_overlay_local_ref_update(struct ufdt *main_tree,
- struct ufdt *overlay_tree) {
- uint32_t phandle_offset = 0;
- phandle_offset = ufdt_get_max_phandle(main_tree);
- if (phandle_offset > 0) {
- ufdt_try_increase_phandle(overlay_tree, phandle_offset);
- }
- int err = ufdt_overlay_do_local_fixups(overlay_tree, phandle_offset);
- if (err < 0) {
- dto_error("failed to perform local fixups in overlay\n");
- return -1;
- }
- return 0;
- }
- static int _ufdt_overlay_fdtps(struct ufdt *main_tree,
- const struct ufdt *overlay_tree) {
- for (int i = 0; i < overlay_tree->num_used_fdtps; i++) {
- void *fdt = overlay_tree->fdtps[i];
- if (ufdt_add_fdt(main_tree, fdt) < 0) {
- return -1;
- }
- }
- return 0;
- }
- static int ufdt_overlay_apply(struct ufdt *main_tree, struct ufdt *overlay_tree,
- size_t overlay_length,
- struct ufdt_node_pool *pool) {
- if (_ufdt_overlay_fdtps(main_tree, overlay_tree) < 0) {
- dto_error("failed to add more fdt into main ufdt tree.\n");
- return -1;
- }
- if (overlay_length < sizeof(struct fdt_header)) {
- dto_error("Overlay_length %zu smaller than header size %zu\n",
- overlay_length, sizeof(struct fdt_header));
- return -1;
- }
- if (ufdt_overlay_local_ref_update(main_tree, overlay_tree) < 0) {
- dto_error("failed to perform local fixups in overlay\n");
- return -1;
- }
- if (ufdt_overlay_do_fixups(main_tree, overlay_tree) < 0) {
- dto_error("failed to perform fixups in overlay\n");
- return -1;
- }
- if (ufdt_overlay_apply_fragments(main_tree, overlay_tree, pool) < 0) {
- dto_error("failed to apply fragments\n");
- return -1;
- }
- return 0;
- }
- struct fdt_header *ufdt_install_blob(void *blob, size_t blob_size) {
- struct fdt_header *pHeader;
- int err;
- dto_debug("ufdt_install_blob (0x%08jx)\n", (uintmax_t)blob);
- if (blob_size < sizeof(struct fdt_header)) {
- dto_error("Blob_size %zu smaller than the header size %zu\n", blob_size,
- sizeof(struct fdt_header));
- return NULL;
- }
- pHeader = (struct fdt_header *)blob;
- err = fdt_check_header(pHeader);
- if (err < 0) {
- if (err == -FDT_ERR_BADVERSION) {
- dto_error("incompatible blob version: %d, should be: %d",
- fdt_version(pHeader), FDT_LAST_SUPPORTED_VERSION);
- } else {
- dto_error("error validating blob: %s", fdt_strerror(err));
- }
- return NULL;
- }
- return pHeader;
- }
- struct fdt_header *ufdt_apply_overlay(struct fdt_header *main_fdt_header,
- size_t main_fdt_size,
- void *overlay_fdtp,
- size_t overlay_size) {
- size_t out_fdt_size;
- if (main_fdt_header == NULL) {
- return NULL;
- }
- if (overlay_size < 8 || overlay_size != fdt_totalsize(overlay_fdtp)) {
- dto_error("Bad overlay size!\n");
- return NULL;
- }
- if (main_fdt_size < 8 || main_fdt_size != fdt_totalsize(main_fdt_header)) {
- dto_error("Bad fdt size!\n");
- return NULL;
- }
- out_fdt_size = fdt_totalsize(main_fdt_header) + overlay_size;
-
- struct fdt_header *out_fdt_header = dto_malloc(out_fdt_size);
- if (out_fdt_header == NULL) {
- dto_error("failed to allocate memory for DTB blob with overlays\n");
- return NULL;
- }
- struct ufdt_node_pool pool;
- ufdt_node_pool_construct(&pool);
- struct ufdt *main_tree = ufdt_from_fdt(main_fdt_header, main_fdt_size, &pool);
- struct ufdt *overlay_tree = ufdt_from_fdt(overlay_fdtp, overlay_size, &pool);
- int err = ufdt_overlay_apply(main_tree, overlay_tree, overlay_size, &pool);
- if (err < 0) {
- goto fail;
- }
- err = ufdt_to_fdt(main_tree, out_fdt_header, out_fdt_size);
- if (err < 0) {
- dto_error("Failed to dump the device tree to out_fdt_header\n");
- goto fail;
- }
- ufdt_destruct(overlay_tree, &pool);
- ufdt_destruct(main_tree, &pool);
- ufdt_node_pool_destruct(&pool);
- return out_fdt_header;
- fail:
- ufdt_destruct(overlay_tree, &pool);
- ufdt_destruct(main_tree, &pool);
- ufdt_node_pool_destruct(&pool);
- dto_free(out_fdt_header);
- return NULL;
- }
|