msg: Pretty print generic netlink header in nl_msg_dump()

Signed-off-by: Thomas Graf <tgraf@suug.ch>

1 files changed, 97 insertions, 41 deletions

diff --git a/lib/msg.c b/lib/msg.c
index b07de63..dafee67 100644
--- a/lib/msg.c
+++ b/lib/msg.c
@@ -767,7 +767,7 @@ 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);
+	limit = 16 - (prefix * 2);
 	prefix_line(ofd, prefix);
 	fprintf(ofd, "    ");
 
@@ -777,7 +777,7 @@ static inline void dump_hex(FILE *ofd, char *start, int len, int prefix)
 		fprintf(ofd, "%02x ", v);
 		ascii[a++] = isprint(v) ? v : '.';
 
-		if (c == limit-1) {
+		if (++c >= limit) {
 			fprintf(ofd, "%s\n", ascii);
 			if (i < (len - 1)) {
 				prefix_line(ofd, prefix);
@@ -785,8 +785,7 @@ static inline void dump_hex(FILE *ofd, char *start, int len, int prefix)
 			}
 			a = c = 0;
 			memset(ascii, 0, sizeof(ascii));
-		} else
-			c++;
+		}
 	}
 
 	if (c != 0) {
@@ -816,12 +815,60 @@ static void print_hdr(FILE *ofd, struct nl_msg *msg)
 	} 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,
+	fprintf(ofd, "    .type = %d <%s>\n", nlh->nlmsg_type, buf);
+	fprintf(ofd, "    .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);
+	fprintf(ofd, "    .seq = %d\n", nlh->nlmsg_seq);
+	fprintf(ofd, "    .port = %d\n", nlh->nlmsg_pid);
+
+}
+
+static void print_genl_hdr(FILE *ofd, void *start)
+{
+	struct genlmsghdr *ghdr = start;
+
+	fprintf(ofd, "  [GENERIC NETLINK HEADER] %zu octets\n", GENL_HDRLEN);
+	fprintf(ofd, "    .cmd = %u\n", ghdr->cmd);
+	fprintf(ofd, "    .version = %u\n", ghdr->version);
+	fprintf(ofd, "    .unused = %#x\n", ghdr->reserved);
+}
+
+static void *print_genl_msg(struct nl_msg *msg, FILE *ofd, struct nlmsghdr *hdr,
+			    struct nl_cache_ops *ops, int *payloadlen)
+{
+	void *data = nlmsg_data(hdr);
+
+	if (*payloadlen < GENL_HDRLEN)
+		return data;
+
+	print_genl_hdr(ofd, data);
+
+	*payloadlen -= GENL_HDRLEN;
+	data += GENL_HDRLEN;
+
+	if (ops) {
+		int hdrsize = ops->co_hdrsize - GENL_HDRLEN;
+
+		if (hdrsize > 0) {
+			if (*payloadlen < hdrsize)
+				return data;
+
+			fprintf(ofd, "  [HEADER] %d octets\n", hdrsize);
+			dump_hex(ofd, data, hdrsize, 0);
+
+			*payloadlen -= hdrsize;
+			data += hdrsize;
+		}
+	}
+
+	return data;
+}
+
+static void dump_attr(FILE *ofd, struct nlattr *attr, int prefix)
+{
+	int len = nla_len(attr);
 
+	dump_hex(ofd, nla_data(attr), len, prefix);
 }
 
 static void dump_attrs(FILE *ofd, struct nlattr *attrs, int attrlen,
@@ -845,7 +892,7 @@ static void dump_attrs(FILE *ofd, struct nlattr *attrs, int attrlen,
 		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);
+			dump_attr(ofd, nla, prefix);
 
 		padlen = nla_padlen(alen);
 		if (padlen > 0) {
@@ -884,6 +931,42 @@ static void dump_error_msg(struct nl_msg *msg, FILE *ofd)
 	}
 }
 
+static void print_msg(struct nl_msg *msg, FILE *ofd, struct nlmsghdr *hdr)
+{
+	struct nl_cache_ops *ops;
+	int payloadlen = nlmsg_len(hdr);
+	int attrlen = 0;
+	void *data;
+
+	data = nlmsg_data(hdr);
+	ops = nl_cache_ops_associate_safe(nlmsg_get_proto(msg),
+					  hdr->nlmsg_type);
+	if (ops) {
+		attrlen = nlmsg_attrlen(hdr, ops->co_hdrsize);
+		payloadlen -= attrlen;
+	}
+
+	if (msg->nm_protocol == NETLINK_GENERIC)
+		data = print_genl_msg(msg, ofd, hdr, ops, &payloadlen);
+
+	if (payloadlen) {
+		fprintf(ofd, "  [PAYLOAD] %d octets\n", payloadlen);
+		dump_hex(ofd, data, 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);
+	}
+
+	if (ops)
+		nl_cache_ops_put(ops);
+}
+
 /**
  * Dump message in human readable format to file descriptor
  * @arg msg		Message to print
@@ -894,45 +977,18 @@ void nl_msg_dump(struct nl_msg *msg, FILE *ofd)
 	struct nlmsghdr *hdr = nlmsg_hdr(msg);
 	
 	fprintf(ofd, 
-	"--------------------------   BEGIN NETLINK MESSAGE "
-	"---------------------------\n");
+	"--------------------------   BEGIN NETLINK MESSAGE ---------------------------\n");
 
-	fprintf(ofd, "  [HEADER] %zu octets\n", sizeof(struct nlmsghdr));
+	fprintf(ofd, "  [NETLINK HEADER] %zu octets\n", sizeof(struct nlmsghdr));
 	print_hdr(ofd, msg);
 
 	if (hdr->nlmsg_type == NLMSG_ERROR)
 		dump_error_msg(msg, ofd);
-	else if (nlmsg_len(hdr) > 0) {
-		struct nl_cache_ops *ops;
-		int payloadlen = nlmsg_len(hdr);
-		int attrlen = 0;
-
-		ops = nl_cache_ops_associate_safe(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);
-		}
-
-		if (ops)
-			nl_cache_ops_put(ops);
-	}
+	else if (nlmsg_len(hdr) > 0)
+		print_msg(msg, ofd, hdr);
 
 	fprintf(ofd, 
-	"---------------------------  END NETLINK MESSAGE   "
-	"---------------------------\n");
+	"---------------------------  END NETLINK MESSAGE   ---------------------------\n");
 }
 
 /** @} */