diff --git a/src/platforms/common/aux_serial.c b/src/platforms/common/aux_serial.c
index 2644378..6d36bd5 100644
--- a/src/platforms/common/aux_serial.c
+++ b/src/platforms/common/aux_serial.c
@@ -273,12 +273,11 @@ void aux_serial_send(const size_t len)
 	}
 }
 
-size_t aux_serial_update_receive_buffer_fullness(void)
+void aux_serial_update_receive_buffer_fullness(void)
 {
 	aux_serial_receive_write_index =
 		AUX_UART_BUFFER_SIZE - dma_get_number_of_data(USBUSART_DMA_BUS, USBUSART_DMA_RX_CHAN);
 	aux_serial_receive_write_index %= AUX_UART_BUFFER_SIZE;
-	return aux_serial_receive_write_index;
 }
 
 bool aux_serial_receive_has_data(void)
@@ -300,6 +299,38 @@ void aux_serial_drain_receive_buffer(void)
 	usbuart_set_led_state(RX_LED_ACT, false);
 }
 
+uint32_t aux_serial_stage_buffer(const char *const fifo, const uint32_t fifo_begin, const uint32_t fifo_end)
+{
+	/*
+	 * To avoid the need of sending ZLP don't transmit full packet.
+	 * Also reserve space for copy function overrun.
+	 */
+	char packet[CDCACM_PACKET_SIZE - 1];
+	uint32_t packet_len = 0;
+	for (uint32_t fifo_index = fifo_begin; fifo_index != fifo_end && packet_len < CDCACM_PACKET_SIZE - 1U;
+		 fifo_index %= AUX_UART_BUFFER_SIZE)
+		packet[packet_len++] = fifo[fifo_index++];
+
+	if (packet_len) {
+		const uint16_t written = usbd_ep_write_packet(usbdev, CDCACM_UART_ENDPOINT, packet, packet_len);
+		return (fifo_begin + written) % AUX_UART_BUFFER_SIZE;
+	}
+	return fifo_begin;
+}
+
+#ifdef ENABLE_DEBUG
+void aux_serial_stage_debug_buffer(void)
+{
+	usb_dbg_out = aux_serial_stage_buffer(usb_dbg_buf, usb_dbg_out, usb_dbg_in);
+}
+#endif
+
+void aux_serial_stage_receive_buffer(void)
+{
+	aux_serial_receive_read_index = aux_serial_stage_buffer(
+		aux_serial_receive_buffer, aux_serial_receive_read_index, aux_serial_receive_write_index);
+}
+
 static void aux_serial_receive_isr(const uint32_t usart, const uint8_t dma_irq)
 {
 	nvic_disable_irq(dma_irq);
diff --git a/src/platforms/common/aux_serial.h b/src/platforms/common/aux_serial.h
index be0d508..c25705f 100644
--- a/src/platforms/common/aux_serial.h
+++ b/src/platforms/common/aux_serial.h
@@ -38,8 +38,12 @@ size_t aux_serial_transmit_buffer_fullness(void);
 /* Send a number of bytes staged into the current transmit bufer */
 void aux_serial_send(size_t len);
 
-size_t aux_serial_update_receive_buffer_fullness(void);
+void aux_serial_update_receive_buffer_fullness(void);
 bool aux_serial_receive_has_data(void);
 void aux_serial_drain_receive_buffer(void);
+#ifdef ENABLE_DEBUG
+void aux_serial_stage_debug_buffer(void);
+#endif
+void aux_serial_stage_receive_buffer(void);
 
 #endif /*AUX_SERIAL_H*/
diff --git a/src/platforms/common/usb_serial.c b/src/platforms/common/usb_serial.c
index 7ba242d..a2a1e53 100644
--- a/src/platforms/common/usb_serial.c
+++ b/src/platforms/common/usb_serial.c
@@ -220,18 +220,6 @@ size_t debug_uart_write(const char *buf, const size_t len)
 	return len;
 }
 
-/*
- * Copy data from fifo into continuous buffer. Return copied length.
- */
-static uint32_t copy_from_fifo(char *dst, const char *src, uint32_t start, uint32_t end, uint32_t len, uint32_t fifo_sz)
-{
-	uint32_t out_len = 0;
-	for (uint32_t buf_out = start; buf_out != end && out_len < len; buf_out %= fifo_sz)
-		dst[out_len++] = src[buf_out++];
-
-	return out_len;
-}
-
 /*
  * Runs deferred processing for AUX serial RX, draining RX FIFO by sending
  * characters to host PC via the debug serial interface. Allowed to write to FIFO OUT pointer.
@@ -239,8 +227,7 @@ static uint32_t copy_from_fifo(char *dst, const char *src, uint32_t start, uint3
 static void debug_uart_send_aux_serial_data(void)
 {
 	aux_serial_receive_complete = false;
-	/* Calculate writing position in the FIFO */
-	const size_t aux_serial_receive_write_index = aux_serial_update_receive_buffer_fullness();
+	aux_serial_update_receive_buffer_fullness();
 
 	/* Forcibly empty fifo if no USB endpoint.
 	 * If fifo empty, nothing further to do. */
@@ -248,30 +235,10 @@ static void debug_uart_send_aux_serial_data(void)
 		aux_serial_drain_receive_buffer();
 		aux_serial_receive_complete = true;
 	} else {
-		/* To avoid the need of sending ZLP don't transmit full packet.
-		 * Also reserve space for copy function overrun.
-		 */
-		char packet_buf[CDCACM_PACKET_SIZE - 1 + sizeof(uint64_t)];
-		uint32_t packet_size;
-
 #ifdef ENABLE_DEBUG
-		/* Copy data from DEBUG FIFO into local usb packet buffer */
-		packet_size = copy_from_fifo(packet_buf, usb_dbg_buf, usb_dbg_out, usb_dbg_in, CDCACM_PACKET_SIZE - 1, AUX_UART_BUFFER_SIZE);
-		/* Send if buffer not empty */
-		if (packet_size)
-		{
-			const uint16_t written = usbd_ep_write_packet(usbdev, CDCACM_UART_ENDPOINT, packet_buf, packet_size);
-			usb_dbg_out = (usb_dbg_out + written) % AUX_UART_BUFFER_SIZE;
-			return;
-		}
+		aux_serial_stage_debug_buffer();
 #endif
-
-		/* Copy data from uart RX FIFO into local usb packet buffer */
-		packet_size = copy_from_fifo(packet_buf, aux_serial_receive_buffer, aux_serial_receive_read_index, aux_serial_receive_write_index, CDCACM_PACKET_SIZE - 1, AUX_UART_BUFFER_SIZE);
-
-		/* Advance fifo out pointer by amount written */
-		const uint16_t written = usbd_ep_write_packet(usbdev, CDCACM_UART_ENDPOINT, packet_buf, packet_size);
-		aux_serial_receive_read_index = (aux_serial_receive_read_index + written) % AUX_UART_BUFFER_SIZE;
+		aux_serial_stage_receive_buffer();
 	}
 }