diff --git a/AdaptiveBrightnessFirmware/Inc/uart_vcp.hpp b/AdaptiveBrightnessFirmware/Inc/uart_vcp.hpp
index c781ec4..3dbd4e4 100644
--- a/AdaptiveBrightnessFirmware/Inc/uart_vcp.hpp
+++ b/AdaptiveBrightnessFirmware/Inc/uart_vcp.hpp
@@ -1,8 +1,7 @@
 #pragma once
 
-#include <array>
-
 #include <cstdint>
+#include <cstring>
 
 #include "usbd_cdc.h"
 #include "usbd_def.h"
@@ -16,9 +15,15 @@ namespace uart::detail {
 
 template<size_t Size>
 struct RingBuffer {
-    size_t head;
-    size_t tail;
-    uint8_t data[Size];
+    volatile size_t head;
+    volatile size_t tail;
+    volatile uint8_t data[Size];
+};
+
+template<size_t Size>
+struct Buffer {
+    volatile size_t size;
+    volatile uint8_t data[Size];
 };
 
 template<class cfg>
@@ -36,80 +41,71 @@ class VirtualComPort {
 
     [[gnu::always_inline]] static bool rxByte(uint8_t& byte)
     {
-        if(m_rxBuffer.head == m_rxBuffer.tail)
+        if(m_receiving || m_rxBuffer.head == m_rxBuffer.tail)
             return false;
 
+        m_reading = true;
         const size_t newTail = (m_rxBuffer.tail + 1) % RX_BUFFER_SIZE;
         byte = m_rxBuffer.data[newTail];
         m_rxBuffer.tail = newTail;
+        m_reading = false;
         return true;
     }
 
     [[gnu::always_inline]] static void txByte(const uint8_t& byte)
     {
-        const size_t newHead = (m_txBuffer.head + 1) % TX_BUFFER_SIZE;
-
-        if(m_txBuffer.tail == newHead) {
+        if(m_txBuffer.size == TX_BUFFER_SIZE) {
             flushTx();
         }
-
-        m_txBuffer.data[newHead] = byte;
-        m_txBuffer.head = newHead;
+        m_txBuffer.data[m_txBuffer.size++] = byte;
     }
 
     [[gnu::always_inline]] static bool peek(uint8_t& byte)
     {
-        if(m_rxBuffer.head == m_rxBuffer.tail)
+        if(m_receiving || m_rxBuffer.head == m_rxBuffer.tail)
             return false;
 
+        m_reading = true;
         const size_t newTail = (m_rxBuffer.tail + 1) % RX_BUFFER_SIZE;
         byte = m_rxBuffer.data[newTail];
-
+        m_reading = false;
         return true;
     }
 
     [[gnu::always_inline]] static void flushTx()
     {
-        if(m_txBuffer.head == m_txBuffer.tail) {
-            return;
-        }
-
         constexpr auto usbReady = []() {
             USBD_CDC_HandleTypeDef* hcdc = static_cast<USBD_CDC_HandleTypeDef*>(hUsbDeviceFS.pClassData);
             return hcdc->TxState != 0;
         };
 
-        constexpr auto txPacket = [usbReady](volatile uint8_t* buffer, size_t length) {
-            USBD_CDC_SetTxBuffer(&hUsbDeviceFS, const_cast<uint8_t*>(buffer), length);
-            USBD_CDC_TransmitPacket(&hUsbDeviceFS);
-            while(!usbReady())
-                ;
-        };
+        while(usbReady())
+            ;
 
-        if(m_txBuffer.head > m_txBuffer.tail) {
-            txPacket(&m_txBuffer.data[m_txBuffer.tail], m_txBuffer.head - m_txBuffer.tail);
-        }
-        else {
-            txPacket(&m_txBuffer.data[m_txBuffer.tail], TX_BUFFER_SIZE - 1 - m_txBuffer.tail);
-            txPacket(m_txBuffer.data, m_txBuffer.head + 1);
-        }
+        std::memcpy(const_cast<uint8_t*>(m_usbAsyncTxBuffer.data), const_cast<uint8_t*>(m_txBuffer.data), m_txBuffer.size);
+        m_usbAsyncTxBuffer.size = m_txBuffer.size;
+        m_txBuffer.size = 0;
 
-        m_txBuffer.tail = m_txBuffer.head;
+        USBD_CDC_SetTxBuffer(&hUsbDeviceFS, const_cast<uint8_t*>(m_usbAsyncTxBuffer.data), m_usbAsyncTxBuffer.size);
+        USBD_CDC_TransmitPacket(&hUsbDeviceFS);
     }
 
   private:
-    static constexpr auto TX_BUFFER_SIZE = 512;
-    static constexpr auto RX_BUFFER_SIZE = 512;
+    static constexpr auto TX_BUFFER_SIZE = 64;
+    static constexpr auto RX_BUFFER_SIZE = 64;
 
-    static volatile RingBuffer<TX_BUFFER_SIZE> m_txBuffer;
-    static volatile RingBuffer<RX_BUFFER_SIZE> m_rxBuffer;
+    static Buffer<TX_BUFFER_SIZE> m_txBuffer;
+    static Buffer<TX_BUFFER_SIZE> m_usbAsyncTxBuffer;
 
-    static std::array<volatile uint8_t, 64> m_usbAsyncRxBuffer;
+    static RingBuffer<RX_BUFFER_SIZE> m_rxBuffer;
+    static Buffer<RX_BUFFER_SIZE> m_usbAsyncRxBuffer;
+    static volatile bool m_receiving;
+    static volatile bool m_reading;
 
     static int8_t CdcInit()
     {
         USBD_CDC_SetTxBuffer(&hUsbDeviceFS, const_cast<uint8_t*>(m_txBuffer.data), 0);
-        USBD_CDC_SetRxBuffer(&hUsbDeviceFS, const_cast<uint8_t*>(m_usbAsyncRxBuffer.data()));
+        USBD_CDC_SetRxBuffer(&hUsbDeviceFS, const_cast<uint8_t*>(m_usbAsyncRxBuffer.data));
         return USBD_OK;
     }
 
@@ -174,36 +170,51 @@ class VirtualComPort {
 
     static int8_t CdcReceive([[maybe_unused]] uint8_t* buf, uint32_t* length)
     {
-        USBD_CDC_ReceivePacket(&hUsbDeviceFS);
+        if(USBD_CDC_ReceivePacket(&hUsbDeviceFS) != USBD_OK)
+            return USBD_FAIL;
 
+        if(m_reading)
+            return USBD_FAIL;
+
+        m_receiving = true;
         for(uint32_t i = 0; i < *length; ++i) {
-            const auto byte = m_usbAsyncRxBuffer[i];
-            rxHandler(byte);
+            rxHandler(m_usbAsyncRxBuffer.data[i]);
         }
+        m_receiving = false;
         return USBD_OK;
     }
 
-    [[gnu::always_inline]] static inline void rxHandler(const uint8_t& data)
+    [[gnu::always_inline]] static inline void rxHandler(const volatile uint8_t& data)
     {
         const size_t newHead = (m_rxBuffer.head + 1) % RX_BUFFER_SIZE;
 
-        if(newHead != m_rxBuffer.tail) {
-            m_rxBuffer.data[newHead] = data;
-            m_rxBuffer.head = newHead;
-        }
-        else {
-            // TODO: Handle overflow
+        // Overflow, overwrites last element
+        if(newHead == m_rxBuffer.tail) {
+            const size_t newTail = (m_rxBuffer.tail + 1) % RX_BUFFER_SIZE;
+            m_rxBuffer.tail = newTail;
         }
+
+        m_rxBuffer.data[newHead] = data;
+        m_rxBuffer.head = newHead;
     }
 };
 
 template<class cfg>
-volatile RingBuffer<VirtualComPort<cfg>::TX_BUFFER_SIZE> VirtualComPort<cfg>::m_txBuffer = {0, 0, {0}};
+Buffer<VirtualComPort<cfg>::TX_BUFFER_SIZE> VirtualComPort<cfg>::m_txBuffer = {0, {0}};
 
 template<class cfg>
-volatile RingBuffer<VirtualComPort<cfg>::RX_BUFFER_SIZE> VirtualComPort<cfg>::m_rxBuffer = {0, 0, {0}};
+Buffer<VirtualComPort<cfg>::TX_BUFFER_SIZE> VirtualComPort<cfg>::m_usbAsyncTxBuffer = {0, {0}};
 
 template<class cfg>
-std::array<volatile uint8_t, 64> VirtualComPort<cfg>::m_usbAsyncRxBuffer = {0};
+RingBuffer<VirtualComPort<cfg>::RX_BUFFER_SIZE> VirtualComPort<cfg>::m_rxBuffer = {0, 0, {0}};
+
+template<class cfg>
+Buffer<VirtualComPort<cfg>::RX_BUFFER_SIZE> VirtualComPort<cfg>::m_usbAsyncRxBuffer = {0, {0}};
+
+template<class cfg>
+volatile bool VirtualComPort<cfg>::m_receiving = false;
+
+template<class cfg>
+volatile bool VirtualComPort<cfg>::m_reading = false;
 
 } // namespace uart::detail