tlv/msgb: Be able to generate and parse TLV as used by LLDP

The Link Layer Discovery Protocol is using a Tag Length Value (TLV)
structure. What is different is that the tag and length do not stop
at byte boundaries.

This commit introduces methods to msgb and tlv to be able to create
such a tag/length header and a way to parse it using the tlv_parse
routine. When it comes to parsing we have an issue that the new
TLV_TYPE_T7L9V should not be mixed with the other types inside the
same tlv definition. This is due the bitshifting required to
identify the tag.

The code likely requires work on big-endian machines but so does
most of the other code as well.

7 files changed, 259 insertions, 2 deletions

diff --git a/.gitignore b/.gitignore
index 71b27f25..13a092d2 100644
--- a/.gitignore
+++ b/.gitignore
@@ -76,6 +76,7 @@ tests/loggingrb/loggingrb_test
 tests/ringbuf/ringbuf_test
 tests/strrb/strrb_test
 tests/vty/vty_test
+tests/tlv/tlv_test
 
 utils/osmo-arfcn
 utils/osmo-auc-gen
diff --git a/include/osmocom/gsm/tlv.h b/include/osmocom/gsm/tlv.h
index a70b0f9c..447d2de2 100644
--- a/include/osmocom/gsm/tlv.h
+++ b/include/osmocom/gsm/tlv.h
@@ -214,6 +214,19 @@ static inline uint8_t *msgb_l16tv_put(struct msgb *msg, uint16_t len, uint8_t ta
 	return buf + len;
 }
 
+/*! \brief put (append) a T7L9 field to \ref msgb */
+static inline uint8_t *msgb_t7l9_put(struct msgb *msg, uint8_t tag, uint16_t len)
+{
+	uint16_t tl;
+
+	tl = tag << 9;
+	tl |= len & 0x1FF;
+
+	msgb_put_u16(msg, tl);
+
+	return msg->tail;
+}
+
 /*! \brief put (append) a V field */
 static inline uint8_t *v_put(uint8_t *buf, uint8_t val)
 {
@@ -377,6 +390,7 @@ enum tlv_type {
 	TLV_TYPE_TvLV,		/*!< \brief tag, variable length, value */
 	TLV_TYPE_SINGLE_TV,	/*!< \brief tag and value (both 4 bit) in 1 byte */
 	TLV_TYPE_vTvLV_GAN,	/*!< \brief variable-length tag, variable-length length */
+	TLV_TYPE_T7L9V,		/*!< \brief 7 bit tag, 9 bit length, value */
 };
 
 /*! \brief Definition of a single IE (Information Element) */
diff --git a/src/gsm/tlv_parser.c b/src/gsm/tlv_parser.c
index 8cb2139d..852c1a71 100644
--- a/src/gsm/tlv_parser.c
+++ b/src/gsm/tlv_parser.c
@@ -51,6 +51,25 @@ int tlv_parse_one(uint8_t *o_tag, uint16_t *o_len, const uint8_t **o_val,
 		return 1;
 	}
 
+	/*
+	 * Check if this could be a T7L9V tag. Do it by
+	 * shifting it to the right once. Note that this
+	 * means that mixing T7L9V with other tags is not
+	 * possible.
+	 */
+	if (def->def[tag >> 1].type == TLV_TYPE_T7L9V)  {
+		*o_tag = tag >> 1;
+		if (buf + 1 > buf + buf_len)
+			return -1;
+		*o_len = (tag & 0x1) << 8;
+		*o_len |= *(buf + 1);
+		*o_val = buf + 2;
+		len = *o_len + 2;
+		if (len > buf_len)
+			return -2;
+		return len;
+	}
+
 	/* FIXME: use tables for knwon IEI */
 	switch (def->def[tag].type) {
 	case TLV_TYPE_T:
diff --git a/tests/Makefile.am b/tests/Makefile.am
index c6216d50..c53898d5 100644
--- a/tests/Makefile.am
+++ b/tests/Makefile.am
@@ -7,7 +7,8 @@ check_PROGRAMS = timer/timer_test sms/sms_test ussd/ussd_test		\
 		 gb/bssgp_fc_test gb/gprs_ns_test                       \
 		 logging/logging_test fr/fr_test	                \
 		 loggingrb/loggingrb_test strrb/strrb_test              \
-		 vty/vty_test comp128/comp128_test utils/utils_test
+		 vty/vty_test comp128/comp128_test utils/utils_test     \
+		 tlv/tlv_test
 
 if ENABLE_MSGFILE
 check_PROGRAMS += msgfile/msgfile_test
@@ -76,6 +77,9 @@ strrb_strrb_test_LDADD = $(top_builddir)/src/libosmocore.la
 vty_vty_test_SOURCES = vty/vty_test.c
 vty_vty_test_LDADD = $(top_builddir)/src/vty/libosmovty.la $(top_builddir)/src/libosmocore.la
 
+tlv_tlv_test_SOURCES = tlv/tlv_test.c
+tlv_tlv_test_LDADD = $(top_builddir)/src/libosmocore.la $(top_builddir)/src/gsm/libosmogsm.la
+
 
 # The `:;' works around a Bash 3.2 bug when the output is not writeable.
 $(srcdir)/package.m4: $(top_srcdir)/configure.ac
@@ -108,7 +112,7 @@ EXTRA_DIST = testsuite.at $(srcdir)/package.m4 $(TESTSUITE)		\
              fr/fr_test.ok loggingrb/logging_test.ok			\
              loggingrb/logging_test.err	strrb/strrb_test.ok		\
 	     vty/vty_test.ok comp128/comp128_test.ok			\
-	     utils/utils_test.ok
+	     utils/utils_test.ok tlv/tlv_test.ok
 
 DISTCLEANFILES = atconfig
 
diff --git a/tests/testsuite.at b/tests/testsuite.at
index 9124f251..54fc8f69 100644
--- a/tests/testsuite.at
+++ b/tests/testsuite.at
@@ -112,6 +112,12 @@ cat $abs_srcdir/vty/vty_test.ok > expout
 AT_CHECK([$abs_top_builddir/tests/vty/vty_test], [0], [expout], [ignore])
 AT_CLEANUP
 
+AT_SETUP([tlv])
+AT_KEYWORDS([tlv])
+cat $abs_srcdir/tlv/tlv_test.ok > expout
+AT_CHECK([$abs_top_builddir/tests/tlv/tlv_test], [0], [expout], [ignore])
+AT_CLEANUP
+
 AT_SETUP([gprs-ns])
 AT_KEYWORDS([gprs-ns])
 cat $abs_srcdir/gb/gprs_ns_test.ok > expout
diff --git a/tests/tlv/tlv_test.c b/tests/tlv/tlv_test.c
new file mode 100644
index 00000000..8d48e782
--- /dev/null
+++ b/tests/tlv/tlv_test.c
@@ -0,0 +1,205 @@
+/* Copyright (C) 2014 sysmocom - s.f.m.c. GmbH. All rights reserved
+ * Author: Daniel Laszlo Sitzer <dlsitzer@sysmocom.de>
+ *
+ * This program is iree software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ */
+
+#include <osmocom/gsm/tlv.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+
+static const struct tlv_definition dummy_tlvdef = {
+	.def = {
+		[1] = { TLV_TYPE_T7L9V },
+		[4] = { TLV_TYPE_TV },
+	},
+};
+
+static const struct tlv_definition lldp_tlvdef = {
+	.def = {
+		[1] = { TLV_TYPE_T7L9V },
+		[2] = { TLV_TYPE_T7L9V },
+		[3] = { TLV_TYPE_T7L9V },
+		[0] = { TLV_TYPE_T7L9V },
+	},
+};
+
+static void test_lldp_tlv(void)
+{
+	struct msgb *m;
+	struct tlv_parsed tp;
+	int nparsed;
+	uint8_t *res;
+
+	printf("%s\n", __func__);
+
+	m = msgb_alloc(128, "lldp_tlv");
+
+	res = msgb_t7l9_put(m, 1, 7);
+	OSMO_ASSERT(m->len == 2);
+	OSMO_ASSERT(m->data[0] == 2);
+	OSMO_ASSERT(m->data[1] == 7);
+	OSMO_ASSERT(res == &m->data[2]);
+
+	m->len += 7;
+	m->tail += 7;
+
+	msgb_tv_put(m, 4, 0x55);
+
+	nparsed = tlv_parse(&tp, &dummy_tlvdef, m->data, m->len, 0, 0);
+	OSMO_ASSERT(nparsed == 2);
+	OSMO_ASSERT(!TLVP_PRESENT(&tp, 0));
+	OSMO_ASSERT(TLVP_PRESENT(&tp, 1));
+	OSMO_ASSERT(TLVP_PRESENT(&tp, 4));
+	OSMO_ASSERT(TLVP_LEN(&tp, 1) == 7);
+	OSMO_ASSERT(TLVP_VAL(&tp, 1) == &m->data[2]);
+	OSMO_ASSERT(*TLVP_VAL(&tp, 4) == 0x55);
+
+	msgb_free(m);
+}
+
+static void test_lldp_tlv_col(void)
+{
+	struct msgb *m;
+	struct tlv_parsed tp;
+	int nparsed;
+	uint8_t *res;
+
+	printf("%s\n", __func__);
+
+	m = msgb_alloc(512, "lldp_tlv");
+
+	res = msgb_t7l9_put(m, 1, 256);
+	OSMO_ASSERT(m->len == 2);
+	OSMO_ASSERT(m->data[0] == 3);
+	OSMO_ASSERT(m->data[1] == 0);
+	OSMO_ASSERT(res == &m->data[2]);
+
+	m->len += 256;
+	m->tail += 256;
+
+	msgb_tv_put(m, 4, 0xAA);
+
+	nparsed = tlv_parse(&tp, &dummy_tlvdef, m->data, m->len, 0, 0);
+	OSMO_ASSERT(nparsed == 2);
+	OSMO_ASSERT(!TLVP_PRESENT(&tp, 0));
+	OSMO_ASSERT(TLVP_PRESENT(&tp, 1));
+	OSMO_ASSERT(TLVP_PRESENT(&tp, 4));
+	OSMO_ASSERT(TLVP_LEN(&tp, 1) == 256);
+	OSMO_ASSERT(TLVP_VAL(&tp, 1) == &m->data[2]);
+	OSMO_ASSERT(*TLVP_VAL(&tp, 4) == 0xAA);
+
+	msgb_free(m);
+}
+
+static struct msgb *create_lldp_frame(const uint8_t *mac)
+{
+	int i;
+	struct msgb *m;
+
+	m = msgb_alloc(512, "lldp_tlv");
+
+	/* Chassis ID TLV */
+	msgb_t7l9_put(m, 1, 7);
+	msgb_put_u8(m, 4); /* Chassis ID Subtype: MAC address */
+	for (i = 0; i < 6; ++i)
+		msgb_put_u8(m, mac[i]);
+
+	/* Port ID TLV */
+	msgb_t7l9_put(m, 2, 7);
+	msgb_put_u8(m, 3); /* Port ID Subtype: MAC address */
+	for (i = 0; i < 6; ++i)
+		msgb_put_u8(m, mac[i]);
+
+	/* TTL TLV */
+	msgb_t7l9_put(m, 3, 2);
+	msgb_put_u16(m, 127);
+
+	/* EOLLDPDU TLV */
+	msgb_t7l9_put(m, 0, 0);
+
+	return m;
+}
+
+static void test_lldp_tlv_lldpdu(void)
+{
+	struct msgb *m;
+	struct tlv_parsed tp;
+	int nparsed;
+	const uint8_t mac[] = {0xF0, 0xDE, 0xF1, 0x02, 0x43, 0x01};
+
+	printf("%s\n", __func__);
+
+	m = create_lldp_frame(mac);
+
+	nparsed = tlv_parse(&tp, &lldp_tlvdef, m->data, m->len, 0, 0);
+	OSMO_ASSERT(nparsed == 4);
+	OSMO_ASSERT(TLVP_PRESENT(&tp, 1));
+	OSMO_ASSERT(TLVP_PRESENT(&tp, 2));
+	OSMO_ASSERT(TLVP_PRESENT(&tp, 3));
+	OSMO_ASSERT(TLVP_PRESENT(&tp, 0));
+	OSMO_ASSERT(TLVP_LEN(&tp, 1) == 1 + ARRAY_SIZE(mac));
+	OSMO_ASSERT(TLVP_LEN(&tp, 2) == 1 + ARRAY_SIZE(mac));
+	OSMO_ASSERT(TLVP_LEN(&tp, 3) == 2);
+	OSMO_ASSERT(TLVP_LEN(&tp, 0) == 0);
+	OSMO_ASSERT(*TLVP_VAL(&tp, 1) == 4);
+	OSMO_ASSERT(*TLVP_VAL(&tp, 2) == 3);
+	OSMO_ASSERT(memcmp(TLVP_VAL(&tp, 1)+1, mac, ARRAY_SIZE(mac)) == 0);
+	OSMO_ASSERT(memcmp(TLVP_VAL(&tp, 2)+1, mac, ARRAY_SIZE(mac)) == 0);
+	OSMO_ASSERT(*TLVP_VAL(&tp, 3) == 0);
+	OSMO_ASSERT(*(TLVP_VAL(&tp, 3)+1) == 127);
+
+	msgb_free(m);
+}
+
+static void test_lldp_truncated(void)
+{
+	struct msgb *m;
+	const uint8_t mac[] = {0xF0, 0xDE, 0xF1, 0x02, 0x43, 0x01};
+	size_t i;
+	int success = 0;
+
+	printf("%s\n", __func__);
+
+	m = create_lldp_frame(mac);
+
+	/* test truncated messages and expect the parse failure */
+	for (i = m->len; i > 0; --i) {
+		int nparsed;
+		struct tlv_parsed tp;
+
+		nparsed = tlv_parse(&tp, &lldp_tlvdef, m->data, i, 0, 0);
+		if (nparsed >= 0) {
+			printf("Success on %zu with %d\n", i, nparsed);
+			success += 1;
+		}
+	}
+
+	/* if we truncate a frame enough it becomes parable again */
+	OSMO_ASSERT(success == 4);
+	msgb_free(m);	
+}
+
+int main(int argc, char *argv[])
+{
+	test_lldp_tlv();
+	test_lldp_tlv_col();
+	test_lldp_tlv_lldpdu();
+	test_lldp_truncated();
+
+	return EXIT_SUCCESS;
+}
diff --git a/tests/tlv/tlv_test.ok b/tests/tlv/tlv_test.ok
new file mode 100644
index 00000000..e0384727
--- /dev/null
+++ b/tests/tlv/tlv_test.ok
@@ -0,0 +1,8 @@
+test_lldp_tlv
+test_lldp_tlv_col
+test_lldp_tlv_lldpdu
+test_lldp_truncated
+Success on 24 with 4
+Success on 22 with 3
+Success on 18 with 2
+Success on 9 with 1