Add TLV handling code

6 files changed, 309 insertions, 0 deletions

diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index e71a6a5..5301b14 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -2,6 +2,7 @@ include_directories(${GMRF_INCLUDE_DIR})
 
 add_library(mmss_proto_babel MODULE
   core.c
+  tlv.c
 )
 target_link_libraries(mmss_proto_babel ${MMSS_PROTOCOL_LIB})
 set_target_properties(mmss_proto_babel PROPERTIES LINK_FLAGS "-Wl,--undefined=mmss_protocol_info")
diff --git a/src/packet.h b/src/packet.h
new file mode 100644
index 0000000..a27b644
--- /dev/null
+++ b/src/packet.h
@@ -0,0 +1,43 @@
+/*
+  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.
+*/
+
+
+#ifndef _GMRF_PROTO_BABEL_PACKET_H_
+#define _GMRF_PROTO_BABEL_PACKET_H_
+
+#include "types.h"
+
+
+#define GP_BABEL_VERSION 0
+
+
+struct __attribute__((packed)) gp_babel_packet {
+	uint16_t version;
+	uint16_t len;
+	uint8_t tlv[];
+};
+
+
+#endif /* _GMRF_PROTO_BABEL_PACKET_H_ */
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;
+}
diff --git a/src/tlv.h b/src/tlv.h
new file mode 100644
index 0000000..d8a154e
--- /dev/null
+++ b/src/tlv.h
@@ -0,0 +1,63 @@
+/*
+  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.
+*/
+
+
+#ifndef _GMRF_PROTO_BABEL_TLV_H_
+#define _GMRF_PROTO_BABEL_TLV_H_
+
+#include "types.h"
+
+
+typedef enum gp_babel_tlv_type {
+	TLV_PAD1 = 0,
+	TLV_PADN,
+	TLV_ACK_REQ,
+	TLV_ACK,
+	TLV_HELLO,
+	TLV_IHU,
+	TLV_NODE_ID,
+	TLV_RESERVED,
+	TLV_UPDATE,
+	TLV_ANNOUNCE_REQ,
+	TLV_SEQNO_REQ,
+} gp_babel_tlv_type_t;
+
+typedef enum gp_babel_addr_enc {
+	ADDR_ENC_UNSPEC = 0,
+	ADDR_ENC_IPV4,
+	ADDR_ENC_IPV6,
+	ADDR_ENC_IPV6LL,
+	ADDR_ENC_GMRF,
+} gp_babel_addr_enc_t;
+
+
+typedef void (*gp_babel_tlv_cb)(gp_babel_tlv_type_t type, const void *data, size_t len, void *arg);
+
+
+bool gp_babel_tlv_parse(const gp_babel_packet_t *packet, gp_babel_tlv_cb cb, void *arg);
+void* gp_babel_tlv_add(gp_babel_packet_t *packet, size_t max_len, gp_babel_tlv_type_t type, size_t len);
+
+
+#endif /* _GMRF_PROTO_BABEL_TLV_H_ */
diff --git a/src/tlv_types.h b/src/tlv_types.h
new file mode 100644
index 0000000..10cbc92
--- /dev/null
+++ b/src/tlv_types.h
@@ -0,0 +1,87 @@
+/*
+  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.
+*/
+
+
+#ifndef _GMRF_PROTO_BABEL_TLV_TYPES_H_
+#define _GMRF_PROTO_BABEL_TLV_TYPES_H_
+
+#include "tlv.h"
+
+
+typedef struct __attribute__((packed)) gp_babel_tlv_ack_req {
+	uint16_t reserved;
+	uint16_t nonce;
+	uint16_t interval;
+} gp_babel_tlv_ack_req_t;
+
+typedef struct __attribute__((packed)) gp_babel_tlv_ack {
+	uint16_t nonce;
+} gp_babel_tlv_ack_t;
+
+typedef struct __attribute__((packed)) gp_babel_tlv_hello {
+	uint16_t reserved;
+	uint16_t seqno;
+	uint16_t interval;
+} gp_babel_tlv_hello_t;
+
+typedef struct __attribute__((packed)) gp_babel_tlv_ihu {
+	uint8_t ae;
+	uint8_t reserved;
+	uint16_t rxcost;
+	uint16_t interval;
+	uint8_t address[];
+} gp_babel_tlv_ihu_t;
+
+typedef struct __attribute__((packed)) gp_babel_tlv_node_id {
+	gp_babel_node_id_t id;
+} gp_babel_tlv_node_id_t;
+
+typedef struct __attribute__((packed)) gp_babel_tlv_update {
+	uint8_t flags;
+	uint8_t reserved;
+	uint16_t interval;
+	uint16_t seqno;
+	uint16_t metric;
+	uint16_t type;
+	uint16_t key;
+	uint8_t data[];
+} gp_babel_tlv_update_t;
+
+typedef struct __attribute__((packed)) gp_babel_tlv_announce_req {
+	uint8_t flags;
+	uint8_t reserved;
+	gp_babel_node_id_t node;
+	uint16_t type;
+	uint16_t key;
+} gp_babel_tlv_announce_req_t;
+
+typedef struct __attribute__((packed)) gp_babel_tlv_seqno_req {
+	uint16_t seqno;
+	gp_babel_node_id_t node;
+	uint16_t type;
+	uint16_t key;
+} gp_babel_tlv_seqno_req_t;
+
+#endif /* _GMRF_PROTO_BABEL_TLV_TYPES_H_ */
diff --git a/src/types.h b/src/types.h
new file mode 100644
index 0000000..5689d9a
--- /dev/null
+++ b/src/types.h
@@ -0,0 +1,39 @@
+/*
+  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.
+*/
+
+
+#ifndef _GMRF_PROTO_BABEL_TYPES_H_
+#define _GMRF_PROTO_BABEL_TYPES_H_
+
+#include <gmrf/gmrf.h>
+
+
+typedef struct __attribute__((packed)) gp_gabel_node_id {
+        uint8_t id[8];
+} gp_babel_node_id_t;
+
+typedef struct gp_babel_packet gp_babel_packet_t;
+
+#endif /* _GMRF_PROTO_BABEL_TYPES_H_ */