Add fdt_find_node_by_type() and fdt_find_compatible_node() to LIBFDT

Signed-off-by: Wolfgang Grandegger <wg@grandegger.com>
Acked-by: Gerald Van Baren <vanbaren@cideas.com>

2 files changed, 149 insertions, 18 deletions

diff --git a/include/libfdt.h b/include/libfdt.h
index e080028..340e89d 100644
--- a/include/libfdt.h
+++ b/include/libfdt.h
@@ -78,6 +78,12 @@ int fdt_subnode_offset_namelen(const void *fdt, int parentoffset,
 int fdt_subnode_offset(const void *fdt, int parentoffset, const char *name);
 
 int fdt_find_node_by_path(const void *fdt, const char *path);
+int fdt_find_node_by_type(const void *fdt, int nodeoffset, const char *type);
+
+int fdt_node_is_compatible(const void *fdt, int nodeoffset,
+			   const char *compat);
+int fdt_find_compatible_node(const void *fdt, int nodeoffset,
+			     const char *type, const char *compat);
 
 struct fdt_property *fdt_get_property(const void *fdt, int nodeoffset,
 				      const char *name, int *lenp);
diff --git a/libfdt/fdt_ro.c b/libfdt/fdt_ro.c
index 1a03109..923c389 100644
--- a/libfdt/fdt_ro.c
+++ b/libfdt/fdt_ro.c
@@ -51,6 +51,33 @@ static int offset_streq(const void *fdt, int offset,
 }
 
 /*
+ * Checks if the property name matches.
+ */
+static int prop_name_eq(const void *fdt, int offset, const char *name,
+			struct fdt_property **prop, int *lenp)
+{
+	int namestroff, len;
+
+	*prop = fdt_offset_ptr_typed(fdt, offset, *prop);
+	if (! *prop)
+		return -FDT_ERR_BADSTRUCTURE;
+
+	namestroff = fdt32_to_cpu((*prop)->nameoff);
+	if (streq(fdt_string(fdt, namestroff), name)) {
+		len = fdt32_to_cpu((*prop)->len);
+		*prop = fdt_offset_ptr(fdt, offset,
+				       sizeof(**prop) + len);
+		if (*prop) {
+			if (lenp)
+				*lenp = len;
+			return 1;
+		} else
+			return -FDT_ERR_BADSTRUCTURE;
+	}
+	return 0;
+}
+
+/*
  * Return a pointer to the string at the given string offset.
  */
 char *fdt_string(const void *fdt, int stroffset)
@@ -59,6 +86,118 @@ char *fdt_string(const void *fdt, int stroffset)
 }
 
 /*
+ * Check if the specified node is compatible by comparing the tokens
+ * in its "compatible" property with the specified string:
+ *
+ *   nodeoffset - starting place of the node
+ *   compat     - the string to match to one of the tokens in the
+ *                "compatible" list.
+ */
+int fdt_node_is_compatible(const void *fdt, int nodeoffset,
+			   const char *compat)
+{
+	const char* cp;
+	int cplen, len;
+
+	cp = fdt_getprop(fdt, nodeoffset, "compatible", &cplen);
+	if (cp == NULL)
+		return 0;
+	while (cplen > 0) {
+		if (strncmp(cp, compat, strlen(compat)) == 0)
+			return 1;
+		len = strlen(cp) + 1;
+		cp += len;
+		cplen -= len;
+	}
+
+	return 0;
+}
+
+/*
+ * Find a node by its device type property. On success, the offset of that
+ * node is returned or an error code otherwise:
+ *
+ *   nodeoffset - the node to start searching from or 0, the node you pass
+ *                will not be searched, only the next one will; typically,
+ *                you pass 0 to start the search and then what the previous
+ *                call returned.
+ *   type       - the device type string to match against.
+ */
+int fdt_find_node_by_type(const void *fdt, int nodeoffset, const char *type)
+{
+	int offset, nextoffset;
+	struct fdt_property *prop;
+	uint32_t tag;
+	int len, ret;
+
+	CHECK_HEADER(fdt);
+
+	tag = fdt_next_tag(fdt, nodeoffset, &nextoffset, NULL);
+	if (tag != FDT_BEGIN_NODE)
+		return -FDT_ERR_BADOFFSET;
+	if (nodeoffset)
+		nodeoffset = 0;	/* start searching with next node */
+
+	while (1) {
+		offset = nextoffset;
+		tag = fdt_next_tag(fdt, offset, &nextoffset, NULL);
+
+		switch (tag) {
+		case FDT_BEGIN_NODE:
+			nodeoffset = offset;
+			break;
+
+		case FDT_PROP:
+			if (nodeoffset == 0)
+				break;
+			ret = prop_name_eq(fdt, offset, "device_type",
+					   &prop, &len);
+			if (ret < 0)
+				return ret;
+			else if (ret > 0 &&
+				 strncmp(prop->data, type, len - 1) == 0)
+			    return nodeoffset;
+			break;
+
+		case FDT_END_NODE:
+		case FDT_NOP:
+			break;
+
+		case FDT_END:
+			return -FDT_ERR_NOTFOUND;
+
+		default:
+			return -FDT_ERR_BADSTRUCTURE;
+		}
+	}
+}
+
+/*
+ * Find a node based on its device type and one of the tokens in its its
+ * "compatible" property. On success, the offset of that node is returned
+ * or an error code otherwise:
+ *
+ *   nodeoffset - the node to start searching from or 0, the node you pass
+ *                will not be searched, only the next one will; typically,
+ *                you pass 0 to start the search and then what the previous
+ *                call returned.
+ *   type       - the device type string to match against.
+ *   compat     - the string to match to one of the tokens in the
+ *                "compatible" list.
+ */
+int fdt_find_compatible_node(const void *fdt, int nodeoffset,
+			     const char *type, const char *compat)
+{
+	int offset;
+
+	offset = fdt_find_node_by_type(fdt, nodeoffset, type);
+	if (offset < 0 || fdt_node_is_compatible(fdt, offset, compat))
+		return offset;
+
+	return -FDT_ERR_NOTFOUND;
+}
+
+/*
  * Return the node offset of the node specified by:
  *   parentoffset - starting place (0 to start at the root)
  *   name         - name being searched for
@@ -187,7 +326,6 @@ struct fdt_property *fdt_get_property(const void *fdt,
 	int level = 0;
 	uint32_t tag;
 	struct fdt_property *prop;
-	int namestroff;
 	int offset, nextoffset;
 	int err;
 
@@ -227,24 +365,11 @@ struct fdt_property *fdt_get_property(const void *fdt,
 			if (level != 0)
 				continue;
 
-			err = -FDT_ERR_BADSTRUCTURE;
-			prop = fdt_offset_ptr_typed(fdt, offset, prop);
-			if (! prop)
-				goto fail;
-			namestroff = fdt32_to_cpu(prop->nameoff);
-			if (streq(fdt_string(fdt, namestroff), name)) {
-				/* Found it! */
-				int len = fdt32_to_cpu(prop->len);
-				prop = fdt_offset_ptr(fdt, offset,
-						      sizeof(*prop)+len);
-				if (! prop)
-					goto fail;
-
-				if (lenp)
-					*lenp = len;
-
+			err = prop_name_eq(fdt, offset, name, &prop, lenp);
+			if (err > 0)
 				return prop;
-			}
+			else if (err < 0)
+				goto fail;
 			break;
 
 		case FDT_NOP: