[RFC PATCH v2 04/74] libfdt: Introduce fdt_get_structured_tag_data()

[Herve Codina <herve.codina@bootlin.com>]

With structured tag, data len can be encoded in the tag itself (no data,
1 cell data, 2 cells) or with a dedicated cell (varlen) added between
the tag and the data.

In multiple places, getting the data related to the tag will be needed.

To avoid code duplication, introduce fdt_get_structured_tag_data() which
purpose is to get the tag data part and its length based on the tag
value.

Signed-off-by: Herve Codina <herve.codina@bootlin.com>
---
 libfdt/fdt.c             | 80 ++++++++++++++++++++++++++--------------
 libfdt/libfdt_internal.h |  3 ++
 2 files changed, 55 insertions(+), 28 deletions(-)

diff --git a/libfdt/fdt.c b/libfdt/fdt.c
index 506e0dd3..7941632f 100644
--- a/libfdt/fdt.c
+++ b/libfdt/fdt.c
@@ -167,11 +167,56 @@ const void *fdt_offset_ptr(const void *fdt, int offset, unsigned int len)
 	return fdt_offset_ptr_(fdt, offset);
 }
 
+int fdt_get_structured_tag_data(uint32_t tag, const void *fdt, int offset,
+				uint32_t *data_len)
+{
+	int data_offset = offset;
+	uint32_t len = 0, sum;
+	const fdt32_t *lenp;
+
+	if (!(tag & FDT_TAG_STRUCTURED))
+		return -FDT_ERR_INTERNAL;
+
+	switch (tag & FDT_TAG_DATA_MASK) {
+	case FDT_TAG_DATA_NONE:
+		break;
+	case FDT_TAG_DATA_1CELL:
+		len = FDT_CELLSIZE;
+		break;
+	case FDT_TAG_DATA_2CELLS:
+		len = 2 * FDT_CELLSIZE;
+		break;
+	case FDT_TAG_DATA_VARLEN:
+		/* Get the length */
+		lenp = fdt_offset_ptr(fdt, offset, sizeof(*lenp));
+		if (!can_assume(VALID_DTB) && !lenp)
+			return -FDT_ERR_BADSTRUCTURE;
+		len = fdt32_to_cpu(*lenp);
+
+		/* Skip the cell encoding the length */
+		data_offset += sizeof(*lenp);
+
+		/* Check for the length value */
+		sum = len + data_offset;
+		if (!can_assume(VALID_DTB) &&
+		    (sum >= INT_MAX || sum < (uint32_t) offset))
+			return -FDT_ERR_BADSTRUCTURE;
+
+		break;
+	}
+
+	if (data_len)
+		*data_len = len;
+
+	return data_offset;
+}
+
 static uint32_t fdt_next_tag_all(const void *fdt, int startoffset, int *nextoffset)
 {
 	const fdt32_t *tagp, *lenp;
 	uint32_t tag, len, sum;
 	int offset = startoffset;
+	int tmp_offset;
 	const char *p;
 
 	*nextoffset = -FDT_ERR_TRUNCATED;
@@ -221,34 +266,13 @@ static uint32_t fdt_next_tag_all(const void *fdt, int startoffset, int *nextoffs
 		if (!(tag & FDT_TAG_STRUCTURED) || !(tag & FDT_TAG_SKIP_SAFE))
 			return FDT_END;
 
-		switch (tag & FDT_TAG_DATA_MASK) {
-		case FDT_TAG_DATA_NONE:
-			break;
-		case FDT_TAG_DATA_1CELL:
-			offset += FDT_CELLSIZE;
-			break;
-		case FDT_TAG_DATA_2CELLS:
-			offset += 2 * FDT_CELLSIZE;
-			break;
-		case FDT_TAG_DATA_VARLEN:
-			/* Get the length */
-			lenp = fdt_offset_ptr(fdt, offset, sizeof(*lenp));
-			if (!can_assume(VALID_DTB) && !lenp)
-				return FDT_END; /* premature end */
-			len = fdt32_to_cpu(*lenp);
-			/*
-			 * Skip the cell encoding the length and the
-			 * following length bytes
-			 */
-			len += sizeof(*lenp);
-			sum = len + offset;
-			if (!can_assume(VALID_DTB) &&
-			    (sum >= INT_MAX || sum < (uint32_t) offset))
-				return FDT_END; /* premature end */
-
-			offset += len;
-			break;
-		}
+		tmp_offset = fdt_get_structured_tag_data(tag, fdt, offset, &len);
+		if (tmp_offset < 0)
+			return FDT_END; /* premature end */
+
+		/* Skip the whole data bloc */
+		offset = tmp_offset + len;
+		break;
 	}
 
 	if (!fdt_offset_ptr(fdt, startoffset, offset - startoffset))
diff --git a/libfdt/libfdt_internal.h b/libfdt/libfdt_internal.h
index ce128fda..d4154119 100644
--- a/libfdt/libfdt_internal.h
+++ b/libfdt/libfdt_internal.h
@@ -23,6 +23,9 @@ int32_t fdt_ro_probe_(const void *fdt);
 uint32_t fdt_next_tag_(const void *fdt, int startoffset, int *nextoffset,
 		       bool *is_unknown);
 
+int fdt_get_structured_tag_data(uint32_t tag, const void *fdt, int offset,
+				uint32_t *data_len);
+
 int fdt_check_node_offset_(const void *fdt, int offset);
 int fdt_check_prop_offset_(const void *fdt, int offset);
 
-- 
2.55.0