diff --git a/src/main/drivers/nvic.h b/src/main/drivers/nvic.h
index 5a501fbc5e..774bbc1c58 100644
--- a/src/main/drivers/nvic.h
+++ b/src/main/drivers/nvic.h
@@ -23,7 +23,10 @@
     // Use all available bits for priority and zero bits to sub-priority
     #ifdef USE_HAL_DRIVER
         #define NVIC_PRIORITY_GROUPING NVIC_PRIORITYGROUP_4
+
+        #define NVIC_BUILD_PRIORITY(base,sub) (((((base)<<(4-(7-(NVIC_PRIORITY_GROUPING))))|((sub)&(0x0f>>(7-(NVIC_PRIORITY_GROUPING)))))<<4)&0xf0)
+
     #else
         #define NVIC_PRIORITY_GROUPING NVIC_PriorityGroup_4
     #endif
-#endif
+#endif
\ No newline at end of file
diff --git a/src/main/vcp_hal/usbd_cdc_interface.c b/src/main/vcp_hal/usbd_cdc_interface.c
index 8fd3abc655..ef111e15e9 100644
--- a/src/main/vcp_hal/usbd_cdc_interface.c
+++ b/src/main/vcp_hal/usbd_cdc_interface.c
@@ -54,6 +54,8 @@
 #include "usbd_cdc_interface.h"
 #include "stdbool.h"
 #include "drivers/time.h"
+#include "drivers/nvic.h"
+#include "build/atomic.h"
 
 /* Private typedef -----------------------------------------------------------*/
 /* Private define ------------------------------------------------------------*/
@@ -366,14 +368,13 @@ uint32_t CDC_Receive_BytesAvailable(void)
 
 uint32_t CDC_Send_FreeBytes(void)
 {
-    /*
-        return the bytes free in the circular buffer
+        uint32_t freeBytes;
 
-        functionally equivalent to:
-        (APP_Rx_ptr_out > APP_Rx_ptr_in ? APP_Rx_ptr_out - APP_Rx_ptr_in : APP_RX_DATA_SIZE - APP_Rx_ptr_in + APP_Rx_ptr_in)
-        but without the impact of the condition check.
-    */
-    return ((UserTxBufPtrOut - UserTxBufPtrIn) + (-((int)(UserTxBufPtrOut <= UserTxBufPtrIn)) & APP_TX_DATA_SIZE)) - 1;
+    ATOMIC_BLOCK(NVIC_BUILD_PRIORITY(6, 0)) {
+        freeBytes = ((UserTxBufPtrOut - UserTxBufPtrIn) + (-((int)(UserTxBufPtrOut <= UserTxBufPtrIn)) & APP_TX_DATA_SIZE)) - 1;
+    }
+
+    return freeBytes;
 }
 
 /**