Protocol: reimplemented push_bytes and push_tlv_header.

Signed-off-by: Ramūnas Mažeikis <ramunasnezinomas@gmail.com>

2 files changed, 24 insertions, 5 deletions

diff --git a/src/common/protocol.c b/src/common/protocol.c
index 8fbd197..e71938c 100644
--- a/src/common/protocol.c
+++ b/src/common/protocol.c
@@ -60,17 +60,36 @@ int push_tlv(struct tlv_packet *packet, enum tlv_type type, char *data)
 
 void clear_data(struct tlv_packet *packet)
 {
-	return 0;
+	packet->offset = 0;
+	packet->type = 0;
+	memset(packet->data, 0, packet->size);
 }
 
-static int push_bytes(struct tlv_packet *packet, size_t size)
+static int push_bytes(struct tlv_packet *packet, char *data, size_t size)
 {
-	return 0;
+	int ret = 0;
+
+	if (packet->offset + size >= packet->size) {
+		ret = E_TLV_OVERFLOW;
+	} else {
+		memcpy(packet->data + packet->offset, data, size);
+		packet->offset += size;
+	}
 }
 
 static int push_tlv_header(struct tlv_packet *packet, enum tlv_type type, size_t size)
 {
-	return 0;
+	int ret = 0;
+
+	if (packet->offset + sizeof(type) + sizeof(size) >= packet->size) {
+		ret = E_TLV_OVERFLOW;
+	} else {
+		memcpy(packet->data + packet->size, type, sizeof(type));
+		packet->offset += sizeof(type);
+		memcpy(packet->data + packet->offset, size, sizeof(size));
+		packet->offset += sizeof(size);
+	}
+	return ret;
 }
 
 int get_tlv(struct tlv_parser *parser, struct tlv *ret)
diff --git a/src/common/protocol_private.h b/src/common/protocol_private.h
index 206046f..de086cd 100644
--- a/src/common/protocol_private.h
+++ b/src/common/protocol_private.h
@@ -3,7 +3,7 @@
 
 #include "protocol_private.h"
 
-int push_bytes(struct tlv_packet *packet, size_t size);
+int push_bytes(struct tlv_packet *packet, char *data, size_t size);
 int push_tlv_header(struct tlv_packet *packet, enum tlv_type type, size_t size);
 
 #endif /* PROTOCOL_PRIVATE_H */
\ No newline at end of file