lib: utils: Add fdt_parse_hart_id() function

Parsing HART id from a CPU DT node is a common requirement for
RISC-V systems. The newly added fdt_parse_hart_id() also helps
reduce duplicate code between fdt_cpu_fixup() function and
fdt_parse_hart_count() function.

Signed-off-by: Anup Patel <anup.patel@wdc.com>
Reviewed-by: Atish Patra <atish.patra@wdc.com>
Reviewed-by: Alistair Francis <alistair.francis@wdc.com>

2 files changed, 33 insertions, 15 deletions

diff --git a/lib/utils/fdt/fdt_fixup.c b/lib/utils/fdt/fdt_fixup.c
index d6e4cc1..9a9b435 100644
--- a/lib/utils/fdt/fdt_fixup.c
+++ b/lib/utils/fdt/fdt_fixup.c
@@ -13,14 +13,14 @@
 #include <sbi/sbi_platform.h>
 #include <sbi/sbi_scratch.h>
 #include <sbi/sbi_string.h>
+#include <sbi_utils/fdt/fdt_fixup.h>
+#include <sbi_utils/fdt/fdt_helper.h>
 
 void fdt_cpu_fixup(void *fdt)
 {
 	struct sbi_scratch *scratch = sbi_scratch_thishart_ptr();
 	const struct sbi_platform *plat = sbi_platform_ptr(scratch);
-	int err, len, cpu_offset, cpus_offset;
-	const fdt32_t *val;
-	const void *prop;
+	int err, cpu_offset, cpus_offset;
 	u32 hartid;
 
 	err = fdt_open_into(fdt, fdt, fdt_totalsize(fdt) + 32);
@@ -32,19 +32,9 @@ void fdt_cpu_fixup(void *fdt)
 		return;
 
 	fdt_for_each_subnode(cpu_offset, fdt, cpus_offset) {
-		prop = fdt_getprop(fdt, cpu_offset, "device_type", &len);
-		if (!prop || !len)
+		err = fdt_parse_hart_id(fdt, cpu_offset, &hartid);
+		if (err)
 			continue;
-		if (sbi_strcmp(prop, "cpu"))
-			continue;
-
-		val = fdt_getprop(fdt, cpu_offset, "reg", &len);
-		if (!val || len < sizeof(fdt32_t))
-			continue;
-
-		if (len > sizeof(fdt32_t))
-			val++;
-		hartid = fdt32_to_cpu(*val);
 
 		if (sbi_platform_hart_invalid(plat, hartid))
 			fdt_setprop_string(fdt, cpu_offset, "status",
diff --git a/lib/utils/fdt/fdt_helper.c b/lib/utils/fdt/fdt_helper.c
index 3186f64..af13810 100644
--- a/lib/utils/fdt/fdt_helper.c
+++ b/lib/utils/fdt/fdt_helper.c
@@ -104,6 +104,34 @@ int fdt_get_node_addr_size(void *fdt, int node, unsigned long *addr,
 	return 0;
 }
 
+int fdt_parse_hart_id(void *fdt, int cpu_offset, u32 *hartid)
+{
+	int len;
+	const void *prop;
+	const fdt32_t *val;
+
+	if (!fdt || cpu_offset < 0)
+		return SBI_EINVAL;
+
+	prop = fdt_getprop(fdt, cpu_offset, "device_type", &len);
+	if (!prop || !len)
+		return SBI_EINVAL;
+	if (sbi_strcmp(prop, "cpu"))
+		return SBI_EINVAL;
+
+	val = fdt_getprop(fdt, cpu_offset, "reg", &len);
+	if (!val || len < sizeof(fdt32_t))
+		return SBI_EINVAL;
+
+	if (len > sizeof(fdt32_t))
+		val++;
+
+	if (hartid)
+		*hartid = fdt32_to_cpu(*val);
+
+	return 0;
+}
+
 int fdt_parse_sifive_uart_node(void *fdt, int nodeoffset,
 			       struct platform_uart_data *uart)
 {