Merge branch '35-Message-Output'

Signed-off-by: Gediminas Jakutis <gediminas@varciai.lt>

3 files changed, 101 insertions, 22 deletions

diff --git a/include/tlv.h b/include/tlv.h
index b52763a..75aa133 100644
--- a/include/tlv.h
+++ b/include/tlv.h
@@ -62,6 +62,8 @@ A few things to note:
 
 
 #include <errno.h>
+#include <stdint.h>
+#include <limits.h>
 
 #define E_TLV_OVERFLOW		(1 << 0)
 #define E_UNKNOWN_TYPE		(1 << 1)
@@ -79,8 +81,19 @@ extern "C" {
  */
 typedef unsigned int msg_idx_t;
 
+#if INT32_MAX == INT_MAX
+#define READ_ENUM(data) htonl((data))
+#else INT_MAX == INT16_MAX
+#define READ_ENUM(data) htons((data))
+#endif
+
 enum tlv_type {
 	/**
+	 * Explicitly states that this tlv is no longer valid for reading.
+	 */
+	INVALID = 0,
+
+	/**
 	 * NULL-terminated string. To be put in a queue to display on the
 	 * screen.
 	 */
@@ -132,8 +145,8 @@ enum tlv_type {
  * */
 struct tlv {
 	enum tlv_type type;
-	size_t length;
-	void *data;
+	uint16_t length;
+	const void *data;
 };
 
 /**
@@ -142,15 +155,22 @@ struct tlv {
  * Related functions return one tlv at a time.
  */
 struct tlv_parser {
-	char *data;
-	size_t offset;
-	size_t size;
+	const char *data;
+	uint16_t offset;
 };
 
 /**
+ * Initialises parser to begin parsing the data.
+ *
+ * Returns the first tlv which should be a meta-tlv. If it is not --- you got
+ * problems.
+ */
+int tlv_parser_init(struct tlv_parser *parser, char *data, struct tlv *ret);
+
+/**
  * Fills tlv structure to represent the next tlv in the packet.
  *
- * Returns END_OF_PACKET if all tlv's were read of E_TLV_OVERFLOW, if the last
+ * Returns END_OF_PACKET if all tlv's were read or E_TLV_OVERFLOW, if the last
  * tlv, according to its declared size should not fit in a packet.
  */
 int tlv_get(struct tlv_parser *parser, struct tlv *ret);
@@ -175,14 +195,14 @@ size_t tlv_raw_size(const struct tlv *t);
  * Pushes tlv to buffer as contiguous data. Check tlv size with tlv_raw_size
  * beforehand. If you don't do that and overflow --- tough tiddy.
  */
-int tlv_get_raw(struct tlv *t, char *buf);
+int tlv_get_raw(const struct tlv *t, char *buf);
 
 /**
  * Pushes data to tlv. Returns E_TLV_OVERFLOW if pushing data would cause the
  * final size to be greater than TLV_SZ_MAX_RAW. In case of such error the data is left
  * untouched.
  */
-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);
 
 /**
  * Pushes a sub-tlv into the packet. 't' can only be REGURAL, HEARTBEAT or
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;
diff --git a/src/device/main.ino b/src/device/main.ino
index 24cfb9c..4376346 100644
--- a/src/device/main.ino
+++ b/src/device/main.ino
@@ -103,7 +103,6 @@ void loop(void)
 		if (progstate.bytes_read > 0) {
 			progstate.parser.data = progstate.in_packet_buf;
 			progstate.parser.offset = 0;
-			progstate.parser.size = progstate.bytes_read;
 			/* Ignore errors for now. */
 			while (tlv_get(&progstate.parser, &progstate.crr_data) == 0) {
 				handle_tlv(&progstate.crr_data);