1 files changed, 254 insertions, 0 deletions
diff --git a/lib/mnl_utils.c b/lib/mnl_utils.c
new file mode 100644
index 0000000..f8e07d2
--- /dev/null
+++ b/lib/mnl_utils.c
@@ -0,0 +1,254 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * mnl_utils.c	Helpers for working with libmnl.
+ */
+
+#include <errno.h>
+#include <string.h>
+#include <time.h>
+#include <libmnl/libmnl.h>
+#include <linux/genetlink.h>
+
+#include "libnetlink.h"
+#include "mnl_utils.h"
+#include "utils.h"
+
+struct mnl_socket *mnlu_socket_open(int bus)
+{
+	struct mnl_socket *nl;
+	int one = 1;
+
+	nl = mnl_socket_open(bus);
+	if (nl == NULL)
+		return NULL;
+
+	mnl_socket_setsockopt(nl, NETLINK_CAP_ACK, &one, sizeof(one));
+	mnl_socket_setsockopt(nl, NETLINK_EXT_ACK, &one, sizeof(one));
+
+	if (mnl_socket_bind(nl, 0, MNL_SOCKET_AUTOPID) < 0)
+		goto err_bind;
+
+	return nl;
+
+err_bind:
+	mnl_socket_close(nl);
+	return NULL;
+}
+
+struct nlmsghdr *mnlu_msg_prepare(void *buf, uint32_t nlmsg_type, uint16_t flags,
+				  void *extra_header, size_t extra_header_size)
+{
+	struct nlmsghdr *nlh;
+	void *eh;
+
+	nlh = mnl_nlmsg_put_header(buf);
+	nlh->nlmsg_type = nlmsg_type;
+	nlh->nlmsg_flags = flags;
+	nlh->nlmsg_seq = time(NULL);
+
+	eh = mnl_nlmsg_put_extra_header(nlh, extra_header_size);
+	memcpy(eh, extra_header, extra_header_size);
+
+	return nlh;
+}
+
+static int mnlu_cb_noop(const struct nlmsghdr *nlh, void *data)
+{
+	return MNL_CB_OK;
+}
+
+static int mnlu_cb_error(const struct nlmsghdr *nlh, void *data)
+{
+	const struct nlmsgerr *err = mnl_nlmsg_get_payload(nlh);
+
+	/* Netlink subsystems returns the errno value with different signess */
+	if (err->error < 0)
+		errno = -err->error;
+	else
+		errno = err->error;
+
+	if (nl_dump_ext_ack(nlh, NULL))
+		return MNL_CB_ERROR;
+
+	return err->error == 0 ? MNL_CB_STOP : MNL_CB_ERROR;
+}
+
+static int mnlu_cb_stop(const struct nlmsghdr *nlh, void *data)
+{
+	int len = *(int *)NLMSG_DATA(nlh);
+
+	if (len < 0) {
+		errno = -len;
+		nl_dump_ext_ack_done(nlh, sizeof(int), len);
+		return MNL_CB_ERROR;
+	}
+	return MNL_CB_STOP;
+}
+
+static mnl_cb_t mnlu_cb_array[NLMSG_MIN_TYPE] = {
+	[NLMSG_NOOP]	= mnlu_cb_noop,
+	[NLMSG_ERROR]	= mnlu_cb_error,
+	[NLMSG_DONE]	= mnlu_cb_stop,
+	[NLMSG_OVERRUN]	= mnlu_cb_noop,
+};
+
+int mnlu_socket_recv_run(struct mnl_socket *nl, unsigned int seq, void *buf, size_t buf_size,
+			 mnl_cb_t cb, void *data)
+{
+	unsigned int portid = mnl_socket_get_portid(nl);
+	int err;
+
+	do {
+		err = mnl_socket_recvfrom(nl, buf, buf_size);
+		if (err <= 0)
+			break;
+		err = mnl_cb_run2(buf, err, seq, portid,
+				  cb, data, mnlu_cb_array,
+				  ARRAY_SIZE(mnlu_cb_array));
+	} while (err > 0);
+
+	return err;
+}
+
+static int get_family_attrs_cb(const struct nlattr *attr, void *data)
+{
+	int type = mnl_attr_get_type(attr);
+	const struct nlattr **tb = data;
+
+	if (mnl_attr_type_valid(attr, CTRL_ATTR_MAX) < 0)
+		return MNL_CB_ERROR;
+
+	if (type == CTRL_ATTR_FAMILY_ID &&
+	    mnl_attr_validate(attr, MNL_TYPE_U16) < 0)
+		return MNL_CB_ERROR;
+	if (type == CTRL_ATTR_MAXATTR &&
+	    mnl_attr_validate(attr, MNL_TYPE_U32) < 0)
+		return MNL_CB_ERROR;
+	tb[type] = attr;
+	return MNL_CB_OK;
+}
+
+static int get_family_cb(const struct nlmsghdr *nlh, void *data)
+{
+	struct genlmsghdr *genl = mnl_nlmsg_get_payload(nlh);
+	struct nlattr *tb[CTRL_ATTR_MAX + 1] = {};
+	struct mnlu_gen_socket *nlg = data;
+
+	mnl_attr_parse(nlh, sizeof(*genl), get_family_attrs_cb, tb);
+	if (!tb[CTRL_ATTR_FAMILY_ID])
+		return MNL_CB_ERROR;
+	if (!tb[CTRL_ATTR_MAXATTR])
+		return MNL_CB_ERROR;
+	nlg->family = mnl_attr_get_u16(tb[CTRL_ATTR_FAMILY_ID]);
+	nlg->maxattr = mnl_attr_get_u32(tb[CTRL_ATTR_MAXATTR]);
+	return MNL_CB_OK;
+}
+
+static int family_get(struct mnlu_gen_socket *nlg, const char *family_name)
+{
+	struct genlmsghdr hdr = {};
+	struct nlmsghdr *nlh;
+	int err;
+
+	hdr.cmd = CTRL_CMD_GETFAMILY;
+	hdr.version = 0x1;
+
+	nlh = mnlu_msg_prepare(nlg->buf, GENL_ID_CTRL,
+			       NLM_F_REQUEST | NLM_F_ACK,
+			       &hdr, sizeof(hdr));
+
+	mnl_attr_put_strz(nlh, CTRL_ATTR_FAMILY_NAME, family_name);
+
+	err = mnl_socket_sendto(nlg->nl, nlh, nlh->nlmsg_len);
+	if (err < 0)
+		return err;
+
+	err = mnlu_socket_recv_run(nlg->nl, nlh->nlmsg_seq, nlg->buf,
+				   MNL_SOCKET_BUFFER_SIZE,
+				   get_family_cb, nlg);
+	return err;
+}
+
+int mnlu_gen_socket_open(struct mnlu_gen_socket *nlg, const char *family_name,
+			 uint8_t version)
+{
+	int err;
+
+	nlg->buf = malloc(MNL_SOCKET_BUFFER_SIZE);
+	if (!nlg->buf)
+		goto err_buf_alloc;
+
+	nlg->nl = mnlu_socket_open(NETLINK_GENERIC);
+	if (!nlg->nl)
+		goto err_socket_open;
+
+	err = family_get(nlg, family_name);
+	if (err)
+		goto err_socket;
+
+	return 0;
+
+err_socket:
+	mnl_socket_close(nlg->nl);
+err_socket_open:
+	free(nlg->buf);
+err_buf_alloc:
+	return -1;
+}
+
+void mnlu_gen_socket_close(struct mnlu_gen_socket *nlg)
+{
+	mnl_socket_close(nlg->nl);
+	free(nlg->buf);
+}
+
+struct nlmsghdr *
+_mnlu_gen_socket_cmd_prepare(struct mnlu_gen_socket *nlg,
+			     uint8_t cmd, uint16_t flags,
+			     uint32_t id, uint8_t version)
+{
+	struct genlmsghdr hdr = {};
+	struct nlmsghdr *nlh;
+
+	hdr.cmd = cmd;
+	hdr.version = version;
+	nlh = mnlu_msg_prepare(nlg->buf, id, flags, &hdr, sizeof(hdr));
+	nlg->seq = nlh->nlmsg_seq;
+	return nlh;
+}
+
+struct nlmsghdr *mnlu_gen_socket_cmd_prepare(struct mnlu_gen_socket *nlg,
+					     uint8_t cmd, uint16_t flags)
+{
+	return _mnlu_gen_socket_cmd_prepare(nlg, cmd, flags, nlg->family,
+					    nlg->version);
+}
+
+int mnlu_gen_socket_sndrcv(struct mnlu_gen_socket *nlg, const struct nlmsghdr *nlh,
+			   mnl_cb_t data_cb, void *data)
+{
+	int err;
+
+	err = mnl_socket_sendto(nlg->nl, nlh, nlh->nlmsg_len);
+	if (err < 0) {
+		perror("Failed to send data");
+		return -errno;
+	}
+
+	err = mnlu_socket_recv_run(nlg->nl, nlh->nlmsg_seq, nlg->buf,
+				   MNL_SOCKET_BUFFER_SIZE,
+				   data_cb, data);
+	if (err < 0) {
+		fprintf(stderr, "kernel answers: %s\n", strerror(errno));
+		return -errno;
+	}
+	return 0;
+}
+
+int mnlu_gen_socket_recv_run(struct mnlu_gen_socket *nlg, mnl_cb_t cb,
+			     void *data)
+{
+	return mnlu_socket_recv_run(nlg->nl, nlg->seq, nlg->buf,
+				    MNL_SOCKET_BUFFER_SIZE,
+				    cb, data);
+}