blob: 9fe9d5418c06b5523a273e9b4ebb6c1f3b68268b [file] [log] [blame] [edit]
/*
* lib/msg.c Netlink Messages Interface
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation version 2.1
* of the License.
*
* Copyright (c) 2003-2008 Thomas Graf <tgraf@suug.ch>
*/
/**
* @ingroup core
* @defgroup msg Messages
* Netlink Message Construction/Parsing Interface
*
* The following information is partly extracted from RFC3549
* (ftp://ftp.rfc-editor.org/in-notes/rfc3549.txt)
*
* @par Message Format
* Netlink messages consist of a byte stream with one or multiple
* Netlink headers and an associated payload. If the payload is too big
* to fit into a single message it, can be split over multiple Netlink
* messages, collectively called a multipart message. For multipart
* messages, the first and all following headers have the \c NLM_F_MULTI
* Netlink header flag set, except for the last header which has the
* Netlink header type \c NLMSG_DONE.
*
* @par
* The Netlink message header (\link nlmsghdr struct nlmsghdr\endlink) is shown below.
* @code
* 0 1 2 3
* 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
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
* | Length |
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
* | Type | Flags |
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
* | Sequence Number |
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
* | Process ID (PID) |
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
* @endcode
*
* @par
* The netlink message header and payload must be aligned properly:
* @code
* <------- NLMSG_ALIGN(hlen) ------> <---- NLMSG_ALIGN(len) --->
* +----------------------------+- - -+- - - - - - - - - - -+- - -+
* | Header | Pad | Payload | Pad |
* | struct nlmsghdr | | | |
* +----------------------------+- - -+- - - - - - - - - - -+- - -+
* @endcode
* @par
* Message Format:
* @code
* <--- nlmsg_total_size(payload) --->
* <-- nlmsg_msg_size(payload) ->
* +----------+- - -+-------------+- - -+-------- - -
* | nlmsghdr | Pad | Payload | Pad | nlmsghdr
* +----------+- - -+-------------+- - -+-------- - -
* nlmsg_data(nlh)---^ ^
* nlmsg_next(nlh)-----------------------+
* @endcode
* @par
* The payload may consist of arbitary data but may have strict
* alignment and formatting rules depening on the specific netlink
* families.
* @par
* @code
* <---------------------- nlmsg_len(nlh) --------------------->
* <------ hdrlen ------> <- nlmsg_attrlen(nlh, hdrlen) ->
* +----------------------+- - -+--------------------------------+
* | Family Header | Pad | Attributes |
* +----------------------+- - -+--------------------------------+
* nlmsg_attrdata(nlh, hdrlen)---^
* @endcode
* @par The ACK Netlink Message
* This message is actually used to denote both an ACK and a NACK.
* Typically, the direction is from FEC to CPC (in response to an ACK
* request message). However, the CPC should be able to send ACKs back
* to FEC when requested.
* @code
* 0 1 2 3
* 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
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
* | Netlink message header |
* | type = NLMSG_ERROR |
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
* | Error code |
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
* | OLD Netlink message header |
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
* @endcode
*
* @par Example
* @code
* // Various methods exist to create/allocate a new netlink
* // message.
* //
* // nlmsg_alloc() will allocate an empty netlink message with
* // a maximum payload size which defaults to the page size of
* // the system. This default size can be modified using the
* // function nlmsg_set_default_size().
* struct nl_msg *msg = nlmsg_alloc();
*
* // Very often, the message type and message flags are known
* // at allocation time while the other fields are auto generated:
* struct nl_msg *msg = nlmsg_alloc_simple(MY_TYPE, MY_FLAGS);
*
* // Alternatively an existing netlink message header can be used
* // to inherit the header values:
* struct nlmsghdr hdr = {
* .nlmsg_type = MY_TYPE,
* .nlmsg_flags = MY_FLAGS,
* };
* struct nl_msg *msg = nlmsg_inherit(&hdr);
*
* // Last but not least, netlink messages received from netlink sockets
* // can be converted into nl_msg objects using nlmsg_convert(). This
* // will create a message with a maximum payload size which equals the
* // length of the existing netlink message, therefore no more data can
* // be appened without calling nlmsg_expand() first.
* struct nl_msg *msg = nlmsg_convert(nlh_from_nl_sock);
*
* // Payload may be added to the message via nlmsg_append(). The fourth
* // parameter specifies the number of alignment bytes the data should
* // be padding with at the end. Common values are 0 to disable it or
* // NLMSG_ALIGNTO to ensure proper netlink message padding.
* nlmsg_append(msg, &mydata, sizeof(mydata), 0);
*
* // Sometimes it may be necessary to reserve room for data but defer
* // the actual copying to a later point, nlmsg_reserve() can be used
* // for this purpose:
* void *data = nlmsg_reserve(msg, sizeof(mydata), NLMSG_ALIGNTO);
*
* // Attributes may be added using the attributes interface.
*
* // After successful use of the message, the memory must be freed
* // using nlmsg_free()
* nlmsg_free(msg);
* @endcode
*
* @par 4) Parsing messages
* @code
* int n;
* unsigned char *buf;
* struct nlmsghdr *hdr;
*
* n = nl_recv(handle, NULL, &buf);
*
* hdr = (struct nlmsghdr *) buf;
* while (nlmsg_ok(hdr, n)) {
* // Process message here...
* hdr = nlmsg_next(hdr, &n);
* }
* @endcode
* @{
*/
#include <netlink-local.h>
#include <netlink/netlink.h>
#include <netlink/utils.h>
#include <netlink/cache.h>
#include <netlink/attr.h>
#include <linux/socket.h>
static size_t default_msg_size;
static void __init init_msg_size(void)
{
default_msg_size = getpagesize();
}
/**
* @name Size Calculations
* @{
*/
/**
* length of netlink message not including padding
* @arg payload length of message payload
*/
int nlmsg_msg_size(int payload)
{
return NLMSG_HDRLEN + payload;
}
/**
* length of netlink message including padding
* @arg payload length of message payload
*/
int nlmsg_total_size(int payload)
{
return NLMSG_ALIGN(nlmsg_msg_size(payload));
}
/**
* length of padding at the message's tail
* @arg payload length of message payload
*/
int nlmsg_padlen(int payload)
{
return nlmsg_total_size(payload) - nlmsg_msg_size(payload);
}
/** @} */
/**
* @name Payload Access
* @{
*/
/**
* head of message payload
* @arg nlh netlink messsage header
*/
void *nlmsg_data(const struct nlmsghdr *nlh)
{
return (unsigned char *) nlh + NLMSG_HDRLEN;
}
void *nlmsg_tail(const struct nlmsghdr *nlh)
{
return (unsigned char *) nlh + NLMSG_ALIGN(nlh->nlmsg_len);
}
/**
* length of message payload
* @arg nlh netlink message header
*/
int nlmsg_len(const struct nlmsghdr *nlh)
{
return nlh->nlmsg_len - NLMSG_HDRLEN;
}
/** @} */
/**
* @name Attribute Access
* @{
*/
/**
* head of attributes data
* @arg nlh netlink message header
* @arg hdrlen length of family specific header
*/
struct nlattr *nlmsg_attrdata(const struct nlmsghdr *nlh, int hdrlen)
{
unsigned char *data = nlmsg_data(nlh);
return (struct nlattr *) (data + NLMSG_ALIGN(hdrlen));
}
/**
* length of attributes data
* @arg nlh netlink message header
* @arg hdrlen length of family specific header
*/
int nlmsg_attrlen(const struct nlmsghdr *nlh, int hdrlen)
{
return nlmsg_len(nlh) - NLMSG_ALIGN(hdrlen);
}
/** @} */
/**
* @name Message Parsing
* @{
*/
int nlmsg_valid_hdr(const struct nlmsghdr *nlh, int hdrlen)
{
if (nlh->nlmsg_len < nlmsg_msg_size(hdrlen))
return 0;
return 1;
}
/**
* check if the netlink message fits into the remaining bytes
* @arg nlh netlink message header
* @arg remaining number of bytes remaining in message stream
*/
int nlmsg_ok(const struct nlmsghdr *nlh, int remaining)
{
return (remaining >= (int)sizeof(struct nlmsghdr) &&
nlh->nlmsg_len >= sizeof(struct nlmsghdr) &&
nlh->nlmsg_len <= remaining);
}
/**
* next netlink message in message stream
* @arg nlh netlink message header
* @arg remaining number of bytes remaining in message stream
*
* @returns the next netlink message in the message stream and
* decrements remaining by the size of the current message.
*/
struct nlmsghdr *nlmsg_next(struct nlmsghdr *nlh, int *remaining)
{
int totlen = NLMSG_ALIGN(nlh->nlmsg_len);
*remaining -= totlen;
return (struct nlmsghdr *) ((unsigned char *) nlh + totlen);
}
/**
* parse attributes of a netlink message
* @arg nlh netlink message header
* @arg hdrlen length of family specific header
* @arg tb destination array with maxtype+1 elements
* @arg maxtype maximum attribute type to be expected
* @arg policy validation policy
*
* See nla_parse()
*/
int nlmsg_parse(struct nlmsghdr *nlh, int hdrlen, struct nlattr *tb[],
int maxtype, struct nla_policy *policy)
{
if (!nlmsg_valid_hdr(nlh, hdrlen))
return -NLE_MSG_TOOSHORT;
return nla_parse(tb, maxtype, nlmsg_attrdata(nlh, hdrlen),
nlmsg_attrlen(nlh, hdrlen), policy);
}
/**
* nlmsg_find_attr - find a specific attribute in a netlink message
* @arg nlh netlink message header
* @arg hdrlen length of familiy specific header
* @arg attrtype type of attribute to look for
*
* Returns the first attribute which matches the specified type.
*/
struct nlattr *nlmsg_find_attr(struct nlmsghdr *nlh, int hdrlen, int attrtype)
{
return nla_find(nlmsg_attrdata(nlh, hdrlen),
nlmsg_attrlen(nlh, hdrlen), attrtype);
}
/**
* nlmsg_validate - validate a netlink message including attributes
* @arg nlh netlinket message header
* @arg hdrlen length of familiy specific header
* @arg maxtype maximum attribute type to be expected
* @arg policy validation policy
*/
int nlmsg_validate(struct nlmsghdr *nlh, int hdrlen, int maxtype,
struct nla_policy *policy)
{
if (!nlmsg_valid_hdr(nlh, hdrlen))
return -NLE_MSG_TOOSHORT;
return nla_validate(nlmsg_attrdata(nlh, hdrlen),
nlmsg_attrlen(nlh, hdrlen), maxtype, policy);
}
/** @} */
/**
* @name Message Building/Access
* @{
*/
static struct nl_msg *__nlmsg_alloc(size_t len)
{
struct nl_msg *nm;
nm = calloc(1, sizeof(*nm));
if (!nm)
goto errout;
nm->nm_refcnt = 1;
nm->nm_nlh = malloc(len);
if (!nm->nm_nlh)
goto errout;
memset(nm->nm_nlh, 0, sizeof(struct nlmsghdr));
nm->nm_protocol = -1;
nm->nm_size = len;
nm->nm_nlh->nlmsg_len = nlmsg_total_size(0);
NL_DBG(2, "msg %p: Allocated new message, maxlen=%zu\n", nm, len);
return nm;
errout:
free(nm);
return NULL;
}
/**
* Allocate a new netlink message with the default maximum payload size.
*
* Allocates a new netlink message without any further payload. The
* maximum payload size defaults to PAGESIZE or as otherwise specified
* with nlmsg_set_default_size().
*
* @return Newly allocated netlink message or NULL.
*/
struct nl_msg *nlmsg_alloc(void)
{
return __nlmsg_alloc(default_msg_size);
}
/**
* Allocate a new netlink message with maximum payload size specified.
*/
struct nl_msg *nlmsg_alloc_size(size_t max)
{
return __nlmsg_alloc(max);
}
/**
* Allocate a new netlink message and inherit netlink message header
* @arg hdr Netlink message header template
*
* Allocates a new netlink message and inherits the original message
* header. If \a hdr is not NULL it will be used as a template for
* the netlink message header, otherwise the header is left blank.
*
* @return Newly allocated netlink message or NULL
*/
struct nl_msg *nlmsg_inherit(struct nlmsghdr *hdr)
{
struct nl_msg *nm;
nm = nlmsg_alloc();
if (nm && hdr) {
struct nlmsghdr *new = nm->nm_nlh;
new->nlmsg_type = hdr->nlmsg_type;
new->nlmsg_flags = hdr->nlmsg_flags;
new->nlmsg_seq = hdr->nlmsg_seq;
new->nlmsg_pid = hdr->nlmsg_pid;
}
return nm;
}
/**
* Allocate a new netlink message
* @arg nlmsgtype Netlink message type
* @arg flags Message flags.
*
* @return Newly allocated netlink message or NULL.
*/
struct nl_msg *nlmsg_alloc_simple(int nlmsgtype, int flags)
{
struct nl_msg *msg;
struct nlmsghdr nlh = {
.nlmsg_type = nlmsgtype,
.nlmsg_flags = flags,
};
msg = nlmsg_inherit(&nlh);
if (msg)
NL_DBG(2, "msg %p: Allocated new simple message\n", msg);
return msg;
}
/**
* Set the default maximum message payload size for allocated messages
* @arg max Size of payload in bytes.
*/
void nlmsg_set_default_size(size_t max)
{
if (max < nlmsg_total_size(0))
max = nlmsg_total_size(0);
default_msg_size = max;
}
/**
* Convert a netlink message received from a netlink socket to a nl_msg
* @arg hdr Netlink message received from netlink socket.
*
* Allocates a new netlink message and copies all of the data pointed to
* by \a hdr into the new message object.
*
* @return Newly allocated netlink message or NULL.
*/
struct nl_msg *nlmsg_convert(struct nlmsghdr *hdr)
{
struct nl_msg *nm;
nm = __nlmsg_alloc(NLMSG_ALIGN(hdr->nlmsg_len));
if (!nm)
goto errout;
memcpy(nm->nm_nlh, hdr, hdr->nlmsg_len);
return nm;
errout:
nlmsg_free(nm);
return NULL;
}
/**
* Reserve room for additional data in a netlink message
* @arg n netlink message
* @arg len length of additional data to reserve room for
* @arg pad number of bytes to align data to
*
* Reserves room for additional data at the tail of the an
* existing netlink message. Eventual padding required will
* be zeroed out.
*
* @return Pointer to start of additional data tailroom or NULL.
*/
void *nlmsg_reserve(struct nl_msg *n, size_t len, int pad)
{
void *buf = n->nm_nlh;
size_t nlmsg_len = n->nm_nlh->nlmsg_len;
size_t tlen;
tlen = pad ? ((len + (pad - 1)) & ~(pad - 1)) : len;
if ((tlen + nlmsg_len) > n->nm_size)
return NULL;
buf += nlmsg_len;
n->nm_nlh->nlmsg_len += tlen;
if (tlen > len)
memset(buf + len, 0, tlen - len);
NL_DBG(2, "msg %p: Reserved %zu bytes, pad=%d, nlmsg_len=%d\n",
n, len, pad, n->nm_nlh->nlmsg_len);
return buf;
}
/**
* Append data to tail of a netlink message
* @arg n netlink message
* @arg data data to add
* @arg len length of data
* @arg pad Number of bytes to align data to.
*
* Extends the netlink message as needed and appends the data of given
* length to the message.
*
* @return 0 on success or a negative error code
*/
int nlmsg_append(struct nl_msg *n, void *data, size_t len, int pad)
{
void *tmp;
tmp = nlmsg_reserve(n, len, pad);
if (tmp == NULL)
return -NLE_NOMEM;
memcpy(tmp, data, len);
NL_DBG(2, "msg %p: Appended %zu bytes with padding %d\n", n, len, pad);
return 0;
}
/**
* Expand maximum payload size of a netlink message
* @arg n Netlink message.
* @arg newlen New maximum payload size.
*
* Reallocates the payload section of a netlink message and increases
* the maximum payload size of the message.
*
* @note Any pointers pointing to old payload block will be stale and
* need to be refetched. Therfore, do not expand while constructing
* nested attributes or while reserved data blocks are held.
*
* @return 0 on success or a negative error code.
*/
int nlmsg_expand(struct nl_msg *n, size_t newlen)
{
void *tmp;
if (newlen <= n->nm_size)
return -NLE_INVAL;
tmp = realloc(n->nm_nlh, newlen);
if (tmp == NULL)
return -NLE_NOMEM;
n->nm_nlh = tmp;
n->nm_size = newlen;
return 0;
}
/**
* Add a netlink message header to a netlink message
* @arg n netlink message
* @arg pid netlink process id or NL_AUTO_PID
* @arg seq sequence number of message or NL_AUTO_SEQ
* @arg type message type
* @arg payload length of message payload
* @arg flags message flags
*
* Adds or overwrites the netlink message header in an existing message
* object. If \a payload is greater-than zero additional room will be
* reserved, f.e. for family specific headers. It can be accesed via
* nlmsg_data().
*
* @return A pointer to the netlink message header or NULL.
*/
struct nlmsghdr *nlmsg_put(struct nl_msg *n, uint32_t pid, uint32_t seq,
int type, int payload, int flags)
{
struct nlmsghdr *nlh;
if (n->nm_nlh->nlmsg_len < NLMSG_HDRLEN)
BUG();
nlh = (struct nlmsghdr *) n->nm_nlh;
nlh->nlmsg_type = type;
nlh->nlmsg_flags = flags;
nlh->nlmsg_pid = pid;
nlh->nlmsg_seq = seq;
NL_DBG(2, "msg %p: Added netlink header type=%d, flags=%d, pid=%d, "
"seq=%d\n", n, type, flags, pid, seq);
if (payload > 0 &&
nlmsg_reserve(n, payload, NLMSG_ALIGNTO) == NULL)
return NULL;
return nlh;
}
/**
* Return actual netlink message
* @arg n netlink message
*
* Returns the actual netlink message casted to the type of the netlink
* message header.
*
* @return A pointer to the netlink message.
*/
struct nlmsghdr *nlmsg_hdr(struct nl_msg *n)
{
return n->nm_nlh;
}
/**
* Acquire a reference on a netlink message
* @arg msg message to acquire reference from
*/
void nlmsg_get(struct nl_msg *msg)
{
msg->nm_refcnt++;
NL_DBG(4, "New reference to message %p, total %d\n",
msg, msg->nm_refcnt);
}
/**
* Release a reference from an netlink message
* @arg msg message to release reference from
*
* Frees memory after the last reference has been released.
*/
void nlmsg_free(struct nl_msg *msg)
{
if (!msg)
return;
msg->nm_refcnt--;
NL_DBG(4, "Returned message reference %p, %d remaining\n",
msg, msg->nm_refcnt);
if (msg->nm_refcnt < 0)
BUG();
if (msg->nm_refcnt <= 0) {
free(msg->nm_nlh);
free(msg);
NL_DBG(2, "msg %p: Freed\n", msg);
}
}
/** @} */
/**
* @name Attributes
* @{
*/
void nlmsg_set_proto(struct nl_msg *msg, int protocol)
{
msg->nm_protocol = protocol;
}
int nlmsg_get_proto(struct nl_msg *msg)
{
return msg->nm_protocol;
}
size_t nlmsg_get_max_size(struct nl_msg *msg)
{
return msg->nm_size;
}
void nlmsg_set_src(struct nl_msg *msg, struct sockaddr_nl *addr)
{
memcpy(&msg->nm_src, addr, sizeof(*addr));
}
struct sockaddr_nl *nlmsg_get_src(struct nl_msg *msg)
{
return &msg->nm_src;
}
void nlmsg_set_dst(struct nl_msg *msg, struct sockaddr_nl *addr)
{
memcpy(&msg->nm_dst, addr, sizeof(*addr));
}
struct sockaddr_nl *nlmsg_get_dst(struct nl_msg *msg)
{
return &msg->nm_dst;
}
void nlmsg_set_creds(struct nl_msg *msg, struct ucred *creds)
{
memcpy(&msg->nm_creds, creds, sizeof(*creds));
msg->nm_flags |= NL_MSG_CRED_PRESENT;
}
struct ucred *nlmsg_get_creds(struct nl_msg *msg)
{
if (msg->nm_flags & NL_MSG_CRED_PRESENT)
return &msg->nm_creds;
return NULL;
}
/** @} */
/**
* @name Netlink Message Type Translations
* @{
*/
static struct trans_tbl nl_msgtypes[] = {
__ADD(NLMSG_NOOP,NOOP)
__ADD(NLMSG_ERROR,ERROR)
__ADD(NLMSG_DONE,DONE)
__ADD(NLMSG_OVERRUN,OVERRUN)
};
char *nl_nlmsgtype2str(int type, char *buf, size_t size)
{
return __type2str(type, buf, size, nl_msgtypes,
ARRAY_SIZE(nl_msgtypes));
}
int nl_str2nlmsgtype(const char *name)
{
return __str2type(name, nl_msgtypes, ARRAY_SIZE(nl_msgtypes));
}
/** @} */
/**
* @name Netlink Message Flags Translations
* @{
*/
char *nl_nlmsg_flags2str(int flags, char *buf, size_t len)
{
memset(buf, 0, len);
#define PRINT_FLAG(f) \
if (flags & NLM_F_##f) { \
flags &= ~NLM_F_##f; \
strncat(buf, #f, len - strlen(buf) - 1); \
if (flags) \
strncat(buf, ",", len - strlen(buf) - 1); \
}
PRINT_FLAG(REQUEST);
PRINT_FLAG(MULTI);
PRINT_FLAG(ACK);
PRINT_FLAG(ECHO);
PRINT_FLAG(ROOT);
PRINT_FLAG(MATCH);
PRINT_FLAG(ATOMIC);
PRINT_FLAG(REPLACE);
PRINT_FLAG(EXCL);
PRINT_FLAG(CREATE);
PRINT_FLAG(APPEND);
if (flags) {
char s[32];
snprintf(s, sizeof(s), "0x%x", flags);
strncat(buf, s, len - strlen(buf) - 1);
}
#undef PRINT_FLAG
return buf;
}
/** @} */
/**
* @name Direct Parsing
* @{
*/
/** @cond SKIP */
struct dp_xdata {
void (*cb)(struct nl_object *, void *);
void *arg;
};
/** @endcond */
static int parse_cb(struct nl_object *obj, struct nl_parser_param *p)
{
struct dp_xdata *x = p->pp_arg;
x->cb(obj, x->arg);
return 0;
}
int nl_msg_parse(struct nl_msg *msg, void (*cb)(struct nl_object *, void *),
void *arg)
{
struct nl_cache_ops *ops;
struct nl_parser_param p = {
.pp_cb = parse_cb
};
struct dp_xdata x = {
.cb = cb,
.arg = arg,
};
ops = nl_cache_ops_associate(nlmsg_get_proto(msg),
nlmsg_hdr(msg)->nlmsg_type);
if (ops == NULL)
return -NLE_MSGTYPE_NOSUPPORT;
p.pp_arg = &x;
return nl_cache_parse(ops, NULL, nlmsg_hdr(msg), &p);
}
/** @} */
/**
* @name Dumping
* @{
*/
static void prefix_line(FILE *ofd, int prefix)
{
int i;
for (i = 0; i < prefix; i++)
fprintf(ofd, " ");
}
static inline void dump_hex(FILE *ofd, char *start, int len, int prefix)
{
int i, a, c, limit;
char ascii[21] = {0};
limit = 18 - (prefix * 2);
prefix_line(ofd, prefix);
fprintf(ofd, " ");
for (i = 0, a = 0, c = 0; i < len; i++) {
int v = *(uint8_t *) (start + i);
fprintf(ofd, "%02x ", v);
ascii[a++] = isprint(v) ? v : '.';
if (c == limit-1) {
fprintf(ofd, "%s\n", ascii);
if (i < (len - 1)) {
prefix_line(ofd, prefix);
fprintf(ofd, " ");
}
a = c = 0;
memset(ascii, 0, sizeof(ascii));
} else
c++;
}
if (c != 0) {
for (i = 0; i < (limit - c); i++)
fprintf(ofd, " ");
fprintf(ofd, "%s\n", ascii);
}
}
static void print_hdr(FILE *ofd, struct nl_msg *msg)
{
struct nlmsghdr *nlh = nlmsg_hdr(msg);
struct nl_cache_ops *ops;
struct nl_msgtype *mt;
char buf[128];
fprintf(ofd, " .nlmsg_len = %d\n", nlh->nlmsg_len);
ops = nl_cache_ops_associate(nlmsg_get_proto(msg), nlh->nlmsg_type);
if (ops) {
mt = nl_msgtype_lookup(ops, nlh->nlmsg_type);
if (!mt)
BUG();
snprintf(buf, sizeof(buf), "%s::%s", ops->co_name, mt->mt_name);
} else
nl_nlmsgtype2str(nlh->nlmsg_type, buf, sizeof(buf));
fprintf(ofd, " .nlmsg_type = %d <%s>\n", nlh->nlmsg_type, buf);
fprintf(ofd, " .nlmsg_flags = %d <%s>\n", nlh->nlmsg_flags,
nl_nlmsg_flags2str(nlh->nlmsg_flags, buf, sizeof(buf)));
fprintf(ofd, " .nlmsg_seq = %d\n", nlh->nlmsg_seq);
fprintf(ofd, " .nlmsg_pid = %d\n", nlh->nlmsg_pid);
}
static void dump_attrs(FILE *ofd, struct nlattr *attrs, int attrlen,
int prefix)
{
int rem;
struct nlattr *nla;
nla_for_each_attr(nla, attrs, attrlen, rem) {
int padlen, alen = nla_len(nla);
prefix_line(ofd, prefix);
fprintf(ofd, " [ATTR %02d%s] %d octets\n", nla_type(nla),
nla->nla_type & NLA_F_NESTED ? " NESTED" : "",
alen);
if (nla->nla_type & NLA_F_NESTED)
dump_attrs(ofd, nla_data(nla), alen, prefix+1);
else
dump_hex(ofd, nla_data(nla), alen, prefix);
padlen = nla_padlen(alen);
if (padlen > 0) {
prefix_line(ofd, prefix);
fprintf(ofd, " [PADDING] %d octets\n",
padlen);
dump_hex(ofd, nla_data(nla) + alen,
padlen, prefix);
}
}
if (rem) {
prefix_line(ofd, prefix);
fprintf(ofd, " [LEFTOVER] %d octets\n", rem);
}
}
/**
* Dump message in human readable format to file descriptor
* @arg msg Message to print
* @arg ofd File descriptor.
*/
void nl_msg_dump(struct nl_msg *msg, FILE *ofd)
{
struct nlmsghdr *hdr = nlmsg_hdr(msg);
fprintf(ofd,
"-------------------------- BEGIN NETLINK MESSAGE "
"---------------------------\n");
fprintf(ofd, " [HEADER] %Zu octets\n", sizeof(struct nlmsghdr));
print_hdr(ofd, msg);
if (hdr->nlmsg_type == NLMSG_ERROR &&
hdr->nlmsg_len >= nlmsg_msg_size(sizeof(struct nlmsgerr))) {
struct nl_msg *errmsg;
struct nlmsgerr *err = nlmsg_data(hdr);
fprintf(ofd, " [ERRORMSG] %Zu octets\n", sizeof(*err));
fprintf(ofd, " .error = %d \"%s\"\n", err->error,
strerror(-err->error));
fprintf(ofd, " [ORIGINAL MESSAGE] %Zu octets\n", sizeof(*hdr));
errmsg = nlmsg_inherit(&err->msg);
print_hdr(ofd, errmsg);
nlmsg_free(errmsg);
} else if (nlmsg_len(hdr) > 0) {
struct nl_cache_ops *ops;
int payloadlen = nlmsg_len(hdr);
int attrlen = 0;
ops = nl_cache_ops_associate(nlmsg_get_proto(msg),
hdr->nlmsg_type);
if (ops) {
attrlen = nlmsg_attrlen(hdr, ops->co_hdrsize);
payloadlen -= attrlen;
}
fprintf(ofd, " [PAYLOAD] %d octets\n", payloadlen);
dump_hex(ofd, nlmsg_data(hdr), payloadlen, 0);
if (attrlen) {
struct nlattr *attrs;
int attrlen;
attrs = nlmsg_attrdata(hdr, ops->co_hdrsize);
attrlen = nlmsg_attrlen(hdr, ops->co_hdrsize);
dump_attrs(ofd, attrs, attrlen, 0);
}
}
fprintf(ofd,
"--------------------------- END NETLINK MESSAGE "
"---------------------------\n");
}
/** @} */
/** @} */