diff --git a/lib/main/STM32_USB_Device_Library/Class/cdc/src/usbd_cdc_core.c b/lib/main/STM32_USB_Device_Library/Class/cdc/src/usbd_cdc_core.c
index 2533cdb877..a82ef08a2f 100644
--- a/lib/main/STM32_USB_Device_Library/Class/cdc/src/usbd_cdc_core.c
+++ b/lib/main/STM32_USB_Device_Library/Class/cdc/src/usbd_cdc_core.c
@@ -184,8 +184,8 @@ __ALIGN_BEGIN uint8_t APP_Rx_Buffer   [APP_RX_DATA_SIZE] __ALIGN_END ;
 #endif /* USB_OTG_HS_INTERNAL_DMA_ENABLED */
 __ALIGN_BEGIN uint8_t CmdBuff[CDC_CMD_PACKET_SZE] __ALIGN_END ;
 
-uint32_t APP_Rx_ptr_in  = 0;
-uint32_t APP_Rx_ptr_out = 0;
+volatile uint32_t APP_Rx_ptr_in  = 0;
+volatile uint32_t APP_Rx_ptr_out = 0;
 uint32_t APP_Rx_length  = 0;
 
 uint8_t  USB_Tx_State = USB_CDC_IDLE;
@@ -482,8 +482,8 @@ uint8_t  usbd_cdc_Init (void  *pdev,
 uint8_t  usbd_cdc_DeInit (void  *pdev, 
                                  uint8_t cfgidx)
 {
-	  (void)pdev;
-	  (void)cfgidx;
+      (void)pdev;
+      (void)cfgidx;
   /* Open EP IN */
   DCD_EP_Close(pdev,
               CDC_IN_EP);
@@ -594,7 +594,7 @@ uint8_t  usbd_cdc_Setup (void  *pdev,
   */
 uint8_t  usbd_cdc_EP0_RxReady (void  *pdev)
 { 
-	(void)pdev;
+    (void)pdev;
   if (cdcCmd != NO_CMD)
   {
     /* Process the data */
@@ -617,60 +617,45 @@ uint8_t  usbd_cdc_EP0_RxReady (void  *pdev)
   */
 uint8_t  usbd_cdc_DataIn (void *pdev, uint8_t epnum)
 {
-	(void)pdev;
-	(void)epnum;
-  uint16_t USB_Tx_ptr;
-  uint16_t USB_Tx_length;
-  
-  if (USB_Tx_State == USB_CDC_BUSY)
-  {
-    if (APP_Rx_length == 0) 
+    (void)pdev;
+    (void)epnum;
+    uint16_t USB_Tx_length;
+
+    if (USB_Tx_State == USB_CDC_BUSY)
     {
-      USB_Tx_State = USB_CDC_IDLE;
-    }
-    else 
-    {
-      if (APP_Rx_length > CDC_DATA_IN_PACKET_SIZE){
-        USB_Tx_ptr = APP_Rx_ptr_out;
-        USB_Tx_length = CDC_DATA_IN_PACKET_SIZE;
-        
-        APP_Rx_ptr_out += CDC_DATA_IN_PACKET_SIZE;
-        APP_Rx_length -= CDC_DATA_IN_PACKET_SIZE;    
-      }
-      else 
-      {
-        USB_Tx_ptr = APP_Rx_ptr_out;
-        USB_Tx_length = APP_Rx_length;
-        
-        APP_Rx_ptr_out += APP_Rx_length;
-        APP_Rx_length = 0;
-        if(USB_Tx_length == CDC_DATA_IN_PACKET_SIZE)
+        if (APP_Rx_length == 0)
         {
-          USB_Tx_State = USB_CDC_ZLP;
+            USB_Tx_State = USB_CDC_IDLE;
+        } else {
+            if (APP_Rx_length > CDC_DATA_IN_PACKET_SIZE) {
+                USB_Tx_length = CDC_DATA_IN_PACKET_SIZE;
+            } else {
+                USB_Tx_length = APP_Rx_length;
+
+                if (USB_Tx_length == CDC_DATA_IN_PACKET_SIZE) {
+                    USB_Tx_State = USB_CDC_ZLP;
+                }
+            }
+
+            /* Prepare the available data buffer to be sent on IN endpoint */
+            DCD_EP_Tx(pdev, CDC_IN_EP, (uint8_t*)&APP_Rx_Buffer[APP_Rx_ptr_out], USB_Tx_length);
+
+            // Advance the out pointer
+            APP_Rx_ptr_out = (APP_Rx_ptr_out + USB_Tx_length) % APP_RX_DATA_SIZE;
+            APP_Rx_length -= USB_Tx_length;
+
+            return USBD_OK;
         }
-      }
-      
-      /* Prepare the available data buffer to be sent on IN endpoint */
-      DCD_EP_Tx (pdev,
-                 CDC_IN_EP,
-                 (uint8_t*)&APP_Rx_Buffer[USB_Tx_ptr],
-                 USB_Tx_length);
-      return USBD_OK;
     }
-  }  
-  
-  /* Avoid any asynchronous transfer during ZLP */
-  if (USB_Tx_State == USB_CDC_ZLP)
-  {
-    /*Send ZLP to indicate the end of the current transfer */
-    DCD_EP_Tx (pdev,
-               CDC_IN_EP,
-               NULL,
-               0);
-    
-    USB_Tx_State = USB_CDC_IDLE;
-  }
-  return USBD_OK;
+
+    /* Avoid any asynchronous transfer during ZLP */
+    if (USB_Tx_State == USB_CDC_ZLP) {
+        /*Send ZLP to indicate the end of the current transfer */
+        DCD_EP_Tx(pdev, CDC_IN_EP, NULL, 0);
+
+        USB_Tx_State = USB_CDC_IDLE;
+    }
+    return USBD_OK;
 }
 
 /**
@@ -731,67 +716,49 @@ uint8_t  usbd_cdc_SOF (void *pdev)
   */
 static void Handle_USBAsynchXfer (void *pdev)
 {
-  uint16_t USB_Tx_ptr;
-  uint16_t USB_Tx_length;
-  
-  if(USB_Tx_State == USB_CDC_IDLE)
-  {
-    if (APP_Rx_ptr_out == APP_RX_DATA_SIZE)
-    {
-      APP_Rx_ptr_out = 0;
-    }
-    
-    if(APP_Rx_ptr_out == APP_Rx_ptr_in) 
-    {
-      USB_Tx_State = USB_CDC_IDLE; 
-      return;
-    }
-    
-    if(APP_Rx_ptr_out > APP_Rx_ptr_in) /* rollback */
-    { 
-      APP_Rx_length = APP_RX_DATA_SIZE - APP_Rx_ptr_out;
-      
-    }
-    else 
-    {
-      APP_Rx_length = APP_Rx_ptr_in - APP_Rx_ptr_out;
-      
-    }
+    uint16_t USB_Tx_length;
+
+    if (USB_Tx_State == USB_CDC_IDLE) {
+        if (APP_Rx_ptr_out == APP_Rx_ptr_in) {
+            // Ring buffer is empty
+            return;
+        }
+
+        if (APP_Rx_ptr_out > APP_Rx_ptr_in) {
+            // Transfer bytes up to the end of the ring buffer
+            APP_Rx_length = APP_RX_DATA_SIZE - APP_Rx_ptr_out;
+        } else {
+            // Transfer all bytes in ring buffer
+            APP_Rx_length = APP_Rx_ptr_in - APP_Rx_ptr_out;
+        }
+
 #ifdef USB_OTG_HS_INTERNAL_DMA_ENABLED
-    APP_Rx_length &= ~0x03;
+        // Only transfer whole 32 bit words of data
+        APP_Rx_length &= ~0x03;
 #endif /* USB_OTG_HS_INTERNAL_DMA_ENABLED */
+
+        if (APP_Rx_length > CDC_DATA_IN_PACKET_SIZE) {
+            USB_Tx_length = CDC_DATA_IN_PACKET_SIZE;
+
+            USB_Tx_State = USB_CDC_BUSY;
+        } else {
+            USB_Tx_length = APP_Rx_length;
+
+            if (USB_Tx_length == CDC_DATA_IN_PACKET_SIZE) {
+                USB_Tx_State = USB_CDC_ZLP;
+            } else {
+                USB_Tx_State = USB_CDC_BUSY;
+            }
+        }
     
-    if (APP_Rx_length > CDC_DATA_IN_PACKET_SIZE)
-    {
-      USB_Tx_ptr = APP_Rx_ptr_out;
-      USB_Tx_length = CDC_DATA_IN_PACKET_SIZE;
-      
-      APP_Rx_ptr_out += CDC_DATA_IN_PACKET_SIZE;	
-      APP_Rx_length -= CDC_DATA_IN_PACKET_SIZE;
-      USB_Tx_State = USB_CDC_BUSY;
+        DCD_EP_Tx (pdev,
+                   CDC_IN_EP,
+                   (uint8_t*)&APP_Rx_Buffer[APP_Rx_ptr_out],
+                   USB_Tx_length);
+
+        APP_Rx_ptr_out = (APP_Rx_ptr_out + USB_Tx_length) % APP_RX_DATA_SIZE;
+        APP_Rx_length -= USB_Tx_length;
     }
-    else
-    {
-      USB_Tx_ptr = APP_Rx_ptr_out;
-      USB_Tx_length = APP_Rx_length;
-      
-      APP_Rx_ptr_out += APP_Rx_length;
-      APP_Rx_length = 0;
-      if(USB_Tx_length == CDC_DATA_IN_PACKET_SIZE)
-      {
-        USB_Tx_State = USB_CDC_ZLP;
-      }
-      else
-      {
-        USB_Tx_State = USB_CDC_BUSY;
-      }
-    }
-    
-    DCD_EP_Tx (pdev,
-               CDC_IN_EP,
-               (uint8_t*)&APP_Rx_Buffer[USB_Tx_ptr],
-               USB_Tx_length);
-  }  
 }
 
 /**
@@ -803,7 +770,7 @@ static void Handle_USBAsynchXfer (void *pdev)
   */
 static uint8_t  *USBD_cdc_GetCfgDesc (uint8_t speed, uint16_t *length)
 {
-	(void)speed;
+    (void)speed;
   *length = sizeof (usbd_cdc_CfgDesc);
   return usbd_cdc_CfgDesc;
 }
diff --git a/src/main/cli/cli.c b/src/main/cli/cli.c
index 8d6b5c94ad..5298958ae7 100644
--- a/src/main/cli/cli.c
+++ b/src/main/cli/cli.c
@@ -185,9 +185,10 @@ static serialPort_t *cliPort = NULL;
 #endif
 #define CLI_OUT_BUFFER_SIZE 64
 
+static bufWriter_t cliWriterDesc;
 static bufWriter_t *cliWriter = NULL;
 static bufWriter_t *cliErrorWriter = NULL;
-static uint8_t cliWriteBuffer[sizeof(*cliWriter) + CLI_OUT_BUFFER_SIZE];
+static uint8_t cliWriteBuffer[CLI_OUT_BUFFER_SIZE];
 
 static char cliBuffer[CLI_IN_BUFFER_SIZE];
 static uint32_t bufferIndex = 0;
@@ -6873,8 +6874,8 @@ void cliEnter(serialPort_t *serialPort)
     cliMode = true;
     cliPort = serialPort;
     setPrintfSerialPort(cliPort);
-    cliWriter = bufWriterInit(cliWriteBuffer, sizeof(cliWriteBuffer), (bufWrite_t)serialWriteBufShim, serialPort);
-    cliErrorWriter = cliWriter;
+    bufWriterInit(&cliWriterDesc, cliWriteBuffer, sizeof(cliWriteBuffer), (bufWrite_t)serialWriteBufShim, serialPort);
+    cliErrorWriter = cliWriter = &cliWriterDesc;
 
 #ifndef MINIMAL_CLI
     cliPrintLine("\r\nEntering CLI Mode, type 'exit' to return, or 'help'");
diff --git a/src/main/drivers/buf_writer.c b/src/main/drivers/buf_writer.c
index a6c89fc7bc..3d2798f6f7 100644
--- a/src/main/drivers/buf_writer.c
+++ b/src/main/drivers/buf_writer.c
@@ -24,15 +24,14 @@
 
 #include "buf_writer.h"
 
-bufWriter_t *bufWriterInit(uint8_t *b, int total_size, bufWrite_t writer, void *arg)
+void bufWriterInit(bufWriter_t *b, uint8_t *data, int size, bufWrite_t writer, void *arg)
 {
     bufWriter_t *buf = (bufWriter_t *)b;
     buf->writer = writer;
     buf->arg = arg;
     buf->at = 0;
-    buf->capacity = total_size - sizeof(*buf);
-
-    return buf;
+    buf->capacity = size;
+    buf->data = data;
 }
 
 void bufWriterAppend(bufWriter_t *b, uint8_t ch)
diff --git a/src/main/drivers/buf_writer.h b/src/main/drivers/buf_writer.h
index 99fc223b82..b877a03cc6 100644
--- a/src/main/drivers/buf_writer.h
+++ b/src/main/drivers/buf_writer.h
@@ -26,16 +26,12 @@ typedef void (*bufWrite_t)(void *arg, void *data, int count);
 typedef struct bufWriter_s {
     bufWrite_t writer;
     void *arg;
+    uint8_t *data;
     uint8_t capacity;
     uint8_t at;
-    uint8_t data[];
 } bufWriter_t;
 
 // Initialise a block of memory as a buffered writer.
-//
-// b should be sizeof(bufWriter_t) + the number of bytes to buffer.
-// total_size should be the total size of b.
-//
-bufWriter_t *bufWriterInit(uint8_t *b, int total_size, bufWrite_t writer, void *p);
+void bufWriterInit(bufWriter_t *b, uint8_t *data, int size, bufWrite_t writer, void *p);
 void bufWriterAppend(bufWriter_t *b, uint8_t ch);
 void bufWriterFlush(bufWriter_t *b);
diff --git a/src/main/vcpf4/usbd_cdc_vcp.c b/src/main/vcpf4/usbd_cdc_vcp.c
index 8f3f56f82f..67f63b143b 100644
--- a/src/main/vcpf4/usbd_cdc_vcp.c
+++ b/src/main/vcpf4/usbd_cdc_vcp.c
@@ -23,9 +23,11 @@
 
 #include "platform.h"
 
+#include "build/atomic.h"
+
 #include "usbd_cdc_vcp.h"
 #include "stm32f4xx_conf.h"
-#include "stdbool.h"
+#include "drivers/nvic.h"
 #include "drivers/time.h"
 
 #ifdef USB_OTG_HS_INTERNAL_DMA_ENABLED
@@ -47,7 +49,7 @@ extern volatile uint32_t APP_Rx_ptr_out;
 /* Increment this buffer position or roll it back to
  start address when writing received data
  in the buffer APP_Rx_Buffer. */
-extern uint32_t APP_Rx_ptr_in;
+extern volatile uint32_t APP_Rx_ptr_in;
 
 /*
     APP TX is the circular buffer for data that is transmitted from the APP (host)
@@ -187,10 +189,7 @@ uint32_t CDC_Send_DATA(const uint8_t *ptrBuffer, uint32_t sendLength)
 
 uint32_t CDC_Send_FreeBytes(void)
 {
-    /*
-        return the bytes free in the circular buffer
-    */
-    return (APP_Rx_ptr_out > APP_Rx_ptr_in ? APP_Rx_ptr_out - APP_Rx_ptr_in : APP_RX_DATA_SIZE + APP_Rx_ptr_out - APP_Rx_ptr_in);
+    return APP_RX_DATA_SIZE - CDC_Receive_BytesAvailable();
 }
 
 /**
@@ -207,8 +206,11 @@ static uint16_t VCP_DataTx(const uint8_t* Buf, uint32_t Len)
         could just check for: USB_CDC_ZLP, but better to be safe
         and wait for any existing transmission to complete.
     */
+    while (USB_Tx_State != 0);
+
     for (uint32_t i = 0; i < Len; i++) {
-        while ((APP_Rx_ptr_in + 1) % APP_RX_DATA_SIZE == APP_Rx_ptr_out || APP_Rx_ptr_out == APP_RX_DATA_SIZE || USB_Tx_State != 0) {
+        // Stall if the ring buffer is full
+        while (((APP_Rx_ptr_in + 1) % APP_RX_DATA_SIZE) == APP_Rx_ptr_out) {
             delay(1);
         }
 
@@ -230,7 +232,7 @@ uint32_t CDC_Receive_DATA(uint8_t* recvBuf, uint32_t len)
 {
     uint32_t count = 0;
 
-    while (APP_Tx_ptr_out != APP_Tx_ptr_in && count < len) {
+    while (APP_Tx_ptr_out != APP_Tx_ptr_in && (count < len)) {
         recvBuf[count] = APP_Tx_Buffer[APP_Tx_ptr_out];
         APP_Tx_ptr_out = (APP_Tx_ptr_out + 1) % APP_TX_DATA_SIZE;
         count++;
@@ -241,7 +243,7 @@ uint32_t CDC_Receive_DATA(uint8_t* recvBuf, uint32_t len)
 uint32_t CDC_Receive_BytesAvailable(void)
 {
     /* return the bytes available in the receive circular buffer */
-    return APP_Tx_ptr_out > APP_Tx_ptr_in ? APP_TX_DATA_SIZE - APP_Tx_ptr_out + APP_Tx_ptr_in : APP_Tx_ptr_in - APP_Tx_ptr_out;
+    return (APP_Tx_ptr_in + APP_TX_DATA_SIZE - APP_Tx_ptr_out) % APP_TX_DATA_SIZE;
 }
 
 /**
diff --git a/src/test/unit/cli_unittest.cc b/src/test/unit/cli_unittest.cc
index 28cab55139..fc7ea5891f 100644
--- a/src/test/unit/cli_unittest.cc
+++ b/src/test/unit/cli_unittest.cc
@@ -327,7 +327,7 @@ uint8_t serialRead(serialPort_t *){return 0;}
 
 void bufWriterAppend(bufWriter_t *, uint8_t ch){ printf("%c", ch); }
 void serialWriteBufShim(void *, const uint8_t *, int) {}
-bufWriter_t *bufWriterInit(uint8_t *, int, bufWrite_t, void *) {return NULL;}
+void bufWriterInit(bufWriter_t *, uint8_t *, int, bufWrite_t, void *) { }
 void setArmingDisabled(armingDisableFlags_e) {}
 
 void waitForSerialPortToFinishTransmitting(serialPort_t *) {}