Protocol: reimplemented push_bytes and push_tlv_header.

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

1 files changed, 23 insertions, 4 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)