shared: Add Generic Netlink helpers for libmnl

mnlg.c and mnlg.h are a copy from iproute2.

The call to nl_dump_ext_ack() and nl_dump_ext_ack_done() have been
removed from the code to avoid additional dependencies.

git://git.kernel.org/pub/scm/network/iproute2/iproute2.git
d035cc1b4e83e2589ea2115cdc2fa7c6d3693a5a

The helpers are needed for the WireGuard VPN plugin.

4 files changed, 362 insertions, 5 deletions

diff --git a/Makefile.plugins b/Makefile.plugins
index 3d4e32f0..a4d255f3 100644
--- a/Makefile.plugins
+++ b/Makefile.plugins
@@ -66,17 +66,21 @@ if WIREGUARD
 builtin_vpn_source = vpn/plugins/vpn.c vpn/plugins/vpn.h
 if WIREGUARD_BUILTIN
 builtin_vpn_modules += wireguard
-builtin_vpn_sources += vpn/plugins/wireguard.c
-builtin_vpn_cflags += -DWIREGUARD=\"@WIREGUARD@\"
+builtin_vpn_sources += src/shared/mnlg.h src/shared/mnlg.c \
+			vpn/plugins/wireguard.c
+builtin_vpn_cflags += @LIBMNL_CFLAGS@ -DWIREGUARD=\"@WIREGUARD@\"
+builtin_vpn_libadd += @LIBMNL_LIBS@
 else
 vpn_plugin_LTLIBRARIES += vpn/plugins/wireguard.la
 vpn_plugin_objects += $(plugins_wireguard_la_OBJECTS)
-vpn_plugins_wireguard_la_SOURCES = vpn/plugins/wireguard.c
-vpn_plugins_wireguard_la_CFLAGS = $(plugin_cflags) \
+vpn_plugins_wireguard_la_SOURCES = src/shared/mnlg.h src/shared/mnlg.c \
+					vpn/plugins/wireguard.c
+vpn_plugins_wireguard_la_CFLAGS = $(plugin_cflags) @LIBMNL_CFLAGS@ \
 					-DWIREGUARD=\"@WIREGUARD@\" \
 					-DVPN_STATEDIR=\""$(vpn_statedir)"\" \
 					-DSCRIPTDIR=\""$(build_scriptdir)"\"
 vpn_plugins_wireguard_la_LDFLAGS = $(plugin_ldflags)
+vpn_plugins_wireguard_la_LIBADD = @LIBMNL_LIBS@
 endif
 endif
 
diff --git a/configure.ac b/configure.ac
index 537ba5f4..3695ef5b 100644
--- a/configure.ac
+++ b/configure.ac
@@ -288,7 +288,8 @@ fi
 AM_CONDITIONAL(XTABLES, test "${found_iptables}" != "no")
 
 found_libmnl="no"
-if (test "${firewall_type}" = "nftables"); then
+if (test "${firewall_type}" = "nftables" -o \
+		"${enable_wireguard}" != "no"); then
 	PKG_CHECK_MODULES(LIBMNL, [libmnl >= 1.0.0], [found_libmnl="yes"],
 		AC_MSG_ERROR([libmnl >= 1.0.0 not found]))
 	AC_SUBST(LIBMNL_CFLAGS)
diff --git a/src/shared/mnlg.c b/src/shared/mnlg.c
new file mode 100644
index 00000000..6b02059d
--- /dev/null
+++ b/src/shared/mnlg.c
@@ -0,0 +1,325 @@
+/*
+ *   mnlg.c	Generic Netlink helpers for libmnl
+ *
+ *              This program is free software; you can redistribute it and/or
+ *              modify it under the terms of the GNU General Public License
+ *              as published by the Free Software Foundation; either version
+ *              2 of the License, or (at your option) any later version.
+ *
+ * Authors:     Jiri Pirko <jiri@mellanox.com>
+ */
+
+#include <stdlib.h>
+#include <stdbool.h>
+#include <string.h>
+#include <errno.h>
+#include <unistd.h>
+#include <time.h>
+#include <libmnl/libmnl.h>
+#include <linux/genetlink.h>
+
+#include "mnlg.h"
+
+#ifndef MNL_ARRAY_SIZE
+#define MNL_ARRAY_SIZE(a) (sizeof(a)/sizeof((a)[0]))
+#endif
+
+#ifndef ARRAY_SIZE
+#define ARRAY_SIZE(a) (sizeof(a)/sizeof((a)[0]))
+#endif
+
+
+struct mnlg_socket {
+	struct mnl_socket *nl;
+	char *buf;
+	uint32_t id;
+	uint8_t version;
+	unsigned int seq;
+	unsigned int portid;
+};
+
+static struct nlmsghdr *__mnlg_msg_prepare(struct mnlg_socket *nlg, uint8_t cmd,
+					   uint16_t flags, uint32_t id,
+					   uint8_t version)
+{
+	struct nlmsghdr *nlh;
+	struct genlmsghdr *genl;
+
+	nlh = mnl_nlmsg_put_header(nlg->buf);
+	nlh->nlmsg_type	= id;
+	nlh->nlmsg_flags = flags;
+	nlg->seq = time(NULL);
+	nlh->nlmsg_seq = nlg->seq;
+
+	genl = mnl_nlmsg_put_extra_header(nlh, sizeof(struct genlmsghdr));
+	genl->cmd = cmd;
+	genl->version = version;
+
+	return nlh;
+}
+
+struct nlmsghdr *mnlg_msg_prepare(struct mnlg_socket *nlg, uint8_t cmd,
+				  uint16_t flags)
+{
+	return __mnlg_msg_prepare(nlg, cmd, flags, nlg->id, nlg->version);
+}
+
+int mnlg_socket_send(struct mnlg_socket *nlg, const struct nlmsghdr *nlh)
+{
+	return mnl_socket_sendto(nlg->nl, nlh, nlh->nlmsg_len);
+}
+
+static int mnlg_cb_noop(const struct nlmsghdr *nlh, void *data)
+{
+	return MNL_CB_OK;
+}
+
+static int mnlg_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;
+
+	return err->error == 0 ? MNL_CB_STOP : MNL_CB_ERROR;
+}
+
+static int mnlg_cb_stop(const struct nlmsghdr *nlh, void *data)
+{
+	int len = *(int *)NLMSG_DATA(nlh);
+
+	if (len < 0) {
+		errno = -len;
+		return MNL_CB_ERROR;
+	}
+	return MNL_CB_STOP;
+}
+
+static mnl_cb_t mnlg_cb_array[NLMSG_MIN_TYPE] = {
+	[NLMSG_NOOP]	= mnlg_cb_noop,
+	[NLMSG_ERROR]	= mnlg_cb_error,
+	[NLMSG_DONE]	= mnlg_cb_stop,
+	[NLMSG_OVERRUN]	= mnlg_cb_noop,
+};
+
+int mnlg_socket_recv_run(struct mnlg_socket *nlg, mnl_cb_t data_cb, void *data)
+{
+	int err;
+
+	do {
+		err = mnl_socket_recvfrom(nlg->nl, nlg->buf,
+					  MNL_SOCKET_BUFFER_SIZE);
+		if (err <= 0)
+			break;
+		err = mnl_cb_run2(nlg->buf, err, nlg->seq, nlg->portid,
+				  data_cb, data, mnlg_cb_array,
+				  ARRAY_SIZE(mnlg_cb_array));
+	} while (err > 0);
+
+	return err;
+}
+
+struct group_info {
+	bool found;
+	uint32_t id;
+	const char *name;
+};
+
+static int parse_mc_grps_cb(const struct nlattr *attr, void *data)
+{
+	const struct nlattr **tb = data;
+	int type = mnl_attr_get_type(attr);
+
+	if (mnl_attr_type_valid(attr, CTRL_ATTR_MCAST_GRP_MAX) < 0)
+		return MNL_CB_OK;
+
+	switch (type) {
+	case CTRL_ATTR_MCAST_GRP_ID:
+		if (mnl_attr_validate(attr, MNL_TYPE_U32) < 0)
+			return MNL_CB_ERROR;
+		break;
+	case CTRL_ATTR_MCAST_GRP_NAME:
+		if (mnl_attr_validate(attr, MNL_TYPE_STRING) < 0)
+			return MNL_CB_ERROR;
+		break;
+	}
+	tb[type] = attr;
+	return MNL_CB_OK;
+}
+
+static void parse_genl_mc_grps(struct nlattr *nested,
+			       struct group_info *group_info)
+{
+	struct nlattr *pos;
+	const char *name;
+
+	mnl_attr_for_each_nested(pos, nested) {
+		struct nlattr *tb[CTRL_ATTR_MCAST_GRP_MAX + 1] = {};
+
+		mnl_attr_parse_nested(pos, parse_mc_grps_cb, tb);
+		if (!tb[CTRL_ATTR_MCAST_GRP_NAME] ||
+		    !tb[CTRL_ATTR_MCAST_GRP_ID])
+			continue;
+
+		name = mnl_attr_get_str(tb[CTRL_ATTR_MCAST_GRP_NAME]);
+		if (strcmp(name, group_info->name) != 0)
+			continue;
+
+		group_info->id = mnl_attr_get_u32(tb[CTRL_ATTR_MCAST_GRP_ID]);
+		group_info->found = true;
+	}
+}
+
+static int get_group_id_attr_cb(const struct nlattr *attr, void *data)
+{
+	const struct nlattr **tb = data;
+	int type = mnl_attr_get_type(attr);
+
+	if (mnl_attr_type_valid(attr, CTRL_ATTR_MAX) < 0)
+		return MNL_CB_ERROR;
+
+	if (type == CTRL_ATTR_MCAST_GROUPS &&
+	    mnl_attr_validate(attr, MNL_TYPE_NESTED) < 0)
+		return MNL_CB_ERROR;
+	tb[type] = attr;
+	return MNL_CB_OK;
+}
+
+static int get_group_id_cb(const struct nlmsghdr *nlh, void *data)
+{
+	struct group_info *group_info = data;
+	struct nlattr *tb[CTRL_ATTR_MAX + 1] = {};
+	struct genlmsghdr *genl = mnl_nlmsg_get_payload(nlh);
+
+	mnl_attr_parse(nlh, sizeof(*genl), get_group_id_attr_cb, tb);
+	if (!tb[CTRL_ATTR_MCAST_GROUPS])
+		return MNL_CB_ERROR;
+	parse_genl_mc_grps(tb[CTRL_ATTR_MCAST_GROUPS], group_info);
+	return MNL_CB_OK;
+}
+
+int mnlg_socket_group_add(struct mnlg_socket *nlg, const char *group_name)
+{
+	struct nlmsghdr *nlh;
+	struct group_info group_info;
+	int err;
+
+	nlh = __mnlg_msg_prepare(nlg, CTRL_CMD_GETFAMILY,
+				 NLM_F_REQUEST | NLM_F_ACK, GENL_ID_CTRL, 1);
+	mnl_attr_put_u16(nlh, CTRL_ATTR_FAMILY_ID, nlg->id);
+
+	err = mnlg_socket_send(nlg, nlh);
+	if (err < 0)
+		return err;
+
+	group_info.found = false;
+	group_info.name = group_name;
+	err = mnlg_socket_recv_run(nlg, get_group_id_cb, &group_info);
+	if (err < 0)
+		return err;
+
+	if (!group_info.found) {
+		errno = ENOENT;
+		return -1;
+	}
+
+	err = mnl_socket_setsockopt(nlg->nl, NETLINK_ADD_MEMBERSHIP,
+				    &group_info.id, sizeof(group_info.id));
+	if (err < 0)
+		return err;
+
+	return 0;
+}
+
+static int get_family_id_attr_cb(const struct nlattr *attr, void *data)
+{
+	const struct nlattr **tb = data;
+	int type = mnl_attr_get_type(attr);
+
+	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;
+	tb[type] = attr;
+	return MNL_CB_OK;
+}
+
+static int get_family_id_cb(const struct nlmsghdr *nlh, void *data)
+{
+	uint32_t *p_id = data;
+	struct nlattr *tb[CTRL_ATTR_MAX + 1] = {};
+	struct genlmsghdr *genl = mnl_nlmsg_get_payload(nlh);
+
+	mnl_attr_parse(nlh, sizeof(*genl), get_family_id_attr_cb, tb);
+	if (!tb[CTRL_ATTR_FAMILY_ID])
+		return MNL_CB_ERROR;
+	*p_id = mnl_attr_get_u16(tb[CTRL_ATTR_FAMILY_ID]);
+	return MNL_CB_OK;
+}
+
+struct mnlg_socket *mnlg_socket_open(const char *family_name, uint8_t version)
+{
+	struct mnlg_socket *nlg;
+	struct nlmsghdr *nlh;
+	int one = 1;
+	int err;
+
+	nlg = malloc(sizeof(*nlg));
+	if (!nlg)
+		return NULL;
+
+	nlg->buf = malloc(MNL_SOCKET_BUFFER_SIZE);
+	if (!nlg->buf)
+		goto err_buf_alloc;
+
+	nlg->nl = mnl_socket_open(NETLINK_GENERIC);
+	if (!nlg->nl)
+		goto err_mnl_socket_open;
+
+	/* Older kernels may no support capped/extended ACK reporting */
+	mnl_socket_setsockopt(nlg->nl, NETLINK_CAP_ACK, &one, sizeof(one));
+	mnl_socket_setsockopt(nlg->nl, NETLINK_EXT_ACK, &one, sizeof(one));
+
+	err = mnl_socket_bind(nlg->nl, 0, MNL_SOCKET_AUTOPID);
+	if (err < 0)
+		goto err_mnl_socket_bind;
+
+	nlg->portid = mnl_socket_get_portid(nlg->nl);
+
+	nlh = __mnlg_msg_prepare(nlg, CTRL_CMD_GETFAMILY,
+				 NLM_F_REQUEST | NLM_F_ACK, GENL_ID_CTRL, 1);
+	mnl_attr_put_strz(nlh, CTRL_ATTR_FAMILY_NAME, family_name);
+
+	err = mnlg_socket_send(nlg, nlh);
+	if (err < 0)
+		goto err_mnlg_socket_send;
+
+	err = mnlg_socket_recv_run(nlg, get_family_id_cb, &nlg->id);
+	if (err < 0)
+		goto err_mnlg_socket_recv_run;
+
+	nlg->version = version;
+	return nlg;
+
+err_mnlg_socket_recv_run:
+err_mnlg_socket_send:
+err_mnl_socket_bind:
+	mnl_socket_close(nlg->nl);
+err_mnl_socket_open:
+	free(nlg->buf);
+err_buf_alloc:
+	free(nlg);
+	return NULL;
+}
+
+void mnlg_socket_close(struct mnlg_socket *nlg)
+{
+	mnl_socket_close(nlg->nl);
+	free(nlg->buf);
+	free(nlg);
+}
diff --git a/src/shared/mnlg.h b/src/shared/mnlg.h
new file mode 100644
index 00000000..4d1babf3
--- /dev/null
+++ b/src/shared/mnlg.h
@@ -0,0 +1,27 @@
+/*
+ *   mnlg.h	Generic Netlink helpers for libmnl
+ *
+ *              This program is free software; you can redistribute it and/or
+ *              modify it under the terms of the GNU General Public License
+ *              as published by the Free Software Foundation; either version
+ *              2 of the License, or (at your option) any later version.
+ *
+ * Authors:     Jiri Pirko <jiri@mellanox.com>
+ */
+
+#ifndef _MNLG_H_
+#define _MNLG_H_
+
+#include <libmnl/libmnl.h>
+
+struct mnlg_socket;
+
+struct nlmsghdr *mnlg_msg_prepare(struct mnlg_socket *nlg, uint8_t cmd,
+				  uint16_t flags);
+int mnlg_socket_send(struct mnlg_socket *nlg, const struct nlmsghdr *nlh);
+int mnlg_socket_recv_run(struct mnlg_socket *nlg, mnl_cb_t data_cb, void *data);
+int mnlg_socket_group_add(struct mnlg_socket *nlg, const char *group_name);
+struct mnlg_socket *mnlg_socket_open(const char *family_name, uint8_t version);
+void mnlg_socket_close(struct mnlg_socket *nlg);
+
+#endif /* _MNLG_H_ */