1 files changed, 66 insertions, 11 deletions

diff --git a/src/common/tlv.c b/src/common/tlv.c
index 5b5ca5e..f02b714 100755
--- a/src/common/tlv.c
+++ b/src/common/tlv.c
@@ -27,32 +27,87 @@
 #include <stdlib.h>
 #include <string.h>
 #include <time.h>
+#include <arpa/inet.h>
 #include "tlv.h"
 #include "net.h"
 #include "utils.h"
 
+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->offset += 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 = data->type;
+	ret->length = data->size;
+}
+
+uint16_t tlv_size(char *buf)
+{
+	struct tlv_header *data = buf;
+	return htons(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;