1 files changed, 666 insertions, 0 deletions

diff --git a/src/shared/netlink.c b/src/shared/netlink.c
new file mode 100644
index 00000000..d72294cc
--- /dev/null
+++ b/src/shared/netlink.c
@@ -0,0 +1,666 @@
+/*
+ *
+ *  Connection Manager
+ *
+ *  Copyright (C) 2011-2012  Intel Corporation. All rights reserved.
+ *  Copyright (C) 2013  BWM CarIT GmbH.
+ *
+ *  This program is free software; you can redistribute it and/or
+ *  modify it under the terms of the GNU Lesser General Public
+ *  License version 2.1 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 Lesser General Public License for more details.
+ *
+ *  You should have received a copy of the GNU Lesser 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
+ *
+ */
+
+/*
+ * This file is a copy from ELL which has been ported to use GLib's
+ * data structures.
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <string.h>
+#include <unistd.h>
+#include <sys/socket.h>
+#include <linux/netlink.h>
+
+#include <gdbus.h>
+
+#include "src/shared/util.h"
+#include "src/shared/netlink.h"
+
+#ifndef SOL_NETLINK
+#define SOL_NETLINK 270
+#endif
+
+struct command {
+	unsigned int id;
+	uint32_t seq;
+	uint32_t len;
+	netlink_command_func_t handler;
+	netlink_destroy_func_t destroy;
+	void *user_data;
+};
+
+struct notify {
+	uint32_t group;
+	netlink_notify_func_t handler;
+	netlink_destroy_func_t destroy;
+	void *user_data;
+};
+
+struct netlink_info {
+	uint32_t pid;
+	GIOChannel *channel;
+	uint32_t next_seq;
+	GQueue *command_queue;
+	GHashTable *command_pending;
+	GHashTable *command_lookup;
+	unsigned int next_command_id;
+	GHashTable *notify_groups;
+	GHashTable *notify_lookup;
+	unsigned int next_notify_id;
+	netlink_debug_func_t debug_handler;
+	netlink_destroy_func_t debug_destroy;
+	void *debug_data;
+};
+
+
+static void destroy_command(struct command *command)
+{
+	if (command->destroy)
+		command->destroy(command->user_data);
+
+	g_free(command);
+}
+
+static void destroy_notify(struct notify *notify)
+{
+	if (notify->destroy)
+		notify->destroy(notify->user_data);
+
+	g_free(notify);
+}
+
+static gboolean can_write_data(GIOChannel *chan,
+				GIOCondition cond, gpointer user_data)
+{
+	struct netlink_info *netlink = user_data;
+	struct command *command;
+	struct sockaddr_nl addr;
+	const void *data;
+	ssize_t written;
+	int sk;
+
+	command = g_queue_pop_head(netlink->command_queue);
+	if (!command)
+		return FALSE;
+
+	sk = g_io_channel_unix_get_fd(chan);
+
+	memset(&addr, 0, sizeof(addr));
+	addr.nl_family = AF_NETLINK;
+	addr.nl_pid = 0;
+
+	data = ((void *) command) + NLMSG_ALIGN(sizeof(struct command));
+
+	written = sendto(sk, data, command->len, 0,
+				(struct sockaddr *) &addr, sizeof(addr));
+	if (written < 0 || (uint32_t) written != command->len) {
+		g_hash_table_remove(netlink->command_lookup,
+					GUINT_TO_POINTER(command->id));
+		destroy_command(command);
+		return FALSE;
+	}
+
+	util_hexdump('<', data, command->len,
+				netlink->debug_handler, netlink->debug_data);
+
+	g_hash_table_replace(netlink->command_pending,
+				GUINT_TO_POINTER(command->seq), command);
+
+	return g_queue_get_length(netlink->command_queue) > 0;
+}
+
+static void do_notify(gpointer key, gpointer value, gpointer user_data)
+{
+	struct nlmsghdr *nlmsg = user_data;
+	struct notify *notify = value;
+
+	if (notify->handler) {
+		notify->handler(nlmsg->nlmsg_type, NLMSG_DATA(nlmsg),
+			nlmsg->nlmsg_len - NLMSG_HDRLEN, notify->user_data);
+	}
+}
+
+static void process_broadcast(struct netlink_info *netlink, uint32_t group,
+						struct nlmsghdr *nlmsg)
+{
+	GHashTable *notify_list;
+
+	notify_list = g_hash_table_lookup(netlink->notify_groups,
+					       GUINT_TO_POINTER(group));
+	if (!notify_list)
+		return;
+
+	g_hash_table_foreach(notify_list, do_notify, nlmsg);
+}
+
+static void process_message(struct netlink_info *netlink,
+				struct nlmsghdr *nlmsg)
+{
+	const void *data = nlmsg;
+	struct command *command;
+
+	command = g_hash_table_lookup(netlink->command_pending,
+					GUINT_TO_POINTER(nlmsg->nlmsg_seq));
+	if (!command)
+		return;
+
+	g_hash_table_remove(netlink->command_pending,
+					GUINT_TO_POINTER(nlmsg->nlmsg_seq));
+
+	if (!command->handler)
+		goto done;
+
+	if (nlmsg->nlmsg_type < NLMSG_MIN_TYPE) {
+		const struct nlmsgerr *err;
+
+		switch (nlmsg->nlmsg_type) {
+		case NLMSG_ERROR:
+			err = data + NLMSG_HDRLEN;
+
+			command->handler(-err->error, 0, NULL, 0,
+							command->user_data);
+			break;
+		}
+	} else {
+		command->handler(0, nlmsg->nlmsg_type, data + NLMSG_HDRLEN,
+					nlmsg->nlmsg_len - NLMSG_HDRLEN,
+					command->user_data);
+	}
+
+done:
+	g_hash_table_remove(netlink->command_lookup,
+				GUINT_TO_POINTER(command->id));
+
+	destroy_command(command);
+}
+
+static void process_multi(struct netlink_info *netlink, struct nlmsghdr *nlmsg)
+{
+	const void *data = nlmsg;
+	struct command *command;
+
+	command = g_hash_table_lookup(netlink->command_pending,
+					GUINT_TO_POINTER(nlmsg->nlmsg_seq));
+	if (!command)
+		return;
+
+	if (!command->handler)
+		goto done;
+
+	if (nlmsg->nlmsg_type < NLMSG_MIN_TYPE) {
+		const struct nlmsgerr *err;
+
+		switch (nlmsg->nlmsg_type) {
+		case NLMSG_DONE:
+		case NLMSG_ERROR:
+			err = data + NLMSG_HDRLEN;
+
+			command->handler(-err->error, 0, NULL, 0,
+							command->user_data);
+			break;
+		}
+	} else {
+		command->handler(0, nlmsg->nlmsg_type, data + NLMSG_HDRLEN,
+					nlmsg->nlmsg_len - NLMSG_HDRLEN,
+					command->user_data);
+		return;
+	}
+
+done:
+	g_hash_table_remove(netlink->command_pending,
+			GUINT_TO_POINTER(nlmsg->nlmsg_seq));
+
+	g_hash_table_remove(netlink->command_lookup,
+			GUINT_TO_POINTER(command->id));
+
+	destroy_command(command);
+}
+
+static gboolean can_read_data(GIOChannel *chan,
+				GIOCondition cond, gpointer data)
+{
+	struct netlink_info *netlink = data;
+	struct cmsghdr *cmsg;
+	struct msghdr msg;
+	struct iovec iov;
+	struct nlmsghdr *nlmsg;
+	unsigned char buffer[4096];
+	unsigned char control[32];
+	uint32_t group = 0;
+	ssize_t len;
+	int sk;
+
+	sk = g_io_channel_unix_get_fd(chan);
+
+	iov.iov_base = buffer;
+	iov.iov_len = sizeof(buffer);
+
+	memset(&msg, 0, sizeof(msg));
+	msg.msg_iov = &iov;
+	msg.msg_iovlen = 1;
+	msg.msg_control = control;
+	msg.msg_controllen = sizeof(control);
+
+	len = recvmsg(sk, &msg, 0);
+	if (len < 0)
+		return FALSE;
+
+	util_hexdump('>', buffer, len, netlink->debug_handler,
+			netlink->debug_data);
+
+	for (cmsg = CMSG_FIRSTHDR(&msg); cmsg;
+					cmsg = CMSG_NXTHDR(&msg, cmsg)) {
+		struct nl_pktinfo *pktinfo;
+
+		if (cmsg->cmsg_level != SOL_NETLINK)
+			continue;
+
+		if (cmsg->cmsg_type != NETLINK_PKTINFO)
+			continue;
+
+		pktinfo = (void *) CMSG_DATA(cmsg);
+
+		group = pktinfo->group;
+	}
+
+	for (nlmsg = iov.iov_base; NLMSG_OK(nlmsg, (uint32_t) len);
+					nlmsg = NLMSG_NEXT(nlmsg, len)) {
+		if (group > 0 && nlmsg->nlmsg_seq == 0) {
+			process_broadcast(netlink, group, nlmsg);
+			continue;
+		}
+
+		if (nlmsg->nlmsg_pid != netlink->pid)
+			continue;
+
+		if (nlmsg->nlmsg_flags & NLM_F_MULTI)
+			process_multi(netlink, nlmsg);
+		else
+			process_message(netlink, nlmsg);
+	}
+
+	return TRUE;
+}
+
+static gboolean netlink_event(GIOChannel *chan,
+				GIOCondition cond, gpointer data)
+{
+	if (cond & (G_IO_NVAL | G_IO_HUP | G_IO_ERR))
+		return FALSE;
+
+	return TRUE;
+}
+
+static int create_netlink_socket(int protocol, uint32_t *pid)
+{
+	struct sockaddr_nl addr;
+	socklen_t addrlen = sizeof(addr);
+	int sk, pktinfo = 1;
+
+	sk = socket(PF_NETLINK, SOCK_DGRAM | SOCK_CLOEXEC | SOCK_NONBLOCK,
+								protocol);
+	if (sk < 0)
+		return -1;
+
+	memset(&addr, 0, sizeof(addr));
+	addr.nl_family = AF_NETLINK;
+
+	if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
+		close(sk);
+		return -1;
+	}
+
+	if (getsockname(sk, (struct sockaddr *) &addr, &addrlen) < 0) {
+		close(sk);
+		return -1;
+	}
+
+	if (setsockopt(sk, SOL_NETLINK, NETLINK_PKTINFO,
+					&pktinfo, sizeof(pktinfo)) < 0) {
+		close(sk);
+		return -1;
+	}
+
+	if (pid)
+		*pid = addr.nl_pid;
+
+	return sk;
+}
+
+struct netlink_info *netlink_new(int protocol)
+{
+	struct netlink_info *netlink;
+	int sk;
+
+	netlink = g_try_new0(struct netlink_info, 1);
+	if (!netlink)
+		return NULL;
+
+	netlink->next_seq = 1;
+	netlink->next_command_id = 1;
+	netlink->next_notify_id = 1;
+
+	sk = create_netlink_socket(protocol, &netlink->pid);
+	if (sk < 0) {
+		g_free(netlink);
+		return NULL;
+	}
+
+	netlink->channel = g_io_channel_unix_new(sk);
+	g_io_channel_set_close_on_unref(netlink->channel, TRUE);
+
+	g_io_channel_set_encoding(netlink->channel, NULL, NULL);
+	g_io_channel_set_buffered(netlink->channel, FALSE);
+
+	g_io_add_watch(netlink->channel, G_IO_IN, can_read_data, netlink);
+	g_io_add_watch(netlink->channel, G_IO_NVAL | G_IO_HUP | G_IO_ERR,
+			netlink_event, netlink);
+
+	netlink->command_queue = g_queue_new();
+	netlink->command_pending = g_hash_table_new(g_direct_hash,
+							g_direct_equal);
+	netlink->command_lookup = g_hash_table_new(g_direct_hash,
+							g_direct_equal);
+
+	netlink->notify_groups = g_hash_table_new(g_direct_hash,
+							g_direct_equal);
+	netlink->notify_lookup = g_hash_table_new(g_direct_hash,
+							g_direct_equal);
+
+	return netlink;
+}
+
+static gboolean cleanup_notify(gpointer key, gpointer value, gpointer user_data)
+{
+	struct notify *notify = value;
+
+	destroy_notify(notify);
+
+	return TRUE;
+
+}
+
+static gboolean cleanup_notify_group(gpointer key, gpointer value,
+					gpointer user_data)
+{
+	GHashTable *notify_list = value;
+
+	g_hash_table_foreach_remove(notify_list, cleanup_notify, user_data);
+	g_hash_table_destroy(notify_list);
+
+	return TRUE;
+}
+
+static gboolean cleanup_command(gpointer key, gpointer value,
+					gpointer user_data)
+{
+	struct command *command = value;
+
+	destroy_command(command);
+
+	return TRUE;
+}
+
+void netlink_destroy(struct netlink_info *netlink)
+{
+	g_hash_table_destroy(netlink->notify_lookup);
+
+	g_hash_table_foreach_remove(netlink->notify_groups,
+				cleanup_notify_group, NULL);
+	g_hash_table_destroy(netlink->notify_groups);
+
+	g_queue_free(netlink->command_queue);
+
+	g_hash_table_destroy(netlink->command_pending);
+
+	g_hash_table_foreach_remove(netlink->command_lookup,
+				cleanup_command, NULL);
+	g_hash_table_destroy(netlink->command_lookup);
+
+	g_io_channel_shutdown(netlink->channel, TRUE, NULL);
+	g_io_channel_unref(netlink->channel);
+
+	g_free(netlink);
+}
+
+unsigned int netlink_send(struct netlink_info *netlink,
+			uint16_t type, uint16_t flags, const void *data,
+			uint32_t len, netlink_command_func_t function,
+			void *user_data, netlink_destroy_func_t destroy)
+{
+	struct command *command;
+	struct nlmsghdr *nlmsg;
+	size_t size;
+
+	if (!netlink)
+		return 0;
+
+	if (!netlink->command_queue ||
+			!netlink->command_pending ||
+			!netlink->command_lookup)
+		return 0;
+
+	size = NLMSG_ALIGN(sizeof(struct command)) +
+					NLMSG_HDRLEN + NLMSG_ALIGN(len);
+
+	command = g_try_malloc0(size);
+	if (!command)
+		return 0;
+
+	command->handler = function;
+	command->destroy = destroy;
+	command->user_data = user_data;
+
+	command->id = netlink->next_command_id;
+
+	g_hash_table_replace(netlink->command_lookup,
+				GUINT_TO_POINTER(command->id), command);
+
+	command->seq = netlink->next_seq++;
+	command->len = NLMSG_HDRLEN + NLMSG_ALIGN(len);
+
+	nlmsg = ((void *) command) + NLMSG_ALIGN(sizeof(struct command));
+
+	nlmsg->nlmsg_len = command->len;
+	nlmsg->nlmsg_type = type;
+	nlmsg->nlmsg_flags = NLM_F_REQUEST | flags;
+	nlmsg->nlmsg_seq = command->seq;
+	nlmsg->nlmsg_pid = netlink->pid;
+
+	if (data && len > 0)
+		memcpy(((void *) nlmsg) + NLMSG_HDRLEN, data, len);
+
+	g_queue_push_tail(netlink->command_queue, command);
+
+	netlink->next_command_id++;
+
+	/* Arm IOChannel to call can_write_data in case it is not armed yet. */
+	if (g_queue_get_length(netlink->command_queue) == 1)
+		g_io_add_watch(netlink->channel, G_IO_OUT, can_write_data,
+				netlink);
+
+	return command->id;
+}
+
+bool netlink_cancel(struct netlink_info *netlink, unsigned int id)
+{
+	struct command *command;
+
+	if (!netlink || id == 0)
+		return false;
+
+	if (!netlink->command_queue ||
+			!netlink->command_pending ||
+			!netlink->command_lookup)
+		return false;
+
+	command = g_hash_table_lookup(netlink->command_lookup,
+					GUINT_TO_POINTER(id));
+	if (!command)
+		return false;
+
+	g_hash_table_remove(netlink->command_lookup, GUINT_TO_POINTER(id));
+
+	if (!g_queue_remove(netlink->command_queue, command)) {
+		g_hash_table_remove(netlink->command_pending,
+					GUINT_TO_POINTER(command->seq));
+	}
+
+	destroy_command(command);
+
+	return true;
+}
+
+static bool add_membership(struct netlink_info *netlink, uint32_t group)
+{
+	int sk, value = group;
+
+	sk = g_io_channel_unix_get_fd(netlink->channel);
+
+	if (setsockopt(sk, SOL_NETLINK, NETLINK_ADD_MEMBERSHIP,
+						&value, sizeof(value)) < 0)
+		return false;
+
+	return true;
+}
+
+static bool drop_membership(struct netlink_info *netlink, uint32_t group)
+{
+	int sk, value = group;
+
+	sk = g_io_channel_unix_get_fd(netlink->channel);
+
+	if (setsockopt(sk, SOL_NETLINK, NETLINK_DROP_MEMBERSHIP,
+						&value, sizeof(value)) < 0)
+		return false;
+
+	return true;
+}
+
+unsigned int netlink_register(struct netlink_info *netlink,
+			uint32_t group, netlink_notify_func_t function,
+			void *user_data, netlink_destroy_func_t destroy)
+{
+	GHashTable *notify_list;
+	struct notify *notify;
+	unsigned int id;
+
+	if (!netlink)
+		return 0;
+
+	if (!netlink->notify_groups || !netlink->notify_lookup)
+		return 0;
+
+	notify_list = g_hash_table_lookup(netlink->notify_groups,
+						GUINT_TO_POINTER(group));
+	if (!notify_list) {
+		notify_list = g_hash_table_new(g_direct_hash, g_direct_equal);
+		if (!notify_list)
+			return 0;
+
+		g_hash_table_replace(netlink->notify_groups,
+					GUINT_TO_POINTER(group), notify_list);
+	}
+
+	notify = g_new(struct notify, 1);
+
+	notify->group = group;
+	notify->handler = function;
+	notify->destroy = destroy;
+	notify->user_data = user_data;
+
+	id = netlink->next_notify_id;
+
+	g_hash_table_replace(netlink->notify_lookup,
+				GUINT_TO_POINTER(id), notify_list);
+	g_hash_table_replace(notify_list, GUINT_TO_POINTER(id), notify);
+
+	if (g_hash_table_size(notify_list) == 1) {
+		if (add_membership(netlink, notify->group) == false)
+			goto remove_notify;
+	}
+
+	netlink->next_notify_id++;
+
+	return id;
+
+remove_notify:
+	g_hash_table_remove(notify_list, GUINT_TO_POINTER(id));
+	g_hash_table_remove(netlink->notify_lookup, GUINT_TO_POINTER(id));
+	g_free(notify);
+
+	return 0;
+}
+
+bool netlink_unregister(struct netlink_info *netlink, unsigned int id)
+{
+	GHashTable *notify_list;
+	struct notify *notify;
+
+	if (!netlink || id == 0)
+		return false;
+
+	if (!netlink->notify_groups || !netlink->notify_lookup)
+		return false;
+
+	notify_list = g_hash_table_lookup(netlink->notify_lookup,
+						GUINT_TO_POINTER(id));
+
+	if (!notify_list)
+		return false;
+
+	g_hash_table_remove(netlink->notify_lookup, GUINT_TO_POINTER(id));
+
+	notify = g_hash_table_lookup(notify_list, GUINT_TO_POINTER(id));
+	if (!notify)
+		return false;
+
+	g_hash_table_remove(notify_list, GUINT_TO_POINTER(id));
+
+	if (g_hash_table_size(notify_list) == 0)
+		drop_membership(netlink, notify->group);
+
+	destroy_notify(notify);
+
+	return true;
+}
+
+bool netlink_set_debug(struct netlink_info *netlink,
+			netlink_debug_func_t function,
+			void *user_data, netlink_destroy_func_t destroy)
+{
+	if (!netlink)
+		return false;
+
+	if (netlink->debug_destroy)
+		netlink->debug_destroy(netlink->debug_data);
+
+	netlink->debug_handler = function;
+	netlink->debug_destroy = destroy;
+	netlink->debug_data = user_data;
+
+	return true;
+}