[mirror_iproute2.git] / lib / mnl_utils.c

// 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, 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_id_attr_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;
	tb[type] = attr;
	return MNL_CB_OK;
}

static int get_family_id_cb(const struct nlmsghdr *nlh, void *data)
{
	struct genlmsghdr *genl = mnl_nlmsg_get_payload(nlh);
	struct nlattr *tb[CTRL_ATTR_MAX + 1] = {};
	uint32_t *p_id = data;

	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;
}

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_id_cb, &nlg->family);
	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)
{
	struct genlmsghdr hdr = {};
	struct nlmsghdr *nlh;

	hdr.cmd = cmd;
	hdr.version = nlg->version;
	nlh = mnlu_msg_prepare(nlg->buf, nlg->family, flags, &hdr, sizeof(hdr));
	nlg->seq = nlh->nlmsg_seq;
	return nlh;
}

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;
}
Commit	Line	Data
72858c7b PM	1	// SPDX-License-Identifier: GPL-2.0+
	2	/*
	3	* mnl_utils.c Helpers for working with libmnl.
	4	*/
	5
6dd778e8	6	#include <errno.h>
dd78dfc7 PM	7	#include <string.h>
dd78dfc7 PM	8	#include <time.h>
72858c7b	9	#include <libmnl/libmnl.h>
b822275a	10	#include <linux/genetlink.h>
72858c7b	11
6dd778e8	12	#include "libnetlink.h"
72858c7b	13	#include "mnl_utils.h"
6dd778e8	14	#include "utils.h"
72858c7b PM	15
	16	struct mnl_socket *mnlu_socket_open(int bus)
	17	{
	18	struct mnl_socket *nl;
	19	int one = 1;
	20
	21	nl = mnl_socket_open(bus);
	22	if (nl == NULL)
	23	return NULL;
	24
	25	mnl_socket_setsockopt(nl, NETLINK_CAP_ACK, &one, sizeof(one));
	26	mnl_socket_setsockopt(nl, NETLINK_EXT_ACK, &one, sizeof(one));
	27
	28	if (mnl_socket_bind(nl, 0, MNL_SOCKET_AUTOPID) < 0)
	29	goto err_bind;
	30
	31	return nl;
	32
	33	err_bind:
	34	mnl_socket_close(nl);
	35	return NULL;
	36	}
dd78dfc7 PM	37
	38	struct nlmsghdr mnlu_msg_prepare(void buf, uint32_t nlmsg_type, uint16_t flags,
	39	void *extra_header, size_t extra_header_size)
	40	{
	41	struct nlmsghdr *nlh;
	42	void *eh;
	43
	44	nlh = mnl_nlmsg_put_header(buf);
	45	nlh->nlmsg_type = nlmsg_type;
	46	nlh->nlmsg_flags = flags;
	47	nlh->nlmsg_seq = time(NULL);
	48
	49	eh = mnl_nlmsg_put_extra_header(nlh, extra_header_size);
	50	memcpy(eh, extra_header, extra_header_size);
	51
	52	return nlh;
	53	}
6dd778e8 PM	54
	55	static int mnlu_cb_noop(const struct nlmsghdr nlh, void data)
	56	{
	57	return MNL_CB_OK;
	58	}
	59
	60	static int mnlu_cb_error(const struct nlmsghdr nlh, void data)
	61	{
	62	const struct nlmsgerr *err = mnl_nlmsg_get_payload(nlh);
	63
	64	/* Netlink subsystems returns the errno value with different signess */
	65	if (err->error < 0)
	66	errno = -err->error;
	67	else
	68	errno = err->error;
	69
	70	if (nl_dump_ext_ack(nlh, NULL))
	71	return MNL_CB_ERROR;
	72
	73	return err->error == 0 ? MNL_CB_STOP : MNL_CB_ERROR;
	74	}
	75
	76	static int mnlu_cb_stop(const struct nlmsghdr nlh, void data)
	77	{
	78	int len = (int )NLMSG_DATA(nlh);
	79
	80	if (len < 0) {
	81	errno = -len;
	82	nl_dump_ext_ack_done(nlh, len);
	83	return MNL_CB_ERROR;
	84	}
	85	return MNL_CB_STOP;
	86	}
	87
	88	static mnl_cb_t mnlu_cb_array[NLMSG_MIN_TYPE] = {
	89	[NLMSG_NOOP] = mnlu_cb_noop,
	90	[NLMSG_ERROR] = mnlu_cb_error,
	91	[NLMSG_DONE] = mnlu_cb_stop,
	92	[NLMSG_OVERRUN] = mnlu_cb_noop,
	93	};
	94
	95	int mnlu_socket_recv_run(struct mnl_socket nl, unsigned int seq, void buf, size_t buf_size,
	96	mnl_cb_t cb, void *data)
	97	{
	98	unsigned int portid = mnl_socket_get_portid(nl);
	99	int err;
	100
	101	do {
	102	err = mnl_socket_recvfrom(nl, buf, buf_size);
	103	if (err <= 0)
	104	break;
	105	err = mnl_cb_run2(buf, err, seq, portid,
	106	cb, data, mnlu_cb_array,
	107	ARRAY_SIZE(mnlu_cb_array));
	108	} while (err > 0);
	109
	110	return err;
	111	}
b822275a PP	112
	113	static int get_family_id_attr_cb(const struct nlattr attr, void data)
	114	{
	115	int type = mnl_attr_get_type(attr);
	116	const struct nlattr **tb = data;
	117
	118	if (mnl_attr_type_valid(attr, CTRL_ATTR_MAX) < 0)
	119	return MNL_CB_ERROR;
	120
	121	if (type == CTRL_ATTR_FAMILY_ID &&
	122	mnl_attr_validate(attr, MNL_TYPE_U16) < 0)
	123	return MNL_CB_ERROR;
	124	tb[type] = attr;
	125	return MNL_CB_OK;
	126	}
	127
	128	static int get_family_id_cb(const struct nlmsghdr nlh, void data)
	129	{
	130	struct genlmsghdr *genl = mnl_nlmsg_get_payload(nlh);
	131	struct nlattr *tb[CTRL_ATTR_MAX + 1] = {};
	132	uint32_t *p_id = data;
	133
	134	mnl_attr_parse(nlh, sizeof(*genl), get_family_id_attr_cb, tb);
	135	if (!tb[CTRL_ATTR_FAMILY_ID])
	136	return MNL_CB_ERROR;
	137	*p_id = mnl_attr_get_u16(tb[CTRL_ATTR_FAMILY_ID]);
	138	return MNL_CB_OK;
	139	}
	140
	141	static int family_get(struct mnlu_gen_socket nlg, const char family_name)
	142	{
	143	struct genlmsghdr hdr = {};
	144	struct nlmsghdr *nlh;
	145	int err;
	146
	147	hdr.cmd = CTRL_CMD_GETFAMILY;
	148	hdr.version = 0x1;
	149
	150	nlh = mnlu_msg_prepare(nlg->buf, GENL_ID_CTRL,
	151	NLM_F_REQUEST \| NLM_F_ACK,
	152	&hdr, sizeof(hdr));
	153
	154	mnl_attr_put_strz(nlh, CTRL_ATTR_FAMILY_NAME, family_name);
	155
	156	err = mnl_socket_sendto(nlg->nl, nlh, nlh->nlmsg_len);
	157	if (err < 0)
	158	return err;
	159
	160	err = mnlu_socket_recv_run(nlg->nl, nlh->nlmsg_seq, nlg->buf,
	161	MNL_SOCKET_BUFFER_SIZE,
	162	get_family_id_cb, &nlg->family);
	163	return err;
	164	}
	165
	166	int mnlu_gen_socket_open(struct mnlu_gen_socket nlg, const char family_name,
	167	uint8_t version)
	168	{
	169	int err;
	170
	171	nlg->buf = malloc(MNL_SOCKET_BUFFER_SIZE);
	172	if (!nlg->buf)
	173	goto err_buf_alloc;
	174
	175	nlg->nl = mnlu_socket_open(NETLINK_GENERIC);
176	if (!nlg->nl)
177	goto err_socket_open;
178
179	err = family_get(nlg, family_name);
180	if (err)
181	goto err_socket;
182
183	return 0;
184
185	err_socket:
186	mnl_socket_close(nlg->nl);
187	err_socket_open:
188	free(nlg->buf);
189	err_buf_alloc:
190	return -1;
191	}
192
193	void mnlu_gen_socket_close(struct mnlu_gen_socket *nlg)
194	{
195	mnl_socket_close(nlg->nl);
196	free(nlg->buf);
197	}
198
199	struct nlmsghdr mnlu_gen_socket_cmd_prepare(struct mnlu_gen_socket nlg,
200	uint8_t cmd, uint16_t flags)
201	{
202	struct genlmsghdr hdr = {};
203	struct nlmsghdr *nlh;
204
205	hdr.cmd = cmd;
206	hdr.version = nlg->version;
207	nlh = mnlu_msg_prepare(nlg->buf, nlg->family, flags, &hdr, sizeof(hdr));
208	nlg->seq = nlh->nlmsg_seq;
209	return nlh;
210	}
211
212	int mnlu_gen_socket_sndrcv(struct mnlu_gen_socket nlg, const struct nlmsghdr nlh,
213	mnl_cb_t data_cb, void *data)
214	{
215	int err;
216
217	err = mnl_socket_sendto(nlg->nl, nlh, nlh->nlmsg_len);
218	if (err < 0) {
219	perror("Failed to send data");
220	return -errno;
221	}
222
223	err = mnlu_socket_recv_run(nlg->nl, nlh->nlmsg_seq, nlg->buf,
224	MNL_SOCKET_BUFFER_SIZE,
225	data_cb, data);
226	if (err < 0) {
227	fprintf(stderr, "kernel answers: %s\n", strerror(errno));
228	return -errno;
229	}
230	return 0;
231	}