[mirror_iproute2.git] / lib / 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)
{
	int 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;

	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, 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] = { { &nlh, sizeof(nlh) }, { req, len } };
	struct msghdr msg = {
		(void*)&nladdr, sizeof(nladdr),
		iov,	2,
		NULL,	0,
		0
	};

	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,
		     int (*filter)(struct sockaddr_nl *, struct nlmsghdr *n, void *),
		     void *arg1,
		     int (*junk)(struct sockaddr_nl *,struct nlmsghdr *n, void *),
		     void *arg2)
{
	char	buf[16384];
	struct sockaddr_nl nladdr;
	struct iovec iov = { buf, sizeof(buf) };

	while (1) {
		int status;
		struct nlmsghdr *h;

		struct msghdr msg = {
			(void*)&nladdr, sizeof(nladdr),
			&iov,	1,
			NULL,	0,
			0
		};

		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;
		}
		if (msg.msg_namelen != sizeof(nladdr)) {
			fprintf(stderr, "sender address length == %d\n", msg.msg_namelen);
			exit(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);
			exit(1);
		}
	}
}

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

	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);
			exit(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);
				exit(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;
				}
				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);
			exit(1);
		}
	}
}

int rtnl_listen(struct rtnl_handle *rtnl, 
	      int (*handler)(struct sockaddr_nl *,struct nlmsghdr *n, void *),
	      void *jarg)
{
	int status;
	struct nlmsghdr *h;
	struct sockaddr_nl nladdr;
	struct iovec iov;
	char   buf[8192];
	struct msghdr msg = {
		(void*)&nladdr, sizeof(nladdr),
		&iov,	1,
		NULL,	0,
		0
	};

	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;
			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);
			exit(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);
				exit(1);
			}

			err = handler(&nladdr, h, jarg);
			if (err < 0)
				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);
			exit(1);
		}
	}
}

int rtnl_from_file(FILE *rtnl, 
	      int (*handler)(struct sockaddr_nl *,struct nlmsghdr *n, void *),
	      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, type;
		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;
		type= h->nlmsg_type;
		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 = (struct rtattr*)(((char*)n) + NLMSG_ALIGN(n->nlmsg_len));
	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, void *data, int alen)
{
	int len = RTA_LENGTH(alen);
	struct rtattr *rta;

	if (NLMSG_ALIGN(n->nlmsg_len) + len > maxlen) {
		fprintf(stderr, "addattr_l ERROR: message exceeded bound of %d\n",maxlen);
		return -1;
	}
	rta = (struct rtattr*)(((char*)n) + NLMSG_ALIGN(n->nlmsg_len));
	rta->rta_type = type;
	rta->rta_len = len;
	memcpy(RTA_DATA(rta), data, alen);
	n->nlmsg_len = NLMSG_ALIGN(n->nlmsg_len) + 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, void *data, int alen)
{
	struct rtattr *subrta;
	int len = RTA_LENGTH(alen);

	if (RTA_ALIGN(rta->rta_len) + 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) + len;
	return 0;
}


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