[PATCH v2 03/11] addr: Add implementations for mpls_ntop and mpls_pton

[David Ahern]

Implementations of mpls_ntop and mpls_pton taken from iproute2.

Signed-off-by: David Ahern <dsahern at gmail.com>
---
 Makefile.am                          |   3 +-
 include/netlink-private/route/mpls.h |  15 +++++
 lib/mpls.c                           | 108 +++++++++++++++++++++++++++++++++++
 3 files changed, 125 insertions(+), 1 deletion(-)
 create mode 100644 include/netlink-private/route/mpls.h
 create mode 100644 lib/mpls.c

diff --git a/Makefile.am b/Makefile.am
index 2b817f4a185b..3fd6ba83ad6c 100644
--- a/Makefile.am
+++ b/Makefile.am
@@ -323,7 +323,8 @@ lib_libnl_3_la_SOURCES = \
 	lib/utils.c \
 	lib/version.c \
 	lib/hash.c \
-	lib/hashtable.c
+	lib/hashtable.c \
+	lib/mpls.c
 EXTRA_lib_libnl_3_la_DEPENDENCIES = \
 	libnl-3.sym
 lib_libnl_3_la_CPPFLAGS = \
diff --git a/include/netlink-private/route/mpls.h b/include/netlink-private/route/mpls.h
new file mode 100644
index 000000000000..502fd34ad4fb
--- /dev/null
+++ b/include/netlink-private/route/mpls.h
@@ -0,0 +1,15 @@
+#ifndef MPLS_H_
+#define MPLS_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+extern const char *mpls_ntop(int af, const void *addr, char *buf, size_t buflen);
+extern int mpls_pton(int af, const char *src, void *addr, size_t alen);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/lib/mpls.c b/lib/mpls.c
new file mode 100644
index 000000000000..cf79ad20ff6a
--- /dev/null
+++ b/lib/mpls.c
@@ -0,0 +1,108 @@
+/*
+ * Adapted from mpls_ntop and mpls_pton copied from iproute2,
+ * lib/mpls_ntop.c and lib/mpls_pton.c
+ */
+#include <errno.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <netinet/in.h>
+#include <netlink/netlink-compat.h>
+#include <netlink-private/route/mpls.h>
+#include <linux-private/linux/mpls.h>
+
+static const char *mpls_ntop1(const struct mpls_label *addr,
+			      char *buf, size_t buflen)
+{
+	size_t destlen = buflen;
+	char *dest = buf;
+	int count = 0;
+
+	while (1) {
+		uint32_t entry = ntohl(addr[count++].entry);
+		uint32_t label = (entry & MPLS_LS_LABEL_MASK) >> MPLS_LS_LABEL_SHIFT;
+		int len = snprintf(dest, destlen, "%u", label);
+
+		if (len >= destlen)
+			break;
+
+		/* Is this the end? */
+		if (entry & MPLS_LS_S_MASK)
+			return buf;
+
+		dest += len;
+		destlen -= len;
+		if (destlen) {
+			*dest = '/';
+			dest++;
+			destlen--;
+		}
+	}
+	errno = E2BIG;
+
+	return NULL;
+}
+
+const char *mpls_ntop(int af, const void *addr, char *buf, size_t buflen)
+{
+	switch(af) {
+	case AF_MPLS:
+		errno = 0;
+		return mpls_ntop1((struct mpls_label *)addr, buf, buflen);
+	}
+
+	errno = EINVAL;
+	return NULL;
+}
+
+static int mpls_pton1(const char *name, struct mpls_label *addr,
+		      unsigned int maxlabels)
+{
+	char *endp;
+	unsigned count;
+
+	for (count = 0; count < maxlabels; count++) {
+		unsigned long label;
+
+		label = strtoul(name, &endp, 0);
+		/* Fail when the label value is out or range */
+		if (label >= (1 << 20))
+			return 0;
+
+		if (endp == name) /* no digits */
+			return 0;
+
+		addr->entry = htonl(label << MPLS_LS_LABEL_SHIFT);
+		if (*endp == '\0') {
+			addr->entry |= htonl(1 << MPLS_LS_S_SHIFT);
+			return (count + 1) * sizeof(struct mpls_label);
+		}
+
+		/* Bad character in the address */
+		if (*endp != '/')
+			return 0;
+
+		name = endp + 1;
+		addr += 1;
+	}
+
+	/* The address was too long */
+	return 0;
+}
+
+int mpls_pton(int af, const char *src, void *addr, size_t alen)
+{
+	unsigned int maxlabels = alen / sizeof(struct mpls_label);
+	int err;
+
+	switch(af) {
+	case AF_MPLS:
+		errno = 0;
+		err = mpls_pton1(src, (struct mpls_label *)addr, maxlabels);
+		break;
+	default:
+		errno = EAFNOSUPPORT;
+		err = -1;
+	}
+
+	return err;
+}
-- 
2.1.4