[PATCH 06/12] netlink: Add l_netlink_message class

[Denis Kenzior <denkenz@gmail.com>]

Initial implementation of the l_netlink_message class.  For now, only
basic message building is supported.  Both RTNL (rtnetlink) and Generic
Netlink (genetlink) messages can be created by including the appropriate
header using l_netlink_message_add_header.
---
 ell/ell.sym           |   6 ++
 ell/netlink-private.h |  11 +++
 ell/netlink.c         | 174 ++++++++++++++++++++++++++++++++++++++++++
 ell/netlink.h         |  12 +++
 4 files changed, 203 insertions(+)

diff --git a/ell/ell.sym b/ell/ell.sym
index bc0c6e10ebca..247786d5ee98 100644
--- a/ell/ell.sym
+++ b/ell/ell.sym
@@ -434,6 +434,12 @@ global:
 	l_netlink_register;
 	l_netlink_unregister;
 	l_netlink_set_debug;
+	l_netlink_message_new;
+	l_netlink_message_new_sized;
+	l_netlink_message_ref;
+	l_netlink_message_unref;
+	l_netlink_message_append;
+	l_netlink_message_add_header;
 	/* path */
 	l_basename;
 	l_path_find;
diff --git a/ell/netlink-private.h b/ell/netlink-private.h
index fdb584e62652..d04af749b9fb 100644
--- a/ell/netlink-private.h
+++ b/ell/netlink-private.h
@@ -31,6 +31,17 @@ enum nlmsgerr_attrs {
 #define NLM_F_ACK_TLVS 0x200
 #endif
 
+struct l_netlink_message {
+	int ref_count;
+	uint32_t size;
+	union { /* The actual data */
+		struct nlmsghdr *hdr;
+		void *data;
+	};
+};
+
 bool netlink_parse_ext_ack_error(const struct nlmsghdr *nlmsg,
 					const char **out_error_msg,
 					uint32_t *out_error_offset);
+int netlink_message_reserve_header(struct l_netlink_message *message,
+					size_t header_len, void **out_header);
diff --git a/ell/netlink.c b/ell/netlink.c
index 2e23c8903509..39c5bc335e19 100644
--- a/ell/netlink.c
+++ b/ell/netlink.c
@@ -12,6 +12,7 @@
 #include <unistd.h>
 #include <sys/socket.h>
 #include <linux/netlink.h>
+#include <limits.h>
 
 #include "useful.h"
 #include "hashmap.h"
@@ -687,3 +688,176 @@ bool netlink_parse_ext_ack_error(const struct nlmsghdr *nlmsg,
 
 	return true;
 }
+
+static int message_grow(struct l_netlink_message *message, uint32_t needed)
+{
+	uint32_t grow_to;
+
+	if (message->size - message->hdr->nlmsg_len >= needed)
+		return 0;
+
+	/*
+	 * Kernel places a practical limit on the size of messages it will
+	 * accept, at least without tweaking SNDBUF.  There's no (known) reason
+	 * to send very large messages, so limit accordingly
+	 */
+	grow_to = message->hdr->nlmsg_len + needed;
+	if (grow_to > (1U << 20))
+		return -EMSGSIZE;
+
+	if (grow_to < l_util_pagesize())
+		grow_to = roundup_pow_of_two(grow_to);
+	else
+		grow_to = align_len(grow_to, l_util_pagesize());
+
+	message->data = l_realloc(message->data, grow_to);
+	message->size = grow_to;
+
+	return 0;
+}
+
+static inline void *message_tail(struct l_netlink_message *message)
+{
+	return message->data + NLMSG_ALIGN(message->hdr->nlmsg_len);
+}
+
+static int add_attribute(struct l_netlink_message *message,
+				uint16_t type, size_t len,
+				void **out_dest)
+{
+	struct nlattr *attr = message_tail(message);
+	int offset = message->hdr->nlmsg_len;
+
+	attr->nla_type = type;
+	attr->nla_len = NLA_HDRLEN + len;
+
+	if (len) {
+		void *dest = message_tail(message) + NLA_HDRLEN;
+
+		memset(dest + len, 0, NLA_ALIGN(len) - len);
+
+		if (out_dest)
+			*out_dest = dest;
+	}
+
+	message->hdr->nlmsg_len += NLA_HDRLEN + NLA_ALIGN(len);
+
+	return offset;
+}
+
+LIB_EXPORT struct l_netlink_message *l_netlink_message_new_sized(uint16_t type,
+					uint16_t flags, size_t initial_len)
+{
+	struct l_netlink_message *message;
+
+	message = l_new(struct l_netlink_message, 1);
+
+	message->size = initial_len + NLMSG_HDRLEN;
+	message->hdr = l_realloc(NULL, message->size);
+	memset(message->hdr, 0, NLMSG_HDRLEN);
+
+	message->hdr->nlmsg_len = NLMSG_HDRLEN;
+	message->hdr->nlmsg_type = type;
+	message->hdr->nlmsg_flags = flags;
+	/* seq and pid will be filled on send */
+	message->hdr->nlmsg_pid = 0;
+
+	return l_netlink_message_ref(message);
+
+}
+
+LIB_EXPORT struct l_netlink_message *l_netlink_message_new(uint16_t type,
+								uint16_t flags)
+{
+	return l_netlink_message_new_sized(type, flags, 256 - NLMSG_HDRLEN);
+}
+
+LIB_EXPORT struct l_netlink_message *l_netlink_message_ref(
+					struct l_netlink_message *message)
+{
+	if (unlikely(!message))
+		return NULL;
+
+	__atomic_fetch_add(&message->ref_count, 1, __ATOMIC_SEQ_CST);
+
+	return message;
+}
+
+LIB_EXPORT void l_netlink_message_unref(struct l_netlink_message *message)
+{
+	if (unlikely(!message))
+		return;
+
+	if (__atomic_sub_fetch(&message->ref_count, 1, __ATOMIC_SEQ_CST))
+		return;
+
+	l_free(message->hdr);
+	l_free(message);
+}
+
+LIB_EXPORT int l_netlink_message_append(struct l_netlink_message *message,
+						uint16_t type,
+						const void *data, size_t len)
+{
+	void *dest;
+	int r;
+
+	if (unlikely(!message))
+		return -EINVAL;
+
+	if (len > USHRT_MAX - NLA_HDRLEN)
+		return -ERANGE;
+
+	r = message_grow(message, NLA_HDRLEN + NLA_ALIGN(len));
+	if (r < 0)
+		return r;
+
+	r = add_attribute(message, type, len, &dest);
+	if (r < 0)
+		return r;
+
+	memcpy(dest, data, len);
+
+	return 0;
+}
+
+int netlink_message_reserve_header(struct l_netlink_message *message,
+					size_t len, void **out_header)
+{
+	int r;
+
+	if (message->hdr->nlmsg_len != NLMSG_HDRLEN)
+		return -EBADE;
+
+	if (len > USHRT_MAX)
+		return -ERANGE;
+
+	r = message_grow(message, NLA_ALIGN(len));
+	if (r < 0)
+		return r;
+
+	if (out_header)
+		*out_header = message_tail(message);
+
+	memset(message_tail(message) + len, 0, NLA_ALIGN(len) - len);
+	message->hdr->nlmsg_len += NLA_ALIGN(len);
+	return 0;
+}
+
+LIB_EXPORT int l_netlink_message_add_header(struct l_netlink_message *message,
+						const void *header,
+						size_t len)
+{
+	int r;
+	void *dest;
+
+	if (unlikely(!message || !len))
+		return -EINVAL;
+
+	r = netlink_message_reserve_header(message, len, &dest);
+	if (r < 0)
+		return r;
+
+	memcpy(dest, header, len);
+	return 0;
+}
diff --git a/ell/netlink.h b/ell/netlink.h
index 3f3dc50e14be..70a55d652560 100644
--- a/ell/netlink.h
+++ b/ell/netlink.h
@@ -44,6 +44,18 @@ bool l_netlink_set_debug(struct l_netlink *netlink,
 			l_netlink_debug_func_t function,
 			void *user_data, l_netlink_destroy_func_t destroy);
 
+struct l_netlink_message *l_netlink_message_new(uint16_t type, uint16_t flags);
+struct l_netlink_message *l_netlink_message_new_sized(uint16_t type,
+							uint16_t flags,
+							size_t initial_size);
+struct l_netlink_message *l_netlink_message_ref(
+					struct l_netlink_message *message);
+void l_netlink_message_unref(struct l_netlink_message *message);
+int l_netlink_message_append(struct l_netlink_message *message, uint16_t type,
+					const void *data, size_t len);
+int l_netlink_message_add_header(struct l_netlink_message *message,
+					const void *header, size_t len);
+
 #ifdef __cplusplus
 }
 #endif
-- 
2.45.2