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-2006 Thomas Graf <tgraf@suug.ch>
  10  */
  11
  12 /**
  13  * @ingroup nl
  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 >= 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 nl_errno(EINVAL);
 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 nl_errno(EINVAL);
 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_nlh = calloc(1, len);
 376         if (!nm->nm_nlh)
 377                 goto errout;
 378
 379         nm->nm_protocol = -1;
 380         nm->nm_size = len;
 381         nm->nm_nlh->nlmsg_len = nlmsg_total_size(0);
 382
 383         NL_DBG(2, "msg %p: Allocated new message, maxlen=%zu\n", nm, len);
 384
 385         return nm;
 386 errout:
 387         free(nm);
 388         nl_errno(ENOMEM);
 389         return NULL;
 390 }
 391
 392 /**
 393  * Allocate a new netlink message with the default maximum payload size.
 394  *
 395  * Allocates a new netlink message without any further payload. The
 396  * maximum payload size defaults to PAGESIZE or as otherwise specified
 397  * with nlmsg_set_default_size().
 398  *
 399  * @return Newly allocated netlink message or NULL.
 400  */
 401 struct nl_msg *nlmsg_alloc(void)
 402 {
 403         return __nlmsg_alloc(default_msg_size);
 404 }
 405
 406 /**
 407  * Allocate a new netlink message with maximum payload size specified.
 408  */
 409 struct nl_msg *nlmsg_alloc_size(size_t max)
 410 {
 411         return __nlmsg_alloc(max);
 412 }
 413
 414 /**
 415  * Allocate a new netlink message and inherit netlink message header
 416  * @arg hdr             Netlink message header template
 417  *
 418  * Allocates a new netlink message and inherits the original message
 419  * header. If \a hdr is not NULL it will be used as a template for
 420  * the netlink message header, otherwise the header is left blank.
 421  *
 422  * @return Newly allocated netlink message or NULL
 423  */
 424 struct nl_msg *nlmsg_inherit(struct nlmsghdr *hdr)
 425 {
 426         struct nl_msg *nm;
 427
 428         nm = nlmsg_alloc();
 429         if (nm && hdr) {
 430                 struct nlmsghdr *new = nm->nm_nlh;
 431
 432                 new->nlmsg_type = hdr->nlmsg_type;
 433                 new->nlmsg_flags = hdr->nlmsg_flags;
 434                 new->nlmsg_seq = hdr->nlmsg_seq;
 435                 new->nlmsg_pid = hdr->nlmsg_pid;
 436         }
 437
 438         return nm;
 439 }
 440
 441 /**
 442  * Allocate a new netlink message
 443  * @arg nlmsgtype       Netlink message type
 444  * @arg flags           Message flags.
 445  *
 446  * @return Newly allocated netlink message or NULL.
 447  */
 448 struct nl_msg *nlmsg_alloc_simple(int nlmsgtype, int flags)
 449 {
 450         struct nl_msg *msg;
 451         struct nlmsghdr nlh = {
 452                 .nlmsg_type = nlmsgtype,
 453                 .nlmsg_flags = flags,
 454         };
 455
 456         msg = nlmsg_inherit(&nlh);
 457         if (msg)
 458                 NL_DBG(2, "msg %p: Allocated new simple message\n", msg);
 459
 460         return msg;
 461 }
 462
 463 /**
 464  * Set the default maximum message payload size for allocated messages
 465  * @arg max             Size of payload in bytes.
 466  */
 467 void nlmsg_set_default_size(size_t max)
 468 {
 469         if (max < nlmsg_total_size(0))
 470                 max = nlmsg_total_size(0);
 471
 472         default_msg_size = max;
 473 }
 474
 475 /**
 476  * Convert a netlink message received from a netlink socket to a nl_msg
 477  * @arg hdr             Netlink message received from netlink socket.
 478  *
 479  * Allocates a new netlink message and copies all of the data pointed to
 480  * by \a hdr into the new message object.
 481  *
 482  * @return Newly allocated netlink message or NULL.
 483  */
 484 struct nl_msg *nlmsg_convert(struct nlmsghdr *hdr)
 485 {
 486         struct nl_msg *nm;
 487
 488         nm = __nlmsg_alloc(NLMSG_ALIGN(hdr->nlmsg_len));
 489         if (!nm)
 490                 goto errout;
 491
 492         memcpy(nm->nm_nlh, hdr, hdr->nlmsg_len);
 493
 494         return nm;
 495 errout:
 496         nlmsg_free(nm);
 497         return NULL;
 498 }
 499
 500 /**
 501  * Reserve room for additional data in a netlink message
 502  * @arg n               netlink message
 503  * @arg len             length of additional data to reserve room for
 504  * @arg pad             number of bytes to align data to
 505  *
 506  * Reserves room for additional data at the tail of the an
 507  * existing netlink message. Eventual padding required will
 508  * be zeroed out.
 509  *
 510  * @return Pointer to start of additional data tailroom or NULL.
 511  */
 512 void *nlmsg_reserve(struct nl_msg *n, size_t len, int pad)
 513 {
 514         void *buf = n->nm_nlh;
 515         size_t nlmsg_len = n->nm_nlh->nlmsg_len;
 516         size_t tlen;
 517
 518         tlen = pad ? ((len + (pad - 1)) & ~(pad - 1)) : len;
 519
 520         if ((tlen + nlmsg_len) > n->nm_size) {
 521                 nl_errno(ENOBUFS);
 522                 return NULL;
 523         }
 524
 525         buf += nlmsg_len;
 526         n->nm_nlh->nlmsg_len += tlen;
 527
 528         if (tlen > len)
 529                 memset(buf + len, 0, tlen - len);
 530
 531         NL_DBG(2, "msg %p: Reserved %zu bytes, pad=%d, nlmsg_len=%d\n",
 532                   n, len, pad, n->nm_nlh->nlmsg_len);
 533
 534         return buf;
 535 }
 536
 537 /**
 538  * Append data to tail of a netlink message
 539  * @arg n               netlink message
 540  * @arg data            data to add
 541  * @arg len             length of data
 542  * @arg pad             Number of bytes to align data to.
 543  *
 544  * Extends the netlink message as needed and appends the data of given
 545  * length to the message.
 546  *
 547  * @return 0 on success or a negative error code
 548  */
 549 int nlmsg_append(struct nl_msg *n, void *data, size_t len, int pad)
 550 {
 551         void *tmp;
 552
 553         tmp = nlmsg_reserve(n, len, pad);
 554         if (tmp == NULL)
 555                 return nl_errno(ENOMEM);
 556
 557         memcpy(tmp, data, len);
 558         NL_DBG(2, "msg %p: Appended %zu bytes with padding %d\n", n, len, pad);
 559
 560         return 0;
 561 }
 562
 563 /**
 564  * Expand maximum payload size of a netlink message
 565  * @arg n               Netlink message.
 566  * @arg newlen          New maximum payload size.
 567  *
 568  * Reallocates the payload section of a netlink message and increases
 569  * the maximum payload size of the message.
 570  *
 571  * @note Any pointers pointing to old payload block will be stale and
 572  *       need to be refetched. Therfore, do not expand while constructing
 573  *       nested attributes or while reserved data blocks are held.
 574  *
 575  * @return 0 on success or a negative error code.
 576  */
 577 int nlmsg_expand(struct nl_msg *n, size_t newlen)
 578 {
 579         void *tmp;
 580
 581         if (newlen <= n->nm_size)
 582                 return nl_errno(EINVAL);
 583
 584         tmp = realloc(n->nm_nlh, newlen);
 585         if (tmp == NULL)
 586                 return nl_errno(ENOMEM);
 587
 588         n->nm_nlh = tmp;
 589         n->nm_size = newlen;
 590
 591         return 0;
 592 }
 593
 594 /**
 595  * Add a netlink message header to a netlink message
 596  * @arg n               netlink message
 597  * @arg pid             netlink process id or NL_AUTO_PID
 598  * @arg seq             sequence number of message or NL_AUTO_SEQ
 599  * @arg type            message type
 600  * @arg payload         length of message payload
 601  * @arg flags           message flags
 602  *
 603  * Adds or overwrites the netlink message header in an existing message
 604  * object. If \a payload is greater-than zero additional room will be
 605  * reserved, f.e. for family specific headers. It can be accesed via
 606  * nlmsg_data().
 607  *
 608  * @return A pointer to the netlink message header or NULL.
 609  */
 610 struct nlmsghdr *nlmsg_put(struct nl_msg *n, uint32_t pid, uint32_t seq,
 611                            int type, int payload, int flags)
 612 {
 613         struct nlmsghdr *nlh;
 614
 615         if (n->nm_nlh->nlmsg_len < NLMSG_HDRLEN)
 616                 BUG();
 617
 618         nlh = (struct nlmsghdr *) n->nm_nlh;
 619         nlh->nlmsg_type = type;
 620         nlh->nlmsg_flags = flags;
 621         nlh->nlmsg_pid = pid;
 622         nlh->nlmsg_seq = seq;
 623
 624         NL_DBG(2, "msg %p: Added netlink header type=%d, flags=%d, pid=%d, "
 625                   "seq=%d\n", n, type, flags, pid, seq);
 626
 627         if (payload > 0 &&
 628             nlmsg_reserve(n, payload, NLMSG_ALIGNTO) == NULL)
 629                 return NULL;
 630
 631         return nlh;
 632 }
 633
 634 /**
 635  * Return actual netlink message
 636  * @arg n               netlink message
 637  *
 638  * Returns the actual netlink message casted to the type of the netlink
 639  * message header.
 640  *
 641  * @return A pointer to the netlink message.
 642  */
 643 struct nlmsghdr *nlmsg_hdr(struct nl_msg *n)
 644 {
 645         return n->nm_nlh;
 646 }
 647
 648 /**
 649  * Free a netlink message
 650  * @arg n               netlink message
 651  *
 652  * Destroys a netlink message and frees up all used memory.
 653  *
 654  * @pre The message must be unused.
 655  */
 656 void nlmsg_free(struct nl_msg *n)
 657 {
 658         if (!n)
 659                 return;
 660
 661         free(n->nm_nlh);
 662         free(n);
 663         NL_DBG(2, "msg %p: Freed\n", n);
 664 }
 665
 666 /** @} */
 667
 668 /**
 669  * @name Attributes
 670  * @{
 671  */
 672
 673 void nlmsg_set_proto(struct nl_msg *msg, int protocol)
 674 {
 675         msg->nm_protocol = protocol;
 676 }
 677
 678 int nlmsg_get_proto(struct nl_msg *msg)
 679 {
 680         return msg->nm_protocol;
 681 }
 682
 683 size_t nlmsg_get_max_size(struct nl_msg *msg)
 684 {
 685         return msg->nm_size;
 686 }
 687
 688 void nlmsg_set_src(struct nl_msg *msg, struct sockaddr_nl *addr)
 689 {
 690         memcpy(&msg->nm_src, addr, sizeof(*addr));
 691 }
 692
 693 struct sockaddr_nl *nlmsg_get_src(struct nl_msg *msg)
 694 {
 695         return &msg->nm_src;
 696 }
 697
 698 void nlmsg_set_dst(struct nl_msg *msg, struct sockaddr_nl *addr)
 699 {
 700         memcpy(&msg->nm_dst, addr, sizeof(*addr));
 701 }
 702
 703 struct sockaddr_nl *nlmsg_get_dst(struct nl_msg *msg)
 704 {
 705         return &msg->nm_dst;
 706 }
 707
 708 void nlmsg_set_creds(struct nl_msg *msg, struct ucred *creds)
 709 {
 710         memcpy(&msg->nm_creds, creds, sizeof(*creds));
 711         msg->nm_flags |= NL_MSG_CRED_PRESENT;
 712 }
 713
 714 struct ucred *nlmsg_get_creds(struct nl_msg *msg)
 715 {
 716         if (msg->nm_flags & NL_MSG_CRED_PRESENT)
 717                 return &msg->nm_creds;
 718         return NULL;
 719 }
 720
 721 /** @} */
 722
 723 /**
 724  * @name Netlink Message Type Translations
 725  * @{
 726  */
 727
 728 static struct trans_tbl nl_msgtypes[] = {
 729         __ADD(NLMSG_NOOP,NOOP)
 730         __ADD(NLMSG_ERROR,ERROR)
 731         __ADD(NLMSG_DONE,DONE)
 732         __ADD(NLMSG_OVERRUN,OVERRUN)
 733 };
 734
 735 char *nl_nlmsgtype2str(int type, char *buf, size_t size)
 736 {
 737         return __type2str(type, buf, size, nl_msgtypes,
 738                           ARRAY_SIZE(nl_msgtypes));
 739 }
 740
 741 int nl_str2nlmsgtype(const char *name)
 742 {
 743         return __str2type(name, nl_msgtypes, ARRAY_SIZE(nl_msgtypes));
 744 }
 745
 746 /** @} */
 747
 748 /**
 749  * @name Netlink Message Flags Translations
 750  * @{
 751  */
 752
 753 char *nl_nlmsg_flags2str(int flags, char *buf, size_t len)
 754 {
 755         memset(buf, 0, len);
 756
 757 #define PRINT_FLAG(f) \
 758         if (flags & NLM_F_##f) { \
 759                 flags &= ~NLM_F_##f; \
 760                 strncat(buf, #f, len - strlen(buf) - 1); \
 761                 if (flags) \
 762                         strncat(buf, ",", len - strlen(buf) - 1); \
 763         }
 764
 765         PRINT_FLAG(REQUEST);
 766         PRINT_FLAG(MULTI);
 767         PRINT_FLAG(ACK);
 768         PRINT_FLAG(ECHO);
 769         PRINT_FLAG(ROOT);
 770         PRINT_FLAG(MATCH);
 771         PRINT_FLAG(ATOMIC);
 772         PRINT_FLAG(REPLACE);
 773         PRINT_FLAG(EXCL);
 774         PRINT_FLAG(CREATE);
 775         PRINT_FLAG(APPEND);
 776
 777         if (flags) {
 778                 char s[32];
 779                 snprintf(s, sizeof(s), "0x%x", flags);
 780                 strncat(buf, s, len - strlen(buf) - 1);
 781         }
 782 #undef PRINT_FLAG
 783
 784         return buf;
 785 }
 786
 787 /** @} */
 788
 789 /**
 790  * @name Direct Parsing
 791  * @{
 792  */
 793
 794 /** @cond SKIP */
 795 struct dp_xdata {
 796         void (*cb)(struct nl_object *, void *);
 797         void *arg;
 798 };
 799 /** @endcond */
 800
 801 static int parse_cb(struct nl_object *obj, struct nl_parser_param *p)
 802 {
 803         struct dp_xdata *x = p->pp_arg;
 804
 805         x->cb(obj, x->arg);
 806         return 0;
 807 }
 808
 809 int nl_msg_parse(struct nl_msg *msg, void (*cb)(struct nl_object *, void *),
 810                  void *arg)
 811 {
 812         struct nl_cache_ops *ops;
 813         struct nl_parser_param p = {
 814                 .pp_cb = parse_cb
 815         };
 816         struct dp_xdata x = {
 817                 .cb = cb,
 818                 .arg = arg,
 819         };
 820
 821         ops = nl_cache_ops_associate(nlmsg_get_proto(msg),
 822                                      nlmsg_hdr(msg)->nlmsg_type);
 823         if (ops == NULL)
 824                 return nl_error(ENOENT, "Unknown message type %d",
 825                                 nlmsg_hdr(msg)->nlmsg_type);
 826         p.pp_arg = &x;
 827
 828         return nl_cache_parse(ops, NULL, nlmsg_hdr(msg), &p);
 829 }
 830
 831 /** @} */
 832
 833 /**
 834  * @name Dumping
 835  * @{
 836  */
 837
 838 static void prefix_line(FILE *ofd, int prefix)
 839 {
 840         int i;
 841
 842         for (i = 0; i < prefix; i++)
 843                 fprintf(ofd, "  ");
 844 }
 845
 846 static inline void dump_hex(FILE *ofd, char *start, int len, int prefix)
 847 {
 848         int i, a, c, limit;
 849         char ascii[21] = {0};
 850
 851         limit = 18 - (prefix * 2);
 852         prefix_line(ofd, prefix);
 853         fprintf(ofd, "    ");
 854
 855         for (i = 0, a = 0, c = 0; i < len; i++) {
 856                 int v = *(uint8_t *) (start + i);
 857
 858                 fprintf(ofd, "%02x ", v);
 859                 ascii[a++] = isprint(v) ? v : '.';
 860
 861                 if (c == limit-1) {
 862                         fprintf(ofd, "%s\n", ascii);
 863                         if (i < (len - 1)) {
 864                                 prefix_line(ofd, prefix);
 865                                 fprintf(ofd, "    ");
 866                         }
 867                         a = c = 0;
 868                         memset(ascii, 0, sizeof(ascii));
 869                 } else
 870                         c++;
 871         }
 872
 873         if (c != 0) {
 874                 for (i = 0; i < (limit - c); i++)
 875                         fprintf(ofd, "   ");
 876                 fprintf(ofd, "%s\n", ascii);
 877         }
 878 }
 879
 880 static void print_hdr(FILE *ofd, struct nl_msg *msg)
 881 {
 882         struct nlmsghdr *nlh = nlmsg_hdr(msg);
 883         struct nl_cache_ops *ops;
 884         struct nl_msgtype *mt;
 885         char buf[128];
 886
 887         fprintf(ofd, "    .nlmsg_len = %d\n", nlh->nlmsg_len);
 888
 889         ops = nl_cache_ops_associate(nlmsg_get_proto(msg), nlh->nlmsg_type);
 890         if (ops) {
 891                 mt = nl_msgtype_lookup(ops, nlh->nlmsg_type);
 892                 if (!mt)
 893                         BUG();
 894
 895                 snprintf(buf, sizeof(buf), "%s::%s", ops->co_name, mt->mt_name);
 896         } else
 897                 nl_nlmsgtype2str(nlh->nlmsg_type, buf, sizeof(buf));
 898
 899         fprintf(ofd, "    .nlmsg_type = %d <%s>\n", nlh->nlmsg_type, buf);
 900         fprintf(ofd, "    .nlmsg_flags = %d <%s>\n", nlh->nlmsg_flags,
 901                 nl_nlmsg_flags2str(nlh->nlmsg_flags, buf, sizeof(buf)));
 902         fprintf(ofd, "    .nlmsg_seq = %d\n", nlh->nlmsg_seq);
 903         fprintf(ofd, "    .nlmsg_pid = %d\n", nlh->nlmsg_pid);
 904
 905 }
 906
 907 static void dump_attrs(FILE *ofd, struct nlattr *attrs, int attrlen,
 908                        int prefix)
 909 {
 910         int rem;
 911         struct nlattr *nla;
 912
 913         nla_for_each_attr(nla, attrs, attrlen, rem) {
 914                 int padlen, alen = nla_len(nla);
 915
 916                 prefix_line(ofd, prefix);
 917                 fprintf(ofd, "  [ATTR %02d%s] %d octets\n", nla_type(nla),
 918                         nla->nla_type & NLA_F_NESTED ? " NESTED" : "",
 919                         alen);
 920
 921                 if (nla->nla_type & NLA_F_NESTED)
 922                         dump_attrs(ofd, nla_data(nla), alen, prefix+1);
 923                 else
 924                         dump_hex(ofd, nla_data(nla), alen, prefix);
 925
 926                 padlen = nla_padlen(alen);
 927                 if (padlen > 0) {
 928                         prefix_line(ofd, prefix);
 929                         fprintf(ofd, "  [PADDING] %d octets\n",
 930                                 padlen);
 931                         dump_hex(ofd, nla_data(nla) + alen,
 932                                  padlen, prefix);
 933                 }
 934         }
 935
 936         if (rem) {
 937                 prefix_line(ofd, prefix);
 938                 fprintf(ofd, "  [LEFTOVER] %d octets\n", rem);
 939         }
 940 }
 941
 942 /**
 943  * Dump message in human readable format to file descriptor
 944  * @arg msg             Message to print
 945  * @arg ofd             File descriptor.
 946  */
 947 void nl_msg_dump(struct nl_msg *msg, FILE *ofd)
 948 {
 949         struct nlmsghdr *hdr = nlmsg_hdr(msg);
 950
 951         fprintf(ofd,
 952         "--------------------------   BEGIN NETLINK MESSAGE "
 953         "---------------------------\n");
 954
 955         fprintf(ofd, "  [HEADER] %Zu octets\n", sizeof(struct nlmsghdr));
 956         print_hdr(ofd, msg);
 957
 958         if (hdr->nlmsg_type == NLMSG_ERROR &&
 959             hdr->nlmsg_len >= nlmsg_msg_size(sizeof(struct nlmsgerr))) {
 960                 struct nl_msg *errmsg;
 961                 struct nlmsgerr *err = nlmsg_data(hdr);
 962
 963                 fprintf(ofd, "  [ERRORMSG] %Zu octets\n", sizeof(*err));
 964                 fprintf(ofd, "    .error = %d \"%s\"\n", err->error,
 965                         strerror(-err->error));
 966                 fprintf(ofd, "  [ORIGINAL MESSAGE] %Zu octets\n", sizeof(*hdr));
 967
 968                 errmsg = nlmsg_inherit(&err->msg);
 969                 print_hdr(ofd, errmsg);
 970                 nlmsg_free(errmsg);
 971         } else if (nlmsg_len(hdr) > 0) {
 972                 struct nl_cache_ops *ops;
 973                 int payloadlen = nlmsg_len(hdr);
 974                 int attrlen = 0;
 975
 976                 ops = nl_cache_ops_associate(nlmsg_get_proto(msg),
 977                                              hdr->nlmsg_type);
 978                 if (ops) {
 979                         attrlen = nlmsg_attrlen(hdr, ops->co_hdrsize);
 980                         payloadlen -= attrlen;
 981                 }
 982
 983                 fprintf(ofd, "  [PAYLOAD] %d octets\n", payloadlen);
 984                 dump_hex(ofd, nlmsg_data(hdr), payloadlen, 0);
 985
 986                 if (attrlen) {
 987                         struct nlattr *attrs;
 988                         int attrlen;
 989
 990                         attrs = nlmsg_attrdata(hdr, ops->co_hdrsize);
 991                         attrlen = nlmsg_attrlen(hdr, ops->co_hdrsize);
 992                         dump_attrs(ofd, attrs, attrlen, 0);
 993                 }
 994         }
 995
 996         fprintf(ofd,
 997         "---------------------------  END NETLINK MESSAGE   "
 998         "---------------------------\n");
 999 }
1000
1001 /** @} */
1002
1003 /** @} */