[PATCH v2 05/15] pbl: fdt: add pbl_load_fdt helper

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

The helper provides a common FDT load mechanism to load the (optional
compressed) DTB to the provided destination address in a user agnostic
manner.

Early FDTs may be useful for other firmware binaries like TEEs (e.g.
OP-TEE) which are loaded and started during the barebox boot phase.

Signed-off-by: Marco Felsch <m.felsch@pengutronix.de>
---
 include/pbl.h |  2 ++
 pbl/fdt.c     | 79 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 81 insertions(+)

diff --git a/include/pbl.h b/include/pbl.h
index 14a463ef8a9f18e42a3332889ca8d9b30d8f1ceb..514f1143706785946e4190f23f95422f23df42e3 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/fdt.c b/pbl/fdt.c
index ac377446caeafb1beaeb211aa8bdec8767ae21fe..3c90959f8e73ea7310248f9b31359d6e8a86ae57 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,80 @@ 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 error;
+
+	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)) {
+		error = fdt_open_into(compressed_dtb->data, buf, bufsize);
+		if (error) {
+			pr_warn("Failed to open uncompressed DTB with %s\n",
+				fdt_strerror(error));
+			return -EINVAL;
+		}
+		return 0;
+	}
+
+	if (bufsize < compressed_dtb->datalen_uncompressed) {
+		pr_warn("FDT buffer to small, min. %u bytes required\n",
+			compressed_dtb->datalen_uncompressed);
+		return -EINVAL;
+	}
+
+	/*
+	 * No error handling required, since this will hang() if uncompress
+	 * fails.
+	 */
+	pbl_dtbz_uncompress(buf, (void *)compressed_dtb->data,
+			    compressed_dtb->datalen);
+
+	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 */
+	error = fdt_open_into(buf, buf, bufsize);
+	if (error) {
+		pr_warn("Failed to open decompressed DTB with %s\n",
+			fdt_strerror(error));
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+int pbl_load_fdt(const void *fdt, void *dest, int destsize)
+{
+	if (destsize == 0 || !dest || !fdt) {
+		pr_warn("Skip early FDT load, invalid input\n");
+		return -EINVAL;
+	}
+
+	if (blob_is_fdt(fdt)) {
+		int error;
+
+		error = fdt_open_into(fdt, dest, destsize);
+		if (error) {
+			pr_warn("Failed to uncompressed DTB with %s\n",
+				fdt_strerror(error));
+			return -EINVAL;
+		}
+		return 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