[RFC PATCH v2 1/7] lib: utils: fdt_helper: add fdt_has_isa_extension() helper

[Yu-Chien Peter Lin]

Add fdt_has_isa_extension() helper to search ISA string of
current hart. This helper is only used before hfeature in
per-hart scratch is initialized.

Signed-off-by: Yu-Chien Peter Lin <peter.lin at sifive.com>
---
 include/sbi_utils/fdt/fdt_helper.h |  2 +
 lib/utils/fdt/fdt_helper.c         | 62 ++++++++++++++++++++++++++++++
 2 files changed, 64 insertions(+)

diff --git a/include/sbi_utils/fdt/fdt_helper.h b/include/sbi_utils/fdt/fdt_helper.h
index 04c850cc..35b5d2ec 100644
--- a/include/sbi_utils/fdt/fdt_helper.h
+++ b/include/sbi_utils/fdt/fdt_helper.h
@@ -57,6 +57,8 @@ int fdt_parse_timebase_frequency(const void *fdt, unsigned long *freq);
 int fdt_parse_isa_extensions(const void *fdt, unsigned int hartid,
 			     unsigned long *extensions);
 
+bool fdt_has_isa_extension(const void *fdt, const char *extension);
+
 int fdt_parse_gaisler_uart_node(const void *fdt, int nodeoffset,
 				struct platform_uart_data *uart);
 
diff --git a/lib/utils/fdt/fdt_helper.c b/lib/utils/fdt/fdt_helper.c
index 799fd48d..8f5d02c1 100644
--- a/lib/utils/fdt/fdt_helper.c
+++ b/lib/utils/fdt/fdt_helper.c
@@ -488,6 +488,68 @@ int fdt_parse_isa_extensions(const void *fdt, unsigned int hartid,
 	return 0;
 }
 
+bool fdt_has_isa_extension(const void *fdt, const char *extension)
+{
+	const struct sbi_hart_ext_data *ext_data = NULL;
+	DECLARE_BITMAP(hart_exts, SBI_HART_EXT_MAX);
+	u32 current_hart = current_hartid();
+	int cpus_offset, cpu_offset, len;
+	const char *isa_str;
+	const fdt32_t *reg;
+	int err, i;
+
+	if (!fdt || !extension)
+		return false;
+
+	for (i = 0; i < SBI_HART_EXT_MAX; i++) {
+		if (!sbi_strncmp(sbi_hart_ext[i].name, extension,
+	                         sbi_strlen(extension))) {
+			ext_data = &sbi_hart_ext[i];
+			break;
+		}
+	}
+
+	if (!ext_data) {
+		sbi_printf("%s: extension %s not found in sbi_hart_ext[]\n",
+			   __func__, extension);
+		return false;
+	}
+
+	cpus_offset = fdt_path_offset(fdt, "/cpus");
+	if (cpus_offset < 0)
+		return false;
+
+	fdt_for_each_subnode(cpu_offset, fdt, cpus_offset) {
+		if (!fdt_node_is_enabled(fdt, cpu_offset))
+			continue;
+
+		reg = fdt_getprop(fdt, cpu_offset, "reg", &len);
+		if (!reg || len < sizeof(fdt32_t))
+			continue;
+
+		if (fdt32_to_cpu(*reg) != current_hart)
+			continue;
+
+		isa_str = fdt_getprop(fdt, cpu_offset, "riscv,isa-extensions", &len);
+		if (isa_str && len > 0) {
+			fdt_parse_isa_extensions_one_hart(isa_str, hart_exts, len);
+			return bitmap_test(hart_exts, ext_data->id);
+		}
+
+		isa_str = fdt_getprop(fdt, cpu_offset, "riscv,isa", &len);
+		if (!isa_str || len <= 0)
+			return false;
+
+		err = fdt_parse_isa_one_hart(isa_str, hart_exts);
+		if (err)
+			return false;
+
+		return bitmap_test(hart_exts, ext_data->id);
+	}
+
+	return false;
+}
+
 static int fdt_parse_uart_node_common(const void *fdt, int nodeoffset,
 				      struct platform_uart_data *uart,
 				      unsigned long default_freq,
-- 
2.39.3