diff --git a/drv/dma_mbuf.c b/drv/dma_mbuf.c
index dbbea46..dd7a3cc 100644
--- a/drv/dma_mbuf.c
+++ b/drv/dma_mbuf.c
@@ -18,6 +18,9 @@
 
 static void mbuf_callback(enum DmaIRQSource src, volatile void* param);
 
+#define byte_index (((uint8_t**)buffer->buffers)[buffer->buffer_index][buffer->buffer_size + 1])
+#define dma_byte_index (((uint8_t**)buffer->buffers)[buffer->dma_buffer_index][buffer->buffer_size + 1])
+
 
 //--local variables-------------------------------------------------------------
 
@@ -32,8 +35,7 @@ void dma_mbuf_configure(volatile struct DmaMultiBuffer* buffer, void** buffers,
 	buffer->buffers = buffers;
 	buffer->dest = dest;
 
-	buffer->buffer_size = buffer_size;
-	buffer->byte_index = 0;
+	buffer->buffer_size = buffer_size - 2;
 
 	buffer->buffer_nb = buffer_nb;
 	buffer->free_buffer_nb = buffer_nb - 1;
@@ -43,6 +45,12 @@ void dma_mbuf_configure(volatile struct DmaMultiBuffer* buffer, void** buffers,
 	buffer->dma = dma;
 	buffer->channel = channel;
 	buffer->config = DMA_CONFIG | priority;
+
+	for (uint8_t i=0; i<buffer->buffer_nb; ++i) {
+		for (uint16_t j=0; j<buffer->buffer_size + 2; ++j) {
+			((uint8_t**)buffer->buffers)[i][j] = 0;
+		}
+	}
 }
 
 uint32_t dma_mbuf_write_byte(volatile struct DmaMultiBuffer* buffer,
@@ -51,7 +59,8 @@ uint32_t dma_mbuf_write_byte(volatile struct DmaMultiBuffer* buffer,
 	dma_enter_critical(buffer->dma, buffer->channel);
 
 	//if the current buffer is full, we need to switch it with an empty one
-	if (buffer->byte_index >= buffer->buffer_size) {
+	volatile uint16_t index = byte_index;
+	if (byte_index >= buffer->buffer_size) {
 
 		//if all buffer full, give up
 		if (buffer->free_buffer_nb == 0) {
@@ -65,13 +74,14 @@ uint32_t dma_mbuf_write_byte(volatile struct DmaMultiBuffer* buffer,
 		}
 		--buffer->free_buffer_nb;
 
-		buffer->byte_index = 0;
+		byte_index = 0;
 	}
 
 	//write the byte
 	uint8_t** buffers = (uint8_t**)buffer->buffers;
-	buffers[buffer->buffer_index][buffer->byte_index] = byte;
-	++buffer->byte_index;
+	buffers[buffer->buffer_index][byte_index] = byte;
+	++byte_index;
+	++index;
 
 	dma_exit_critical(buffer->dma, buffer->channel);
 	return 0;
@@ -81,14 +91,14 @@ void dma_mbuf_refresh(volatile struct DmaMultiBuffer* buffer)
 {
 	//no more data to send, stop here
 	if (buffer->dma_buffer_index == buffer->buffer_index
-			&& buffer->byte_index == 0) {
+			&& byte_index == 0) {
 		return;
 	}
 
 	//else start a new transfer
 	dma_configure(buffer->dma, buffer->channel, buffer->config, buffer->dest,
 			buffer->buffers[buffer->dma_buffer_index],
-			buffer->byte_index, mbuf_callback, buffer);
+			dma_byte_index, mbuf_callback, buffer);
 
 	//if the newly transfering buffer was being written to, switch the current
 	//buffer. Since we just ended a transfer, the next buffer should be empty
@@ -98,7 +108,9 @@ void dma_mbuf_refresh(volatile struct DmaMultiBuffer* buffer)
 		if (buffer->buffer_index >= buffer->buffer_nb) {
 			buffer->buffer_index = 0;
 		}
-		buffer->byte_index = 0;
+		--buffer->free_buffer_nb;
+
+		byte_index = 0;
 	}
 
 	return;
diff --git a/drv/dma_mbuf.h b/drv/dma_mbuf.h
index b50b3af..f26a6d4 100644
--- a/drv/dma_mbuf.h
+++ b/drv/dma_mbuf.h
@@ -25,7 +25,6 @@ struct DmaMultiBuffer {
 	volatile void* dest;      //destination peripheral register
 
 	uint16_t buffer_size;     //size of a single buffer
-	uint16_t byte_index;      //index of the current byte in the current buffer
 
 	uint8_t buffer_nb;        //total number of buffers
 	uint8_t free_buffer_nb;   //number of buffers not currently used