libnl_2: Fix nested attribute setting

Change-Id: I9dbf72a25e8a939ab6aa35eaf4adbf86d9ed97a1
Signed-off-by: Dmitry Shmidt <dimitrysh@google.com>

4 files changed, 64 insertions, 75 deletions

diff --git a/libnl_2/Android.mk b/libnl_2/Android.mk
index 1fbde21..1745f5a 100644
--- a/libnl_2/Android.mk
+++ b/libnl_2/Android.mk
@@ -11,7 +11,8 @@ LOCAL_SRC_FILES := \
         msg.c \
         netlink.c \
         object.c \
-        socket.c
+        socket.c \
+        dbg.c
 
 LOCAL_C_INCLUDES += \
         external/libnl-headers
diff --git a/libnl_2/attr.c b/libnl_2/attr.c
index d416350..f3a2b58 100644
--- a/libnl_2/attr.c
+++ b/libnl_2/attr.c
@@ -66,38 +66,30 @@ int nla_len(const struct nlattr *nla)
 	return nla->nla_len - NLA_HDRLEN;
 }
 
+int nla_padlen(int payload)
+{
+	return NLA_ALIGN(payload) - payload;
+}
+
 /* Start a new level of nested attributes. */
 struct nlattr *nla_nest_start(struct nl_msg *msg, int attrtype)
 {
-	if (!nla_put(msg, attrtype, 0, NULL)) {
-		/* Get ref to last (nested start) attr	*/
-		int padding;
-		struct nlattr *nla;
-
-		padding = nlmsg_padlen(nlmsg_datalen(nlmsg_hdr(msg)));
-		nla = (struct nlattr *) \
-			((char *) nlmsg_tail(msg->nm_nlh) - padding);
-		return nla;
+	struct nlattr *start = (struct nlattr *)nlmsg_tail(msg->nm_nlh);
+	int rc;
 
-	} else
+	rc = nla_put(msg, attrtype, 0, NULL);
+	if (rc < 0)
 		return NULL;
 
+	return start;
 }
 
 /* Finalize nesting of attributes. */
 int nla_nest_end(struct nl_msg *msg, struct nlattr *start)
 {
-	struct nlattr *container;
-
-	/* Adjust nested attribute container size */
-	container = (unsigned char *) start - sizeof(struct nlattr);
-	container->nla_len = (unsigned char *) \
-		nlmsg_tail(nlmsg_hdr(msg)) - (unsigned char *)container;
-
-	/* Fix attribute size */
-	start->nla_len = (unsigned char *) \
-		nlmsg_tail(nlmsg_hdr(msg)) - (unsigned char *)start;
-
+	/* Set attribute size */
+	start->nla_len = (unsigned char *)nlmsg_tail(nlmsg_hdr(msg)) -
+				(unsigned char *)start;
 	return 0;
 }
 
@@ -134,14 +126,13 @@ int nla_parse(struct nlattr *tb[], int maxtype, struct nlattr *head,
 	int rem;
 
 	/* First clear table */
-	memset(tb, 0, (maxtype+1) * sizeof(struct nlattr *));
+	memset(tb, 0, (maxtype + 1) * sizeof(struct nlattr *));
 
 	nla_for_each_attr(pos, head, len, rem) {
-		const int type = nla_type(pos);
+		int type = nla_type(pos);
 
-		if (type <= maxtype)
+		if ((type <= maxtype) && (type != 0))
 			tb[type] = pos;
-
 	}
 
 	return 0;
@@ -179,15 +170,10 @@ int nla_put(struct nl_msg *msg, int attrtype, int datalen, const void *data)
  * nested message may not have a family specific header */
 int nla_put_nested(struct nl_msg *msg, int attrtype, struct nl_msg *nested)
 {
-	int rc = -1;
-	const int NO_HEADER = 0;
+	int rc;
 
-	rc = nla_put(
-		msg,
-		attrtype,
-		nlmsg_attrlen(nlmsg_hdr(nested), NO_HEADER),
-		(const void *) nlmsg_attrdata(nlmsg_hdr(nested), NO_HEADER)
-		);
+	rc = nla_put(msg, attrtype, nlmsg_attrlen(nlmsg_hdr(nested), 0),
+			nlmsg_attrdata(nlmsg_hdr(nested), 0));
 	return rc;
 
 }
@@ -195,43 +181,37 @@ int nla_put_nested(struct nl_msg *msg, int attrtype, struct nl_msg *nested)
 /* Return type of the attribute. */
 int nla_type(const struct nlattr *nla)
 {
-	return (int) nla->nla_type;
+	return (int)nla->nla_type & NLA_TYPE_MASK;
 }
 
 /* Reserves room for an attribute in specified netlink message and fills
  * in the attribute header (type,length). Return NULL if insufficient space */
-struct nlattr *nla_reserve(struct nl_msg * msg, int attrtype, int data_len)
+struct nlattr *nla_reserve(struct nl_msg *msg, int attrtype, int data_len)
 {
 
 	struct nlattr *nla;
-	const unsigned int NEW_SIZE = \
-		msg->nm_nlh->nlmsg_len + NLA_ALIGN(NLA_HDRLEN + data_len);
+	const unsigned int NEW_SIZE = NLMSG_ALIGN(msg->nm_nlh->nlmsg_len) +
+					NLA_ALIGN(NLA_HDRLEN + data_len);
 
 	/* Check enough space for attribute */
-	if (NEW_SIZE <= msg->nm_size) {
-		const int fam_hdrlen = msg->nm_nlh->nlmsg_len - NLMSG_HDRLEN;
-		msg->nm_nlh->nlmsg_len = NEW_SIZE;
-		nla = nlmsg_attrdata(msg->nm_nlh, fam_hdrlen);
-		nla->nla_type = attrtype;
-		nla->nla_len = NLA_HDRLEN + data_len;
-	} else
-		goto fail;
+	if (NEW_SIZE > msg->nm_size)
+		return NULL;
 
+	nla = (struct nlattr *)nlmsg_tail(msg->nm_nlh);
+	nla->nla_type = attrtype;
+	nla->nla_len = NLA_HDRLEN + data_len;
+	memset((unsigned char *)nla + nla->nla_len, 0, nla_padlen(data_len));
+	msg->nm_nlh->nlmsg_len = NEW_SIZE;
 	return nla;
-fail:
-	return NULL;
-
 }
 
 /* Copy attribute payload to another memory area. */
 int nla_memcpy(void *dest, struct nlattr *src, int count)
 {
-	int rc;
-	void *ret_dest = memcpy(dest, nla_data(src), count);
-	if (!ret_dest)
-		return count;
-	else
+	if (!src || !dest)
 		return 0;
+	if (count > nla_len(src))
+		count = nla_len(src);
+	memcpy(dest, nla_data(src), count);
+	return count;
 }
-
-
diff --git a/libnl_2/dbg.c b/libnl_2/dbg.c
new file mode 100644
index 0000000..9764de6
--- /dev/null
+++ b/libnl_2/dbg.c
@@ -0,0 +1,12 @@
+#include "netlink/netlink.h"
+#include <android/log.h>
+
+void libnl_printf(int level, char *format, ...)
+{
+	va_list ap;
+
+	level = ANDROID_LOG_ERROR;
+	va_start(ap, format);
+	__android_log_vprint(level, "libnl_2", format, ap);
+	va_end(ap);
+}
diff --git a/libnl_2/msg.c b/libnl_2/msg.c
index d7276b7..283da6e 100644
--- a/libnl_2/msg.c
+++ b/libnl_2/msg.c
@@ -91,39 +91,41 @@ struct nlmsghdr *nlmsg_hdr(struct nl_msg *n)
 struct nlattr *nlmsg_attrdata(const struct nlmsghdr *nlh, int hdrlen)
 {
 	unsigned char *data = nlmsg_data(nlh);
-	return (struct nlattr *) (data + NLMSG_ALIGN(hdrlen));
+	return (struct nlattr *)(data + NLMSG_ALIGN(hdrlen));
 }
 
 /* Returns pointer to end of netlink message */
 void *nlmsg_tail(const struct nlmsghdr *nlh)
 {
-	return (void *)((char *) nlh + nlh->nlmsg_len);
+	return (void *)((char *)nlh + NLMSG_ALIGN(nlh->nlmsg_len));
 }
 
 /* Next netlink message in message stream */
 struct nlmsghdr *nlmsg_next(struct nlmsghdr *nlh, int *remaining)
 {
 	struct nlmsghdr *next_nlh = NULL;
+	int len = nlmsg_len(nlh);
+
+	len = NLMSG_ALIGN(len);
 	if (*remaining > 0 &&
-		nlmsg_len(nlh) <= *remaining &&
-		nlmsg_len(nlh) >= (int) sizeof(struct nlmsghdr)) {
-		next_nlh = (struct nlmsghdr *) \
-			((char *) nlh + nlmsg_len(nlh));
-
-		if (next_nlh && nlmsg_len(nlh) <= *remaining) {
-			*remaining -= nlmsg_len(nlh);
-			next_nlh = (struct nlmsghdr *) \
-				((char *) nlh + nlmsg_len(nlh));
-		}
+	    len <= *remaining &&
+	    len >= (int) sizeof(struct nlmsghdr)) {
+		next_nlh = (struct nlmsghdr *)((char *)nlh + len);
+		*remaining -= len;
 	}
 
 	return next_nlh;
 }
 
+int nlmsg_datalen(const struct nlmsghdr *nlh)
+{
+	return nlh->nlmsg_len - NLMSG_HDRLEN;
+}
+
 /* Length of attributes data */
 int nlmsg_attrlen(const struct nlmsghdr *nlh, int hdrlen)
 {
-	return nlmsg_len(nlh) - NLMSG_HDRLEN - hdrlen;
+	return nlmsg_datalen(nlh) - NLMSG_ALIGN(hdrlen);
 }
 
 /* Length of netlink message */
@@ -145,9 +147,3 @@ int nlmsg_padlen(int payload)
 {
 	return NLMSG_ALIGN(payload) - payload;
 }
-
-int nlmsg_datalen(const struct nlmsghdr *nlh)
-{
-	return nlh->nlmsg_len - NLMSG_HDRLEN;
-}
-