feat(fdt-wrappers): add CPU enumeration utility function

This change adds a new utility function - `fdtw_for_each_cpu` - to
invoke a callback for every CPU node listed in a flattened device tree
(FDT) with the node identifier and the MPIDR of the core it describes.

Signed-off-by: Chris Kay <chris.kay@arm.com>
Change-Id: Iabb5c0f0c9d11928a4a7a41cdc7d1e09aadeb2bc

2 files changed, 47 insertions, 0 deletions

diff --git a/common/fdt_wrappers.c b/common/fdt_wrappers.c
index dd7a0faef..64e01ea6d 100644
--- a/common/fdt_wrappers.c
+++ b/common/fdt_wrappers.c
@@ -572,3 +572,47 @@ uint64_t fdtw_translate_address(const void *dtb, int node,
 	/* Translate the local device address recursively */
 	return fdtw_translate_address(dtb, local_bus_node, global_address);
 }
+
+/*
+ * For every CPU node (`/cpus/cpu@n`) in an FDT, execute a callback passing a
+ * pointer to the FDT and the offset of the CPU node. If the return value of the
+ * callback is negative, it is treated as an error and the loop is aborted. In
+ * this situation, the value of the callback is returned from the function.
+ *
+ * Returns `0` on success, or a negative integer representing an error code.
+ */
+int fdtw_for_each_cpu(const void *dtb,
+		      int (*callback)(const void *dtb, int node, uintptr_t mpidr))
+{
+	int ret = 0;
+	int parent, node = 0;
+
+	parent = fdt_path_offset(dtb, "/cpus");
+	if (parent < 0) {
+		return parent;
+	}
+
+	fdt_for_each_subnode(node, dtb, parent) {
+		const char *name;
+		int len;
+
+		uintptr_t mpidr = 0U;
+
+		name = fdt_get_name(dtb, node, &len);
+		if (strncmp(name, "cpu@", 4) != 0) {
+			continue;
+		}
+
+		ret = fdt_get_reg_props_by_index(dtb, node, 0, &mpidr, NULL);
+		if (ret < 0) {
+			break;
+		}
+
+		ret = callback(dtb, node, mpidr);
+		if (ret < 0) {
+			break;
+		}
+	}
+
+	return ret;
+}
diff --git a/include/common/fdt_wrappers.h b/include/common/fdt_wrappers.h
index 98e7a3e6f..9c7180c5e 100644
--- a/include/common/fdt_wrappers.h
+++ b/include/common/fdt_wrappers.h
@@ -41,6 +41,9 @@ int fdt_get_stdout_node_offset(const void *dtb);
 uint64_t fdtw_translate_address(const void *dtb, int bus_node,
 				uint64_t base_address);
 
+int fdtw_for_each_cpu(const void *fdt,
+		      int (*callback)(const void *dtb, int node, uintptr_t mpidr));
+
 static inline uint32_t fdt_blob_size(const void *dtb)
 {
 	const uint32_t *dtb_header = dtb;