cmm/src/rtnl.c

/*
 *
 *  Copyright (C) 2007 Mindspeed Technologies, Inc.
 *
 * 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.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */
#include "cmm.h"

/*********************************************************************************************/

/*
The following routines were derived from the libnetlink.c file in the iproute2 package.
Copyright notice from that file follows...
*/

/*
 * 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>
 *
 */

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

int cmm_rtnl_fd(struct rtnl_handle *rth)
{
	return rth->fd;
}

unsigned int cmm_rtnl_rcvbufsiz(struct rtnl_handle *rth, unsigned int size)
{
	socklen_t socklen = sizeof(size);
	unsigned int read_size = 0;

	if (setsockopt(rth->fd, SOL_SOCKET, SO_RCVBUFFORCE, &size, socklen) < 0) {
		/* if this didn't work, we try at least to get the system
		 * wide maximum (or whatever the user requested) */
		setsockopt(rth->fd, SOL_SOCKET, SO_RCVBUF, &size, socklen);
	}

	getsockopt(rth->fd, SOL_SOCKET, SO_RCVBUF, &read_size, &socklen);

	return read_size;
}

int cmm_rtnl_set_nonblocking_mode(struct rtnl_handle *rth)
{
	if (fcntl(rth->fd, F_SETFL, O_NONBLOCK) < 0)
	{
		cmm_print(DEBUG_ERROR, "%s: fcntl(%d) failed %s\n", __func__, rth->fd, strerror(errno));
		goto err0;
	}

	rth->flags = RTNL_NON_BLOCKING_MODE;

	return 0;

err0:
	return -1;
}
int cmm_nl_open(struct rtnl_handle *rth, unsigned subscriptions, unsigned int group)
{
	struct sockaddr_nl nladdr;

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

	rth->fd = socket(PF_NETLINK, SOCK_RAW, group);
	if (rth->fd < 0) {
		cmm_print(DEBUG_ERROR, "%s::%d: socket() %s\n", __func__, __LINE__, strerror(errno));
		goto err0;
	}

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

	if (bind(rth->fd, (struct sockaddr*)&nladdr, sizeof(nladdr)) < 0) {
		cmm_print(DEBUG_ERROR, "%s::%d: bind() %s\n", __func__, __LINE__, strerror(errno));
		goto err1;
	}

	return 0;

err1:
	close(rth->fd);

err0:
	return -1;

}

int inline cmm_rtnl_open(struct rtnl_handle *rth, unsigned subscriptions)
{
	return cmm_nl_open(rth, subscriptions, NETLINK_ROUTE);
}

int cmm_rtnl_neigh_dump_request(struct rtnl_handle *rth, int family)
{
	struct ndmsg ndm = {
		.ndm_family = family,
		.ndm_ifindex = 0,
		.ndm_state = 0,
		.ndm_flags = 0,
		.ndm_type = 0,
	};

	return cmm_rtnl_dump_request(rth, RTM_GETNEIGH, &ndm, sizeof(struct ndmsg));
}

void cmm_nlh_init(struct nlmsghdr *nlh, unsigned int len, unsigned short type, unsigned short flags)
{
	memset(nlh, 0, sizeof(struct nlmsghdr));
	nlh->nlmsg_len = NLMSG_SPACE(len);
	nlh->nlmsg_type = type;
	nlh->nlmsg_flags = flags;
	nlh->nlmsg_pid = 0;
	nlh->nlmsg_seq = 0;
}

int cmm_rtnl_send(struct rtnl_handle *rth, struct nlmsghdr *nlh)
{
	struct sockaddr_nl nladdr;
	struct iovec iov = {
		.iov_base = nlh, .iov_len = nlh->nlmsg_len,
	};
	struct msghdr msg= {
                .msg_name = &nladdr,
                .msg_namelen =  sizeof(nladdr),
                .msg_iov = &iov,
                .msg_iovlen = 1,
        };
	int rc;

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

	if ((rc = sendmsg(rth->fd, &msg, 0)) < 0) {
		cmm_print(DEBUG_ERROR, "%s::%d: sendmsg() %s\n", __func__, __LINE__, strerror(errno));
	}

	return rc;
}

int cmm_rtnl_dump_request(struct rtnl_handle *rth, int type, void *req, int len)
{
	char buf[NLMSG_SPACE(len)] __attribute__ ((aligned (4)));
	struct nlmsghdr *nlh = (struct nlmsghdr *)buf;

	cmm_nlh_init(nlh, len, type, NLM_F_REQUEST | NLM_F_DUMP);

	memcpy(NLMSG_DATA(nlh), req, len);

	return cmm_rtnl_send(rth, nlh);
}

int cmm_rtnl_listen(struct rtnl_handle *rth, rtnl_filter_t handler, void *jarg)
{
	int len;
	struct nlmsghdr *nlh;
	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[64 * 1024] __attribute__ ((aligned (4)));
	int rc;

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

	iov.iov_base = buf;
	do {
		iov.iov_len = sizeof(buf);
		len = recvmsg(rth->fd, &msg, 0);

		if (len < 0) {
			if (errno == EINTR)
				continue;

			if (errno == EAGAIN)
				goto err;

			cmm_print(DEBUG_ERROR, "%s::%d: recvmsg() %s\n", __func__, __LINE__, strerror(errno));

			goto err;
		}

		if (len == 0) {
			cmm_print(DEBUG_ERROR, "%s::%d: EOF on netlink\n", __func__, __LINE__);
			goto err;
		}

		if (msg.msg_namelen != sizeof(nladdr)) {
			cmm_print(DEBUG_ERROR, "%s::%d: wrong sender address len(%d)\n", __func__, __LINE__, msg.msg_namelen);
			goto err;
		}

		nlh = (struct nlmsghdr *)buf;

		while (NLMSG_OK(nlh, len)) {

			if (nlh->nlmsg_type == NLMSG_ERROR) {
				struct nlmsgerr *err = NLMSG_DATA(nlh);

				cmm_print(DEBUG_ERROR, "%s::%d: netlink error %s\n", __func__, __LINE__, strerror(-err->error));
				goto err;
			}

			if (nlh->nlmsg_type == NLMSG_DONE)
				goto out;

			rc = handler(&nladdr, nlh, jarg);
			if (rc <= RTNL_CB_STOP) {
				if (rc == RTNL_CB_STOP)
					goto out;
 
				goto err;
			}

			nlh = NLMSG_NEXT(nlh, len);
		}

		if (msg.msg_flags & MSG_TRUNC) {
			cmm_print(DEBUG_ERROR, "%s::%d: truncated message\n", __func__, __LINE__);
			goto err;
		}

	} while (!(rth->flags & RTNL_NON_BLOCKING_MODE));

out:
	return 0;

err:
	return -1;
}

int cmm_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)
		cmm_print(DEBUG_ERROR, "%s::%d: payload too long %d %d\n", __func__, __LINE__, len, rta->rta_len);

	return 0;
}

struct rtattr *cmm_get_rtattr(struct rtattr *rta, int len, int type)
{
	while (RTA_OK(rta, len)) {

		if (rta->rta_type == type)
			goto found;

		rta = RTA_NEXT(rta, len);
	}

	return NULL;

found:
	return rta;
}


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

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

int cmm_addattr16(struct nlmsghdr *n, int maxlen, int type, __u16 data)
{
	int len = RTA_LENGTH(2);
	struct rtattr *rta;

	rta = NLMSG_TAIL(n);
	rta->rta_type = type;
	rta->rta_len = len;
	memcpy(RTA_DATA(rta), &data, 2);
	n->nlmsg_len = NLMSG_ALIGN(n->nlmsg_len) + len;
	return 0;
}

int cmm_addattr32(struct nlmsghdr *n, int maxlen, int type, __u32 data)
{
	int len = RTA_LENGTH(4);
	struct rtattr *rta;

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