• Home
  • Line#
  • Scopes#
  • Navigate#
  • Raw
  • Download
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 
init_msg_size(void)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  */
nlmsg_msg_size(int payload)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  */
nlmsg_total_size(int payload)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  */
nlmsg_padlen(int payload)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  */
nlmsg_data(const struct nlmsghdr * nlh)218 void *nlmsg_data(const struct nlmsghdr *nlh)
219 {
220 	return (unsigned char *) nlh + NLMSG_HDRLEN;
221 }
222 
nlmsg_tail(const struct nlmsghdr * nlh)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  */
nlmsg_len(const struct nlmsghdr * nlh)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  */
nlmsg_attrdata(const struct nlmsghdr * nlh,int hdrlen)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  */
nlmsg_attrlen(const struct nlmsghdr * nlh,int hdrlen)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 
nlmsg_valid_hdr(const struct nlmsghdr * nlh,int hdrlen)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  */
nlmsg_ok(const struct nlmsghdr * nlh,int remaining)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  */
nlmsg_next(struct nlmsghdr * nlh,int * remaining)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  */
nlmsg_parse(struct nlmsghdr * nlh,int hdrlen,struct nlattr * tb[],int maxtype,struct nla_policy * policy)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  */
nlmsg_find_attr(struct nlmsghdr * nlh,int hdrlen,int attrtype)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  */
nlmsg_validate(struct nlmsghdr * nlh,int hdrlen,int maxtype,struct nla_policy * policy)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 
__nlmsg_alloc(size_t len)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  */
nlmsg_alloc(void)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  */
nlmsg_alloc_size(size_t max)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  */
nlmsg_inherit(struct nlmsghdr * hdr)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  */
nlmsg_alloc_simple(int nlmsgtype,int flags)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  */
nlmsg_set_default_size(size_t max)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  */
nlmsg_convert(struct nlmsghdr * hdr)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  */
nlmsg_reserve(struct nl_msg * n,size_t len,int pad)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  */
nlmsg_append(struct nl_msg * n,void * data,size_t len,int pad)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  */
nlmsg_expand(struct nl_msg * n,size_t newlen)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  */
nlmsg_put(struct nl_msg * n,uint32_t pid,uint32_t seq,int type,int payload,int flags)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  */
nlmsg_hdr(struct nl_msg * n)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  */
nlmsg_get(struct nl_msg * msg)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  */
nlmsg_free(struct nl_msg * msg)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 
nlmsg_set_proto(struct nl_msg * msg,int protocol)692 void nlmsg_set_proto(struct nl_msg *msg, int protocol)
693 {
694 	msg->nm_protocol = protocol;
695 }
696 
nlmsg_get_proto(struct nl_msg * msg)697 int nlmsg_get_proto(struct nl_msg *msg)
698 {
699 	return msg->nm_protocol;
700 }
701 
nlmsg_get_max_size(struct nl_msg * msg)702 size_t nlmsg_get_max_size(struct nl_msg *msg)
703 {
704 	return msg->nm_size;
705 }
706 
nlmsg_set_src(struct nl_msg * msg,struct sockaddr_nl * addr)707 void nlmsg_set_src(struct nl_msg *msg, struct sockaddr_nl *addr)
708 {
709 	memcpy(&msg->nm_src, addr, sizeof(*addr));
710 }
711 
nlmsg_get_src(struct nl_msg * msg)712 struct sockaddr_nl *nlmsg_get_src(struct nl_msg *msg)
713 {
714 	return &msg->nm_src;
715 }
716 
nlmsg_set_dst(struct nl_msg * msg,struct sockaddr_nl * addr)717 void nlmsg_set_dst(struct nl_msg *msg, struct sockaddr_nl *addr)
718 {
719 	memcpy(&msg->nm_dst, addr, sizeof(*addr));
720 }
721 
nlmsg_get_dst(struct nl_msg * msg)722 struct sockaddr_nl *nlmsg_get_dst(struct nl_msg *msg)
723 {
724 	return &msg->nm_dst;
725 }
726 
nlmsg_set_creds(struct nl_msg * msg,struct ucred * creds)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 
nlmsg_get_creds(struct nl_msg * msg)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 
nl_nlmsgtype2str(int type,char * buf,size_t size)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 
nl_str2nlmsgtype(const char * name)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 
nl_nlmsg_flags2str(int flags,char * buf,size_t len)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 
parse_cb(struct nl_object * obj,struct nl_parser_param * p)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 
nl_msg_parse(struct nl_msg * msg,void (* cb)(struct nl_object *,void *),void * arg)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 
prefix_line(FILE * ofd,int prefix)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 
dump_hex(FILE * ofd,char * start,int len,int prefix)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 
print_hdr(FILE * ofd,struct nl_msg * msg)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 
dump_attrs(FILE * ofd,struct nlattr * attrs,int attrlen,int prefix)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  */
nl_msg_dump(struct nl_msg * msg,FILE * ofd)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 /** @} */
1022