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