From 19e966b8622e21c6beb2872d3e4373de9dbfef45 Mon Sep 17 00:00:00 2001
From: Anup Patel <anup.patel@wdc.com>
Date: Sun, 26 Apr 2020 12:39:13 +0530
Subject: [PATCH] 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>
---
 include/sbi_utils/fdt/fdt_helper.h |  4 ++++
 lib/utils/fdt/fdt_fixup.c          | 20 +++++---------------
 lib/utils/fdt/fdt_helper.c         | 28 ++++++++++++++++++++++++++++
 3 files changed, 37 insertions(+), 15 deletions(-)

diff --git a/include/sbi_utils/fdt/fdt_helper.h b/include/sbi_utils/fdt/fdt_helper.h
index 3a15aca..98c789b 100644
--- a/include/sbi_utils/fdt/fdt_helper.h
+++ b/include/sbi_utils/fdt/fdt_helper.h
@@ -10,6 +10,8 @@
 #ifndef __FDT_HELPER_H__
 #define __FDT_HELPER_H__
 
+#include <sbi/sbi_types.h>
+
 struct fdt_match {
 	const char *compatible;
 	void *data;
@@ -37,6 +39,8 @@ int fdt_find_match(void *fdt, const struct fdt_match *match_table,
 int fdt_get_node_addr_size(void *fdt, int node, unsigned long *addr,
 			   unsigned long *size);
 
+int fdt_parse_hart_id(void *fdt, int cpu_offset, u32 *hartid);
+
 int fdt_parse_sifive_uart_node(void *fdt, int nodeoffset,
 			       struct platform_uart_data *uart);
 
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)
 {
-- 
2.7.4