[PATCH 2/4] of: fdt: implement fdt_machine_is_compatible

[Ahmad Fatoum]

When finding compatible bootloader spec files, barebox will unflatten
each DTB in turn, allocating objects for each property and node, only to
compare a single property and then free all the allocations again.

Given that this operation is repeated for every device tree until a
match is found, it's a good idea to be able to compare machine
(top-level) compatibles without having to unflatten the whole FDT.

Implemnt fdt_machine_is_compatible() that does just that. This
intentionally opencodes the device tree iteration as to minimize
code and runtime size. Using libfdt without LTO would be slower
and bigger.

Signed-off-by: Ahmad Fatoum <a.fatoum at pengutronix.de>
---
 drivers/of/fdt.c | 95 ++++++++++++++++++++++++++++++++++++++++++++++++
 include/of.h     |  3 ++
 2 files changed, 98 insertions(+)

diff --git a/drivers/of/fdt.c b/drivers/of/fdt.c
index 676b8a5535bf..aa3d2e967acd 100644
--- a/drivers/of/fdt.c
+++ b/drivers/of/fdt.c
@@ -668,3 +668,98 @@ void fdt_print_reserve_map(const void *__fdt)
 			return;
 	}
 }
+
+static int fdt_string_is_compatible(const char *haystack, int haystack_len,
+				    const char *needle, int needle_len)
+{
+	const char *p;
+	int index = 0;
+
+	while (haystack_len >= needle_len) {
+		if (memcmp(needle, haystack, needle_len + 1) == 0)
+			return OF_DEVICE_COMPATIBLE_MAX_SCORE - (index << 2);
+
+		p = memchr(haystack, '\0', haystack_len);
+		if (!p)
+			return 0;
+		haystack_len -= (p - haystack) + 1;
+		haystack = p + 1;
+		index++;
+	}
+
+	return 0;
+}
+
+bool fdt_machine_is_compatible(const struct fdt_header *fdt, size_t fdt_size, const char *compat)
+{
+	uint32_t tag;
+	const struct fdt_property *fdt_prop;
+	const char *name;
+	uint32_t dt_struct;
+	const struct fdt_node_header *fnh;
+	const void *dt_strings;
+	struct fdt_header f;
+	int ret, len;
+	int expect = FDT_BEGIN_NODE;
+	int compat_len = strlen(compat);
+
+	ret = fdt_parse_header(fdt, fdt_size, &f);
+	if (ret < 0)
+		return ERR_PTR(ret);
+
+	dt_struct = f.off_dt_struct;
+	dt_strings = (const void *)fdt + f.off_dt_strings;
+
+	while (1) {
+		const __be32 *tagp = (const void *)fdt + dt_struct;
+		if (!dt_ptr_ok(fdt, tagp))
+			return false;
+
+		tag = be32_to_cpu(*tagp);
+		if (tag != FDT_NOP && tag != expect)
+			return false;
+
+		switch (tag) {
+		case FDT_BEGIN_NODE:
+			fnh = (const void *)fdt + dt_struct;
+
+			/* The root node must have an empty name */
+			if (fnh->name[0] != '\0')
+				return false;
+
+			dt_struct = dt_struct_advance(&f, dt_struct,
+					sizeof(struct fdt_node_header) + 1);
+
+			expect = FDT_PROP;
+			break;
+
+		case FDT_PROP:
+			fdt_prop = (const void *)fdt + dt_struct;
+			len = fdt32_to_cpu(fdt_prop->len);
+
+			name = dt_string(&f, dt_strings, fdt32_to_cpu(fdt_prop->nameoff));
+			if (!name)
+				return false;
+
+			if (strcmp(name, "compatible")) {
+				dt_struct = dt_struct_advance(&f, dt_struct,
+							      sizeof(struct fdt_property) + len);
+				break;
+			}
+
+			return fdt_string_is_compatible(fdt_prop->data, len, compat, compat_len);
+
+		case FDT_NOP:
+			dt_struct = dt_struct_advance(&f, dt_struct, FDT_TAGSIZE);
+			break;
+
+		default:
+			return false;
+		}
+
+		if (!dt_struct)
+			return false;
+	}
+
+	return false;
+}
diff --git a/include/of.h b/include/of.h
index 3391403a347f..b00940c8532e 100644
--- a/include/of.h
+++ b/include/of.h
@@ -64,6 +64,9 @@ void of_clean_reserve_map(void);
 void fdt_add_reserve_map(void *fdt);
 void fdt_print_reserve_map(const void *fdt);
 
+bool fdt_machine_is_compatible(const struct fdt_header *fdt, size_t fdt_size, const char *compat);
+
+
 struct device;
 struct driver;
 struct resource;
-- 
2.39.2