From dea8ee0d3f13f8d1638745b76e86bd7617bf92e7 Mon Sep 17 00:00:00 2001
From: Ruchika Gupta <ruchika.gupta@linaro.org>
Date: Fri, 8 Apr 2022 13:16:16 +0530
Subject: [PATCH] feat(fdt-wrappers): add function to find or add a sudnode

This change adds a new utility function - `fdtw_find_or_add_subnode`
to find a subnode. If the subnode is not present, the function adds
it in the flattened device tree.

Signed-off-by: Ruchika Gupta <ruchika.gupta@linaro.org>
Change-Id: Idf3ceddc57761ac015763d4a8b004877bcad766a
---
 common/fdt_wrappers.c         | 23 ++++++++++++++++++++++-
 include/common/fdt_wrappers.h |  4 +++-
 2 files changed, 25 insertions(+), 2 deletions(-)

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;