diff --git a/src/main/drivers/serial_usb_vcp.c b/src/main/drivers/serial_usb_vcp.c
index 61b3979967..8360b0068f 100644
--- a/src/main/drivers/serial_usb_vcp.c
+++ b/src/main/drivers/serial_usb_vcp.c
@@ -77,13 +77,10 @@ static uint8_t usbVcpRead(serialPort_t *instance)
 
     uint8_t buf[1];
 
-    uint32_t rxed = 0;
-
-    while (rxed < 1) {
-        rxed += CDC_Receive_DATA((uint8_t*)buf + rxed, 1 - rxed);
+    while (true) {
+        if (CDC_Receive_DATA(buf, 1))
+            return buf[0];
     }
-
-    return buf[0];
 }
 
 static void usbVcpWriteBuf(serialPort_t *instance, void *data, int count)
@@ -95,8 +92,10 @@ static void usbVcpWriteBuf(serialPort_t *instance, void *data, int count)
     }
 
     uint32_t start = millis();
-    for (uint8_t *p = data; count > 0; ) {
-        uint32_t txed = CDC_Send_DATA(p, count);
+    uint8_t *p = data;
+    uint32_t txed = 0;
+    while (count > 0) {
+        txed = CDC_Send_DATA(p, count);
         count -= txed;
         p += txed;
 
@@ -119,14 +118,19 @@ static bool usbVcpFlush(vcpPort_t *port)
         return false;
     }
 
-    uint8_t txed = 0;
     uint32_t start = millis();
+    uint8_t *p = port->txBuf;
+    uint32_t txed = 0;
+    while (count > 0) {
+        txed = CDC_Send_DATA(p, count);
+        count -= txed;
+        p += txed;
 
-    do {
-        txed += CDC_Send_DATA(&port->txBuf[txed], count-txed);
-    } while (txed < count && (millis() - start < USB_TIMEOUT));
-
-    return txed == count;
+        if (millis() - start > USB_TIMEOUT) {
+            break;
+        }
+    }
+    return count == 0;
 }
 
 static void usbVcpWrite(serialPort_t *instance, uint8_t c)