summaryrefslogtreecommitdiff
path: root/src/shared/netlink.c
diff options
context:
space:
mode:
Diffstat (limited to 'src/shared/netlink.c')
-rw-r--r--src/shared/netlink.c666
1 files changed, 666 insertions, 0 deletions
diff --git a/src/shared/netlink.c b/src/shared/netlink.c
new file mode 100644
index 0000000..d72294c
--- /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;
+}