From fe9a3d9a107ccc7ff50f830ca2a61889828e79b9 Mon Sep 17 00:00:00 2001
From: Jef Driesen <jef@libdivecomputer.org>
Date: Mon, 10 Aug 2020 12:10:29 +0200
Subject: [PATCH] Add a function to insert data anywhere in the buffer

The memory buffer already supported appending and prepending data, but
not inserting data anywhere in the buffer. That's exactly what the new
function does. The free space is still maintained at either the start or
the end of the buffer.
---
 include/libdivecomputer/buffer.h |  3 ++
 src/buffer.c                     | 66 ++++++++++++++++++++++++++++++++
 src/libdivecomputer.symbols      |  1 +
 3 files changed, 70 insertions(+)

diff --git a/include/libdivecomputer/buffer.h b/include/libdivecomputer/buffer.h
index 2af4822..8cbf27f 100644
--- a/include/libdivecomputer/buffer.h
+++ b/include/libdivecomputer/buffer.h
@@ -51,6 +51,9 @@ dc_buffer_append (dc_buffer_t *buffer, const unsigned char data[], size_t size);
 int
 dc_buffer_prepend (dc_buffer_t *buffer, const unsigned char data[], size_t size);
 
+int
+dc_buffer_insert (dc_buffer_t *buffer, size_t offset, const unsigned char data[], size_t size);
+
 int
 dc_buffer_slice (dc_buffer_t *buffer, size_t offset, size_t size);
 
diff --git a/src/buffer.c b/src/buffer.c
index 5b203fb..86cc8c4 100644
--- a/src/buffer.c
+++ b/src/buffer.c
@@ -231,6 +231,72 @@ dc_buffer_prepend (dc_buffer_t *buffer, const unsigned char data[], size_t size)
 }
 
 
+int
+dc_buffer_insert (dc_buffer_t *buffer, size_t offset, const unsigned char data[], size_t size)
+{
+	if (buffer == NULL)
+		return 0;
+
+	if (offset > buffer->size)
+		return 0;
+
+	size_t head = buffer->offset;
+	size_t tail = buffer->capacity - (buffer->offset + buffer->size);
+
+	unsigned char *ptr = buffer->data + buffer->offset;
+
+	if (size <= head) {
+		if (buffer->size)
+			memmove (ptr - size, ptr, offset);
+		buffer->offset -= size;
+	} else if (size <= tail) {
+		if (buffer->size)
+			memmove (ptr + offset + size, ptr + offset, buffer->size - offset);
+	} else if (size <= tail + head) {
+		size_t n = buffer->size + size;
+		size_t available = buffer->capacity - n;
+
+		size_t tmp_offset = head > tail ? available : 0;
+
+		unsigned char *tmp = buffer->data;
+
+		if (buffer->size) {
+			memmove (tmp + tmp_offset, ptr, offset);
+			memmove (tmp + tmp_offset + offset + size, ptr + offset, buffer->size - offset);
+		}
+
+		buffer->offset = tmp_offset;
+	} else {
+		size_t n = buffer->size + size;
+		size_t capacity = dc_buffer_expand_calc (buffer, n);
+		size_t available = capacity - n;
+
+		size_t tmp_offset = head > tail ? available : 0;
+
+		unsigned char *tmp = (unsigned char *) malloc (capacity);
+		if (tmp == NULL)
+			return 0;
+
+		if (buffer->size) {
+			memcpy (tmp + tmp_offset, ptr, offset);
+			memcpy (tmp + tmp_offset + offset + size, ptr + offset, buffer->size - offset);
+		}
+
+		free (buffer->data);
+		buffer->data = tmp;
+		buffer->capacity = capacity;
+		buffer->offset = tmp_offset;
+	}
+
+	if (size)
+		memcpy (buffer->data + buffer->offset + offset, data, size);
+
+	buffer->size += size;
+
+	return 1;
+}
+
+
 int
 dc_buffer_slice (dc_buffer_t *buffer, size_t offset, size_t size)
 {
diff --git a/src/libdivecomputer.symbols b/src/libdivecomputer.symbols
index 85caae5..800e217 100644
--- a/src/libdivecomputer.symbols
+++ b/src/libdivecomputer.symbols
@@ -8,6 +8,7 @@ dc_buffer_reserve
 dc_buffer_resize
 dc_buffer_append
 dc_buffer_prepend
+dc_buffer_insert
 dc_buffer_slice
 dc_buffer_get_size
 dc_buffer_get_data