[PATCH 08/12] netlink: Support adding nested attributes to messages

[Denis Kenzior <denkenz@gmail.com>]

---
 ell/ell.sym           |  2 ++
 ell/netlink-private.h |  2 ++
 ell/netlink.c         | 54 +++++++++++++++++++++++++++++++++++++++++++
 ell/netlink.h         |  3 +++
 4 files changed, 61 insertions(+)

diff --git a/ell/ell.sym b/ell/ell.sym
index b99b6e552eaa..0dad73f5546e 100644
--- a/ell/ell.sym
+++ b/ell/ell.sym
@@ -441,6 +441,8 @@ global:
 	l_netlink_message_append;
 	l_netlink_message_appendv;
 	l_netlink_message_add_header;
+	l_netlink_message_enter_nested;
+	l_netlink_message_leave_nested;
 	/* path */
 	l_basename;
 	l_path_find;
diff --git a/ell/netlink-private.h b/ell/netlink-private.h
index d04af749b9fb..db4317786a8f 100644
--- a/ell/netlink-private.h
+++ b/ell/netlink-private.h
@@ -38,6 +38,8 @@ struct l_netlink_message {
 		struct nlmsghdr *hdr;
 		void *data;
 	};
+	uint32_t nest_offset[4];
+	uint8_t nest_level;
 };
 
 bool netlink_parse_ext_ack_error(const struct nlmsghdr *nlmsg,
diff --git a/ell/netlink.c b/ell/netlink.c
index d7294a95f20d..f73b876b4ef0 100644
--- a/ell/netlink.c
+++ b/ell/netlink.c
@@ -727,6 +727,15 @@ static int add_attribute(struct l_netlink_message *message,
 {
 	struct nlattr *attr = message_tail(message);
 	int offset = message->hdr->nlmsg_len;
+	int i;
+
+	for (i = 0; i < message->nest_level; i++) {
+		uint32_t nested_len = offset + NLA_HDRLEN + NLA_ALIGN(len) -
+					message->nest_offset[i];
+
+		if (nested_len > USHRT_MAX)
+			return -ERANGE;
+	}
 
 	attr->nla_type = type;
 	attr->nla_len = NLA_HDRLEN + len;
@@ -895,3 +904,48 @@ LIB_EXPORT int l_netlink_message_add_header(struct l_netlink_message *message,
 	memcpy(dest, header, len);
 	return 0;
 }
+
+LIB_EXPORT int l_netlink_message_enter_nested(struct l_netlink_message *message,
+						uint16_t type)
+{
+	int r;
+
+	if (unlikely(!message))
+		return -EINVAL;
+
+	if (unlikely(message->nest_level == L_ARRAY_SIZE(message->nest_offset)))
+		return -EOVERFLOW;
+
+	r = message_grow(message, NLA_HDRLEN);
+	if (r < 0)
+		return r;
+
+	r = add_attribute(message, type | NLA_F_NESTED, 0, NULL);
+	if (r < 0)
+		return false;
+
+	message->nest_offset[message->nest_level] = r;
+	message->nest_level += 1;
+
+	return 0;
+}
+
+LIB_EXPORT int l_netlink_message_leave_nested(struct l_netlink_message *message)
+{
+	struct nlattr *nla;
+	uint32_t offset;
+
+	if (unlikely(!message))
+		return -EINVAL;
+
+	if (unlikely(message->nest_level == 0))
+		return -EOVERFLOW;
+
+	message->nest_level -= 1;
+	offset = message->nest_offset[message->nest_level];
+
+	nla = message->data + offset;
+	nla->nla_len = message->hdr->nlmsg_len - offset;
+
+	return 0;
+}
diff --git a/ell/netlink.h b/ell/netlink.h
index 4d1c4761b3bb..593cddf1aee8 100644
--- a/ell/netlink.h
+++ b/ell/netlink.h
@@ -58,6 +58,9 @@ int l_netlink_message_appendv(struct l_netlink_message *message,
 					const struct iovec *iov, size_t iov_len);
 int l_netlink_message_add_header(struct l_netlink_message *message,
 					const void *header, size_t len);
+int l_netlink_message_enter_nested(struct l_netlink_message *message,
+					uint16_t type);
+int l_netlink_message_leave_nested(struct l_netlink_message *message);
 
 #ifdef __cplusplus
 }
-- 
2.45.2