1 files changed, 73 insertions, 13 deletions

diff --git a/src/common/tlv.c b/src/common/tlv.c
index c25f008..72d0399 100644..100755
--- a/src/common/tlv.c
+++ b/src/common/tlv.c
@@ -31,33 +31,93 @@
 #include "net.h"
 #include "utils.h"
 
+#ifdef unix
+#include <arpa/inet.h>
+#else
+#include <lwip/def.h>
+#endif
+
+struct tlv_header {
+	enum tlv_type type;
+	uint16_t size;
+};
+
+/**
+ * Returns tlv_size from packet
+ */
+uint16_t tlv_size(char *buf);
+
+/**
+ * Returns tlv_type from packet.
+ */
+enum tlv_type tlv_get_type(char *buf);
+
+/* Changes to be made:
+ *	* Trust tlv size from raw data
+ *	* Don't fail on 0 length. In that case data points to NULL
+ *	* tlv_parser should be opaque.
+ *	* Make a constructor that takes a user-supplied buffer.
+ */
 int tlv_get(struct tlv_parser *parser, struct tlv *ret)
 {
 	int retval = 0;
 
-	if (parser->offset + sizeof(ret->type) + sizeof(ret->length) >= parser->size) {
-		retval = E_TLV_OVERFLOW;
-	} else if (parser -> offset == parser->size) {
-		retval = END_OF_PACKET;
+	size_t packet_size;
+	size_t final_size;
+	struct tlv *crr_tlv = parser->data + parser->offset;
+
+	packet_size = tlv_size(parser->data);
+	final_size = parser->offset + crr_tlv->length + sizeof(struct tlv_header);
+	if (final_size >= packet_size) {
+		return retval = E_TLV_OVERFLOW;
 	} else {
-		memcpy(&ret->type, parser->data + parser->offset, sizeof(ret->type));
-		parser->size += sizeof(ret->type);
-		ret->length = memcpy(&ret->length, parser->data + parser->offset, sizeof(ret->length));
-		parser->offset += sizeof(ret->length);
-		if (parser->offset + ret->length >= parser->size) {
-			retval = E_TLV_OVERFLOW;
+		if (crr_tlv->length != 0) {
+			ret->data = parser->data + parser->offset + sizeof(struct tlv_header);
 		} else {
-			memcpy(ret->data, parser->data, ret->length);
+			ret->data = NULL;
+		}
+		ret->type = crr_tlv->type;
+		ret->length = crr_tlv->length;
+		parser->offset += sizeof(struct tlv_header) + ret->length;
+		if (final_size == packet_size) {
+			retval = END_OF_PACKET;
 		}
 	}
+
 	return retval;
 }
 
+int tlv_parser_init(struct tlv_parser *parser, char *buf, struct tlv *ret)
+{
+	/* Data points to the beggining of the buffer for size and
+	 * type acquisition purposes.
+	 * */
+	struct tlv_header *data = buf;
+	 /* It is known that the first tlv will be of the meta kind.
+	  * We can safely skip the header.
+	  * */
+	parser->data = buf + sizeof(struct tlv_header);
+	ret->type = READ_ENUM(data->type);
+	ret->length = ntohs(data->size);
+}
+
+uint16_t tlv_size(char *buf)
+{
+	struct tlv_header *data = buf;
+	return ntohs(data->size);
+}
+
+enum tlv_type tlv_get_type(char *buf)
+{
+	struct tlv_header *data = buf;
+	return READ_ENUM(data->type);
+}
+
 size_t tlv_data_size(struct tlv_parser *parser)
 {
 	size_t size;
 
-	if (parser->offset + sizeof(enum tlv_type) + sizeof(size_t) >= parser->size) {
+	if (parser->offset + sizeof(enum tlv_type) + sizeof(size_t) >= tlv_size(parser)) {
 		size = 0;
 	} else {
 		memcpy(&size, parser->data + parser->offset + sizeof(enum tlv_type), sizeof(size_t));
@@ -92,7 +152,7 @@ size_t tlv_raw_size(const struct tlv *t)
 	return sizeof(*t) + t->length;
 }
 
-int tlv_push_data(struct tlv *t, const char *data, size_t size)
+int tlv_push_data(struct tlv *t, const char *data, uint16_t size)
 {
 	int ret = 0;
 	size_t final_size = tlv_raw_size(t) + size;