src/shared/netlink.c

   1 /*
   2  *
   3  *  Connection Manager
   4  *
   5  *  Copyright (C) 2011-2012  Intel Corporation. All rights reserved.
   6  *  Copyright (C) 2013  BWM CarIT GmbH.
   7  *
   8  *  This program is free software; you can redistribute it and/or
   9  *  modify it under the terms of the GNU Lesser General Public
  10  *  License version 2.1 as published by the Free Software Foundation.
  11  *
  12  *  This program is distributed in the hope that it will be useful,
  13  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  14  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  15  *  GNU Lesser General Public License for more details.
  16  *
  17  *  You should have received a copy of the GNU Lesser General Public
  18  *  License along with this program; if not, write to the Free Software
  19  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  20  *
  21  */
  22
  23 /*
  24  * This file is a copy from ELL which has been ported to use GLib's
  25  * data structures.
  26  */
  27
  28 #ifdef HAVE_CONFIG_H
  29 #include <config.h>
  30 #endif
  31
  32 #include <string.h>
  33 #include <unistd.h>
  34 #include <sys/socket.h>
  35 #include <linux/netlink.h>
  36
  37 #include <gdbus.h>
  38
  39 #include "src/shared/util.h"
  40 #include "src/shared/netlink.h"
  41
  42 #ifndef SOL_NETLINK
  43 #define SOL_NETLINK 270
  44 #endif
  45
  46 struct command {
  47         unsigned int id;
  48         uint32_t seq;
  49         uint32_t len;
  50         netlink_command_func_t handler;
  51         netlink_destroy_func_t destroy;
  52         void *user_data;
  53 };
  54
  55 struct notify {
  56         uint32_t group;
  57         netlink_notify_func_t handler;
  58         netlink_destroy_func_t destroy;
  59         void *user_data;
  60 };
  61
  62 struct netlink_info {
  63         uint32_t pid;
  64         GIOChannel *channel;
  65         uint32_t next_seq;
  66         GQueue *command_queue;
  67         GHashTable *command_pending;
  68         GHashTable *command_lookup;
  69         unsigned int next_command_id;
  70         GHashTable *notify_groups;
  71         GHashTable *notify_lookup;
  72         unsigned int next_notify_id;
  73         netlink_debug_func_t debug_handler;
  74         netlink_destroy_func_t debug_destroy;
  75         void *debug_data;
  76 };
  77
  78
  79 static void destroy_command(struct command *command)
  80 {
  81         if (command->destroy)
  82                 command->destroy(command->user_data);
  83
  84         g_free(command);
  85 }
  86
  87 static void destroy_notify(struct notify *notify)
  88 {
  89         if (notify->destroy)
  90                 notify->destroy(notify->user_data);
  91
  92         g_free(notify);
  93 }
  94
  95 static gboolean can_write_data(GIOChannel *chan,
  96                                 GIOCondition cond, gpointer user_data)
  97 {
  98         struct netlink_info *netlink = user_data;
  99         struct command *command;
 100         struct sockaddr_nl addr;
 101         const void *data;
 102         ssize_t written;
 103         int sk;
 104
 105         command = g_queue_pop_head(netlink->command_queue);
 106         if (!command)
 107                 return FALSE;
 108
 109         sk = g_io_channel_unix_get_fd(chan);
 110
 111         memset(&addr, 0, sizeof(addr));
 112         addr.nl_family = AF_NETLINK;
 113         addr.nl_pid = 0;
 114
 115         data = ((void *) command) + NLMSG_ALIGN(sizeof(struct command));
 116
 117         written = sendto(sk, data, command->len, 0,
 118                                 (struct sockaddr *) &addr, sizeof(addr));
 119         if (written < 0 || (uint32_t) written != command->len) {
 120                 g_hash_table_remove(netlink->command_lookup,
 121                                         GUINT_TO_POINTER(command->id));
 122                 destroy_command(command);
 123                 return FALSE;
 124         }
 125
 126         util_hexdump('<', data, command->len,
 127                                 netlink->debug_handler, netlink->debug_data);
 128
 129         g_hash_table_replace(netlink->command_pending,
 130                                 GUINT_TO_POINTER(command->seq), command);
 131
 132         return g_queue_get_length(netlink->command_queue) > 0;
 133 }
 134
 135 static void do_notify(gpointer key, gpointer value, gpointer user_data)
 136 {
 137         struct nlmsghdr *nlmsg = user_data;
 138         struct notify *notify = value;
 139
 140         if (notify->handler) {
 141                 notify->handler(nlmsg->nlmsg_type, NLMSG_DATA(nlmsg),
 142                         nlmsg->nlmsg_len - NLMSG_HDRLEN, notify->user_data);
 143         }
 144 }
 145
 146 static void process_broadcast(struct netlink_info *netlink, uint32_t group,
 147                                                 struct nlmsghdr *nlmsg)
 148 {
 149         GHashTable *notify_list;
 150
 151         notify_list = g_hash_table_lookup(netlink->notify_groups,
 152                                                GUINT_TO_POINTER(group));
 153         if (!notify_list)
 154                 return;
 155
 156         g_hash_table_foreach(notify_list, do_notify, nlmsg);
 157 }
 158
 159 static void process_message(struct netlink_info *netlink,
 160                                 struct nlmsghdr *nlmsg)
 161 {
 162         const void *data = nlmsg;
 163         struct command *command;
 164
 165         command = g_hash_table_lookup(netlink->command_pending,
 166                                         GUINT_TO_POINTER(nlmsg->nlmsg_seq));
 167         if (!command)
 168                 return;
 169
 170         g_hash_table_remove(netlink->command_pending,
 171                                         GUINT_TO_POINTER(nlmsg->nlmsg_seq));
 172
 173         if (!command->handler)
 174                 goto done;
 175
 176         if (nlmsg->nlmsg_type < NLMSG_MIN_TYPE) {
 177                 const struct nlmsgerr *err;
 178
 179                 switch (nlmsg->nlmsg_type) {
 180                 case NLMSG_ERROR:
 181                         err = data + NLMSG_HDRLEN;
 182
 183                         command->handler(-err->error, 0, NULL, 0,
 184                                                         command->user_data);
 185                         break;
 186                 }
 187         } else {
 188                 command->handler(0, nlmsg->nlmsg_type, data + NLMSG_HDRLEN,
 189                                         nlmsg->nlmsg_len - NLMSG_HDRLEN,
 190                                         command->user_data);
 191         }
 192
 193 done:
 194         g_hash_table_remove(netlink->command_lookup,
 195                                 GUINT_TO_POINTER(command->id));
 196
 197         destroy_command(command);
 198 }
 199
 200 static void process_multi(struct netlink_info *netlink, struct nlmsghdr *nlmsg)
 201 {
 202         const void *data = nlmsg;
 203         struct command *command;
 204
 205         command = g_hash_table_lookup(netlink->command_pending,
 206                                         GUINT_TO_POINTER(nlmsg->nlmsg_seq));
 207         if (!command)
 208                 return;
 209
 210         if (!command->handler)
 211                 goto done;
 212
 213         if (nlmsg->nlmsg_type < NLMSG_MIN_TYPE) {
 214                 const struct nlmsgerr *err;
 215
 216                 switch (nlmsg->nlmsg_type) {
 217                 case NLMSG_DONE:
 218                 case NLMSG_ERROR:
 219                         err = data + NLMSG_HDRLEN;
 220
 221                         command->handler(-err->error, 0, NULL, 0,
 222                                                         command->user_data);
 223                         break;
 224                 }
 225         } else {
 226                 command->handler(0, nlmsg->nlmsg_type, data + NLMSG_HDRLEN,
 227                                         nlmsg->nlmsg_len - NLMSG_HDRLEN,
 228                                         command->user_data);
 229                 return;
 230         }
 231
 232 done:
 233         g_hash_table_remove(netlink->command_pending,
 234                         GUINT_TO_POINTER(nlmsg->nlmsg_seq));
 235
 236         g_hash_table_remove(netlink->command_lookup,
 237                         GUINT_TO_POINTER(command->id));
 238
 239         destroy_command(command);
 240 }
 241
 242 static gboolean can_read_data(GIOChannel *chan,
 243                                 GIOCondition cond, gpointer data)
 244 {
 245         struct netlink_info *netlink = data;
 246         struct cmsghdr *cmsg;
 247         struct msghdr msg;
 248         struct iovec iov;
 249         struct nlmsghdr *nlmsg;
 250         unsigned char buffer[4096];
 251         unsigned char control[32];
 252         uint32_t group = 0;
 253         ssize_t len;
 254         int sk;
 255
 256         sk = g_io_channel_unix_get_fd(chan);
 257
 258         iov.iov_base = buffer;
 259         iov.iov_len = sizeof(buffer);
 260
 261         memset(&msg, 0, sizeof(msg));
 262         msg.msg_iov = &iov;
 263         msg.msg_iovlen = 1;
 264         msg.msg_control = control;
 265         msg.msg_controllen = sizeof(control);
 266
 267         len = recvmsg(sk, &msg, 0);
 268         if (len < 0)
 269                 return FALSE;
 270
 271         util_hexdump('>', buffer, len, netlink->debug_handler,
 272                         netlink->debug_data);
 273
 274         for (cmsg = CMSG_FIRSTHDR(&msg); cmsg;
 275                                         cmsg = CMSG_NXTHDR(&msg, cmsg)) {
 276                 struct nl_pktinfo *pktinfo;
 277
 278                 if (cmsg->cmsg_level != SOL_NETLINK)
 279                         continue;
 280
 281                 if (cmsg->cmsg_type != NETLINK_PKTINFO)
 282                         continue;
 283
 284                 pktinfo = (void *) CMSG_DATA(cmsg);
 285
 286                 group = pktinfo->group;
 287         }
 288
 289         for (nlmsg = iov.iov_base; NLMSG_OK(nlmsg, (uint32_t) len);
 290                                         nlmsg = NLMSG_NEXT(nlmsg, len)) {
 291                 if (group > 0 && nlmsg->nlmsg_seq == 0) {
 292                         process_broadcast(netlink, group, nlmsg);
 293                         continue;
 294                 }
 295
 296                 if (nlmsg->nlmsg_pid != netlink->pid)
 297                         continue;
 298
 299                 if (nlmsg->nlmsg_flags & NLM_F_MULTI)
 300                         process_multi(netlink, nlmsg);
 301                 else
 302                         process_message(netlink, nlmsg);
 303         }
 304
 305         return TRUE;
 306 }
 307
 308 static gboolean netlink_event(GIOChannel *chan,
 309                                 GIOCondition cond, gpointer data)
 310 {
 311         if (cond & (G_IO_NVAL | G_IO_HUP | G_IO_ERR))
 312                 return FALSE;
 313
 314         return TRUE;
 315 }
 316
 317 static int create_netlink_socket(int protocol, uint32_t *pid)
 318 {
 319         struct sockaddr_nl addr;
 320         socklen_t addrlen = sizeof(addr);
 321         int sk, pktinfo = 1;
 322
 323         sk = socket(PF_NETLINK, SOCK_DGRAM | SOCK_CLOEXEC | SOCK_NONBLOCK,
 324                                                                 protocol);
 325         if (sk < 0)
 326                 return -1;
 327
 328         memset(&addr, 0, sizeof(addr));
 329         addr.nl_family = AF_NETLINK;
 330
 331         if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
 332                 close(sk);
 333                 return -1;
 334         }
 335
 336         if (getsockname(sk, (struct sockaddr *) &addr, &addrlen) < 0) {
 337                 close(sk);
 338                 return -1;
 339         }
 340
 341         if (setsockopt(sk, SOL_NETLINK, NETLINK_PKTINFO,
 342                                         &pktinfo, sizeof(pktinfo)) < 0) {
 343                 close(sk);
 344                 return -1;
 345         }
 346
 347         if (pid)
 348                 *pid = addr.nl_pid;
 349
 350         return sk;
 351 }
 352
 353 struct netlink_info *netlink_new(int protocol)
 354 {
 355         struct netlink_info *netlink;
 356         int sk;
 357
 358         netlink = g_try_new0(struct netlink_info, 1);
 359         if (!netlink)
 360                 return NULL;
 361
 362         netlink->next_seq = 1;
 363         netlink->next_command_id = 1;
 364         netlink->next_notify_id = 1;
 365
 366         sk = create_netlink_socket(protocol, &netlink->pid);
 367         if (sk < 0) {
 368                 g_free(netlink);
 369                 return NULL;
 370         }
 371
 372         netlink->channel = g_io_channel_unix_new(sk);
 373         g_io_channel_set_close_on_unref(netlink->channel, TRUE);
 374
 375         g_io_channel_set_encoding(netlink->channel, NULL, NULL);
 376         g_io_channel_set_buffered(netlink->channel, FALSE);
 377
 378         g_io_add_watch(netlink->channel, G_IO_IN, can_read_data, netlink);
 379         g_io_add_watch(netlink->channel, G_IO_NVAL | G_IO_HUP | G_IO_ERR,
 380                         netlink_event, netlink);
 381
 382         netlink->command_queue = g_queue_new();
 383         netlink->command_pending = g_hash_table_new(g_direct_hash,
 384                                                         g_direct_equal);
 385         netlink->command_lookup = g_hash_table_new(g_direct_hash,
 386                                                         g_direct_equal);
 387
 388         netlink->notify_groups = g_hash_table_new(g_direct_hash,
 389                                                         g_direct_equal);
 390         netlink->notify_lookup = g_hash_table_new(g_direct_hash,
 391                                                         g_direct_equal);
 392
 393         return netlink;
 394 }
 395
 396 static gboolean cleanup_notify(gpointer key, gpointer value, gpointer user_data)
 397 {
 398         struct notify *notify = value;
 399
 400         destroy_notify(notify);
 401
 402         return TRUE;
 403
 404 }
 405
 406 static gboolean cleanup_notify_group(gpointer key, gpointer value,
 407                                         gpointer user_data)
 408 {
 409         GHashTable *notify_list = value;
 410
 411         g_hash_table_foreach_remove(notify_list, cleanup_notify, user_data);
 412         g_hash_table_destroy(notify_list);
 413
 414         return TRUE;
 415 }
 416
 417 static gboolean cleanup_command(gpointer key, gpointer value,
 418                                         gpointer user_data)
 419 {
 420         struct command *command = value;
 421
 422         destroy_command(command);
 423
 424         return TRUE;
 425 }
 426
 427 void netlink_destroy(struct netlink_info *netlink)
 428 {
 429         g_hash_table_destroy(netlink->notify_lookup);
 430
 431         g_hash_table_foreach_remove(netlink->notify_groups,
 432                                 cleanup_notify_group, NULL);
 433         g_hash_table_destroy(netlink->notify_groups);
 434
 435         g_queue_free(netlink->command_queue);
 436
 437         g_hash_table_destroy(netlink->command_pending);
 438
 439         g_hash_table_foreach_remove(netlink->command_lookup,
 440                                 cleanup_command, NULL);
 441         g_hash_table_destroy(netlink->command_lookup);
 442
 443         g_io_channel_shutdown(netlink->channel, TRUE, NULL);
 444         g_io_channel_unref(netlink->channel);
 445
 446         g_free(netlink);
 447 }
 448
 449 unsigned int netlink_send(struct netlink_info *netlink,
 450                         uint16_t type, uint16_t flags, const void *data,
 451                         uint32_t len, netlink_command_func_t function,
 452                         void *user_data, netlink_destroy_func_t destroy)
 453 {
 454         struct command *command;
 455         struct nlmsghdr *nlmsg;
 456         size_t size;
 457
 458         if (!netlink)
 459                 return 0;
 460
 461         if (!netlink->command_queue ||
 462                         !netlink->command_pending ||
 463                         !netlink->command_lookup)
 464                 return 0;
 465
 466         size = NLMSG_ALIGN(sizeof(struct command)) +
 467                                         NLMSG_HDRLEN + NLMSG_ALIGN(len);
 468
 469         command = g_try_malloc0(size);
 470         if (!command)
 471                 return 0;
 472
 473         command->handler = function;
 474         command->destroy = destroy;
 475         command->user_data = user_data;
 476
 477         command->id = netlink->next_command_id;
 478
 479         g_hash_table_replace(netlink->command_lookup,
 480                                 GUINT_TO_POINTER(command->id), command);
 481
 482         command->seq = netlink->next_seq++;
 483         command->len = NLMSG_HDRLEN + NLMSG_ALIGN(len);
 484
 485         nlmsg = ((void *) command) + NLMSG_ALIGN(sizeof(struct command));
 486
 487         nlmsg->nlmsg_len = command->len;
 488         nlmsg->nlmsg_type = type;
 489         nlmsg->nlmsg_flags = NLM_F_REQUEST | flags;
 490         nlmsg->nlmsg_seq = command->seq;
 491         nlmsg->nlmsg_pid = netlink->pid;
 492
 493         if (data && len > 0)
 494                 memcpy(((void *) nlmsg) + NLMSG_HDRLEN, data, len);
 495
 496         g_queue_push_tail(netlink->command_queue, command);
 497
 498         netlink->next_command_id++;
 499
 500         /* Arm IOChannel to call can_write_data in case it is not armed yet. */
 501         if (g_queue_get_length(netlink->command_queue) == 1)
 502                 g_io_add_watch(netlink->channel, G_IO_OUT, can_write_data,
 503                                 netlink);
 504
 505         return command->id;
 506 }
 507
 508 bool netlink_cancel(struct netlink_info *netlink, unsigned int id)
 509 {
 510         struct command *command;
 511
 512         if (!netlink || id == 0)
 513                 return false;
 514
 515         if (!netlink->command_queue ||
 516                         !netlink->command_pending ||
 517                         !netlink->command_lookup)
 518                 return false;
 519
 520         command = g_hash_table_lookup(netlink->command_lookup,
 521                                         GUINT_TO_POINTER(id));
 522         if (!command)
 523                 return false;
 524
 525         g_hash_table_remove(netlink->command_lookup, GUINT_TO_POINTER(id));
 526
 527         if (!g_queue_remove(netlink->command_queue, command)) {
 528                 g_hash_table_remove(netlink->command_pending,
 529                                         GUINT_TO_POINTER(command->seq));
 530         }
 531
 532         destroy_command(command);
 533
 534         return true;
 535 }
 536
 537 static bool add_membership(struct netlink_info *netlink, uint32_t group)
 538 {
 539         int sk, value = group;
 540
 541         sk = g_io_channel_unix_get_fd(netlink->channel);
 542
 543         if (setsockopt(sk, SOL_NETLINK, NETLINK_ADD_MEMBERSHIP,
 544                                                 &value, sizeof(value)) < 0)
 545                 return false;
 546
 547         return true;
 548 }
 549
 550 static bool drop_membership(struct netlink_info *netlink, uint32_t group)
 551 {
 552         int sk, value = group;
 553
 554         sk = g_io_channel_unix_get_fd(netlink->channel);
 555
 556         if (setsockopt(sk, SOL_NETLINK, NETLINK_DROP_MEMBERSHIP,
 557                                                 &value, sizeof(value)) < 0)
 558                 return false;
 559
 560         return true;
 561 }
 562
 563 unsigned int netlink_register(struct netlink_info *netlink,
 564                         uint32_t group, netlink_notify_func_t function,
 565                         void *user_data, netlink_destroy_func_t destroy)
 566 {
 567         GHashTable *notify_list;
 568         struct notify *notify;
 569         unsigned int id;
 570
 571         if (!netlink)
 572                 return 0;
 573
 574         if (!netlink->notify_groups || !netlink->notify_lookup)
 575                 return 0;
 576
 577         notify_list = g_hash_table_lookup(netlink->notify_groups,
 578                                                 GUINT_TO_POINTER(group));
 579         if (!notify_list) {
 580                 notify_list = g_hash_table_new(g_direct_hash, g_direct_equal);
 581                 if (!notify_list)
 582                         return 0;
 583
 584                 g_hash_table_replace(netlink->notify_groups,
 585                                         GUINT_TO_POINTER(group), notify_list);
 586         }
 587
 588         notify = g_new(struct notify, 1);
 589
 590         notify->group = group;
 591         notify->handler = function;
 592         notify->destroy = destroy;
 593         notify->user_data = user_data;
 594
 595         id = netlink->next_notify_id;
 596
 597         g_hash_table_replace(netlink->notify_lookup,
 598                                 GUINT_TO_POINTER(id), notify_list);
 599         g_hash_table_replace(notify_list, GUINT_TO_POINTER(id), notify);
 600
 601         if (g_hash_table_size(notify_list) == 1) {
 602                 if (add_membership(netlink, notify->group) == false)
 603                         goto remove_notify;
 604         }
 605
 606         netlink->next_notify_id++;
 607
 608         return id;
 609
 610 remove_notify:
 611         g_hash_table_remove(notify_list, GUINT_TO_POINTER(id));
 612         g_hash_table_remove(netlink->notify_lookup, GUINT_TO_POINTER(id));
 613         g_free(notify);
 614
 615         return 0;
 616 }
 617
 618 bool netlink_unregister(struct netlink_info *netlink, unsigned int id)
 619 {
 620         GHashTable *notify_list;
 621         struct notify *notify;
 622
 623         if (!netlink || id == 0)
 624                 return false;
 625
 626         if (!netlink->notify_groups || !netlink->notify_lookup)
 627                 return false;
 628
 629         notify_list = g_hash_table_lookup(netlink->notify_lookup,
 630                                                 GUINT_TO_POINTER(id));
 631
 632         if (!notify_list)
 633                 return false;
 634
 635         g_hash_table_remove(netlink->notify_lookup, GUINT_TO_POINTER(id));
 636
 637         notify = g_hash_table_lookup(notify_list, GUINT_TO_POINTER(id));
 638         if (!notify)
 639                 return false;
 640
 641         g_hash_table_remove(notify_list, GUINT_TO_POINTER(id));
 642
 643         if (g_hash_table_size(notify_list) == 0)
 644                 drop_membership(netlink, notify->group);
 645
 646         destroy_notify(notify);
 647
 648         return true;
 649 }
 650
 651 bool netlink_set_debug(struct netlink_info *netlink,
 652                         netlink_debug_func_t function,
 653                         void *user_data, netlink_destroy_func_t destroy)
 654 {
 655         if (!netlink)
 656                 return false;
 657
 658         if (netlink->debug_destroy)
 659                 netlink->debug_destroy(netlink->debug_data);
 660
 661         netlink->debug_handler = function;
 662         netlink->debug_destroy = destroy;
 663         netlink->debug_data = user_data;
 664
 665         return true;
 666 }