[PATCH 09/23] pbl: fdt: add pbl_load_fdt helper

[Marco Felsch <m.felsch@pengutronix.de>]

The helper provides a common FDT load mechanism to load the (compressed)
DTB to the provided destination address. Early FDTs may required by
other BL3x binaries like BL31 (TF-A) and BL32 (OP-TEE).

Signed-off-by: Marco Felsch <m.felsch@pengutronix.de>
---
 arch/Kconfig  |  3 +++
 include/pbl.h |  2 ++
 pbl/Kconfig   | 11 +++++++++
 pbl/fdt.c     | 75 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 4 files changed, 91 insertions(+)

diff --git a/arch/Kconfig b/arch/Kconfig
index 55618bf896c25f2afa2786706e27fce6ab4b3a39..edd574879b244d3843f1b5a06e1e25c5d4acbc8d 100644
--- a/arch/Kconfig
+++ b/arch/Kconfig
@@ -62,6 +62,9 @@ config ARCH_DMA_DEFAULT_COHERENT
 config ARCH_HAS_DMA_WRITE_COMBINE
 	bool
 
+config ARCH_HAS_EARLY_FDT_SUPPORT
+	bool
+
 config ARCH_HAS_ASAN_FIBER_API
 	bool
 
diff --git a/include/pbl.h b/include/pbl.h
index 9db4652e833bf5e82fd42a1cae19e94c4f2f868f..bf0c9e2ba29428361cb16a572295e66f20acdd82 100644
--- a/include/pbl.h
+++ b/include/pbl.h
@@ -31,6 +31,8 @@ fdt_device_get_match_data(const void *fdt, const char *nodepath,
 
 int pbl_barebox_verify(const void *compressed_start, unsigned int len,
 		       const void *hash, unsigned int hash_len);
+int pbl_load_fdt(const void *fdt, void *dest, int destsize);
+
 #endif
 
 void __noreturn barebox_pbl_entry(ulong, ulong, void *);
diff --git a/pbl/Kconfig b/pbl/Kconfig
index cab9325d16e8625bcca10125b3281062abffedbc..ea2086f33ea405ecd7b00065eaf9874218152b3a 100644
--- a/pbl/Kconfig
+++ b/pbl/Kconfig
@@ -65,6 +65,17 @@ config PBL_VERIFY_PIGGY
 	depends on ARM || MIPS || RISCV
 	bool "Verify barebox proper hash before decompression" if COMPILE_TEST
 
+config PBL_EARLY_FDT_LOAD
+	bool "Enable early FDT loading"
+	depends on ARCH_HAS_EARLY_FDT_SUPPORT
+	select LIBFDT
+	help
+	  This enables the support to decompress and load the barebox builtin DTB
+	  during the pbl (BL2) phase.
+
+	  This is useful for BL31 and BL32 firmwares which may use the DTB for
+	  device configuration.
+
 config PBL_CLOCKSOURCE
 	bool
 
diff --git a/pbl/fdt.c b/pbl/fdt.c
index ac377446caeafb1beaeb211aa8bdec8767ae21fe..0cd9f5a0f11723e9f9b6848e8553da481b92f2be 100644
--- a/pbl/fdt.c
+++ b/pbl/fdt.c
@@ -1,8 +1,10 @@
 // SPDX-License-Identifier: GPL-2.0
+#include <compressed-dtb.h>
 #include <linux/libfdt.h>
 #include <pbl.h>
 #include <linux/printk.h>
 #include <stdio.h>
+#include <uncompress.h>
 
 static const __be32 *fdt_parse_reg(const __be32 *reg, uint32_t n,
 				   uint64_t *val)
@@ -185,3 +187,76 @@ const void *fdt_device_get_match_data(const void *fdt, const char *nodepath,
 
 	return NULL;
 }
+
+static int pbl_open_dtbz_into(const void *fdt, void *buf, int bufsize)
+{
+	const struct barebox_boarddata_compressed_dtb *compressed_dtb;
+	int ret;
+
+	if (!fdt_blob_can_be_decompressed(fdt)) {
+		pr_warn("DTB can't be decompressed\n");
+		return -EINVAL;
+	}
+
+	compressed_dtb = fdt;
+
+	if (IS_ENABLED(CONFIG_IMAGE_COMPRESSION_NONE)) {
+		ret = fdt_open_into(compressed_dtb->data, buf, bufsize);
+		if (ret)
+			pr_warn("Failed to open uncompressed DTB with %s\n",
+				fdt_strerror(ret));
+		return ret ? -EINVAL : 0;
+	}
+
+	if (bufsize < compressed_dtb->datalen_uncompressed) {
+		pr_warn("FDT buffer to small, min. %u bytes required\n",
+			compressed_dtb->datalen_uncompressed);
+		return -EINVAL;
+	}
+
+	ret = pbl_dtbz_uncompress(buf, (void *)compressed_dtb->data,
+				  compressed_dtb->datalen);
+	if (ret) {
+		pr_warn("Failed to uncompress DTB err=%d\n", ret);
+		return ret;
+	}
+
+	if (!blob_is_fdt(buf)) {
+		pr_warn("Failed to determine FDT type for uncompressed DTB\n");
+		return -EINVAL;
+	}
+
+	/* Required to setup size data structures to allow runtime adaptions */
+	ret = fdt_open_into(buf, buf, bufsize);
+	if (ret)
+		pr_warn("Failed to open decompressed DTB with %s\n", fdt_strerror(ret));
+
+	return ret ? -EINVAL : 0;
+}
+
+int pbl_load_fdt(const void *fdt, void *dest, int destsize)
+{
+	int ret;
+
+	if (!IS_ENABLED(CONFIG_PBL_EARLY_FDT_LOAD))
+		return -ENOTSUPP;
+
+	if (destsize == 0) {
+		pr_warn("Skip early FDT load due to dest buffer size of 0-bytes\n");
+		return -EINVAL;
+	}
+
+	if (blob_is_fdt(fdt)) {
+		ret = fdt_open_into(fdt, dest, destsize);
+		if (ret)
+			pr_warn("Failed to uncompressed DTB with %s\n",
+				fdt_strerror(ret));
+		return ret ? -EINVAL : 0;
+	} else if (blob_is_compressed_fdt(fdt)) {
+		return pbl_open_dtbz_into(fdt, dest, destsize);
+	}
+
+	pr_warn("FDT detection failed\n");
+
+	return -EINVAL;
+}

-- 
2.47.3