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