[PATCH v4 01/14] lib: Add TLV parser

[Roberto Sassu]

From: Roberto Sassu <roberto.sassu@xxxxxxxxxx>

Add a parser of a generic TLV format:

+-----------------+------------------+-----------------+
| data type (u64) | num fields (u64) | total len (u64) | # header
+--------------+--+---------+--------+---------+-------+
| field1 (u64) | len1 (u64) | value1 (u8 len1) |
+--------------+------------+------------------+
|     ...      |    ...     |        ...       |         # data
+--------------+------------+------------------+
| fieldN (u64) | lenN (u64) | valueN (u8 lenN) |
+--------------+------------+------------------+

Each adopter can define its own data types and fields. The TLV parser does
not need to be aware of those, and calls a callback function with the
callback data, both supplied by the adopter, for every encountered field
during parsing. The adopter can decide in the callback function how each
defined field should be handled/parsed.

Normally, calling tlv_parse() is sufficient for most of the use cases. In
addition, tlv_parse_hdr() and tlv_parse_data() are also provided for more
advanced use cases.

Nesting TLVs is also possible, a callback function can call tlv_parse() to
parse the inner structure.

Signed-off-by: Roberto Sassu <roberto.sassu@xxxxxxxxxx>
---
 MAINTAINERS                     |   8 ++
 include/linux/tlv_parser.h      |  28 +++++
 include/uapi/linux/tlv_parser.h |  59 +++++++++
 lib/Kconfig                     |   3 +
 lib/Makefile                    |   3 +
 lib/tlv_parser.c                | 214 ++++++++++++++++++++++++++++++++
 lib/tlv_parser.h                |  17 +++
 7 files changed, 332 insertions(+)
 create mode 100644 include/linux/tlv_parser.h
 create mode 100644 include/uapi/linux/tlv_parser.h
 create mode 100644 lib/tlv_parser.c
 create mode 100644 lib/tlv_parser.h

diff --git a/MAINTAINERS b/MAINTAINERS
index 96a3b60cfc67..b1ca23ab8732 100644
--- a/MAINTAINERS
+++ b/MAINTAINERS
@@ -22148,6 +22148,14 @@ W:	http://sourceforge.net/projects/tlan/
 F:	Documentation/networking/device_drivers/ethernet/ti/tlan.rst
 F:	drivers/net/ethernet/ti/tlan.*
 
+TLV PARSER
+M:	Roberto Sassu <roberto.sassu@xxxxxxxxxx>
+L:	linux-kernel@xxxxxxxxxxxxxxx
+S:	Maintained
+F:	include/linux/tlv_parser.h
+F:	include/uapi/linux/tlv_parser.h
+F:	lib/tlv_parser.*
+
 TMIO/SDHI MMC DRIVER
 M:	Wolfram Sang <wsa+renesas@xxxxxxxxxxxxxxxxxxxx>
 L:	linux-mmc@xxxxxxxxxxxxxxx
diff --git a/include/linux/tlv_parser.h b/include/linux/tlv_parser.h
new file mode 100644
index 000000000000..565743b3cb30
--- /dev/null
+++ b/include/linux/tlv_parser.h
@@ -0,0 +1,28 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (C) 2023-2024 Huawei Technologies Duesseldorf GmbH
+ *
+ * Author: Roberto Sassu <roberto.sassu@xxxxxxxxxx>
+ *
+ * Header file of TLV parser.
+ */
+
+#ifndef _LINUX_TLV_PARSER_H
+#define _LINUX_TLV_PARSER_H
+
+#include <uapi/linux/tlv_parser.h>
+
+typedef int (*parse_callback)(void *, __u64, const __u8 *, __u64);
+
+int tlv_parse_hdr(const __u8 **data, size_t *data_len, __u64 *parsed_data_type,
+		  __u64 *parsed_num_entries, __u64 *parsed_total_len,
+		  const char **data_types, __u64 num_data_types);
+int tlv_parse_data(parse_callback callback, void *callback_data,
+		   __u64 num_entries, const __u8 *data, size_t data_len,
+		   const char **fields, __u64 num_fields);
+int tlv_parse(__u64 expected_data_type, parse_callback callback,
+	      void *callback_data, const __u8 *data, size_t data_len,
+	      const char **data_types, __u64 num_data_types,
+	      const char **fields, __u64 num_fields);
+
+#endif /* _LINUX_TLV_PARSER_H */
diff --git a/include/uapi/linux/tlv_parser.h b/include/uapi/linux/tlv_parser.h
new file mode 100644
index 000000000000..3968c96f2518
--- /dev/null
+++ b/include/uapi/linux/tlv_parser.h
@@ -0,0 +1,59 @@
+/* SPDX-License-Identifier: GPL-2.0 WITH Linux-syscall-note */
+/*
+ * Copyright (C) 2023-2024 Huawei Technologies Duesseldorf GmbH
+ *
+ * Author: Roberto Sassu <roberto.sassu@xxxxxxxxxx>
+ *
+ * Implement the user space interface for the TLV parser.
+ */
+
+#ifndef _UAPI_LINUX_TLV_PARSER_H
+#define _UAPI_LINUX_TLV_PARSER_H
+
+#include <linux/types.h>
+
+/*
+ * TLV format:
+ *
+ * +-----------------+------------------+-----------------+
+ * | data type (u64) | num fields (u64) | total len (u64) | # header
+ * +--------------+--+---------+--------+---------+-------+
+ * | field1 (u64) | len1 (u64) | value1 (u8 len1) |
+ * +--------------+------------+------------------+
+ * |     ...      |    ...     |        ...       |         # data
+ * +--------------+------------+------------------+
+ * | fieldN (u64) | lenN (u64) | valueN (u8 lenN) |
+ * +--------------+------------+------------------+
+ */
+
+/**
+ * struct tlv_hdr - Header of TLV format
+ * @data_type: Type of data to parse
+ * @num_entries: Number of data entries provided
+ * @_reserved: Reserved for future use (must be equal to zero)
+ * @total_len: Total length of the data blob, excluding the header
+ *
+ * This structure represents the header of the TLV data format.
+ */
+struct tlv_hdr {
+	__u64 data_type;
+	__u64 num_entries;
+	__u64 _reserved;
+	__u64 total_len;
+} __attribute__((packed));
+
+/**
+ * struct tlv_data_entry - Data entry of TLV format
+ * @field: Data field identifier
+ * @length: Data length
+ * @data: Data
+ *
+ * This structure represents a TLV entry of the data part of TLV data format.
+ */
+struct tlv_data_entry {
+	__u64 field;
+	__u64 length;
+	__u8 data[];
+} __attribute__((packed));
+
+#endif /* _UAPI_LINUX_TLV_PARSER_H */
diff --git a/lib/Kconfig b/lib/Kconfig
index 5ddda7c2ed9b..d4d3c4435427 100644
--- a/lib/Kconfig
+++ b/lib/Kconfig
@@ -785,3 +785,6 @@ config POLYNOMIAL
 
 config FIRMWARE_TABLE
 	bool
+
+config TLV_PARSER
+	bool
diff --git a/lib/Makefile b/lib/Makefile
index 6b09731d8e61..23f0b770a639 100644
--- a/lib/Makefile
+++ b/lib/Makefile
@@ -444,3 +444,6 @@ $(obj)/$(TEST_FORTIFY_LOG): $(addprefix $(obj)/, $(TEST_FORTIFY_LOGS)) FORCE
 ifeq ($(CONFIG_FORTIFY_SOURCE),y)
 $(obj)/string.o: $(obj)/$(TEST_FORTIFY_LOG)
 endif
+
+obj-$(CONFIG_TLV_PARSER) += tlv_parser.o
+CFLAGS_tlv_parser.o += -I lib
diff --git a/lib/tlv_parser.c b/lib/tlv_parser.c
new file mode 100644
index 000000000000..9565542932eb
--- /dev/null
+++ b/lib/tlv_parser.c
@@ -0,0 +1,214 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2023-2024 Huawei Technologies Duesseldorf GmbH
+ *
+ * Author: Roberto Sassu <roberto.sassu@xxxxxxxxxx>
+ *
+ * Implement the TLV parser.
+ */
+
+#define pr_fmt(fmt) "TLV PARSER: "fmt
+#include <tlv_parser.h>
+
+/**
+ * tlv_parse_hdr - Parse TLV header
+ * @data: Data to parse (updated)
+ * @data_len: Length of @data (updated)
+ * @parsed_data_type: Parsed data type (updated)
+ * @parsed_num_entries: Parsed number of data entries (updated)
+ * @parsed_total_len: Parsed length of TLV data, excluding the header (updated)
+ * @data_types: Array of data type strings
+ * @num_data_types: Number of elements of @data_types
+ *
+ * Parse the header of the TLV data format, move the data pointer to the TLV
+ * data part, decrease the data length by the length of the header, and provide
+ * the data type, number of entries and the total data length extracted from the
+ * header.
+ *
+ * Return: Zero on success, a negative value on error.
+ */
+int tlv_parse_hdr(const __u8 **data, size_t *data_len, __u64 *parsed_data_type,
+		  __u64 *parsed_num_entries, __u64 *parsed_total_len,
+		  const char **data_types, __u64 num_data_types)
+{
+	struct tlv_hdr *hdr;
+
+	if (*data_len < sizeof(*hdr)) {
+		pr_debug("Data blob too short, %lu bytes, expected %lu\n",
+			 *data_len, sizeof(*hdr));
+		return -EBADMSG;
+	}
+
+	hdr = (struct tlv_hdr *)*data;
+
+	*data += sizeof(*hdr);
+	*data_len -= sizeof(*hdr);
+
+	*parsed_data_type = __be64_to_cpu(hdr->data_type);
+	if (*parsed_data_type >= num_data_types) {
+		pr_debug("Invalid data type %llu, max: %llu\n",
+			 *parsed_data_type, num_data_types - 1);
+		return -EBADMSG;
+	}
+
+	*parsed_num_entries = __be64_to_cpu(hdr->num_entries);
+
+	if (hdr->_reserved != 0) {
+		pr_debug("_reserved must be zero\n");
+		return -EBADMSG;
+	}
+
+	*parsed_total_len = __be64_to_cpu(hdr->total_len);
+	if (*parsed_total_len > *data_len) {
+		pr_debug("Invalid total length %llu, expected: %lu\n",
+			 *parsed_total_len, *data_len);
+		return -EBADMSG;
+	}
+
+	pr_debug("Header: type: %s, num entries: %llu, total len: %lld\n",
+		 data_types[*parsed_data_type], *parsed_num_entries,
+		 *parsed_total_len);
+
+	return 0;
+}
+
+/**
+ * tlv_parse_data - Parse TLV data
+ * @callback: Callback function to call to parse the entries
+ * @callback_data: Opaque data to supply to the callback function
+ * @num_entries: Number of data entries to parse
+ * @data: Data to parse
+ * @data_len: Length of @data
+ * @fields: Array of field strings
+ * @num_fields: Number of elements of @fields
+ *
+ * Parse the data part of the TLV data format and call the supplied callback
+ * function for each data entry, passing also the opaque data pointer.
+ *
+ * The callback function decides how to process data depending on the field.
+ *
+ * Return: Zero on success, a negative value on error.
+ */
+int tlv_parse_data(parse_callback callback, void *callback_data,
+		   __u64 num_entries, const __u8 *data, size_t data_len,
+		   const char **fields, __u64 num_fields)
+{
+	const __u8 *data_ptr = data;
+	struct tlv_data_entry *entry;
+	__u64 parsed_field, len, i, max_num_entries;
+	int ret;
+
+	max_num_entries = data_len / sizeof(*entry);
+
+	/* Finite termination on num_entries. */
+	if (num_entries > max_num_entries)
+		return -EBADMSG;
+
+	for (i = 0; i < num_entries; i++) {
+		if (data_len < sizeof(*entry))
+			return -EBADMSG;
+
+		entry = (struct tlv_data_entry *)data_ptr;
+		data_ptr += sizeof(*entry);
+		data_len -= sizeof(*entry);
+
+		parsed_field = __be64_to_cpu(entry->field);
+		if (parsed_field >= num_fields) {
+			pr_debug("Invalid field %llu, max: %llu\n",
+				 parsed_field, num_fields - 1);
+			return -EBADMSG;
+		}
+
+		len = __be64_to_cpu(entry->length);
+
+		if (data_len < len)
+			return -EBADMSG;
+
+		pr_debug("Data: field: %s, len: %llu\n", fields[parsed_field],
+			 len);
+
+		if (!len)
+			continue;
+
+		ret = callback(callback_data, parsed_field, data_ptr, len);
+		if (ret < 0) {
+			pr_debug("Parsing of field %s failed, ret: %d\n",
+				 fields[parsed_field], ret);
+			return ret;
+		}
+
+		data_ptr += len;
+		data_len -= len;
+	}
+
+	if (data_len) {
+		pr_debug("Excess data: %lu bytes\n", data_len);
+		return -EBADMSG;
+	}
+
+	return 0;
+}
+
+/**
+ * tlv_parse - Parse data in TLV format
+ * @expected_data_type: Desired data type
+ * @callback: Callback function to call to parse the data entries
+ * @callback_data: Opaque data to supply to the callback function
+ * @data: Data to parse
+ * @data_len: Length of @data
+ * @data_types: Array of data type strings
+ * @num_data_types: Number of elements of @data_types
+ * @fields: Array of field strings
+ * @num_fields: Number of elements of @fields
+ *
+ * Parse data in TLV format and call tlv_parse_data() each time the header has
+ * the same data type as the expected one.
+ *
+ * Return: Zero on success, a negative value on error.
+ */
+int tlv_parse(__u64 expected_data_type, parse_callback callback,
+	      void *callback_data, const __u8 *data, size_t data_len,
+	      const char **data_types, __u64 num_data_types,
+	      const char **fields, __u64 num_fields)
+{
+	__u64 parsed_data_type, parsed_num_entries, parsed_total_len;
+	const __u8 *data_ptr = data;
+	int ret = 0;
+
+	pr_debug("Start parsing data blob, size: %lu, expected data type: %s\n",
+		 data_len, data_types[expected_data_type]);
+
+	while (data_len) {
+		ret = tlv_parse_hdr(&data_ptr, &data_len, &parsed_data_type,
+				    &parsed_num_entries, &parsed_total_len,
+				    data_types, num_data_types);
+		if (ret < 0)
+			goto out;
+
+		/* Skip data with a different data type than expected. */
+		if (parsed_data_type != expected_data_type) {
+			/*
+			 * tlv_parse_hdr() already checked that
+			 * parsed_total_len <= data_len.
+			 */
+			data_ptr += parsed_total_len;
+			data_len -= parsed_total_len;
+			continue;
+		}
+
+		pr_debug("Found data type %s at offset %ld\n",
+			 data_types[parsed_data_type], data_ptr - data);
+
+		ret = tlv_parse_data(callback, callback_data,
+				     parsed_num_entries, data_ptr,
+				     parsed_total_len, fields, num_fields);
+		if (ret < 0)
+			goto out;
+
+		data_ptr += parsed_total_len;
+		data_len -= parsed_total_len;
+	}
+out:
+	pr_debug("End of parsing data blob, ret: %d\n", ret);
+	return ret;
+}
diff --git a/lib/tlv_parser.h b/lib/tlv_parser.h
new file mode 100644
index 000000000000..8fa8127bd13e
--- /dev/null
+++ b/lib/tlv_parser.h
@@ -0,0 +1,17 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (C) 2023-2024 Huawei Technologies Duesseldorf GmbH
+ *
+ * Author: Roberto Sassu <roberto.sassu@xxxxxxxxxx>
+ *
+ * Header file of TLV parser.
+ */
+
+#ifndef _LIB_TLV_PARSER_H
+#define _LIB_TLV_PARSER_H
+
+#include <linux/kernel.h>
+#include <linux/err.h>
+#include <linux/tlv_parser.h>
+
+#endif /* _LIB_TLV_PARSER_H */
-- 
2.34.1