lib/msg.c

   1 /*
   2  * lib/msg.c            Netlink Messages Interface
   3  *
   4  *      This library is free software; you can redistribute it and/or
   5  *      modify it under the terms of the GNU Lesser General Public
   6  *      License as published by the Free Software Foundation version 2.1
   7  *      of the License.
   8  *
   9  * Copyright (c) 2003-2008 Thomas Graf <tgraf@suug.ch>
  10  */
  11
  12 /**
  13  * @ingroup core
  14  * @defgroup msg Messages
  15  * Netlink Message Construction/Parsing Interface
  16  *
  17  * The following information is partly extracted from RFC3549
  18  * (ftp://ftp.rfc-editor.org/in-notes/rfc3549.txt)
  19  *
  20  * @par Message Format
  21  * Netlink messages consist of a byte stream with one or multiple
  22  * Netlink headers and an associated payload.  If the payload is too big
  23  * to fit into a single message it, can be split over multiple Netlink
  24  * messages, collectively called a multipart message.  For multipart
  25  * messages, the first and all following headers have the \c NLM_F_MULTI
  26  * Netlink header flag set, except for the last header which has the
  27  * Netlink header type \c NLMSG_DONE.
  28  *
  29  * @par
  30  * The Netlink message header (\link nlmsghdr struct nlmsghdr\endlink) is shown below.
  31  * @code
  32  * 0                   1                   2                   3
  33  * 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
  34  * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
  35  * |                          Length                             |
  36  * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
  37  * |            Type              |           Flags              |
  38  * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
  39  * |                      Sequence Number                        |
  40  * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
  41  * |                      Process ID (PID)                       |
  42  * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
  43  * @endcode
  44  *
  45  * @par
  46  * The netlink message header and payload must be aligned properly:
  47  * @code
  48  *  <------- NLMSG_ALIGN(hlen) ------> <---- NLMSG_ALIGN(len) --->
  49  * +----------------------------+- - -+- - - - - - - - - - -+- - -+
  50  * |           Header           | Pad |       Payload       | Pad |
  51  * |      struct nlmsghdr       |     |                     |     |
  52  * +----------------------------+- - -+- - - - - - - - - - -+- - -+
  53  * @endcode
  54  * @par
  55  * Message Format:
  56  * @code
  57  *    <--- nlmsg_total_size(payload)  --->
  58  *    <-- nlmsg_msg_size(payload) ->
  59  *   +----------+- - -+-------------+- - -+-------- - -
  60  *   | nlmsghdr | Pad |   Payload   | Pad | nlmsghdr
  61  *   +----------+- - -+-------------+- - -+-------- - -
  62  *   nlmsg_data(nlh)---^                   ^
  63  *   nlmsg_next(nlh)-----------------------+
  64  * @endcode
  65  * @par
  66  * The payload may consist of arbitary data but may have strict
  67  * alignment and formatting rules depening on the specific netlink
  68  * families.
  69  * @par
  70  * @code
  71  *    <---------------------- nlmsg_len(nlh) --------------------->
  72  *    <------ hdrlen ------>       <- nlmsg_attrlen(nlh, hdrlen) ->
  73  *   +----------------------+- - -+--------------------------------+
  74  *   |     Family Header    | Pad |           Attributes           |
  75  *   +----------------------+- - -+--------------------------------+
  76  *   nlmsg_attrdata(nlh, hdrlen)---^
  77  * @endcode
  78  * @par The ACK Netlink Message
  79  * This message is actually used to denote both an ACK and a NACK.
  80  * Typically, the direction is from FEC to CPC (in response to an ACK
  81  * request message).  However, the CPC should be able to send ACKs back
  82  * to FEC when requested.
  83  * @code
  84  *  0                   1                   2                   3
  85  *  0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
  86  * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
  87  * |                       Netlink message header                  |
  88  * |                       type = NLMSG_ERROR                      |
  89  * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
  90  * |                          Error code                           |
  91  * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
  92  * |                       OLD Netlink message header              |
  93  * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
  94  * @endcode
  95  *
  96  * @par Example
  97  * @code
  98  * // Various methods exist to create/allocate a new netlink
  99  * // message.
 100  * //
 101  * // nlmsg_alloc() will allocate an empty netlink message with
 102  * // a maximum payload size which defaults to the page size of
 103  * // the system. This default size can be modified using the
 104  * // function nlmsg_set_default_size().
 105  * struct nl_msg *msg = nlmsg_alloc();
 106  *
 107  * // Very often, the message type and message flags are known
 108  * // at allocation time while the other fields are auto generated:
 109  * struct nl_msg *msg = nlmsg_alloc_simple(MY_TYPE, MY_FLAGS);
 110  *
 111  * // Alternatively an existing netlink message header can be used
 112  * // to inherit the header values:
 113  * struct nlmsghdr hdr = {
 114  *      .nlmsg_type = MY_TYPE,
 115  *      .nlmsg_flags = MY_FLAGS,
 116  * };
 117  * struct nl_msg *msg = nlmsg_inherit(&hdr);
 118  *
 119  * // Last but not least, netlink messages received from netlink sockets
 120  * // can be converted into nl_msg objects using nlmsg_convert(). This
 121  * // will create a message with a maximum payload size which equals the
 122  * // length of the existing netlink message, therefore no more data can
 123  * // be appened without calling nlmsg_expand() first.
 124  * struct nl_msg *msg = nlmsg_convert(nlh_from_nl_sock);
 125  *
 126  * // Payload may be added to the message via nlmsg_append(). The fourth
 127  * // parameter specifies the number of alignment bytes the data should
 128  * // be padding with at the end. Common values are 0 to disable it or
 129  * // NLMSG_ALIGNTO to ensure proper netlink message padding.
 130  * nlmsg_append(msg, &mydata, sizeof(mydata), 0);
 131  *
 132  * // Sometimes it may be necessary to reserve room for data but defer
 133  * // the actual copying to a later point, nlmsg_reserve() can be used
 134  * // for this purpose:
 135  * void *data = nlmsg_reserve(msg, sizeof(mydata), NLMSG_ALIGNTO);
 136  *
 137  * // Attributes may be added using the attributes interface.
 138  *
 139  * // After successful use of the message, the memory must be freed
 140  * // using nlmsg_free()
 141  * nlmsg_free(msg);
 142  * @endcode
 143  *
 144  * @par 4) Parsing messages
 145  * @code
 146  * int n;
 147  * unsigned char *buf;
 148  * struct nlmsghdr *hdr;
 149  *
 150  * n = nl_recv(handle, NULL, &buf);
 151  *
 152  * hdr = (struct nlmsghdr *) buf;
 153  * while (nlmsg_ok(hdr, n)) {
 154  *      // Process message here...
 155  *      hdr = nlmsg_next(hdr, &n);
 156  * }
 157  * @endcode
 158  * @{
 159  */
 160
 161 #include <netlink-local.h>
 162 #include <netlink/netlink.h>
 163 #include <netlink/utils.h>
 164 #include <netlink/cache.h>
 165 #include <netlink/attr.h>
 166 #include <linux/socket.h>
 167
 168 static size_t default_msg_size;
 169
 170 static void __init init_msg_size(void)
 171 {
 172         default_msg_size = getpagesize();
 173 }
 174
 175 /**
 176  * @name Size Calculations
 177  * @{
 178  */
 179
 180 /**
 181  * length of netlink message not including padding
 182  * @arg payload         length of message payload
 183  */
 184 int nlmsg_msg_size(int payload)
 185 {
 186         return NLMSG_HDRLEN + payload;
 187 }
 188
 189 /**
 190  * length of netlink message including padding
 191  * @arg payload         length of message payload
 192  */
 193 int nlmsg_total_size(int payload)
 194 {
 195         return NLMSG_ALIGN(nlmsg_msg_size(payload));
 196 }
 197
 198 /**
 199  * length of padding at the message's tail
 200  * @arg payload         length of message payload
 201  */
 202 int nlmsg_padlen(int payload)
 203 {
 204         return nlmsg_total_size(payload) - nlmsg_msg_size(payload);
 205 }
 206
 207 /** @} */
 208
 209 /**
 210  * @name Payload Access
 211  * @{
 212  */
 213
 214 /**
 215  * head of message payload
 216  * @arg nlh             netlink messsage header
 217  */
 218 void *nlmsg_data(const struct nlmsghdr *nlh)
 219 {
 220         return (unsigned char *) nlh + NLMSG_HDRLEN;
 221 }
 222
 223 void *nlmsg_tail(const struct nlmsghdr *nlh)
 224 {
 225         return (unsigned char *) nlh + NLMSG_ALIGN(nlh->nlmsg_len);
 226 }
 227
 228 /**
 229  * length of message payload
 230  * @arg nlh             netlink message header
 231  */
 232 int nlmsg_len(const struct nlmsghdr *nlh)
 233 {
 234         return nlh->nlmsg_len - NLMSG_HDRLEN;
 235 }
 236
 237 /** @} */
 238
 239 /**
 240  * @name Attribute Access
 241  * @{
 242  */
 243
 244 /**
 245  * head of attributes data
 246  * @arg nlh             netlink message header
 247  * @arg hdrlen          length of family specific header
 248  */
 249 struct nlattr *nlmsg_attrdata(const struct nlmsghdr *nlh, int hdrlen)
 250 {
 251         unsigned char *data = nlmsg_data(nlh);
 252         return (struct nlattr *) (data + NLMSG_ALIGN(hdrlen));
 253 }
 254
 255 /**
 256  * length of attributes data
 257  * @arg nlh             netlink message header
 258  * @arg hdrlen          length of family specific header
 259  */
 260 int nlmsg_attrlen(const struct nlmsghdr *nlh, int hdrlen)
 261 {
 262         return nlmsg_len(nlh) - NLMSG_ALIGN(hdrlen);
 263 }
 264
 265 /** @} */
 266
 267 /**
 268  * @name Message Parsing
 269  * @{
 270  */
 271
 272 int nlmsg_valid_hdr(const struct nlmsghdr *nlh, int hdrlen)
 273 {
 274         if (nlh->nlmsg_len < nlmsg_msg_size(hdrlen))
 275                 return 0;
 276
 277         return 1;
 278 }
 279
 280 /**
 281  * check if the netlink message fits into the remaining bytes
 282  * @arg nlh             netlink message header
 283  * @arg remaining       number of bytes remaining in message stream
 284  */
 285 int nlmsg_ok(const struct nlmsghdr *nlh, int remaining)
 286 {
 287         return (remaining >= (int)sizeof(struct nlmsghdr) &&
 288                 nlh->nlmsg_len >= sizeof(struct nlmsghdr) &&
 289                 nlh->nlmsg_len <= remaining);
 290 }
 291
 292 /**
 293  * next netlink message in message stream
 294  * @arg nlh             netlink message header
 295  * @arg remaining       number of bytes remaining in message stream
 296  *
 297  * @returns the next netlink message in the message stream and
 298  * decrements remaining by the size of the current message.
 299  */
 300 struct nlmsghdr *nlmsg_next(struct nlmsghdr *nlh, int *remaining)
 301 {
 302         int totlen = NLMSG_ALIGN(nlh->nlmsg_len);
 303
 304         *remaining -= totlen;
 305
 306         return (struct nlmsghdr *) ((unsigned char *) nlh + totlen);
 307 }
 308
 309 /**
 310  * parse attributes of a netlink message
 311  * @arg nlh             netlink message header
 312  * @arg hdrlen          length of family specific header
 313  * @arg tb              destination array with maxtype+1 elements
 314  * @arg maxtype         maximum attribute type to be expected
 315  * @arg policy          validation policy
 316  *
 317  * See nla_parse()
 318  */
 319 int nlmsg_parse(struct nlmsghdr *nlh, int hdrlen, struct nlattr *tb[],
 320                 int maxtype, struct nla_policy *policy)
 321 {
 322         if (!nlmsg_valid_hdr(nlh, hdrlen))
 323                 return -NLE_MSG_TOOSHORT;
 324
 325         return nla_parse(tb, maxtype, nlmsg_attrdata(nlh, hdrlen),
 326                          nlmsg_attrlen(nlh, hdrlen), policy);
 327 }
 328
 329 /**
 330  * nlmsg_find_attr - find a specific attribute in a netlink message
 331  * @arg nlh             netlink message header
 332  * @arg hdrlen          length of familiy specific header
 333  * @arg attrtype        type of attribute to look for
 334  *
 335  * Returns the first attribute which matches the specified type.
 336  */
 337 struct nlattr *nlmsg_find_attr(struct nlmsghdr *nlh, int hdrlen, int attrtype)
 338 {
 339         return nla_find(nlmsg_attrdata(nlh, hdrlen),
 340                         nlmsg_attrlen(nlh, hdrlen), attrtype);
 341 }
 342
 343 /**
 344  * nlmsg_validate - validate a netlink message including attributes
 345  * @arg nlh             netlinket message header
 346  * @arg hdrlen          length of familiy specific header
 347  * @arg maxtype         maximum attribute type to be expected
 348  * @arg policy          validation policy
 349  */
 350 int nlmsg_validate(struct nlmsghdr *nlh, int hdrlen, int maxtype,
 351                    struct nla_policy *policy)
 352 {
 353         if (!nlmsg_valid_hdr(nlh, hdrlen))
 354                 return -NLE_MSG_TOOSHORT;
 355
 356         return nla_validate(nlmsg_attrdata(nlh, hdrlen),
 357                             nlmsg_attrlen(nlh, hdrlen), maxtype, policy);
 358 }
 359
 360 /** @} */
 361
 362 /**
 363  * @name Message Building/Access
 364  * @{
 365  */
 366
 367 static struct nl_msg *__nlmsg_alloc(size_t len)
 368 {
 369         struct nl_msg *nm;
 370
 371         nm = calloc(1, sizeof(*nm));
 372         if (!nm)
 373                 goto errout;
 374
 375         nm->nm_refcnt = 1;
 376
 377         nm->nm_nlh = malloc(len);
 378         if (!nm->nm_nlh)
 379                 goto errout;
 380
 381         memset(nm->nm_nlh, 0, sizeof(struct nlmsghdr));
 382
 383         nm->nm_protocol = -1;
 384         nm->nm_size = len;
 385         nm->nm_nlh->nlmsg_len = nlmsg_total_size(0);
 386
 387         NL_DBG(2, "msg %p: Allocated new message, maxlen=%zu\n", nm, len);
 388
 389         return nm;
 390 errout:
 391         free(nm);
 392         return NULL;
 393 }
 394
 395 /**
 396  * Allocate a new netlink message with the default maximum payload size.
 397  *
 398  * Allocates a new netlink message without any further payload. The
 399  * maximum payload size defaults to PAGESIZE or as otherwise specified
 400  * with nlmsg_set_default_size().
 401  *
 402  * @return Newly allocated netlink message or NULL.
 403  */
 404 struct nl_msg *nlmsg_alloc(void)
 405 {
 406         return __nlmsg_alloc(default_msg_size);
 407 }
 408
 409 /**
 410  * Allocate a new netlink message with maximum payload size specified.
 411  */
 412 struct nl_msg *nlmsg_alloc_size(size_t max)
 413 {
 414         return __nlmsg_alloc(max);
 415 }
 416
 417 /**
 418  * Allocate a new netlink message and inherit netlink message header
 419  * @arg hdr             Netlink message header template
 420  *
 421  * Allocates a new netlink message and inherits the original message
 422  * header. If \a hdr is not NULL it will be used as a template for
 423  * the netlink message header, otherwise the header is left blank.
 424  *
 425  * @return Newly allocated netlink message or NULL
 426  */
 427 struct nl_msg *nlmsg_inherit(struct nlmsghdr *hdr)
 428 {
 429         struct nl_msg *nm;
 430
 431         nm = nlmsg_alloc();
 432         if (nm && hdr) {
 433                 struct nlmsghdr *new = nm->nm_nlh;
 434
 435                 new->nlmsg_type = hdr->nlmsg_type;
 436                 new->nlmsg_flags = hdr->nlmsg_flags;
 437                 new->nlmsg_seq = hdr->nlmsg_seq;
 438                 new->nlmsg_pid = hdr->nlmsg_pid;
 439         }
 440
 441         return nm;
 442 }
 443
 444 /**
 445  * Allocate a new netlink message
 446  * @arg nlmsgtype       Netlink message type
 447  * @arg flags           Message flags.
 448  *
 449  * @return Newly allocated netlink message or NULL.
 450  */
 451 struct nl_msg *nlmsg_alloc_simple(int nlmsgtype, int flags)
 452 {
 453         struct nl_msg *msg;
 454         struct nlmsghdr nlh = {
 455                 .nlmsg_type = nlmsgtype,
 456                 .nlmsg_flags = flags,
 457         };
 458
 459         msg = nlmsg_inherit(&nlh);
 460         if (msg)
 461                 NL_DBG(2, "msg %p: Allocated new simple message\n", msg);
 462
 463         return msg;
 464 }
 465
 466 /**
 467  * Set the default maximum message payload size for allocated messages
 468  * @arg max             Size of payload in bytes.
 469  */
 470 void nlmsg_set_default_size(size_t max)
 471 {
 472         if (max < nlmsg_total_size(0))
 473                 max = nlmsg_total_size(0);
 474
 475         default_msg_size = max;
 476 }
 477
 478 /**
 479  * Convert a netlink message received from a netlink socket to a nl_msg
 480  * @arg hdr             Netlink message received from netlink socket.
 481  *
 482  * Allocates a new netlink message and copies all of the data pointed to
 483  * by \a hdr into the new message object.
 484  *
 485  * @return Newly allocated netlink message or NULL.
 486  */
 487 struct nl_msg *nlmsg_convert(struct nlmsghdr *hdr)
 488 {
 489         struct nl_msg *nm;
 490
 491         nm = __nlmsg_alloc(NLMSG_ALIGN(hdr->nlmsg_len));
 492         if (!nm)
 493                 goto errout;
 494
 495         memcpy(nm->nm_nlh, hdr, hdr->nlmsg_len);
 496
 497         return nm;
 498 errout:
 499         nlmsg_free(nm);
 500         return NULL;
 501 }
 502
 503 /**
 504  * Reserve room for additional data in a netlink message
 505  * @arg n               netlink message
 506  * @arg len             length of additional data to reserve room for
 507  * @arg pad             number of bytes to align data to
 508  *
 509  * Reserves room for additional data at the tail of the an
 510  * existing netlink message. Eventual padding required will
 511  * be zeroed out.
 512  *
 513  * @return Pointer to start of additional data tailroom or NULL.
 514  */
 515 void *nlmsg_reserve(struct nl_msg *n, size_t len, int pad)
 516 {
 517         void *buf = n->nm_nlh;
 518         size_t nlmsg_len = n->nm_nlh->nlmsg_len;
 519         size_t tlen;
 520
 521         tlen = pad ? ((len + (pad - 1)) & ~(pad - 1)) : len;
 522
 523         if ((tlen + nlmsg_len) > n->nm_size)
 524                 return NULL;
 525
 526         buf += nlmsg_len;
 527         n->nm_nlh->nlmsg_len += tlen;
 528
 529         if (tlen > len)
 530                 memset(buf + len, 0, tlen - len);
 531
 532         NL_DBG(2, "msg %p: Reserved %zu bytes, pad=%d, nlmsg_len=%d\n",
 533                   n, len, pad, n->nm_nlh->nlmsg_len);
 534
 535         return buf;
 536 }
 537
 538 /**
 539  * Append data to tail of a netlink message
 540  * @arg n               netlink message
 541  * @arg data            data to add
 542  * @arg len             length of data
 543  * @arg pad             Number of bytes to align data to.
 544  *
 545  * Extends the netlink message as needed and appends the data of given
 546  * length to the message.
 547  *
 548  * @return 0 on success or a negative error code
 549  */
 550 int nlmsg_append(struct nl_msg *n, void *data, size_t len, int pad)
 551 {
 552         void *tmp;
 553
 554         tmp = nlmsg_reserve(n, len, pad);
 555         if (tmp == NULL)
 556                 return -NLE_NOMEM;
 557
 558         memcpy(tmp, data, len);
 559         NL_DBG(2, "msg %p: Appended %zu bytes with padding %d\n", n, len, pad);
 560
 561         return 0;
 562 }
 563
 564 /**
 565  * Expand maximum payload size of a netlink message
 566  * @arg n               Netlink message.
 567  * @arg newlen          New maximum payload size.
 568  *
 569  * Reallocates the payload section of a netlink message and increases
 570  * the maximum payload size of the message.
 571  *
 572  * @note Any pointers pointing to old payload block will be stale and
 573  *       need to be refetched. Therfore, do not expand while constructing
 574  *       nested attributes or while reserved data blocks are held.
 575  *
 576  * @return 0 on success or a negative error code.
 577  */
 578 int nlmsg_expand(struct nl_msg *n, size_t newlen)
 579 {
 580         void *tmp;
 581
 582         if (newlen <= n->nm_size)
 583                 return -NLE_INVAL;
 584
 585         tmp = realloc(n->nm_nlh, newlen);
 586         if (tmp == NULL)
 587                 return -NLE_NOMEM;
 588
 589         n->nm_nlh = tmp;
 590         n->nm_size = newlen;
 591
 592         return 0;
 593 }
 594
 595 /**
 596  * Add a netlink message header to a netlink message
 597  * @arg n               netlink message
 598  * @arg pid             netlink process id or NL_AUTO_PID
 599  * @arg seq             sequence number of message or NL_AUTO_SEQ
 600  * @arg type            message type
 601  * @arg payload         length of message payload
 602  * @arg flags           message flags
 603  *
 604  * Adds or overwrites the netlink message header in an existing message
 605  * object. If \a payload is greater-than zero additional room will be
 606  * reserved, f.e. for family specific headers. It can be accesed via
 607  * nlmsg_data().
 608  *
 609  * @return A pointer to the netlink message header or NULL.
 610  */
 611 struct nlmsghdr *nlmsg_put(struct nl_msg *n, uint32_t pid, uint32_t seq,
 612                            int type, int payload, int flags)
 613 {
 614         struct nlmsghdr *nlh;
 615
 616         if (n->nm_nlh->nlmsg_len < NLMSG_HDRLEN)
 617                 BUG();
 618
 619         nlh = (struct nlmsghdr *) n->nm_nlh;
 620         nlh->nlmsg_type = type;
 621         nlh->nlmsg_flags = flags;
 622         nlh->nlmsg_pid = pid;
 623         nlh->nlmsg_seq = seq;
 624
 625         NL_DBG(2, "msg %p: Added netlink header type=%d, flags=%d, pid=%d, "
 626                   "seq=%d\n", n, type, flags, pid, seq);
 627
 628         if (payload > 0 &&
 629             nlmsg_reserve(n, payload, NLMSG_ALIGNTO) == NULL)
 630                 return NULL;
 631
 632         return nlh;
 633 }
 634
 635 /**
 636  * Return actual netlink message
 637  * @arg n               netlink message
 638  *
 639  * Returns the actual netlink message casted to the type of the netlink
 640  * message header.
 641  *
 642  * @return A pointer to the netlink message.
 643  */
 644 struct nlmsghdr *nlmsg_hdr(struct nl_msg *n)
 645 {
 646         return n->nm_nlh;
 647 }
 648
 649 /**
 650  * Acquire a reference on a netlink message
 651  * @arg msg             message to acquire reference from
 652  */
 653 void nlmsg_get(struct nl_msg *msg)
 654 {
 655         msg->nm_refcnt++;
 656         NL_DBG(4, "New reference to message %p, total %d\n",
 657                msg, msg->nm_refcnt);
 658 }
 659
 660 /**
 661  * Release a reference from an netlink message
 662  * @arg msg             message to release reference from
 663  *
 664  * Frees memory after the last reference has been released.
 665  */
 666 void nlmsg_free(struct nl_msg *msg)
 667 {
 668         if (!msg)
 669                 return;
 670
 671         msg->nm_refcnt--;
 672         NL_DBG(4, "Returned message reference %p, %d remaining\n",
 673                msg, msg->nm_refcnt);
 674
 675         if (msg->nm_refcnt < 0)
 676                 BUG();
 677
 678         if (msg->nm_refcnt <= 0) {
 679                 free(msg->nm_nlh);
 680                 free(msg);
 681                 NL_DBG(2, "msg %p: Freed\n", msg);
 682         }
 683 }
 684
 685 /** @} */
 686
 687 /**
 688  * @name Attributes
 689  * @{
 690  */
 691
 692 void nlmsg_set_proto(struct nl_msg *msg, int protocol)
 693 {
 694         msg->nm_protocol = protocol;
 695 }
 696
 697 int nlmsg_get_proto(struct nl_msg *msg)
 698 {
 699         return msg->nm_protocol;
 700 }
 701
 702 size_t nlmsg_get_max_size(struct nl_msg *msg)
 703 {
 704         return msg->nm_size;
 705 }
 706
 707 void nlmsg_set_src(struct nl_msg *msg, struct sockaddr_nl *addr)
 708 {
 709         memcpy(&msg->nm_src, addr, sizeof(*addr));
 710 }
 711
 712 struct sockaddr_nl *nlmsg_get_src(struct nl_msg *msg)
 713 {
 714         return &msg->nm_src;
 715 }
 716
 717 void nlmsg_set_dst(struct nl_msg *msg, struct sockaddr_nl *addr)
 718 {
 719         memcpy(&msg->nm_dst, addr, sizeof(*addr));
 720 }
 721
 722 struct sockaddr_nl *nlmsg_get_dst(struct nl_msg *msg)
 723 {
 724         return &msg->nm_dst;
 725 }
 726
 727 void nlmsg_set_creds(struct nl_msg *msg, struct ucred *creds)
 728 {
 729         memcpy(&msg->nm_creds, creds, sizeof(*creds));
 730         msg->nm_flags |= NL_MSG_CRED_PRESENT;
 731 }
 732
 733 struct ucred *nlmsg_get_creds(struct nl_msg *msg)
 734 {
 735         if (msg->nm_flags & NL_MSG_CRED_PRESENT)
 736                 return &msg->nm_creds;
 737         return NULL;
 738 }
 739
 740 /** @} */
 741
 742 /**
 743  * @name Netlink Message Type Translations
 744  * @{
 745  */
 746
 747 static struct trans_tbl nl_msgtypes[] = {
 748         __ADD(NLMSG_NOOP,NOOP)
 749         __ADD(NLMSG_ERROR,ERROR)
 750         __ADD(NLMSG_DONE,DONE)
 751         __ADD(NLMSG_OVERRUN,OVERRUN)
 752 };
 753
 754 char *nl_nlmsgtype2str(int type, char *buf, size_t size)
 755 {
 756         return __type2str(type, buf, size, nl_msgtypes,
 757                           ARRAY_SIZE(nl_msgtypes));
 758 }
 759
 760 int nl_str2nlmsgtype(const char *name)
 761 {
 762         return __str2type(name, nl_msgtypes, ARRAY_SIZE(nl_msgtypes));
 763 }
 764
 765 /** @} */
 766
 767 /**
 768  * @name Netlink Message Flags Translations
 769  * @{
 770  */
 771
 772 char *nl_nlmsg_flags2str(int flags, char *buf, size_t len)
 773 {
 774         memset(buf, 0, len);
 775
 776 #define PRINT_FLAG(f) \
 777         if (flags & NLM_F_##f) { \
 778                 flags &= ~NLM_F_##f; \
 779                 strncat(buf, #f, len - strlen(buf) - 1); \
 780                 if (flags) \
 781                         strncat(buf, ",", len - strlen(buf) - 1); \
 782         }
 783
 784         PRINT_FLAG(REQUEST);
 785         PRINT_FLAG(MULTI);
 786         PRINT_FLAG(ACK);
 787         PRINT_FLAG(ECHO);
 788         PRINT_FLAG(ROOT);
 789         PRINT_FLAG(MATCH);
 790         PRINT_FLAG(ATOMIC);
 791         PRINT_FLAG(REPLACE);
 792         PRINT_FLAG(EXCL);
 793         PRINT_FLAG(CREATE);
 794         PRINT_FLAG(APPEND);
 795
 796         if (flags) {
 797                 char s[32];
 798                 snprintf(s, sizeof(s), "0x%x", flags);
 799                 strncat(buf, s, len - strlen(buf) - 1);
 800         }
 801 #undef PRINT_FLAG
 802
 803         return buf;
 804 }
 805
 806 /** @} */
 807
 808 /**
 809  * @name Direct Parsing
 810  * @{
 811  */
 812
 813 /** @cond SKIP */
 814 struct dp_xdata {
 815         void (*cb)(struct nl_object *, void *);
 816         void *arg;
 817 };
 818 /** @endcond */
 819
 820 static int parse_cb(struct nl_object *obj, struct nl_parser_param *p)
 821 {
 822         struct dp_xdata *x = p->pp_arg;
 823
 824         x->cb(obj, x->arg);
 825         return 0;
 826 }
 827
 828 int nl_msg_parse(struct nl_msg *msg, void (*cb)(struct nl_object *, void *),
 829                  void *arg)
 830 {
 831         struct nl_cache_ops *ops;
 832         struct nl_parser_param p = {
 833                 .pp_cb = parse_cb
 834         };
 835         struct dp_xdata x = {
 836                 .cb = cb,
 837                 .arg = arg,
 838         };
 839
 840         ops = nl_cache_ops_associate(nlmsg_get_proto(msg),
 841                                      nlmsg_hdr(msg)->nlmsg_type);
 842         if (ops == NULL)
 843                 return -NLE_MSGTYPE_NOSUPPORT;
 844         p.pp_arg = &x;
 845
 846         return nl_cache_parse(ops, NULL, nlmsg_hdr(msg), &p);
 847 }
 848
 849 /** @} */
 850
 851 /**
 852  * @name Dumping
 853  * @{
 854  */
 855
 856 static void prefix_line(FILE *ofd, int prefix)
 857 {
 858         int i;
 859
 860         for (i = 0; i < prefix; i++)
 861                 fprintf(ofd, "  ");
 862 }
 863
 864 static inline void dump_hex(FILE *ofd, char *start, int len, int prefix)
 865 {
 866         int i, a, c, limit;
 867         char ascii[21] = {0};
 868
 869         limit = 18 - (prefix * 2);
 870         prefix_line(ofd, prefix);
 871         fprintf(ofd, "    ");
 872
 873         for (i = 0, a = 0, c = 0; i < len; i++) {
 874                 int v = *(uint8_t *) (start + i);
 875
 876                 fprintf(ofd, "%02x ", v);
 877                 ascii[a++] = isprint(v) ? v : '.';
 878
 879                 if (c == limit-1) {
 880                         fprintf(ofd, "%s\n", ascii);
 881                         if (i < (len - 1)) {
 882                                 prefix_line(ofd, prefix);
 883                                 fprintf(ofd, "    ");
 884                         }
 885                         a = c = 0;
 886                         memset(ascii, 0, sizeof(ascii));
 887                 } else
 888                         c++;
 889         }
 890
 891         if (c != 0) {
 892                 for (i = 0; i < (limit - c); i++)
 893                         fprintf(ofd, "   ");
 894                 fprintf(ofd, "%s\n", ascii);
 895         }
 896 }
 897
 898 static void print_hdr(FILE *ofd, struct nl_msg *msg)
 899 {
 900         struct nlmsghdr *nlh = nlmsg_hdr(msg);
 901         struct nl_cache_ops *ops;
 902         struct nl_msgtype *mt;
 903         char buf[128];
 904
 905         fprintf(ofd, "    .nlmsg_len = %d\n", nlh->nlmsg_len);
 906
 907         ops = nl_cache_ops_associate(nlmsg_get_proto(msg), nlh->nlmsg_type);
 908         if (ops) {
 909                 mt = nl_msgtype_lookup(ops, nlh->nlmsg_type);
 910                 if (!mt)
 911                         BUG();
 912
 913                 snprintf(buf, sizeof(buf), "%s::%s", ops->co_name, mt->mt_name);
 914         } else
 915                 nl_nlmsgtype2str(nlh->nlmsg_type, buf, sizeof(buf));
 916
 917         fprintf(ofd, "    .nlmsg_type = %d <%s>\n", nlh->nlmsg_type, buf);
 918         fprintf(ofd, "    .nlmsg_flags = %d <%s>\n", nlh->nlmsg_flags,
 919                 nl_nlmsg_flags2str(nlh->nlmsg_flags, buf, sizeof(buf)));
 920         fprintf(ofd, "    .nlmsg_seq = %d\n", nlh->nlmsg_seq);
 921         fprintf(ofd, "    .nlmsg_pid = %d\n", nlh->nlmsg_pid);
 922
 923 }
 924
 925 static void dump_attrs(FILE *ofd, struct nlattr *attrs, int attrlen,
 926                        int prefix)
 927 {
 928         int rem;
 929         struct nlattr *nla;
 930
 931         nla_for_each_attr(nla, attrs, attrlen, rem) {
 932                 int padlen, alen = nla_len(nla);
 933
 934                 prefix_line(ofd, prefix);
 935                 fprintf(ofd, "  [ATTR %02d%s] %d octets\n", nla_type(nla),
 936                         nla->nla_type & NLA_F_NESTED ? " NESTED" : "",
 937                         alen);
 938
 939                 if (nla->nla_type & NLA_F_NESTED)
 940                         dump_attrs(ofd, nla_data(nla), alen, prefix+1);
 941                 else
 942                         dump_hex(ofd, nla_data(nla), alen, prefix);
 943
 944                 padlen = nla_padlen(alen);
 945                 if (padlen > 0) {
 946                         prefix_line(ofd, prefix);
 947                         fprintf(ofd, "  [PADDING] %d octets\n",
 948                                 padlen);
 949                         dump_hex(ofd, nla_data(nla) + alen,
 950                                  padlen, prefix);
 951                 }
 952         }
 953
 954         if (rem) {
 955                 prefix_line(ofd, prefix);
 956                 fprintf(ofd, "  [LEFTOVER] %d octets\n", rem);
 957         }
 958 }
 959
 960 /**
 961  * Dump message in human readable format to file descriptor
 962  * @arg msg             Message to print
 963  * @arg ofd             File descriptor.
 964  */
 965 void nl_msg_dump(struct nl_msg *msg, FILE *ofd)
 966 {
 967         struct nlmsghdr *hdr = nlmsg_hdr(msg);
 968
 969         fprintf(ofd,
 970         "--------------------------   BEGIN NETLINK MESSAGE "
 971         "---------------------------\n");
 972
 973         fprintf(ofd, "  [HEADER] %Zu octets\n", sizeof(struct nlmsghdr));
 974         print_hdr(ofd, msg);
 975
 976         if (hdr->nlmsg_type == NLMSG_ERROR &&
 977             hdr->nlmsg_len >= nlmsg_msg_size(sizeof(struct nlmsgerr))) {
 978                 struct nl_msg *errmsg;
 979                 struct nlmsgerr *err = nlmsg_data(hdr);
 980
 981                 fprintf(ofd, "  [ERRORMSG] %Zu octets\n", sizeof(*err));
 982                 fprintf(ofd, "    .error = %d \"%s\"\n", err->error,
 983                         strerror(-err->error));
 984                 fprintf(ofd, "  [ORIGINAL MESSAGE] %Zu octets\n", sizeof(*hdr));
 985
 986                 errmsg = nlmsg_inherit(&err->msg);
 987                 print_hdr(ofd, errmsg);
 988                 nlmsg_free(errmsg);
 989         } else if (nlmsg_len(hdr) > 0) {
 990                 struct nl_cache_ops *ops;
 991                 int payloadlen = nlmsg_len(hdr);
 992                 int attrlen = 0;
 993
 994                 ops = nl_cache_ops_associate(nlmsg_get_proto(msg),
 995                                              hdr->nlmsg_type);
 996                 if (ops) {
 997                         attrlen = nlmsg_attrlen(hdr, ops->co_hdrsize);
 998                         payloadlen -= attrlen;
 999                 }
1000
1001                 fprintf(ofd, "  [PAYLOAD] %d octets\n", payloadlen);
1002                 dump_hex(ofd, nlmsg_data(hdr), payloadlen, 0);
1003
1004                 if (attrlen) {
1005                         struct nlattr *attrs;
1006                         int attrlen;
1007
1008                         attrs = nlmsg_attrdata(hdr, ops->co_hdrsize);
1009                         attrlen = nlmsg_attrlen(hdr, ops->co_hdrsize);
1010                         dump_attrs(ofd, attrs, attrlen, 0);
1011                 }
1012         }
1013
1014         fprintf(ofd,
1015         "---------------------------  END NETLINK MESSAGE   "
1016         "---------------------------\n");
1017 }
1018
1019 /** @} */
1020
1021 /** @} */