diff --git a/common/fdt_wrappers.c b/common/fdt_wrappers.c index 2a9673f01..1b065b1e2 100644 --- a/common/fdt_wrappers.c +++ b/common/fdt_wrappers.c @@ -1,5 +1,5 @@ /* - * Copyright (c) 2018-2021, ARM Limited and Contributors. All rights reserved. + * Copyright (c) 2018-2022, ARM Limited and Contributors. All rights reserved. * * SPDX-License-Identifier: BSD-3-Clause */ @@ -618,3 +618,24 @@ int fdtw_for_each_cpu(const void *dtb, return ret; } + +/* + * Find a given node in device tree. If not present, add it. + * Returns offset of node found/added on success, and < 0 on error. + */ +int fdtw_find_or_add_subnode(void *fdt, int parentoffset, const char *name) +{ + int offset; + + offset = fdt_subnode_offset(fdt, parentoffset, name); + + if (offset == -FDT_ERR_NOTFOUND) { + offset = fdt_add_subnode(fdt, parentoffset, name); + } + + if (offset < 0) { + ERROR("%s: %s: %s\n", __func__, name, fdt_strerror(offset)); + } + + return offset; +} diff --git a/include/common/fdt_wrappers.h b/include/common/fdt_wrappers.h index 9c7180c5e..2929fc23d 100644 --- a/include/common/fdt_wrappers.h +++ b/include/common/fdt_wrappers.h @@ -1,5 +1,5 @@ /* - * Copyright (c) 2018-2021, ARM Limited and Contributors. All rights reserved. + * Copyright (c) 2018-2022, ARM Limited and Contributors. All rights reserved. * * SPDX-License-Identifier: BSD-3-Clause */ @@ -44,6 +44,8 @@ uint64_t fdtw_translate_address(const void *dtb, int bus_node, int fdtw_for_each_cpu(const void *fdt, int (*callback)(const void *dtb, int node, uintptr_t mpidr)); +int fdtw_find_or_add_subnode(void *fdt, int parentoffset, const char *name); + static inline uint32_t fdt_blob_size(const void *dtb) { const uint32_t *dtb_header = dtb;