[people/ms/rstp.git] / libnetlink.c

/*
 * libnetlink.c	RTnetlink service routines.
 *
 *		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:	Alexey Kuznetsov, <kuznet@ms2.inr.ac.ru>
 *
 */

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <syslog.h>
#include <fcntl.h>
#include <net/if_arp.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <string.h>
#include <errno.h>
#include <time.h>
#include <sys/uio.h>

#include "libnetlink.h"

void rtnl_close(struct rtnl_handle *rth)
{
	close(rth->fd);
}

int rtnl_open_byproto(struct rtnl_handle *rth, unsigned subscriptions,
		      int protocol)
{
	socklen_t addr_len;
	int sndbuf = 32768;
	int rcvbuf = 32768;

	memset(rth, 0, sizeof(*rth));

	rth->fd = socket(AF_NETLINK, SOCK_RAW, protocol);
	if (rth->fd < 0) {
		perror("Cannot open netlink socket");
		return -1;
	}

	if (setsockopt(rth->fd, SOL_SOCKET, SO_SNDBUF, &sndbuf, sizeof(sndbuf))
	    < 0) {
		perror("SO_SNDBUF");
		return -1;
	}

	if (setsockopt(rth->fd, SOL_SOCKET, SO_RCVBUF, &rcvbuf, sizeof(rcvbuf))
	    < 0) {
		perror("SO_RCVBUF");
		return -1;
	}

	memset(&rth->local, 0, sizeof(rth->local));
	rth->local.nl_family = AF_NETLINK;
	rth->local.nl_groups = subscriptions;

	if (bind(rth->fd, (struct sockaddr *)&rth->local, sizeof(rth->local)) <
	    0) {
		perror("Cannot bind netlink socket");
		return -1;
	}
	addr_len = sizeof(rth->local);
	if (getsockname(rth->fd, (struct sockaddr *)&rth->local, &addr_len) < 0) {
		perror("Cannot getsockname");
		return -1;
	}
	if (addr_len != sizeof(rth->local)) {
		fprintf(stderr, "Wrong address length %d\n", addr_len);
		return -1;
	}
	if (rth->local.nl_family != AF_NETLINK) {
		fprintf(stderr, "Wrong address family %d\n",
			rth->local.nl_family);
		return -1;
	}
	rth->seq = time(NULL);
	return 0;
}

int rtnl_open(struct rtnl_handle *rth, unsigned subscriptions)
{
	return rtnl_open_byproto(rth, subscriptions, NETLINK_ROUTE);
}

int rtnl_wilddump_request(struct rtnl_handle *rth, int family, int type)
{
	struct {
		struct nlmsghdr nlh;
		struct rtgenmsg g;
	} req;
	struct sockaddr_nl nladdr;

	memset(&nladdr, 0, sizeof(nladdr));
	nladdr.nl_family = AF_NETLINK;

	memset(&req, 0, sizeof(req));
	req.nlh.nlmsg_len = sizeof(req);
	req.nlh.nlmsg_type = type;
	req.nlh.nlmsg_flags = NLM_F_ROOT | NLM_F_MATCH | NLM_F_REQUEST;
	req.nlh.nlmsg_pid = 0;
	req.nlh.nlmsg_seq = rth->dump = ++rth->seq;
	req.g.rtgen_family = family;

	return sendto(rth->fd, (void *)&req, sizeof(req), 0,
		      (struct sockaddr *)&nladdr, sizeof(nladdr));
}

int rtnl_send(struct rtnl_handle *rth, const char *buf, int len)
{
	struct sockaddr_nl nladdr;

	memset(&nladdr, 0, sizeof(nladdr));
	nladdr.nl_family = AF_NETLINK;

	return sendto(rth->fd, buf, len, 0, (struct sockaddr *)&nladdr,
		      sizeof(nladdr));
}

int rtnl_dump_request(struct rtnl_handle *rth, int type, void *req, int len)
{
	struct nlmsghdr nlh;
	struct sockaddr_nl nladdr;
	struct iovec iov[2] = {
		{.iov_base = &nlh,.iov_len = sizeof(nlh)}
		,
		{.iov_base = req,.iov_len = len}
	};
	struct msghdr msg = {
		.msg_name = &nladdr,
		.msg_namelen = sizeof(nladdr),
		.msg_iov = iov,
		.msg_iovlen = 2,
	};

	memset(&nladdr, 0, sizeof(nladdr));
	nladdr.nl_family = AF_NETLINK;

	nlh.nlmsg_len = NLMSG_LENGTH(len);
	nlh.nlmsg_type = type;
	nlh.nlmsg_flags = NLM_F_ROOT | NLM_F_MATCH | NLM_F_REQUEST;
	nlh.nlmsg_pid = 0;
	nlh.nlmsg_seq = rth->dump = ++rth->seq;

	return sendmsg(rth->fd, &msg, 0);
}

int rtnl_dump_filter(struct rtnl_handle *rth,
		     rtnl_filter_t filter,
		     void *arg1, rtnl_filter_t junk, void *arg2)
{
	struct sockaddr_nl nladdr;
	struct iovec iov;
	struct msghdr msg = {
		.msg_name = &nladdr,
		.msg_namelen = sizeof(nladdr),
		.msg_iov = &iov,
		.msg_iovlen = 1,
	};
	char buf[16384];

	iov.iov_base = buf;
	while (1) {
		int status;
		struct nlmsghdr *h;

		iov.iov_len = sizeof(buf);
		status = recvmsg(rth->fd, &msg, 0);

		if (status < 0) {
			if (errno == EINTR)
				continue;
			perror("OVERRUN");
			continue;
		}

		if (status == 0) {
			fprintf(stderr, "EOF on netlink\n");
			return -1;
		}

		h = (struct nlmsghdr *)buf;
		while (NLMSG_OK(h, status)) {
			int err;

			if (nladdr.nl_pid != 0 ||
			    h->nlmsg_pid != rth->local.nl_pid ||
			    h->nlmsg_seq != rth->dump) {
				if (junk) {
					err = junk(&nladdr, h, arg2);
					if (err < 0)
						return err;
				}
				goto skip_it;
			}

			if (h->nlmsg_type == NLMSG_DONE)
				return 0;
			if (h->nlmsg_type == NLMSG_ERROR) {
				struct nlmsgerr *err =
				    (struct nlmsgerr *)NLMSG_DATA(h);
				if (h->nlmsg_len <
				    NLMSG_LENGTH(sizeof(struct nlmsgerr))) {
					fprintf(stderr, "ERROR truncated\n");
				} else {
					errno = -err->error;
					perror("RTNETLINK answers");
				}
				return -1;
			}
			err = filter(&nladdr, h, arg1);
			if (err < 0)
				return err;

		      skip_it:
			h = NLMSG_NEXT(h, status);
		}
		if (msg.msg_flags & MSG_TRUNC) {
			fprintf(stderr, "Message truncated\n");
			continue;
		}
		if (status) {
			fprintf(stderr, "!!!Remnant of size %d\n", status);
			return -1;
		}
	}
}

int rtnl_talk(struct rtnl_handle *rtnl, struct nlmsghdr *n, pid_t peer,
	      unsigned groups, struct nlmsghdr *answer,
	      rtnl_filter_t junk, void *jarg)
{
	int status;
	unsigned seq;
	struct nlmsghdr *h;
	struct sockaddr_nl nladdr;
	struct iovec iov = {
		.iov_base = (void *)n,
		.iov_len = n->nlmsg_len
	};
	struct msghdr msg = {
		.msg_name = &nladdr,
		.msg_namelen = sizeof(nladdr),
		.msg_iov = &iov,
		.msg_iovlen = 1,
	};
	char buf[16384];

	memset(&nladdr, 0, sizeof(nladdr));
	nladdr.nl_family = AF_NETLINK;
	nladdr.nl_pid = peer;
	nladdr.nl_groups = groups;

	n->nlmsg_seq = seq = ++rtnl->seq;

	if (answer == NULL)
		n->nlmsg_flags |= NLM_F_ACK;

	status = sendmsg(rtnl->fd, &msg, 0);

	if (status < 0) {
		perror("Cannot talk to rtnetlink");
		return -1;
	}

	memset(buf, 0, sizeof(buf));

	iov.iov_base = buf;

	while (1) {
		iov.iov_len = sizeof(buf);
		status = recvmsg(rtnl->fd, &msg, 0);

		if (status < 0) {
			if (errno == EINTR)
				continue;
			perror("OVERRUN");
			continue;
		}
		if (status == 0) {
			fprintf(stderr, "EOF on netlink\n");
			return -1;
		}
		if (msg.msg_namelen != sizeof(nladdr)) {
			fprintf(stderr, "sender address length == %d\n",
				msg.msg_namelen);
			return -1;
		}
		for (h = (struct nlmsghdr *)buf; status >= sizeof(*h);) {
			int err;
			int len = h->nlmsg_len;
			int l = len - sizeof(*h);

			if (l < 0 || len > status) {
				if (msg.msg_flags & MSG_TRUNC) {
					fprintf(stderr, "Truncated message\n");
					return -1;
				}
				fprintf(stderr,
					"!!!malformed message: len=%d\n", len);
				return -1;
			}

			if (nladdr.nl_pid != peer ||
			    h->nlmsg_pid != rtnl->local.nl_pid ||
			    h->nlmsg_seq != seq) {
				if (junk) {
					err = junk(&nladdr, h, jarg);
					if (err < 0)
						return err;
				}
				/* Don't forget to skip that message. */
				status -= NLMSG_ALIGN(len);
				h = (struct nlmsghdr *)((char *)h +
							NLMSG_ALIGN(len));
				continue;
			}

			if (h->nlmsg_type == NLMSG_ERROR) {
				struct nlmsgerr *err =
				    (struct nlmsgerr *)NLMSG_DATA(h);
				if (l < sizeof(struct nlmsgerr)) {
					fprintf(stderr, "ERROR truncated\n");
				} else {
					errno = -err->error;
					if (errno == 0) {
						if (answer)
							memcpy(answer, h,
							       h->nlmsg_len);
						return 0;
					}
					perror("RTNETLINK answers");
				}
				return -1;
			}
			if (answer) {
				memcpy(answer, h, h->nlmsg_len);
				return 0;
			}

			fprintf(stderr, "Unexpected reply!!!\n");

			status -= NLMSG_ALIGN(len);
			h = (struct nlmsghdr *)((char *)h + NLMSG_ALIGN(len));
		}
		if (msg.msg_flags & MSG_TRUNC) {
			fprintf(stderr, "Message truncated\n");
			continue;
		}
		if (status) {
			fprintf(stderr, "!!!Remnant of size %d\n", status);
			return -1;
		}
	}
}

int rtnl_listen(struct rtnl_handle *rtnl, rtnl_filter_t handler, void *jarg)
{
	int status;
	struct nlmsghdr *h;
	struct sockaddr_nl nladdr;
	struct iovec iov;
	struct msghdr msg = {
		.msg_name = &nladdr,
		.msg_namelen = sizeof(nladdr),
		.msg_iov = &iov,
		.msg_iovlen = 1,
	};
	char buf[8192];

	memset(&nladdr, 0, sizeof(nladdr));
	nladdr.nl_family = AF_NETLINK;
	nladdr.nl_pid = 0;
	nladdr.nl_groups = 0;

	iov.iov_base = buf;
	while (1) {
		iov.iov_len = sizeof(buf);
		status = recvmsg(rtnl->fd, &msg, 0);

		if (status < 0) {
			if (errno == EINTR)
				continue;
			if (errno == EAGAIN)
				return 0;
			perror("OVERRUN");
			return -1;
		}
		if (status == 0) {
			fprintf(stderr, "EOF on netlink\n");
			return -1;
		}
		if (msg.msg_namelen != sizeof(nladdr)) {
			fprintf(stderr, "Sender address length == %d\n",
				msg.msg_namelen);
			return -1;
		}
		for (h = (struct nlmsghdr *)buf; status >= sizeof(*h);) {
			int err;
			int len = h->nlmsg_len;
			int l = len - sizeof(*h);

			if (l < 0 || len > status) {
				if (msg.msg_flags & MSG_TRUNC) {
					fprintf(stderr, "Truncated message\n");
					return -1;
				}
				fprintf(stderr,
					"!!!malformed message: len=%d\n", len);
				return -1;
			}

			err = handler(&nladdr, h, jarg);
			if (err < 0) {
				fprintf(stderr, "Handler returned %d\n", err);
				return err;
			}

			status -= NLMSG_ALIGN(len);
			h = (struct nlmsghdr *)((char *)h + NLMSG_ALIGN(len));
		}
		if (msg.msg_flags & MSG_TRUNC) {
			fprintf(stderr, "Message truncated\n");
			continue;
		}
		if (status) {
			fprintf(stderr, "!!!Remnant of size %d\n", status);
			return -1;
		}
	}
}

int rtnl_from_file(FILE * rtnl, rtnl_filter_t handler, void *jarg)
{
	int status;
	struct sockaddr_nl nladdr;
	char buf[8192];
	struct nlmsghdr *h = (void *)buf;

	memset(&nladdr, 0, sizeof(nladdr));
	nladdr.nl_family = AF_NETLINK;
	nladdr.nl_pid = 0;
	nladdr.nl_groups = 0;

	while (1) {
		int err, len;
		int l;

		status = fread(&buf, 1, sizeof(*h), rtnl);

		if (status < 0) {
			if (errno == EINTR)
				continue;
			perror("rtnl_from_file: fread");
			return -1;
		}
		if (status == 0)
			return 0;

		len = h->nlmsg_len;
		l = len - sizeof(*h);

		if (l < 0 || len > sizeof(buf)) {
			fprintf(stderr, "!!!malformed message: len=%d @%lu\n",
				len, ftell(rtnl));
			return -1;
		}

		status = fread(NLMSG_DATA(h), 1, NLMSG_ALIGN(l), rtnl);

		if (status < 0) {
			perror("rtnl_from_file: fread");
			return -1;
		}
		if (status < l) {
			fprintf(stderr, "rtnl-from_file: truncated message\n");
			return -1;
		}

		err = handler(&nladdr, h, jarg);
		if (err < 0)
			return err;
	}
}

int addattr32(struct nlmsghdr *n, int maxlen, int type, __u32 data)
{
	int len = RTA_LENGTH(4);
	struct rtattr *rta;
	if (NLMSG_ALIGN(n->nlmsg_len) + len > maxlen) {
		fprintf(stderr,
			"addattr32: Error! max allowed bound %d exceeded\n",
			maxlen);
		return -1;
	}
	rta = NLMSG_TAIL(n);
	rta->rta_type = type;
	rta->rta_len = len;
	memcpy(RTA_DATA(rta), &data, 4);
	n->nlmsg_len = NLMSG_ALIGN(n->nlmsg_len) + len;
	return 0;
}

int addattr_l(struct nlmsghdr *n, int maxlen, int type, const void *data,
	      int alen)
{
	int len = RTA_LENGTH(alen);
	struct rtattr *rta;

	if (NLMSG_ALIGN(n->nlmsg_len) + RTA_ALIGN(len) > maxlen) {
		fprintf(stderr,
			"addattr_l ERROR: message exceeded bound of %d\n",
			maxlen);
		return -1;
	}
	rta = NLMSG_TAIL(n);
	rta->rta_type = type;
	rta->rta_len = len;
	memcpy(RTA_DATA(rta), data, alen);
	n->nlmsg_len = NLMSG_ALIGN(n->nlmsg_len) + RTA_ALIGN(len);
	return 0;
}

int addraw_l(struct nlmsghdr *n, int maxlen, const void *data, int len)
{
	if (NLMSG_ALIGN(n->nlmsg_len) + NLMSG_ALIGN(len) > maxlen) {
		fprintf(stderr,
			"addraw_l ERROR: message exceeded bound of %d\n",
			maxlen);
		return -1;
	}

	memcpy(NLMSG_TAIL(n), data, len);
	memset((void *)NLMSG_TAIL(n) + len, 0, NLMSG_ALIGN(len) - len);
	n->nlmsg_len = NLMSG_ALIGN(n->nlmsg_len) + NLMSG_ALIGN(len);
	return 0;
}

int rta_addattr32(struct rtattr *rta, int maxlen, int type, __u32 data)
{
	int len = RTA_LENGTH(4);
	struct rtattr *subrta;

	if (RTA_ALIGN(rta->rta_len) + len > maxlen) {
		fprintf(stderr,
			"rta_addattr32: Error! max allowed bound %d exceeded\n",
			maxlen);
		return -1;
	}
	subrta = (struct rtattr *)(((char *)rta) + RTA_ALIGN(rta->rta_len));
	subrta->rta_type = type;
	subrta->rta_len = len;
	memcpy(RTA_DATA(subrta), &data, 4);
	rta->rta_len = NLMSG_ALIGN(rta->rta_len) + len;
	return 0;
}

int rta_addattr_l(struct rtattr *rta, int maxlen, int type,
		  const void *data, int alen)
{
	struct rtattr *subrta;
	int len = RTA_LENGTH(alen);

	if (RTA_ALIGN(rta->rta_len) + RTA_ALIGN(len) > maxlen) {
		fprintf(stderr,
			"rta_addattr_l: Error! max allowed bound %d exceeded\n",
			maxlen);
		return -1;
	}
	subrta = (struct rtattr *)(((char *)rta) + RTA_ALIGN(rta->rta_len));
	subrta->rta_type = type;
	subrta->rta_len = len;
	memcpy(RTA_DATA(subrta), data, alen);
	rta->rta_len = NLMSG_ALIGN(rta->rta_len) + RTA_ALIGN(len);
	return 0;
}

int parse_rtattr(struct rtattr *tb[], int max, struct rtattr *rta, int len)
{
	memset(tb, 0, sizeof(struct rtattr *) * (max + 1));
	while (RTA_OK(rta, len)) {
		if (rta->rta_type <= max)
			tb[rta->rta_type] = rta;
		rta = RTA_NEXT(rta, len);
	}
	if (len)
		fprintf(stderr, "!!!Deficit %d, rta_len=%d\n", len,
			rta->rta_len);
	return 0;
}

int parse_rtattr_byindex(struct rtattr *tb[], int max, struct rtattr *rta,
			 int len)
{
	int i = 0;

	memset(tb, 0, sizeof(struct rtattr *) * max);
	while (RTA_OK(rta, len)) {
		if (rta->rta_type <= max && i < max)
			tb[i++] = rta;
		rta = RTA_NEXT(rta, len);
	}
	if (len)
		fprintf(stderr, "!!!Deficit %d, rta_len=%d\n", len,
			rta->rta_len);
	return i;
}
Commit	Line	Data
ad02a0eb SH	1	/*
	2	* libnetlink.c RTnetlink service routines.
	3	*
	4	* This program is free software; you can redistribute it and/or
	5	* modify it under the terms of the GNU General Public License
	6	* as published by the Free Software Foundation; either version
	7	* 2 of the License, or (at your option) any later version.
	8	*
	9	* Authors: Alexey Kuznetsov, <kuznet@ms2.inr.ac.ru>
	10	*
	11	*/
	12
	13	#include <stdio.h>
	14	#include <stdlib.h>
	15	#include <unistd.h>
	16	#include <syslog.h>
	17	#include <fcntl.h>
	18	#include <net/if_arp.h>
	19	#include <sys/socket.h>
	20	#include <netinet/in.h>
	21	#include <string.h>
	22	#include <errno.h>
	23	#include <time.h>
	24	#include <sys/uio.h>
	25
	26	#include "libnetlink.h"
	27
	28	void rtnl_close(struct rtnl_handle *rth)
	29	{
	30	close(rth->fd);
	31	}
	32
	33	int rtnl_open_byproto(struct rtnl_handle *rth, unsigned subscriptions,
	34	int protocol)
	35	{
	36	socklen_t addr_len;
	37	int sndbuf = 32768;
	38	int rcvbuf = 32768;
	39
1b9e819c	40	memset(rth, 0, sizeof(*rth));
ad02a0eb SH	41
	42	rth->fd = socket(AF_NETLINK, SOCK_RAW, protocol);
	43	if (rth->fd < 0) {
	44	perror("Cannot open netlink socket");
	45	return -1;
	46	}
	47
11904a35 SH	48	if (setsockopt(rth->fd, SOL_SOCKET, SO_SNDBUF, &sndbuf, sizeof(sndbuf))
11904a35 SH	49	< 0) {
ad02a0eb SH	50	perror("SO_SNDBUF");
	51	return -1;
	52	}
	53
11904a35 SH	54	if (setsockopt(rth->fd, SOL_SOCKET, SO_RCVBUF, &rcvbuf, sizeof(rcvbuf))
11904a35 SH	55	< 0) {
ad02a0eb SH	56	perror("SO_RCVBUF");
	57	return -1;
	58	}
	59
	60	memset(&rth->local, 0, sizeof(rth->local));
	61	rth->local.nl_family = AF_NETLINK;
	62	rth->local.nl_groups = subscriptions;
	63
11904a35 SH	64	if (bind(rth->fd, (struct sockaddr *)&rth->local, sizeof(rth->local)) <
11904a35 SH	65	0) {
ad02a0eb SH	66	perror("Cannot bind netlink socket");
	67	return -1;
	68	}
	69	addr_len = sizeof(rth->local);
11904a35	70	if (getsockname(rth->fd, (struct sockaddr *)&rth->local, &addr_len) < 0) {
ad02a0eb SH	71	perror("Cannot getsockname");
	72	return -1;
	73	}
	74	if (addr_len != sizeof(rth->local)) {
	75	fprintf(stderr, "Wrong address length %d\n", addr_len);
	76	return -1;
	77	}
	78	if (rth->local.nl_family != AF_NETLINK) {
11904a35 SH	79	fprintf(stderr, "Wrong address family %d\n",
11904a35 SH	80	rth->local.nl_family);
ad02a0eb SH	81	return -1;
	82	}
	83	rth->seq = time(NULL);
	84	return 0;
	85	}
	86
	87	int rtnl_open(struct rtnl_handle *rth, unsigned subscriptions)
	88	{
	89	return rtnl_open_byproto(rth, subscriptions, NETLINK_ROUTE);
	90	}
	91
	92	int rtnl_wilddump_request(struct rtnl_handle *rth, int family, int type)
	93	{
	94	struct {
	95	struct nlmsghdr nlh;
	96	struct rtgenmsg g;
	97	} req;
	98	struct sockaddr_nl nladdr;
	99
	100	memset(&nladdr, 0, sizeof(nladdr));
	101	nladdr.nl_family = AF_NETLINK;
	102
	103	memset(&req, 0, sizeof(req));
	104	req.nlh.nlmsg_len = sizeof(req);
	105	req.nlh.nlmsg_type = type;
11904a35	106	req.nlh.nlmsg_flags = NLM_F_ROOT \| NLM_F_MATCH \| NLM_F_REQUEST;
ad02a0eb SH	107	req.nlh.nlmsg_pid = 0;
	108	req.nlh.nlmsg_seq = rth->dump = ++rth->seq;
	109	req.g.rtgen_family = family;
	110
11904a35 SH	111	return sendto(rth->fd, (void *)&req, sizeof(req), 0,
11904a35 SH	112	(struct sockaddr *)&nladdr, sizeof(nladdr));
ad02a0eb SH	113	}
	114
	115	int rtnl_send(struct rtnl_handle rth, const char buf, int len)
	116	{
	117	struct sockaddr_nl nladdr;
	118
	119	memset(&nladdr, 0, sizeof(nladdr));
	120	nladdr.nl_family = AF_NETLINK;
	121
11904a35 SH	122	return sendto(rth->fd, buf, len, 0, (struct sockaddr *)&nladdr,
11904a35 SH	123	sizeof(nladdr));
ad02a0eb SH	124	}
	125
	126	int rtnl_dump_request(struct rtnl_handle rth, int type, void req, int len)
	127	{
	128	struct nlmsghdr nlh;
	129	struct sockaddr_nl nladdr;
	130	struct iovec iov[2] = {
11904a35 SH	131	{.iov_base = &nlh,.iov_len = sizeof(nlh)}
	132	,
	133	{.iov_base = req,.iov_len = len}
ad02a0eb SH	134	};
	135	struct msghdr msg = {
	136	.msg_name = &nladdr,
11904a35	137	.msg_namelen = sizeof(nladdr),
ad02a0eb SH	138	.msg_iov = iov,
	139	.msg_iovlen = 2,
	140	};
	141
	142	memset(&nladdr, 0, sizeof(nladdr));
	143	nladdr.nl_family = AF_NETLINK;
	144
	145	nlh.nlmsg_len = NLMSG_LENGTH(len);
	146	nlh.nlmsg_type = type;
11904a35	147	nlh.nlmsg_flags = NLM_F_ROOT \| NLM_F_MATCH \| NLM_F_REQUEST;
ad02a0eb SH	148	nlh.nlmsg_pid = 0;
	149	nlh.nlmsg_seq = rth->dump = ++rth->seq;
	150
	151	return sendmsg(rth->fd, &msg, 0);
	152	}
	153
	154	int rtnl_dump_filter(struct rtnl_handle *rth,
	155	rtnl_filter_t filter,
11904a35	156	void arg1, rtnl_filter_t junk, void arg2)
ad02a0eb SH	157	{
	158	struct sockaddr_nl nladdr;
	159	struct iovec iov;
	160	struct msghdr msg = {
	161	.msg_name = &nladdr,
	162	.msg_namelen = sizeof(nladdr),
	163	.msg_iov = &iov,
	164	.msg_iovlen = 1,
	165	};
	166	char buf[16384];
	167
	168	iov.iov_base = buf;
	169	while (1) {
	170	int status;
	171	struct nlmsghdr *h;
	172
	173	iov.iov_len = sizeof(buf);
	174	status = recvmsg(rth->fd, &msg, 0);
	175
	176	if (status < 0) {
	177	if (errno == EINTR)
	178	continue;
	179	perror("OVERRUN");
	180	continue;
	181	}
	182
	183	if (status == 0) {
	184	fprintf(stderr, "EOF on netlink\n");
	185	return -1;
	186	}
	187
11904a35	188	h = (struct nlmsghdr *)buf;
ad02a0eb SH	189	while (NLMSG_OK(h, status)) {
	190	int err;
	191
	192	if (nladdr.nl_pid != 0 \|\|
	193	h->nlmsg_pid != rth->local.nl_pid \|\|
	194	h->nlmsg_seq != rth->dump) {
	195	if (junk) {
	196	err = junk(&nladdr, h, arg2);
	197	if (err < 0)
	198	return err;
	199	}
	200	goto skip_it;
	201	}
	202
	203	if (h->nlmsg_type == NLMSG_DONE)
	204	return 0;
	205	if (h->nlmsg_type == NLMSG_ERROR) {
11904a35 SH	206	struct nlmsgerr *err =
	207	(struct nlmsgerr *)NLMSG_DATA(h);
	208	if (h->nlmsg_len <
	209	NLMSG_LENGTH(sizeof(struct nlmsgerr))) {
ad02a0eb SH	210	fprintf(stderr, "ERROR truncated\n");
	211	} else {
	212	errno = -err->error;
	213	perror("RTNETLINK answers");
	214	}
	215	return -1;
	216	}
	217	err = filter(&nladdr, h, arg1);
	218	if (err < 0)
	219	return err;
	220
11904a35	221	skip_it:
ad02a0eb SH	222	h = NLMSG_NEXT(h, status);
	223	}
	224	if (msg.msg_flags & MSG_TRUNC) {
	225	fprintf(stderr, "Message truncated\n");
	226	continue;
	227	}
	228	if (status) {
	229	fprintf(stderr, "!!!Remnant of size %d\n", status);
5167b82c	230	return -1;
ad02a0eb SH	231	}
	232	}
	233	}
	234
	235	int rtnl_talk(struct rtnl_handle rtnl, struct nlmsghdr n, pid_t peer,
	236	unsigned groups, struct nlmsghdr *answer,
11904a35	237	rtnl_filter_t junk, void *jarg)
ad02a0eb SH	238	{
	239	int status;
	240	unsigned seq;
	241	struct nlmsghdr *h;
	242	struct sockaddr_nl nladdr;
	243	struct iovec iov = {
11904a35	244	.iov_base = (void *)n,
ad02a0eb SH	245	.iov_len = n->nlmsg_len
	246	};
	247	struct msghdr msg = {
	248	.msg_name = &nladdr,
	249	.msg_namelen = sizeof(nladdr),
	250	.msg_iov = &iov,
	251	.msg_iovlen = 1,
	252	};
11904a35	253	char buf[16384];
ad02a0eb SH	254
	255	memset(&nladdr, 0, sizeof(nladdr));
	256	nladdr.nl_family = AF_NETLINK;
	257	nladdr.nl_pid = peer;
	258	nladdr.nl_groups = groups;
	259
	260	n->nlmsg_seq = seq = ++rtnl->seq;
	261
	262	if (answer == NULL)
	263	n->nlmsg_flags \|= NLM_F_ACK;
	264
	265	status = sendmsg(rtnl->fd, &msg, 0);
	266
	267	if (status < 0) {
	268	perror("Cannot talk to rtnetlink");
	269	return -1;
	270	}
	271
11904a35	272	memset(buf, 0, sizeof(buf));
ad02a0eb SH	273
	274	iov.iov_base = buf;
	275
	276	while (1) {
	277	iov.iov_len = sizeof(buf);
	278	status = recvmsg(rtnl->fd, &msg, 0);
	279
	280	if (status < 0) {
	281	if (errno == EINTR)
	282	continue;
	283	perror("OVERRUN");
	284	continue;
	285	}
	286	if (status == 0) {
	287	fprintf(stderr, "EOF on netlink\n");
	288	return -1;
	289	}
	290	if (msg.msg_namelen != sizeof(nladdr)) {
11904a35 SH	291	fprintf(stderr, "sender address length == %d\n",
11904a35 SH	292	msg.msg_namelen);
5167b82c	293	return -1;
ad02a0eb	294	}
11904a35	295	for (h = (struct nlmsghdr )buf; status >= sizeof(h);) {
ad02a0eb SH	296	int err;
	297	int len = h->nlmsg_len;
	298	int l = len - sizeof(*h);
	299
11904a35	300	if (l < 0 \|\| len > status) {
ad02a0eb SH	301	if (msg.msg_flags & MSG_TRUNC) {
	302	fprintf(stderr, "Truncated message\n");
	303	return -1;
	304	}
11904a35 SH	305	fprintf(stderr,
11904a35 SH	306	"!!!malformed message: len=%d\n", len);
5167b82c	307	return -1;
ad02a0eb SH	308	}
	309
	310	if (nladdr.nl_pid != peer \|\|
	311	h->nlmsg_pid != rtnl->local.nl_pid \|\|
	312	h->nlmsg_seq != seq) {
	313	if (junk) {
	314	err = junk(&nladdr, h, jarg);
	315	if (err < 0)
	316	return err;
	317	}
	318	/* Don't forget to skip that message. */
	319	status -= NLMSG_ALIGN(len);
11904a35 SH	320	h = (struct nlmsghdr )((char )h +
11904a35 SH	321	NLMSG_ALIGN(len));
ad02a0eb SH	322	continue;
	323	}
	324
	325	if (h->nlmsg_type == NLMSG_ERROR) {
11904a35 SH	326	struct nlmsgerr *err =
11904a35 SH	327	(struct nlmsgerr *)NLMSG_DATA(h);
ad02a0eb SH	328	if (l < sizeof(struct nlmsgerr)) {
	329	fprintf(stderr, "ERROR truncated\n");
	330	} else {
	331	errno = -err->error;
	332	if (errno == 0) {
	333	if (answer)
11904a35 SH	334	memcpy(answer, h,
11904a35 SH	335	h->nlmsg_len);
ad02a0eb SH	336	return 0;
	337	}
	338	perror("RTNETLINK answers");
	339	}
	340	return -1;
	341	}
	342	if (answer) {
	343	memcpy(answer, h, h->nlmsg_len);
	344	return 0;
	345	}
	346
	347	fprintf(stderr, "Unexpected reply!!!\n");
	348
	349	status -= NLMSG_ALIGN(len);
11904a35	350	h = (struct nlmsghdr )((char )h + NLMSG_ALIGN(len));
ad02a0eb SH	351	}
	352	if (msg.msg_flags & MSG_TRUNC) {
	353	fprintf(stderr, "Message truncated\n");
	354	continue;
	355	}
	356	if (status) {
	357	fprintf(stderr, "!!!Remnant of size %d\n", status);
5167b82c	358	return -1;
ad02a0eb SH	359	}
	360	}
	361	}
	362
11904a35	363	int rtnl_listen(struct rtnl_handle rtnl, rtnl_filter_t handler, void jarg)
ad02a0eb SH	364	{
	365	int status;
	366	struct nlmsghdr *h;
	367	struct sockaddr_nl nladdr;
	368	struct iovec iov;
	369	struct msghdr msg = {
	370	.msg_name = &nladdr,
	371	.msg_namelen = sizeof(nladdr),
	372	.msg_iov = &iov,
	373	.msg_iovlen = 1,
	374	};
11904a35	375	char buf[8192];
ad02a0eb SH	376
	377	memset(&nladdr, 0, sizeof(nladdr));
	378	nladdr.nl_family = AF_NETLINK;
	379	nladdr.nl_pid = 0;
	380	nladdr.nl_groups = 0;
	381
	382	iov.iov_base = buf;
	383	while (1) {
	384	iov.iov_len = sizeof(buf);
	385	status = recvmsg(rtnl->fd, &msg, 0);
	386
	387	if (status < 0) {
	388	if (errno == EINTR)
	389	continue;
11904a35 SH	390	if (errno == EAGAIN)
11904a35 SH	391	return 0;
ad02a0eb	392	perror("OVERRUN");
11904a35	393	return -1;
ad02a0eb SH	394	}
	395	if (status == 0) {
	396	fprintf(stderr, "EOF on netlink\n");
	397	return -1;
	398	}
	399	if (msg.msg_namelen != sizeof(nladdr)) {
11904a35 SH	400	fprintf(stderr, "Sender address length == %d\n",
11904a35 SH	401	msg.msg_namelen);
5167b82c	402	return -1;
ad02a0eb	403	}
11904a35	404	for (h = (struct nlmsghdr )buf; status >= sizeof(h);) {
ad02a0eb SH	405	int err;
	406	int len = h->nlmsg_len;
	407	int l = len - sizeof(*h);
	408
11904a35	409	if (l < 0 \|\| len > status) {
ad02a0eb SH	410	if (msg.msg_flags & MSG_TRUNC) {
	411	fprintf(stderr, "Truncated message\n");
	412	return -1;
	413	}
11904a35 SH	414	fprintf(stderr,
11904a35 SH	415	"!!!malformed message: len=%d\n", len);
5167b82c	416	return -1;
ad02a0eb SH	417	}
	418
	419	err = handler(&nladdr, h, jarg);
	420	if (err < 0) {
11904a35 SH	421	fprintf(stderr, "Handler returned %d\n", err);
	422	return err;
	423	}
ad02a0eb SH	424
ad02a0eb SH	425	status -= NLMSG_ALIGN(len);
11904a35	426	h = (struct nlmsghdr )((char )h + NLMSG_ALIGN(len));
ad02a0eb SH	427	}
	428	if (msg.msg_flags & MSG_TRUNC) {
	429	fprintf(stderr, "Message truncated\n");
	430	continue;
	431	}
	432	if (status) {
	433	fprintf(stderr, "!!!Remnant of size %d\n", status);
5167b82c	434	return -1;
ad02a0eb SH	435	}
	436	}
	437	}
	438
11904a35	439	int rtnl_from_file(FILE * rtnl, rtnl_filter_t handler, void *jarg)
ad02a0eb SH	440	{
	441	int status;
	442	struct sockaddr_nl nladdr;
11904a35 SH	443	char buf[8192];
11904a35 SH	444	struct nlmsghdr h = (void )buf;
ad02a0eb SH	445
	446	memset(&nladdr, 0, sizeof(nladdr));
	447	nladdr.nl_family = AF_NETLINK;
	448	nladdr.nl_pid = 0;
	449	nladdr.nl_groups = 0;
	450
	451	while (1) {
394cd57d	452	int err, len;
ad02a0eb SH	453	int l;
	454
	455	status = fread(&buf, 1, sizeof(*h), rtnl);
	456
	457	if (status < 0) {
	458	if (errno == EINTR)
	459	continue;
	460	perror("rtnl_from_file: fread");
	461	return -1;
	462	}
	463	if (status == 0)
	464	return 0;
	465
	466	len = h->nlmsg_len;
ad02a0eb SH	467	l = len - sizeof(*h);
ad02a0eb SH	468
11904a35	469	if (l < 0 \|\| len > sizeof(buf)) {
ad02a0eb SH	470	fprintf(stderr, "!!!malformed message: len=%d @%lu\n",
	471	len, ftell(rtnl));
	472	return -1;
	473	}
	474
	475	status = fread(NLMSG_DATA(h), 1, NLMSG_ALIGN(l), rtnl);
	476
	477	if (status < 0) {
	478	perror("rtnl_from_file: fread");
	479	return -1;
	480	}
	481	if (status < l) {
	482	fprintf(stderr, "rtnl-from_file: truncated message\n");
	483	return -1;
	484	}
	485
	486	err = handler(&nladdr, h, jarg);
	487	if (err < 0)
	488	return err;
	489	}
	490	}
	491
	492	int addattr32(struct nlmsghdr *n, int maxlen, int type, __u32 data)
	493	{
	494	int len = RTA_LENGTH(4);
	495	struct rtattr *rta;
	496	if (NLMSG_ALIGN(n->nlmsg_len) + len > maxlen) {
11904a35 SH	497	fprintf(stderr,
	498	"addattr32: Error! max allowed bound %d exceeded\n",
	499	maxlen);
ad02a0eb SH	500	return -1;
	501	}
	502	rta = NLMSG_TAIL(n);
	503	rta->rta_type = type;
	504	rta->rta_len = len;
	505	memcpy(RTA_DATA(rta), &data, 4);
	506	n->nlmsg_len = NLMSG_ALIGN(n->nlmsg_len) + len;
	507	return 0;
	508	}
	509
	510	int addattr_l(struct nlmsghdr n, int maxlen, int type, const void data,
	511	int alen)
	512	{
	513	int len = RTA_LENGTH(alen);
	514	struct rtattr *rta;
	515
	516	if (NLMSG_ALIGN(n->nlmsg_len) + RTA_ALIGN(len) > maxlen) {
11904a35 SH	517	fprintf(stderr,
	518	"addattr_l ERROR: message exceeded bound of %d\n",
	519	maxlen);
ad02a0eb SH	520	return -1;
	521	}
	522	rta = NLMSG_TAIL(n);
	523	rta->rta_type = type;
	524	rta->rta_len = len;
	525	memcpy(RTA_DATA(rta), data, alen);
	526	n->nlmsg_len = NLMSG_ALIGN(n->nlmsg_len) + RTA_ALIGN(len);
	527	return 0;
	528	}
	529
	530	int addraw_l(struct nlmsghdr n, int maxlen, const void data, int len)
	531	{
	532	if (NLMSG_ALIGN(n->nlmsg_len) + NLMSG_ALIGN(len) > maxlen) {
11904a35 SH	533	fprintf(stderr,
	534	"addraw_l ERROR: message exceeded bound of %d\n",
	535	maxlen);
ad02a0eb SH	536	return -1;
	537	}
	538
	539	memcpy(NLMSG_TAIL(n), data, len);
11904a35	540	memset((void *)NLMSG_TAIL(n) + len, 0, NLMSG_ALIGN(len) - len);
ad02a0eb SH	541	n->nlmsg_len = NLMSG_ALIGN(n->nlmsg_len) + NLMSG_ALIGN(len);
	542	return 0;
	543	}
	544
	545	int rta_addattr32(struct rtattr *rta, int maxlen, int type, __u32 data)
	546	{
	547	int len = RTA_LENGTH(4);
	548	struct rtattr *subrta;
	549
	550	if (RTA_ALIGN(rta->rta_len) + len > maxlen) {
11904a35 SH	551	fprintf(stderr,
	552	"rta_addattr32: Error! max allowed bound %d exceeded\n",
	553	maxlen);
ad02a0eb SH	554	return -1;
ad02a0eb SH	555	}
11904a35	556	subrta = (struct rtattr )(((char )rta) + RTA_ALIGN(rta->rta_len));
ad02a0eb SH	557	subrta->rta_type = type;
	558	subrta->rta_len = len;
	559	memcpy(RTA_DATA(subrta), &data, 4);
	560	rta->rta_len = NLMSG_ALIGN(rta->rta_len) + len;
	561	return 0;
	562	}
	563
	564	int rta_addattr_l(struct rtattr *rta, int maxlen, int type,
	565	const void *data, int alen)
	566	{
	567	struct rtattr *subrta;
	568	int len = RTA_LENGTH(alen);
	569
	570	if (RTA_ALIGN(rta->rta_len) + RTA_ALIGN(len) > maxlen) {
11904a35 SH	571	fprintf(stderr,
	572	"rta_addattr_l: Error! max allowed bound %d exceeded\n",
	573	maxlen);
ad02a0eb SH	574	return -1;
ad02a0eb SH	575	}
11904a35	576	subrta = (struct rtattr )(((char )rta) + RTA_ALIGN(rta->rta_len));
ad02a0eb SH	577	subrta->rta_type = type;
	578	subrta->rta_len = len;
	579	memcpy(RTA_DATA(subrta), data, alen);
	580	rta->rta_len = NLMSG_ALIGN(rta->rta_len) + RTA_ALIGN(len);
	581	return 0;
	582	}
	583
	584	int parse_rtattr(struct rtattr tb[], int max, struct rtattr rta, int len)
	585	{
	586	memset(tb, 0, sizeof(struct rtattr ) (max + 1));
	587	while (RTA_OK(rta, len)) {
	588	if (rta->rta_type <= max)
	589	tb[rta->rta_type] = rta;
11904a35	590	rta = RTA_NEXT(rta, len);
ad02a0eb SH	591	}
ad02a0eb SH	592	if (len)
11904a35 SH	593	fprintf(stderr, "!!!Deficit %d, rta_len=%d\n", len,
11904a35 SH	594	rta->rta_len);
ad02a0eb SH	595	return 0;
	596	}
	597
11904a35 SH	598	int parse_rtattr_byindex(struct rtattr tb[], int max, struct rtattr rta,
11904a35 SH	599	int len)
ad02a0eb SH	600	{
	601	int i = 0;
	602
	603	memset(tb, 0, sizeof(struct rtattr ) max);
	604	while (RTA_OK(rta, len)) {
	605	if (rta->rta_type <= max && i < max)
	606	tb[i++] = rta;
11904a35	607	rta = RTA_NEXT(rta, len);
ad02a0eb SH	608	}
ad02a0eb SH	609	if (len)
11904a35 SH	610	fprintf(stderr, "!!!Deficit %d, rta_len=%d\n", len,
11904a35 SH	611	rta->rta_len);
ad02a0eb SH	612	return i;
ad02a0eb SH	613	}