From ea89f59bebc6529e3270eebcc86825ee1eb7055e Mon Sep 17 00:00:00 2001
From: Guus Sliepen <guus@meshlink.io>
Date: Sun, 18 Oct 2015 13:44:43 +0200
Subject: [PATCH] Allow putting data in a buffer at an arbitrary offset.

---
 utcp.c | 38 +++++++++++++++++++++++++++++---------
 1 file changed, 29 insertions(+), 9 deletions(-)

diff --git a/utcp.c b/utcp.c
index ce05690..e6f6cf9 100644
--- a/utcp.c
+++ b/utcp.c
@@ -130,16 +130,30 @@ static int32_t seqdiff(uint32_t a, uint32_t b) {
 // TODO: convert to ringbuffers to avoid memmove() operations.
 
 // Store data into the buffer
-static ssize_t buffer_put(struct buffer *buf, const void *data, size_t len) {
+static ssize_t buffer_put_at(struct buffer *buf, size_t offset, const void *data, size_t len) {
 	if(buf->maxsize <= buf->used)
 		return 0;
-	if(len > buf->maxsize - buf->used)
-		len = buf->maxsize - buf->used;
-	if(len > buf->size - buf->used) {
+
+	debug("buffer_put_at %zu %zu %zu\n", buf->used, offset, len);
+
+	size_t required = offset + len;
+	if(required > buf->maxsize) {
+		if(offset >= buf->maxsize)
+			return 0;
+		abort();
+		len = buf->maxsize - offset;
+		required = buf->maxsize;
+	}
+
+	if(required > buf->size) {
 		size_t newsize = buf->size;
-		do {
-			newsize *= 2;
-		} while(newsize < buf->used + len);
+		if(!newsize) {
+			newsize = required;
+		} else {
+			do {
+				newsize *= 2;
+			} while(newsize < buf->used + len);
+		}
 		if(newsize > buf->maxsize)
 			newsize = buf->maxsize;
 		char *newdata = realloc(buf->data, newsize);
@@ -148,11 +162,17 @@ static ssize_t buffer_put(struct buffer *buf, const void *data, size_t len) {
 		buf->data = newdata;
 		buf->size = newsize;
 	}
-	memcpy(buf->data + buf->used, data, len);
-	buf->used += len;
+
+	memcpy(buf->data + offset, data, len);
+	if(required > buf->used)
+		buf->used = required;
 	return len;
 }
 
+static ssize_t buffer_put(struct buffer *buf, const void *data, size_t len) {
+	return buffer_put_at(buf, buf->used, data, len);
+}
+
 // Get data from the buffer. data can be NULL.
 static ssize_t buffer_get(struct buffer *buf, void *data, size_t len) {
 	if(len > buf->used)
-- 
2.39.5