Add TLV handling code

1 files changed, 76 insertions, 0 deletions

diff --git a/src/tlv.c b/src/tlv.c
new file mode 100644
index 0000000..22d04b3
--- /dev/null
+++ b/src/tlv.c
@@ -0,0 +1,76 @@
+/*
+  Copyright (c) 2013, Matthias Schiffer <mschiffer@universe-factory.net>
+  All rights reserved.
+
+  Redistribution and use in source and binary forms, with or without
+  modification, are permitted provided that the following conditions are met:
+
+    1. Redistributions of source code must retain the above copyright notice,
+       this list of conditions and the following disclaimer.
+    2. Redistributions in binary form must reproduce the above copyright notice,
+       this list of conditions and the following disclaimer in the documentation
+       and/or other materials provided with the distribution.
+
+  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+  OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+
+#include "tlv.h"
+#include "packet.h"
+
+#include <string.h>
+
+#include <arpa/inet.h>
+
+
+bool gp_babel_tlv_parse(const gp_babel_packet_t *packet, gp_babel_tlv_cb cb, void *arg) {
+	if (packet->version != htons(GP_BABEL_VERSION))
+		return false;
+
+	const uint8_t *data = packet->tlv + 2, *end = packet->tlv + ntohs(packet->len);
+
+	while (data <= end) {
+		if (data[-2] == TLV_PAD1) {
+			/* Pad1 */
+			data++;
+			continue;
+		}
+
+		if (data + data[-1] > end) {
+			/* warn */
+			break;
+		}
+
+		if (data[-2] != TLV_PADN)
+			cb(data[-2], data, data[-1], arg);
+
+		data += data[-1] + 2;
+	}
+
+	return true;
+}
+
+void* gp_babel_tlv_add(gp_babel_packet_t *packet, size_t max_len, gp_babel_tlv_type_t type, size_t len) {
+	size_t pktlen = ntohs(packet->len);
+
+	if (pktlen+len+2 > max_len)
+		return NULL;
+
+	uint8_t *data = packet->tlv+pktlen+2;
+
+	data[-2] = type;
+	data[-1] = len;
+
+	packet->len = htons(pktlen+len+2);
+
+	return data;
+}