dbus/kdbus-common.c

   1 /* -*- mode: C; c-file-style: "gnu"; indent-tabs-mode: nil; -*- */
   2 /* kdbus-common.c  kdbus related utils for daemon and libdbus
   3  *
   4  * Copyright (C) 2013  Samsung Electronics
   5  *
   6  * Licensed under the Academic Free License version 2.1
   7  *
   8  * This program is free software; you can redistribute it and/or modify
   9  * it under the terms of the GNU General Public License as published by
  10  * the Free Software Foundation; either version 2 of the License, or
  11  * (at your option) any later version and under the terms of the GNU
  12  * Lesser General Public License as published by the
  13  * Free Software Foundation; either version 2.1 of the License, or (at
  14  * your option) any later version.
  15  *
  16  * This program is distributed in the hope that it will be useful,
  17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  19  * GNU General Public License for more details.
  20  *
  21  * You should have received a copy of the GNU General Public License
  22  * along with this program; if not, write to the Free Software
  23  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
  24  *
  25  */
  26 #include <config.h>
  27 #include <linux/kdbus.h>
  28 #include "kdbus-common.h"
  29 #include "dbus-transport-kdbus.h"
  30 #include "dbus-valgrind-internal.h"
  31 #include <string.h>
  32 #include <stddef.h>
  33 #include <stdlib.h>
  34 #include <errno.h>
  35 #include <unistd.h>
  36 #include <dbus/dbus-internals.h>
  37 #include <dbus/dbus-shared.h>
  38 #include "dbus-signals.h"
  39 #include <stdio.h>
  40 #include <sys/ioctl.h>
  41 #include <sys/types.h>
  42 #include <sys/stat.h>
  43 #include <fcntl.h>
  44 #include <sys/mman.h>
  45
  46 struct kdbus_t
  47 {
  48   int fd;                                                     /**< File descriptor */
  49   void *mmap_ptr;                   /**< Mapped memory where kdbus (kernel) writes
  50                                      *   messages incoming to us.
  51                                      */
  52   size_t pool_size;                                     /**< Size of mapped memory */
  53   __u64 id;                                       /**< unique id of the connection */
  54   char bus_id[sizeof (((struct kdbus_cmd_hello *)(0))->id128)];  /**< id of the bus */
  55   struct kdbus_bloom_parameter bloom;                         /**< bloom parameters*/
  56 };
  57
  58 /* ALIGN8 and KDBUS_FOREACH taken from systemd */
  59 #define ALIGN8(l) (((l) + 7) & ~7)
  60 #define KDBUS_FOREACH(iter, first, _size)                               \
  61         for (iter = (first);                                            \
  62              ((uint8_t *)(iter) < (uint8_t *)(first) + (_size)) &&      \
  63                ((uint8_t *)(iter) >= (uint8_t *)(first));               \
  64              iter = (void*)(((uint8_t *)iter) + ALIGN8((iter)->size)))
  65
  66 static int
  67 safe_ioctl (int fd,
  68             unsigned long request,
  69             void *data)
  70 {
  71   int ret;
  72
  73   do {
  74     ret = ioctl (fd, request, data);
  75   }
  76   while (-1 == ret && EINTR == errno);
  77
  78   return ret;
  79 }
  80
  81 static int
  82 free_by_offset (kdbus_t  *kdbus,
  83                 __u64     offset)
  84 {
  85   struct kdbus_cmd_free cmd;
  86
  87   /*
  88    * Kdbus requires to initialize ioctl params partially. Some parts
  89    * are for data passed from user to kernel, and other parts
  90    * for data passed from kernel to user.
  91    *
  92    * Valgrind detects when uninitialized data is passed to kernel
  93    * and has no way to know that it is meant to be filled by kernel.
  94    * Thus, we initialize params for Valgrind to stop complaining.
  95    */
  96   VALGRIND_MAKE_MEM_DEFINED (&cmd, sizeof (cmd));
  97
  98   cmd.size = sizeof (cmd);
  99   cmd.offset = offset;
 100   cmd.flags = 0;
 101
 102   if (safe_ioctl (kdbus->fd, KDBUS_CMD_FREE, &cmd )!= 0)
 103     return errno;
 104
 105   return 0;
 106 }
 107
 108 static void make_item_name (const char *name, struct kdbus_item *item)
 109 {
 110   size_t len = strlen (name) + 1;
 111   item->size = KDBUS_ITEM_HEADER_SIZE + len;
 112   item->type = KDBUS_ITEM_NAME;
 113
 114   memcpy (item->str, name, len);
 115 }
 116
 117 /**
 118  * Adds an item in the current position of items array.
 119  *
 120  * @param item item to fill
 121  * @param item_type type of the item
 122  * @param string value of the item
 123  * @param string_size size of the value
 124  * @returns pointer to the next item
 125  */
 126 struct kdbus_item *
 127 _kdbus_item_add_string (struct kdbus_item *item,
 128                         __u64              item_type,
 129                         const char        *item_string,
 130                         __u64              item_string_size)
 131 {
 132   item->size = KDBUS_ITEM_HEADER_SIZE + item_string_size;
 133   item->type = item_type;
 134   memcpy (item->str, item_string, item_string_size);
 135   return KDBUS_ITEM_NEXT (item);
 136 }
 137
 138 struct kdbus_item *
 139 _kdbus_item_add_payload_memfd (struct kdbus_item *item,
 140                                __u64              start,
 141                                __u64              size,
 142                                int                fd)
 143 {
 144   item->type = KDBUS_ITEM_PAYLOAD_MEMFD;
 145   item->size = KDBUS_ITEM_HEADER_SIZE + sizeof (struct kdbus_memfd);
 146   item->memfd.start = start;
 147   item->memfd.size = size;
 148   item->memfd.fd = fd;
 149   return KDBUS_ITEM_NEXT (item);
 150 }
 151
 152 struct kdbus_item *
 153 _kdbus_item_add_payload_vec (struct kdbus_item *item,
 154                              __u64              size,
 155                              __u64              address_or_offset)
 156 {
 157   item->type = KDBUS_ITEM_PAYLOAD_VEC;
 158   item->size = KDBUS_ITEM_HEADER_SIZE + sizeof (struct kdbus_vec);
 159   item->vec.size = size;
 160   item->vec.address = address_or_offset;
 161   return KDBUS_ITEM_NEXT (item);
 162 }
 163
 164 struct kdbus_item *
 165 _kdbus_item_add_fds (struct kdbus_item *item,
 166                      const int         *fds,
 167                      int                fds_count)
 168 {
 169   item->type = KDBUS_ITEM_FDS;
 170   item->size = KDBUS_ITEM_HEADER_SIZE + (__u64)fds_count * sizeof (int);
 171   memcpy (item->fds, fds, fds_count * sizeof (int));
 172   return KDBUS_ITEM_NEXT (item);
 173 }
 174
 175 struct kdbus_item *
 176 _kdbus_item_add_bloom_filter (struct kdbus_item          *item,
 177                               kdbus_t                    *kdbus,
 178                               struct kdbus_bloom_filter **out_ptr)
 179 {
 180   item->type = KDBUS_ITEM_BLOOM_FILTER;
 181   item->size = KDBUS_ITEM_HEADER_SIZE
 182                + sizeof (struct kdbus_bloom_filter)
 183                + kdbus->bloom.size;
 184   memset (item->bloom_filter.data, 0, kdbus->bloom.size);
 185   item->bloom_filter.generation = 0;
 186   *out_ptr = &item->bloom_filter;
 187   return KDBUS_ITEM_NEXT (item);
 188 }
 189
 190 kdbus_bloom_data_t *
 191 _kdbus_bloom_filter_get_data (struct kdbus_bloom_filter *bloom_filter)
 192 {
 193   return bloom_filter->data;
 194 }
 195
 196 struct kdbus_item *
 197 _kdbus_item_add_name_change (struct kdbus_item *item,
 198                              __u64              old_id,
 199                              __u64              old_id_flags,
 200                              __u64              new_id,
 201                              __u64              new_id_flags)
 202 {
 203   item->size = KDBUS_ITEM_HEADER_SIZE + sizeof (struct kdbus_notify_name_change);
 204   item->type = KDBUS_ITEM_NAME_CHANGE;
 205   item->name_change.old_id.id = old_id;
 206   item->name_change.old_id.flags = old_id_flags;
 207   item->name_change.new_id.id = new_id;
 208   item->name_change.new_id.flags = new_id_flags;
 209   return KDBUS_ITEM_NEXT (item);
 210 }
 211
 212 struct kdbus_item *
 213 _kdbus_item_add_id_add (struct kdbus_item *item,
 214                         __u64              id,
 215                         __u64              id_flags)
 216 {
 217   item->size = KDBUS_ITEM_HEADER_SIZE + sizeof (struct kdbus_notify_id_change);
 218   item->type = KDBUS_ITEM_ID_ADD;
 219   item->id_change.id = id;
 220   item->id_change.flags = id_flags;
 221   return KDBUS_ITEM_NEXT (item);
 222 }
 223
 224 struct kdbus_item *
 225 _kdbus_item_add_id (struct kdbus_item *item,
 226                     __u64              id)
 227 {
 228   item->size = KDBUS_ITEM_HEADER_SIZE + sizeof (id);
 229   item->type = KDBUS_ITEM_ID;
 230   item->id = id;
 231   return KDBUS_ITEM_NEXT (item);
 232 }
 233
 234 struct kdbus_item *
 235 _kdbus_item_add_bloom_mask (struct kdbus_item   *item,
 236                             kdbus_t             *kdbus,
 237                             kdbus_bloom_data_t **bloom)
 238 {
 239   item->size = KDBUS_ITEM_HEADER_SIZE + kdbus->bloom.size;
 240   item->type = KDBUS_ITEM_BLOOM_MASK;
 241   memset (item->data64, 0, kdbus->bloom.size);
 242   if (NULL != bloom)
 243     *bloom = item->data64;
 244   return KDBUS_ITEM_NEXT (item);
 245 }
 246
 247 static inline void *
 248 get_from_offset (kdbus_t *kdbus,
 249                  __u64    offset)
 250 {
 251   return ((char *)kdbus->mmap_ptr) + offset;
 252 }
 253
 254 kdbus_t *
 255 _kdbus_new ()
 256 {
 257   return dbus_new (kdbus_t, 1);
 258 }
 259
 260 void
 261 _kdbus_free (kdbus_t *kdbus)
 262 {
 263   dbus_free (kdbus);
 264 }
 265
 266 /**
 267  * Opens a connection to the kdbus bus
 268  *
 269  * @param kdbus kdbus object
 270  * @param path the path to kdbus bus
 271  * @returns 0 on success, -errno on failure
 272  */
 273 int
 274 _kdbus_open (kdbus_t *kdbus, const char *path)
 275 {
 276   int fd = open (path, O_RDWR|O_CLOEXEC|O_NONBLOCK);
 277   if (-1 == fd)
 278     return -errno;
 279
 280   kdbus->fd = fd;
 281   return 0;
 282 }
 283
 284 int
 285 _kdbus_close (kdbus_t *kdbus)
 286 {
 287   int ret;
 288   int errclose = 0;
 289   int errunmap = 0;
 290
 291   do
 292   {
 293     ret = close (kdbus->fd);
 294   } while (-1 == ret && EINTR == errno);
 295   if (-1 == ret)
 296     errclose = errno;
 297
 298   ret = munmap (kdbus->mmap_ptr, kdbus->pool_size);
 299   if (-1 == ret)
 300     errunmap = errno;
 301
 302   if (0 != errclose)
 303     return -errclose;
 304   if (0 != errunmap)
 305     return -errunmap;
 306   return 0;
 307 }
 308
 309 int
 310 _kdbus_get_fd (kdbus_t *kdbus)
 311 {
 312   return kdbus->fd;
 313 }
 314
 315 __u64
 316 _kdbus_get_id (kdbus_t *kdbus)
 317 {
 318   return kdbus->id;
 319 }
 320
 321 char *
 322 _kdbus_get_bus_id (kdbus_t *kdbus)
 323 {
 324   return kdbus->bus_id;
 325 }
 326
 327 __u64
 328 _kdbus_get_bus_id_size (void)
 329 {
 330   return sizeof (((struct kdbus_t *)(0))->bus_id);
 331 }
 332
 333 int
 334 _kdbus_hello (kdbus_t       *kdbus,
 335               __u64          flags,
 336               __u64          attach_flags_send,
 337               __u64          attach_flags_recv,
 338               __u64          pool_size,
 339               const char    *activator_name,
 340               const char    *connection_name)
 341 {
 342   struct kdbus_cmd_hello *hello;
 343   struct kdbus_item *item, *items;
 344   __u64 hello_size;
 345   size_t activator_name_size = 0;
 346   size_t connection_name_size = 0;
 347   __u64 offset;
 348   __u64 items_size;
 349
 350   hello_size = sizeof (struct kdbus_cmd_hello);
 351
 352   if (NULL != activator_name)
 353     {
 354       activator_name_size = strlen (activator_name) + 1;
 355       hello_size += KDBUS_ITEM_SIZE (activator_name_size);
 356     }
 357
 358   if (NULL != connection_name)
 359     {
 360       connection_name_size  = strlen (connection_name) + 1;
 361       hello_size += KDBUS_ITEM_SIZE (connection_name_size);
 362     }
 363
 364   hello = dbus_malloc (hello_size);
 365   if (NULL == hello)
 366     return -ENOMEM;
 367
 368   VALGRIND_MAKE_MEM_DEFINED (hello, hello_size);
 369
 370   hello->flags = flags;
 371   hello->attach_flags_send = attach_flags_send;
 372   hello->attach_flags_recv = attach_flags_recv;
 373   hello->pool_size = pool_size;
 374
 375   item = hello->items;
 376   if (connection_name_size > 0)
 377     item = _kdbus_item_add_string (item,
 378                                    KDBUS_ITEM_CONN_DESCRIPTION,
 379                                    connection_name,
 380                                    connection_name_size);
 381   if (activator_name_size > 0)
 382     {
 383       _kdbus_item_add_string (item,
 384                               KDBUS_ITEM_NAME,
 385                               activator_name,
 386                               activator_name_size);
 387       hello->flags |= KDBUS_HELLO_ACTIVATOR;
 388     }
 389
 390   hello->size = hello_size;
 391
 392   if (safe_ioctl (kdbus->fd, KDBUS_CMD_HELLO, hello) != 0)
 393     {
 394       dbus_free (hello);
 395       return -errno;
 396     }
 397
 398   kdbus->id = hello->id;
 399   memcpy (kdbus->bus_id, hello->id128, sizeof (kdbus->bus_id));
 400
 401   offset = hello->offset;
 402   items_size = hello->items_size;
 403   dbus_free (hello);
 404
 405   kdbus->mmap_ptr = mmap (NULL, pool_size, PROT_READ, MAP_SHARED, kdbus->fd, 0);
 406   if (MAP_FAILED == kdbus->mmap_ptr)
 407       return -errno;
 408
 409   kdbus->pool_size = pool_size;
 410
 411   items = get_from_offset (kdbus, offset);
 412   KDBUS_FOREACH (item, items, items_size)
 413     {
 414       if (KDBUS_ITEM_BLOOM_PARAMETER == item->type)
 415         kdbus->bloom = item->bloom_parameter;
 416     }
 417
 418   free_by_offset (kdbus, offset);
 419
 420   return 0;
 421 }
 422
 423 int
 424 _kdbus_send (kdbus_t           *kdbus,
 425              __u64              flags,
 426              struct kdbus_msg  *msg,
 427              struct kdbus_msg **msg_reply)
 428 {
 429   struct kdbus_cmd_send cmd;
 430
 431   VALGRIND_MAKE_MEM_DEFINED (&cmd, sizeof (cmd));
 432
 433   cmd.size = sizeof (cmd);
 434   cmd.msg_address = (uintptr_t)msg;
 435   cmd.flags = flags;
 436
 437   if (-1 == safe_ioctl (kdbus->fd, KDBUS_CMD_SEND, &cmd))
 438     return errno;
 439
 440   if (flags & KDBUS_SEND_SYNC_REPLY)
 441     {
 442       if (NULL != msg_reply)
 443         *msg_reply = get_from_offset (kdbus, cmd.reply.offset);
 444       else
 445         free_by_offset (kdbus, cmd.reply.offset);
 446     }
 447
 448   return 0;
 449 }
 450
 451 int
 452 _kdbus_recv (kdbus_t           *kdbus,
 453              __u64              flags,
 454              __s64              priority,
 455              struct kdbus_msg **msg)
 456 {
 457   struct kdbus_cmd_recv cmd;
 458
 459   VALGRIND_MAKE_MEM_DEFINED (&cmd, sizeof (cmd));
 460
 461   cmd.size = sizeof (cmd);
 462   cmd.flags = flags;
 463   cmd.priority = priority;
 464
 465   if (-1 == safe_ioctl (kdbus->fd, KDBUS_CMD_RECV, &cmd))
 466     return errno;
 467
 468   *msg = get_from_offset (kdbus, cmd.msg.offset);
 469
 470   return 0;
 471 }
 472
 473 /**
 474  * Provides list of names on the bus.
 475  *
 476  * @param kdbus kdbus object
 477  * @param flags specification of required names. See enum kdbus_list_flags in kdbus.h.
 478  * @param name_list data from kdbus response - the list
 479  * @param list_size size of name_list in bytes
 480  * @return 0 on success, errno on failure
 481  *
 482  * Note that name_list is allocated in kdbus pool and must be freed by _kdbus_free_mem.
 483  */
 484 int
 485 _kdbus_list (kdbus_t            *kdbus,
 486              __u64               flags,
 487              struct kdbus_info **name_list,
 488              __u64              *list_size)
 489 {
 490   struct kdbus_cmd_list cmd;
 491
 492   VALGRIND_MAKE_MEM_DEFINED (&cmd, sizeof (cmd));
 493
 494   cmd.size = sizeof (cmd);
 495   cmd.flags = flags;
 496
 497   if (-1 == safe_ioctl (kdbus->fd, KDBUS_CMD_LIST, &cmd))
 498     return errno;
 499
 500   *name_list = get_from_offset (kdbus, cmd.offset);
 501   *list_size = cmd.list_size;
 502
 503   return 0;
 504 }
 505
 506 __u64
 507 _kdbus_compute_match_items_size (kdbus_t       *kdbus,
 508                                  dbus_bool_t    with_bloom_mask,
 509                                  __u64          sender_id,
 510                                  const char    *sender_name)
 511 {
 512   __u64 size = 0;
 513
 514   if (with_bloom_mask)
 515     size += KDBUS_ITEM_SIZE (kdbus->bloom.size);
 516
 517   if (KDBUS_MATCH_ID_ANY != sender_id) /* unique name present */
 518     size += KDBUS_ITEM_SIZE (sizeof (sender_id));
 519   else if (NULL != sender_name)
 520     size += KDBUS_ITEM_SIZE (strlen (sender_name) + 1);
 521
 522   return size;
 523 }
 524
 525 struct kdbus_cmd_match *
 526 _kdbus_new_cmd_match (kdbus_t       *kdbus,
 527                       __u64          items_size,
 528                       __u64          flags,
 529                       __u64          cookie)
 530 {
 531   struct kdbus_cmd_match *cmd;
 532   __u64 cmd_size = sizeof (*cmd) + items_size;
 533   cmd = dbus_malloc (cmd_size);
 534   if (NULL == cmd)
 535     return NULL;
 536
 537   VALGRIND_MAKE_MEM_DEFINED (cmd, cmd_size);
 538
 539   cmd->size = cmd_size;
 540   cmd->flags = flags;
 541   cmd->cookie = cookie;
 542
 543   return cmd;
 544 }
 545
 546 void
 547 _kdbus_free_cmd_match (struct kdbus_cmd_match *cmd)
 548 {
 549   dbus_free (cmd);
 550 }
 551
 552 int
 553 _kdbus_add_match_name_acquired (kdbus_t *kdbus,
 554                             __u64    flags,
 555                             __u64    cookie,
 556                             __u64    old_id,
 557                             __u64    old_id_flags,
 558                             __u64    new_id,
 559                             __u64    new_id_flags,
 560                             const char *name)
 561 {
 562   struct kdbus_cmd_match *cmd;
 563   struct kdbus_item *item;
 564   int ret;
 565   unsigned int len = 0;
 566
 567   if (name)
 568     len = strlen(name) + 1;
 569   cmd = _kdbus_new_cmd_match (kdbus,
 570                               KDBUS_ITEM_SIZE (sizeof (struct kdbus_notify_name_change) + len),
 571                               flags,
 572                               cookie);
 573   if (NULL == cmd)
 574     return ENOMEM;
 575
 576   item = cmd->items;
 577   _kdbus_item_add_name_change (item,
 578                                old_id, old_id_flags,
 579                                new_id, new_id_flags);
 580   item->size += len;
 581   if (name)
 582     memcpy(item->name_change.name, name, len);
 583
 584   ret = safe_ioctl (kdbus->fd, KDBUS_CMD_MATCH_ADD, cmd);
 585   if (ret != 0)
 586     goto err_name_acquired;
 587
 588   item->type = KDBUS_ITEM_NAME_ADD;
 589   ret = safe_ioctl (kdbus->fd, KDBUS_CMD_MATCH_ADD, cmd);
 590   if (ret != 0)
 591     goto err_name_acquired;
 592
 593 err_name_acquired:
 594   if (0 != ret)
 595     ret = errno;
 596   _kdbus_free_cmd_match (cmd);
 597   return ret;
 598 }
 599
 600 int
 601 _kdbus_add_match_name_lost (kdbus_t *kdbus,
 602                             __u64    flags,
 603                             __u64    cookie,
 604                             __u64    old_id,
 605                             __u64    old_id_flags,
 606                             __u64    new_id,
 607                             __u64    new_id_flags,
 608                             const char *name)
 609 {
 610   struct kdbus_cmd_match *cmd;
 611   struct kdbus_item *item;
 612   int ret;
 613   unsigned int len = 0;
 614
 615   if (name)
 616     len = strlen(name) + 1;
 617
 618   cmd = _kdbus_new_cmd_match (kdbus,
 619                               KDBUS_ITEM_SIZE (sizeof (struct kdbus_notify_name_change) + len),
 620                               flags,
 621                               cookie);
 622   if (NULL == cmd)
 623     return ENOMEM;
 624
 625   item = cmd->items;
 626   _kdbus_item_add_name_change (item,
 627                                old_id, old_id_flags,
 628                                new_id, new_id_flags);
 629
 630   item->size += len;
 631   if (name)
 632   memcpy(item->name_change.name, name, len);
 633
 634   ret = safe_ioctl (kdbus->fd, KDBUS_CMD_MATCH_ADD, cmd);
 635   if (ret != 0)
 636     goto err_name_remove;
 637
 638   item->type = KDBUS_ITEM_NAME_REMOVE;
 639   ret = safe_ioctl (kdbus->fd, KDBUS_CMD_MATCH_ADD, cmd);
 640   if (ret != 0)
 641     goto err_name_remove;
 642
 643 err_name_remove:
 644   if (0 != ret)
 645     ret = errno;
 646   _kdbus_free_cmd_match (cmd);
 647   return ret;
 648 }
 649
 650 int
 651 _kdbus_add_match_name_change (kdbus_t *kdbus,
 652                               __u64    flags,
 653                               __u64    cookie,
 654                               __u64    old_id,
 655                               __u64    old_id_flags,
 656                               __u64    new_id,
 657                               __u64    new_id_flags,
 658                               const char *name)
 659 {
 660   struct kdbus_cmd_match *cmd;
 661   struct kdbus_item *item;
 662   int ret;
 663   unsigned int len = 0;
 664
 665   if (name)
 666     len = strlen(name) + 1;
 667
 668   /* name = NULL or Well-known Name */
 669   if (name == NULL || (name[0] != ':')) {
 670     cmd = _kdbus_new_cmd_match (kdbus,
 671                                 KDBUS_ITEM_SIZE (sizeof (struct kdbus_notify_name_change) + len),
 672                                 flags,
 673                                 cookie);
 674     if (NULL == cmd)
 675       return ENOMEM;
 676
 677     item = cmd->items;
 678     _kdbus_item_add_name_change (item,
 679                                  old_id, old_id_flags,
 680                                  new_id, new_id_flags);
 681
 682     item->size += len;
 683     if (name)
 684       memcpy(item->name_change.name, name, len);
 685
 686     ret = safe_ioctl (kdbus->fd, KDBUS_CMD_MATCH_ADD, cmd);
 687     if (0 == ret)
 688       {
 689         item->type = KDBUS_ITEM_NAME_ADD;
 690         ret = safe_ioctl (kdbus->fd, KDBUS_CMD_MATCH_ADD, cmd);
 691         if (0 == ret)
 692           {
 693            item->type = KDBUS_ITEM_NAME_REMOVE;
 694            ret = safe_ioctl (kdbus->fd, KDBUS_CMD_MATCH_ADD, cmd);
 695           }
 696       }
 697
 698     if (0 != ret)
 699       ret = errno;
 700
 701     _kdbus_free_cmd_match (cmd);
 702     return ret;
 703   }
 704
 705   return FALSE;
 706 }
 707
 708 int
 709 _kdbus_add_match_id_change (kdbus_t *kdbus,
 710                             __u64    flags,
 711                             __u64    cookie,
 712                             __u64    id,
 713                             __u64    id_flags,
 714                             const char *name)
 715 {
 716   struct kdbus_cmd_match *cmd;
 717   struct kdbus_item *item;
 718   int ret;
 719
 720   /* name = NULL or Unique Name */
 721   if (name == NULL || name[0] == ':')
 722     {
 723     cmd = _kdbus_new_cmd_match (kdbus,
 724                                 KDBUS_ITEM_SIZE (sizeof (struct kdbus_notify_id_change)),
 725                                 flags,
 726                                 cookie);
 727     if (NULL == cmd)
 728       return ENOMEM;
 729
 730     item = cmd->items;
 731     _kdbus_item_add_id_add (item, id, id_flags);
 732
 733     if (name)
 734       item->id_change.id = strtoull (name + 3, NULL, 10);
 735
 736     ret = safe_ioctl (kdbus->fd, KDBUS_CMD_MATCH_ADD, cmd);
 737     if (0 == ret)
 738       {
 739         item->type = KDBUS_ITEM_ID_REMOVE;
 740         ret = safe_ioctl (kdbus->fd, KDBUS_CMD_MATCH_ADD, cmd);
 741       }
 742
 743    if (0 != ret)
 744      ret = errno;
 745
 746     _kdbus_free_cmd_match (cmd);
 747     return ret;
 748   }
 749   return FALSE;
 750 }
 751
 752 int _kdbus_add_match (kdbus_t *kdbus,
 753                       struct kdbus_cmd_match *cmd)
 754 {
 755   int ret = safe_ioctl (kdbus->fd, KDBUS_CMD_MATCH_ADD, cmd);
 756   if (0 != ret)
 757     return errno;
 758
 759   return 0;
 760 }
 761
 762 /**
 763  * Allocates and initializes kdbus message structure.
 764  * @param kdbus kdbus object
 765  * @param size_for_items size of items that will be attached to this message
 766  * @param flags flags for message
 767  * @returns initialized kdbus message or NULL if malloc failed
 768  */
 769 struct kdbus_msg *
 770 _kdbus_new_msg (kdbus_t                *kdbus,
 771                 __u64                   size_for_items,
 772                 __u64                   flags,
 773                 __s64                   priority,
 774                 __u64                   dst_id,
 775                 __u64                   src_id,
 776                 enum kdbus_payload_type payload_type,
 777                 __u64                   cookie,
 778                 __u64                   timeout_ns_or_cookie_reply)
 779 {
 780   struct kdbus_msg *msg;
 781   __u64 msg_size = sizeof (struct kdbus_msg) + size_for_items;
 782
 783   msg = dbus_malloc (msg_size);
 784   if (NULL == msg)
 785     return NULL;
 786
 787   msg->size = msg_size;
 788   msg->flags = flags;
 789   msg->priority = priority;
 790   msg->dst_id = dst_id;
 791   msg->src_id = src_id;
 792   msg->payload_type = payload_type;
 793   msg->cookie = cookie;
 794   msg->timeout_ns = timeout_ns_or_cookie_reply;
 795
 796   return msg;
 797 }
 798
 799 void
 800 _kdbus_free_msg (struct kdbus_msg *msg)
 801 {
 802   dbus_free (msg);
 803 }
 804
 805 int
 806 _kdbus_free_mem (kdbus_t *kdbus, void *mem)
 807 {
 808   char *base_ptr = kdbus->mmap_ptr;
 809   char *mem_ptr = (char *)mem;
 810
 811   return free_by_offset (kdbus, mem_ptr - base_ptr);
 812 }
 813
 814 /**
 815  * Computes size of items that will be attached to a message.
 816  *
 817  * @param kdbus kdbus object
 818  * @param destination Well-known name or NULL. If NULL, dst_id must be supplied.
 819  * @param dst_id Numeric id of recipient. Ignored if name is not NULL.
 820  * @param body_size Size of message body (may be 0).
 821  * @param use_memfd Flag to build memfd message.
 822  * @param fds_count Number of file descriptors sent in the message.
 823  * @returns size in bytes needed for the message object
 824  */
 825 __u64
 826 _kdbus_compute_msg_items_size (kdbus_t       *kdbus,
 827                                const char    *destination,
 828                                __u64          dst_id,
 829                                __u64          body_size,
 830                                dbus_bool_t    use_memfd,
 831                                int            fds_count)
 832 {
 833   __u64 items_size = 0;
 834
 835   /*  header */
 836   items_size += KDBUS_ITEM_SIZE (sizeof (struct kdbus_vec));
 837
 838   if (use_memfd)
 839     {
 840       /* body */
 841       items_size += KDBUS_ITEM_SIZE (sizeof (struct kdbus_memfd));
 842
 843       /* footer */
 844       items_size += KDBUS_ITEM_SIZE (sizeof (struct kdbus_vec));
 845     }
 846   else
 847     {
 848       __u64 vectors = (body_size + KDBUS_MSG_MAX_PAYLOAD_VEC_SIZE - 1)
 849                        / KDBUS_MSG_MAX_PAYLOAD_VEC_SIZE;
 850       /* subsequent vectors -> parts of body */
 851       items_size += vectors * KDBUS_ITEM_SIZE (sizeof (struct kdbus_vec));
 852     }
 853
 854   if (fds_count > 0)
 855     items_size += KDBUS_ITEM_SIZE (sizeof (int) * fds_count);
 856
 857   if (destination)
 858     items_size += KDBUS_ITEM_SIZE (strlen (destination) + 1);
 859   else if (KDBUS_DST_ID_BROADCAST == dst_id)
 860     items_size += KDBUS_ITEM_SIZE (sizeof (struct kdbus_bloom_filter))
 861                   + kdbus->bloom.size;
 862   return items_size;
 863 }
 864
 865 /**
 866  *
 867  * Asks the bus to assign the given name to the connection.
 868  *
 869  * Use same flags as original dbus version with one exception below.
 870  * Result flag #DBUS_REQUEST_NAME_REPLY_ALREADY_OWNER is currently
 871  * never returned by kdbus, instead DBUS_REQUEST_NAME_REPLY_PRIMARY_OWNER
 872  * is returned by kdbus.
 873  *
 874  * @param transport transport of the connection
 875  * @param name the name to request
 876  * @param flags flags
 877  * @returns a DBus result code on success, -errno on error
 878  */
 879 int
 880 _kdbus_request_name (kdbus_t     *kdbus,
 881                      const char  *name,
 882                      const __u64  flags)
 883 {
 884   struct kdbus_cmd *cmd_name;
 885   size_t len = strlen (name) + 1;
 886   __u64 size = sizeof (*cmd_name) + KDBUS_ITEM_SIZE (len);
 887   __u64 flags_kdbus = 0;
 888
 889   cmd_name = alloca (size);
 890   cmd_name->size = size;
 891
 892   if (flags & DBUS_NAME_FLAG_ALLOW_REPLACEMENT)
 893     flags_kdbus |= KDBUS_NAME_ALLOW_REPLACEMENT;
 894   if (!(flags & DBUS_NAME_FLAG_DO_NOT_QUEUE))
 895     flags_kdbus |= KDBUS_NAME_QUEUE;
 896   if (flags & DBUS_NAME_FLAG_REPLACE_EXISTING)
 897     flags_kdbus |= KDBUS_NAME_REPLACE_EXISTING;
 898
 899   cmd_name->flags = flags_kdbus;
 900   make_item_name (name, &(cmd_name->items[0]));
 901
 902   _dbus_verbose ("Request name - flags sent: 0x%llx       !!!!!!!!!\n", cmd_name->flags);
 903
 904   if (ioctl (kdbus->fd, KDBUS_CMD_NAME_ACQUIRE, cmd_name) < 0)
 905     {
 906       _dbus_verbose ("error acquiring name '%s': %m, %d\n", name, errno);
 907       if (errno == EEXIST)
 908         return DBUS_REQUEST_NAME_REPLY_EXISTS;
 909       if (errno == EALREADY)
 910         return DBUS_REQUEST_NAME_REPLY_ALREADY_OWNER;
 911       return -errno;
 912     }
 913   else if ((cmd_name->return_flags & KDBUS_NAME_PRIMARY)
 914        && !(cmd_name->return_flags & KDBUS_NAME_ACQUIRED))
 915     return DBUS_REQUEST_NAME_REPLY_ALREADY_OWNER;
 916
 917   _dbus_verbose ("Request name - received flag: 0x%llx       !!!!!!!!!\n", cmd_name->flags);
 918
 919   if (cmd_name->return_flags & KDBUS_NAME_IN_QUEUE)
 920     return DBUS_REQUEST_NAME_REPLY_IN_QUEUE;
 921
 922   return DBUS_REQUEST_NAME_REPLY_PRIMARY_OWNER;
 923 }
 924
 925 /**
 926  *
 927  * Releases well-known name - the connections resign from the name
 928  * which can be then assigned to another connection or the connection
 929  * is being removed from the queue for that name
 930  *
 931  * @param name the name to request
 932  * @param id unique id of the connection for which the name is being released
 933  * @returns a DBus result code on success, -errno on error
 934  */
 935 int
 936 _kdbus_release_name (kdbus_t    *kdbus,
 937                      const char *name)
 938 {
 939   struct kdbus_cmd *cmd_name;
 940   size_t len = strlen (name)+1;
 941   __u64 size = sizeof (*cmd_name) + KDBUS_ITEM_SIZE (len);
 942
 943   cmd_name = alloca (size);
 944   cmd_name->size = size;
 945   cmd_name->flags = 0;
 946   make_item_name (name, &(cmd_name->items[0]));
 947
 948   if (ioctl (kdbus->fd, KDBUS_CMD_NAME_RELEASE, cmd_name))
 949     {
 950       if ((errno == ESRCH))
 951         return DBUS_RELEASE_NAME_REPLY_NON_EXISTENT;
 952       else if (errno == EADDRINUSE)
 953         return DBUS_RELEASE_NAME_REPLY_NOT_OWNER;
 954       _dbus_verbose ("error releasing name '%s'. Error: %m, %d\n", name, errno);
 955       return -errno;
 956     }
 957
 958   _dbus_verbose ("Name '%s' released\n", name);
 959
 960   return DBUS_RELEASE_NAME_REPLY_RELEASED;
 961 }
 962
 963 static int
 964 decode_connection_info (struct kdbus_info *connection_info,
 965                         struct nameInfo   *pInfo,
 966                         dbus_bool_t        get_sec_label)
 967 {
 968   struct kdbus_item *item;
 969
 970   memset (pInfo, 0, sizeof (*pInfo));
 971
 972   pInfo->uniqueId = connection_info->id;
 973   pInfo->flags = connection_info->flags;
 974
 975   item = connection_info->items;
 976
 977   while ((uint8_t *)item < ((uint8_t *)connection_info) + connection_info->size)
 978     {
 979       switch (item->type)
 980         {
 981           case KDBUS_ITEM_PIDS:
 982             pInfo->processId = item->pids.pid;
 983             break;
 984                   case KDBUS_ITEM_CREDS:
 985                           pInfo->userId = item->creds.uid;
 986                           break;
 987           case KDBUS_ITEM_SECLABEL:
 988               if (get_sec_label)
 989                 {
 990                   pInfo->sec_label_len = item->size - KDBUS_ITEM_HEADER_SIZE;
 991                   if (0 != pInfo->sec_label_len)
 992                     {
 993                       pInfo->sec_label = dbus_malloc (pInfo->sec_label_len);
 994                       if (NULL == pInfo->sec_label)
 995                         return ENOMEM;
 996
 997                       memcpy (pInfo->sec_label, item->data, pInfo->sec_label_len);
 998                     }
 999                 }
1000               break;
1001         }
1002
1003       item = KDBUS_ITEM_NEXT (item);
1004     }
1005   return 0;
1006 }
1007
1008 static int
1009 process_connection_info_cmd (kdbus_t               *kdbus,
1010                              struct kdbus_cmd_info *cmd,
1011                              struct nameInfo       *pInfo,
1012                              dbus_bool_t            get_sec_label)
1013 {
1014   int ret;
1015   struct kdbus_info *kdbus_info;
1016
1017   if (NULL == cmd)
1018     return -1;
1019
1020   ret = safe_ioctl (kdbus->fd, KDBUS_CMD_CONN_INFO, cmd);
1021
1022   if (ret < 0)
1023   {
1024     pInfo->uniqueId = 0;
1025     dbus_free(cmd);
1026     return errno;
1027   }
1028
1029   kdbus_info = get_from_offset (kdbus, cmd->offset);
1030   ret = decode_connection_info (kdbus_info,
1031                                 pInfo,
1032                                 get_sec_label);
1033   if (ret != 0) {
1034     dbus_free(cmd);
1035     return ret;
1036   }
1037
1038   ret = free_by_offset (kdbus, cmd->offset);
1039   if (ret != 0)
1040     {
1041       _dbus_verbose ("kdbus error freeing pool: %d (%m)\n", errno);
1042       if (get_sec_label)
1043         {
1044           free (pInfo->sec_label);
1045           pInfo->sec_label = NULL;
1046         }
1047     }
1048
1049   dbus_free (cmd);
1050
1051   return ret;
1052 }
1053
1054 /*
1055  * In this function either id is equal to 0 AND name is not NULL,
1056  * or id is greater than 0 AND name is NULL.
1057  * Thus, condition NULL != name is equivalent to 0 == id.
1058  */
1059 static struct kdbus_cmd_info *
1060 prepare_connection_info_cmd (__u64          id,
1061                              const char    *name,
1062                              dbus_bool_t    get_sec_label)
1063 {
1064   struct kdbus_cmd_info *cmd;
1065   __u64 size = sizeof (*cmd);
1066
1067   if (NULL != name)
1068     size += KDBUS_ITEM_SIZE (strlen (name) + 1);
1069
1070   cmd = dbus_malloc (size);
1071   if (NULL == cmd)
1072     return NULL;
1073
1074   cmd->size = size;
1075   cmd->id = id;
1076   if (NULL != name)
1077     make_item_name (name, &(cmd->items[0]));
1078
1079   cmd->attach_flags = KDBUS_ATTACH_CREDS | KDBUS_ATTACH_PIDS;
1080   if (get_sec_label)
1081     cmd->attach_flags |= KDBUS_ATTACH_SECLABEL;
1082
1083   cmd->flags = 0;
1084
1085   return cmd;
1086 }
1087
1088 /**
1089  * Gets connection info for the given unique id.
1090  *
1091  * @param kdbus kdbus object
1092  * @param id unique id to query for
1093  * @param get_sec_label #TRUE if sec_label field in pInfo should be filled
1094  * @param pInfo nameInfo structure address to store info about the name
1095  * @return 0 on success, errno if failed
1096  *
1097  * @note If you specify #TRUE in get_sec_label param, you must free
1098  * pInfo.sec_label with dbus_free() after use.
1099  */
1100 int
1101 _kdbus_connection_info_by_id (kdbus_t         *kdbus,
1102                               __u64            id,
1103                               dbus_bool_t      get_sec_label,
1104                               struct nameInfo *pInfo)
1105 {
1106   struct kdbus_cmd_info *cmd = prepare_connection_info_cmd (id, NULL, get_sec_label);
1107
1108   return process_connection_info_cmd (kdbus, cmd, pInfo, get_sec_label);
1109 }
1110
1111 /**
1112  * Gets connection info for the given name
1113  *
1114  * @param kdbus kdbus object
1115  * @param name name to query for
1116  * @param get_sec_label #TRUE if sec_label field in pInfo should be filled
1117  * @param pInfo nameInfo structure address to store info about the name
1118  * @return 0 on success, errno if failed
1119  *
1120  * @note If you specify #TRUE in get_sec_label param, you must free
1121  * pInfo.sec_label with dbus_free() after use.
1122  */
1123 int
1124 _kdbus_connection_info_by_name (kdbus_t         *kdbus,
1125                                 const char      *name,
1126                                 dbus_bool_t      get_sec_label,
1127                                 struct nameInfo *pInfo)
1128 {
1129   struct kdbus_cmd_info *cmd;
1130
1131   /* if name starts with ":1." it is a unique name and should be send as number */
1132   if ((name[0] == ':') && (name[1] == '1') && (name[2] == '.'))
1133   {
1134     return _kdbus_connection_info_by_id (kdbus,
1135                                          strtoull (&name[3], NULL, 10),
1136                                          get_sec_label,
1137                                          pInfo);
1138   }
1139
1140   cmd = prepare_connection_info_cmd (0, name, get_sec_label);
1141
1142   return process_connection_info_cmd (kdbus, cmd, pInfo, get_sec_label);
1143 }
1144
1145 /*
1146  * Removes match rule in kdbus on behalf of sender of the message
1147  * @param kdbus kdbus object
1148  * @param cookie cookie of the rules to be removed
1149  */
1150 int
1151 _kdbus_remove_match (kdbus_t    *kdbus,
1152                      __u64       cookie)
1153 {
1154   struct kdbus_cmd_match cmd;
1155
1156   VALGRIND_MAKE_MEM_DEFINED (&cmd, sizeof (cmd));
1157
1158   cmd.cookie = cookie;
1159   cmd.size = sizeof (struct kdbus_cmd_match);
1160   cmd.flags = 0;
1161
1162   if (safe_ioctl (kdbus->fd, KDBUS_CMD_MATCH_REMOVE, &cmd) != 0)
1163     return errno;
1164
1165   return 0;
1166 }
1167
1168 /************************* BLOOM FILTERS ***********************/
1169
1170 /*
1171  * Macros for SipHash algorithm
1172  */
1173 #define ROTL(x,b) (uint64_t)( ((x) << (b)) | ( (x) >> (64 - (b))) )
1174
1175 #define U32TO8_LE(p, v)         \
1176     (p)[0] = (unsigned char)((v)      ); (p)[1] = (unsigned char)((v) >>  8); \
1177     (p)[2] = (unsigned char)((v) >> 16); (p)[3] = (unsigned char)((v) >> 24);
1178
1179 #define U64TO8_LE(p, v)         \
1180   U32TO8_LE((p),     (uint32_t)((v)      ));   \
1181   U32TO8_LE((p) + 4, (uint32_t)((v) >> 32));
1182
1183 #define U8TO64_LE(p) \
1184   (((uint64_t)((p)[0])      ) | \
1185    ((uint64_t)((p)[1]) <<  8) | \
1186    ((uint64_t)((p)[2]) << 16) | \
1187    ((uint64_t)((p)[3]) << 24) | \
1188    ((uint64_t)((p)[4]) << 32) | \
1189    ((uint64_t)((p)[5]) << 40) | \
1190    ((uint64_t)((p)[6]) << 48) | \
1191    ((uint64_t)((p)[7]) << 56))
1192
1193 #define SIPROUND            \
1194   do {              \
1195     v0 += v1; v1=ROTL(v1,13); v1 ^= v0; v0=ROTL(v0,32); \
1196     v2 += v3; v3=ROTL(v3,16); v3 ^= v2;     \
1197     v0 += v3; v3=ROTL(v3,21); v3 ^= v0;     \
1198     v2 += v1; v1=ROTL(v1,17); v1 ^= v2; v2=ROTL(v2,32); \
1199   } while (0)
1200
1201
1202 /*
1203  * Hash keys for bloom filters
1204  */
1205 static const unsigned char hash_keys[8][16] =
1206 {
1207   {0xb9,0x66,0x0b,0xf0,0x46,0x70,0x47,0xc1,0x88,0x75,0xc4,0x9c,0x54,0xb9,0xbd,0x15},
1208   {0xaa,0xa1,0x54,0xa2,0xe0,0x71,0x4b,0x39,0xbf,0xe1,0xdd,0x2e,0x9f,0xc5,0x4a,0x3b},
1209   {0x63,0xfd,0xae,0xbe,0xcd,0x82,0x48,0x12,0xa1,0x6e,0x41,0x26,0xcb,0xfa,0xa0,0xc8},
1210   {0x23,0xbe,0x45,0x29,0x32,0xd2,0x46,0x2d,0x82,0x03,0x52,0x28,0xfe,0x37,0x17,0xf5},
1211   {0x56,0x3b,0xbf,0xee,0x5a,0x4f,0x43,0x39,0xaf,0xaa,0x94,0x08,0xdf,0xf0,0xfc,0x10},
1212   {0x31,0x80,0xc8,0x73,0xc7,0xea,0x46,0xd3,0xaa,0x25,0x75,0x0f,0x9e,0x4c,0x09,0x29},
1213   {0x7d,0xf7,0x18,0x4b,0x7b,0xa4,0x44,0xd5,0x85,0x3c,0x06,0xe0,0x65,0x53,0x96,0x6d},
1214   {0xf2,0x77,0xe9,0x6f,0x93,0xb5,0x4e,0x71,0x9a,0x0c,0x34,0x88,0x39,0x25,0xbf,0x35}
1215 };
1216
1217 /*
1218  * SipHash algorithm
1219  */
1220 static void
1221 _g_siphash24 (unsigned char       out[8],
1222               const void         *_in,
1223               size_t              inlen,
1224               const unsigned char k[16])
1225 {
1226   uint64_t v0 = 0x736f6d6570736575ULL;
1227   uint64_t v1 = 0x646f72616e646f6dULL;
1228   uint64_t v2 = 0x6c7967656e657261ULL;
1229   uint64_t v3 = 0x7465646279746573ULL;
1230   uint64_t b;
1231   uint64_t k0 = U8TO64_LE (k);
1232   uint64_t k1 = U8TO64_LE (k + 8);
1233   uint64_t m;
1234   const unsigned char *in = _in;
1235   const unsigned char *end = in + inlen - (inlen % sizeof (uint64_t));
1236   const int left = inlen & 7;
1237   b = ((uint64_t) inlen) << 56;
1238   v3 ^= k1;
1239   v2 ^= k0;
1240   v1 ^= k1;
1241   v0 ^= k0;
1242
1243   for (; in != end; in += 8)
1244     {
1245       m = U8TO64_LE (in);
1246       v3 ^= m;
1247       SIPROUND;
1248       SIPROUND;
1249       v0 ^= m;
1250     }
1251
1252   switch (left)
1253     {
1254       case 7: b |= ((uint64_t) in[6]) << 48;
1255       case 6: b |= ((uint64_t) in[5]) << 40;
1256       case 5: b |= ((uint64_t) in[4]) << 32;
1257       case 4: b |= ((uint64_t) in[3]) << 24;
1258       case 3: b |= ((uint64_t) in[2]) << 16;
1259       case 2: b |= ((uint64_t) in[1]) <<  8;
1260       case 1: b |= ((uint64_t) in[0]); break;
1261       case 0: break;
1262     }
1263
1264   v3 ^= b;
1265   SIPROUND;
1266   SIPROUND;
1267   v0 ^= b;
1268
1269   v2 ^= 0xff;
1270   SIPROUND;
1271   SIPROUND;
1272   SIPROUND;
1273   SIPROUND;
1274   b = v0 ^ v1 ^ v2  ^ v3;
1275   U64TO8_LE (out, b);
1276 }
1277
1278 void
1279 _kdbus_bloom_add_data (kdbus_t            *kdbus,
1280                        kdbus_bloom_data_t *bloom_data,
1281                        const void         *data,
1282                        size_t              data_size)
1283 {
1284   unsigned char hash[8];
1285   uint64_t bit_num;
1286   unsigned int bytes_num = 0;
1287   unsigned int cnt_1, cnt_2;
1288   unsigned int hash_index = 0;
1289
1290   unsigned int c = 0;
1291   uint64_t p = 0;
1292
1293   bit_num = kdbus->bloom.size * 8;
1294
1295   if (bit_num > 1)
1296     bytes_num = ((__builtin_clzll (bit_num) ^ 63U) + 7) / 8;
1297
1298   for (cnt_1 = 0; cnt_1 < kdbus->bloom.n_hash; cnt_1++)
1299     {
1300       for (cnt_2 = 0, hash_index = 0; cnt_2 < bytes_num; cnt_2++)
1301         {
1302           if (c <= 0)
1303             {
1304               _g_siphash24 (hash, data, data_size, hash_keys[hash_index++]);
1305               c += 8;
1306             }
1307
1308           p = (p << 8ULL) | (uint64_t) hash[8 - c];
1309           c--;
1310         }
1311
1312       p &= bit_num - 1;
1313       bloom_data[p >> 6] |= 1ULL << (p & 63);
1314     }
1315 }