ofono/gisi/netlink.c

/*
 *
 *  oFono - Open Source Telephony
 *
 *  Copyright (C) 2009-2010  Nokia Corporation and/or its subsidiary(-ies).
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License version 2 as
 *  published by the Free Software Foundation.
 *
 *  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., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 */

#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <sys/types.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/socket.h>
#ifndef SOL_NETLINK
#define SOL_NETLINK 270 /* libc!? */
#endif
#include "phonet.h"
#include <linux/rtnetlink.h>
#include <linux/if.h>
#include <sys/ioctl.h>
#include <errno.h>
#include <glib.h>

#include "netlink.h"

#ifndef ARPHRD_PHONET
#define ARPHRD_PHONET (820)
#endif

/*
 * GCC -Wcast-align does not like rtlink alignment macros,
 * fixed macros by Andrzej Zaborowski <balrogg@gmail.com>.
 */
#undef IFA_RTA
#define IFA_RTA(r)  ((struct rtattr *)(void *)(((char *)(r)) \
	+ NLMSG_ALIGN(sizeof(struct ifaddrmsg))))

#undef IFLA_RTA
#define IFLA_RTA(r)  ((struct rtattr *)(void *)(((char *)(r)) \
	+ NLMSG_ALIGN(sizeof(struct ifinfomsg))))

#undef NLMSG_NEXT
#define NLMSG_NEXT(nlh, len)	 ((len) -= NLMSG_ALIGN((nlh)->nlmsg_len), \
	(struct nlmsghdr *)(void *)(((char *)(nlh)) \
	+ NLMSG_ALIGN((nlh)->nlmsg_len)))

#undef RTA_NEXT
#define RTA_NEXT(rta, attrlen)	((attrlen) -= RTA_ALIGN((rta)->rta_len), \
	(struct rtattr *)(void *)(((char *)(rta)) \
	+ RTA_ALIGN((rta)->rta_len)))

#define SIZE_NLMSG (16384)

struct _GIsiPhonetNetlink {
	GIsiModem *modem;
	GIsiPhonetNetlinkFunc callback;
	void *opaque;
	guint watch;
};

static GSList *netlink_list;

static void bring_up(unsigned ifindex)
{
	struct ifreq req = { .ifr_ifindex = ifindex, };
	int fd = socket(PF_LOCAL, SOCK_DGRAM, 0);

	if (ioctl(fd, SIOCGIFNAME, &req)
		|| ioctl(fd, SIOCGIFFLAGS, &req))
		goto error;

	req.ifr_flags |= IFF_UP | IFF_RUNNING;
	ioctl(fd, SIOCSIFFLAGS, &req);
error:
	close(fd);
}

static int pn_netlink_socket(void)
{
	int fd;
	int bufsize = SIZE_NLMSG;

	fd = socket(PF_NETLINK, SOCK_DGRAM, NETLINK_ROUTE);
	if (fd == -1)
		return -1;

	if (setsockopt(fd, SOL_SOCKET, SO_RCVBUF, &bufsize, sizeof(bufsize))) {
		int error = errno;
		close(fd), fd = -1;
		errno = error;
	}

	return fd;
}

static void pn_netlink_link(GIsiPhonetNetlink *self, struct nlmsghdr *nlh)
{
	const struct ifinfomsg *ifi;
	const struct rtattr *rta;
	int len;
	const char *ifname = NULL;
	enum GIsiPhonetLinkState st;
	unsigned interface;

	ifi = NLMSG_DATA(nlh);
	len = IFA_PAYLOAD(nlh);

	if (ifi->ifi_type != ARPHRD_PHONET)
		return;

	interface = g_isi_modem_index(self->modem);
	if (interface != 0 && interface != (unsigned)ifi->ifi_index)
		return;

#define UP (IFF_UP | IFF_LOWER_UP | IFF_RUNNING)

	if (nlh->nlmsg_type == RTM_DELLINK)
		st = PN_LINK_REMOVED;
	else if ((ifi->ifi_flags & UP) != UP)
		st = PN_LINK_DOWN;
	else
		st = PN_LINK_UP;

	for (rta = IFLA_RTA(ifi); RTA_OK(rta, len);
		rta = RTA_NEXT(rta, len)) {

		if (rta->rta_type == IFLA_IFNAME)
			ifname = RTA_DATA(rta);
	}

	if (ifname && self->modem)
		self->callback(self->modem, st, ifname, self->opaque);

#undef UP
}

/* Parser Netlink messages */
static gboolean pn_netlink_process(GIOChannel *channel, GIOCondition cond,
					gpointer data)
{
	struct {
		struct nlmsghdr nlh;
		char buf[SIZE_NLMSG];
	} resp;
	struct iovec iov = { &resp, sizeof(resp), };
	struct msghdr msg = { .msg_iov = &iov, .msg_iovlen = 1, };
	ssize_t ret;
	struct nlmsghdr *nlh;
	int fd = g_io_channel_unix_get_fd(channel);
	GIsiPhonetNetlink *self = data;

	if (cond & (G_IO_NVAL|G_IO_HUP))
		return FALSE;

	ret = recvmsg(fd, &msg, 0);
	if (ret == -1)
		return TRUE;

	if (msg.msg_flags & MSG_TRUNC) {
		g_printerr("Netlink message of %zu bytes truncated at %zu\n",
				ret, sizeof(resp));
		return TRUE;
	}

	for (nlh = &resp.nlh; NLMSG_OK(nlh, (size_t)ret);
			nlh = NLMSG_NEXT(nlh, ret)) {

		if (nlh->nlmsg_type == NLMSG_DONE)
			break;

		switch (nlh->nlmsg_type) {
		case NLMSG_ERROR: {
			struct nlmsgerr *err = NLMSG_DATA(nlh);
			if (err->error)
				g_printerr("Netlink error: %s",
						strerror(-err->error));
			return TRUE;
		}
		case RTM_NEWLINK:
		case RTM_DELLINK:
			pn_netlink_link(self, nlh);
			break;
		}
	}
	return TRUE;
}

/* Dump current links */
static int pn_netlink_getlink(int fd)
{
	struct {
		struct nlmsghdr nlh;
		struct ifinfomsg ifi;
	} req = {
		.nlh = {
			.nlmsg_type = RTM_GETLINK,
			.nlmsg_len = sizeof(req),
			.nlmsg_flags = NLM_F_REQUEST | NLM_F_ROOT | NLM_F_MATCH,
			.nlmsg_pid = getpid(),
		},
		.ifi = {
			.ifi_family = AF_UNSPEC,
			.ifi_type = ARPHRD_PHONET,
			.ifi_change = 0xffFFffFF,
		}
	};

	struct sockaddr_nl addr = { .nl_family = AF_NETLINK, };

	return sendto(fd, &req, sizeof(req), 0,
		      (struct sockaddr *)&addr, sizeof(addr));
}

GIsiPhonetNetlink *g_isi_pn_netlink_by_modem(GIsiModem *modem)
{
	GSList *m;

	for (m = netlink_list; m; m = m->next) {
		GIsiPhonetNetlink *self = m->data;

		if (g_isi_modem_index(modem) == g_isi_modem_index(self->modem))
			return self;
	}

	return NULL;
}

GIsiPhonetNetlink *g_isi_pn_netlink_start(GIsiModem *modem,
						GIsiPhonetNetlinkFunc cb,
						void *data)
{
	GIOChannel *chan;
	GIsiPhonetNetlink *self;
	int fd;
	unsigned group = RTNLGRP_LINK;
	unsigned interface;

	fd = pn_netlink_socket();
	if (fd == -1)
		return NULL;

	self = g_try_new0(GIsiPhonetNetlink, 1);
	if (self == NULL)
		goto error;

	fcntl(fd, F_SETFL, O_NONBLOCK | fcntl(fd, F_GETFL));

	if (setsockopt(fd, SOL_NETLINK, NETLINK_ADD_MEMBERSHIP,
		       &group, sizeof(group)))
		goto error;

	interface = g_isi_modem_index(modem);
	if (interface)
		bring_up(interface);

	pn_netlink_getlink(fd);

	chan = g_io_channel_unix_new(fd);
	if (chan == NULL)
		goto error;

	g_io_channel_set_close_on_unref(chan, TRUE);
	g_io_channel_set_encoding(chan, NULL, NULL);
	g_io_channel_set_buffered(chan, FALSE);

	self->callback = cb;
	self->opaque = data;
	self->modem = modem;
	self->watch = g_io_add_watch(chan, G_IO_IN|G_IO_ERR|G_IO_HUP,
					pn_netlink_process, self);
	g_io_channel_unref(chan);

	netlink_list = g_slist_prepend(netlink_list, self);

	return self;

error:
	close(fd);
	free(self);
	return NULL;
}

void g_isi_pn_netlink_stop(GIsiPhonetNetlink *self)
{
	if (self == NULL)
		return;

	netlink_list = g_slist_remove(netlink_list, self);
	g_source_remove(self->watch);
	g_free(self);
}

static int pn_netlink_getack(int fd)
{
	struct {
		struct nlmsghdr nlh;
		char buf[SIZE_NLMSG];
	} resp;
	struct iovec iov = { &resp, sizeof(resp), };
	struct msghdr msg = { .msg_iov = &iov, .msg_iovlen = 1, };
	ssize_t ret;
	struct nlmsghdr *nlh = &resp.nlh;

	ret = recvmsg(fd, &msg, 0);
	if (ret == -1)
		return -errno;

	if (msg.msg_flags & MSG_TRUNC)
		return -EIO;

	for (; NLMSG_OK(nlh, (size_t)ret); nlh = NLMSG_NEXT(nlh, ret)) {

		if (nlh->nlmsg_type == NLMSG_DONE)
			return 0;

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

	return -EIO;
}

/* Set local address */
static int pn_netlink_setaddr(uint32_t ifa_index, uint8_t ifa_local)
{
	struct ifaddrmsg *ifa;
	struct rtattr *rta;
	uint32_t reqlen = NLMSG_LENGTH(NLMSG_ALIGN(sizeof(*ifa))
				+ RTA_SPACE(1));
	struct req {
		struct nlmsghdr nlh;
		char buf[512];
	} req = {
		.nlh = {
			.nlmsg_flags = NLM_F_REQUEST | NLM_F_ACK,
			.nlmsg_type = RTM_NEWADDR,
			.nlmsg_pid = getpid(),
			.nlmsg_len = reqlen,
		},
	};
	int fd;
	int error;
	struct sockaddr_nl addr = { .nl_family = AF_NETLINK, };

	ifa = NLMSG_DATA(&req.nlh);
	ifa->ifa_family = AF_PHONET;
	ifa->ifa_prefixlen = 0;
	ifa->ifa_index = ifa_index;

	rta = IFA_RTA(ifa);
	rta->rta_type = IFA_LOCAL;
	rta->rta_len = RTA_LENGTH(1);
	*(uint8_t *)RTA_DATA(rta) = ifa_local;

	fd = pn_netlink_socket();
	if (fd == -1)
		return -errno;

	if (sendto(fd, &req, reqlen, 0, (void *)&addr, sizeof(addr)) == -1)
		error = -errno;
	else
		error = pn_netlink_getack(fd);

	close(fd);

	return error;
}

int g_isi_pn_netlink_set_address(GIsiModem *modem, uint8_t local)
{
	uint32_t ifindex = g_isi_modem_index(modem);

	if (ifindex == 0)
		return -ENODEV;

	if (local != PN_DEV_PC && local != PN_DEV_SOS)
		return -EINVAL;

	return pn_netlink_setaddr(ifindex, local);
}