fix wild bug

1 files changed, 17 insertions, 11 deletions

diff --git a/i2ctcp/i2ctcpv1.c b/i2ctcp/i2ctcpv1.c
index 36a5dbd..b406d8d 100644
--- a/i2ctcp/i2ctcpv1.c
+++ b/i2ctcp/i2ctcpv1.c
@@ -7,10 +7,6 @@
 #include "i2ctcpv1.h"
 
 int i2ctcp_read(i2ctcp_msg_t * target, const char * buf, size_t buf_sz) {
-	// a new reader is used per buffer block passed to this function
-	mpack_reader_t reader;
-	mpack_reader_init_data(&reader, buf, buf_sz);
-
 	// at start of message
 	if (target->_rdata == 0) {
 		// NOTE: The entire start of a message needs to be readable from the buffer
@@ -21,17 +17,27 @@ int i2ctcp_read(i2ctcp_msg_t * target, const char * buf, size_t buf_sz) {
 		// message. The check here is kind of optional.
 		if (buf_sz < 4) return -1;
 
+		// mpack reader is used for the first buffer block, as it contains the data
+		// size info
+		mpack_reader_t reader;
+		mpack_reader_init_data(&reader, buf, buf_sz);
+
 		target->addr = mpack_expect_u16(&reader);
 		target->length = target->_rdata = mpack_expect_bin(&reader);
 		target->data = (char *) malloc(target->length);
-	}
 
-	// continue reading chunks of target->data until the amount of bytes
-	// specified in target->length
-	size_t to_read = MIN(mpack_reader_remaining(&reader, NULL), target->_rdata);
-	char * data = target->data + target->length - target->_rdata;
-	mpack_read_bytes(&reader, data, to_read);
-	target->_rdata -= to_read;
+		// read remaining data in (header) packet
+		size_t to_read = mpack_reader_remaining(&reader, NULL);
+		mpack_read_bytes(&reader, target->data, to_read);
+		target->_rdata -= to_read;
+	} else {
+		// continue reading chunks of target->data until the amount of bytes
+		// specified in target->length
+		size_t to_read = MIN(buf_sz, target->_rdata);
+		char * data = target->data + target->length - target->_rdata;
+		memcpy(data, buf, to_read);
+		target->_rdata -= to_read;
+	}
 
 	// if rdata = 0, the message was completely read
 	return target->_rdata;