diff --git a/firmware/application/usb_serial_io.c b/firmware/application/usb_serial_io.c
index aab6f4e9..3195cab7 100644
--- a/firmware/application/usb_serial_io.c
+++ b/firmware/application/usb_serial_io.c
@@ -58,19 +58,18 @@ void bulk_out_receive(void) {
 void serial_bulk_transfer_complete(void* user_data, unsigned int bytes_transferred) {
     (void)user_data;
 
-    chSysLockFromIsr();
-
     for (unsigned int i = 0; i < bytes_transferred; i++) {
         msg_t ret;
         do {
+            chSysLockFromIsr();
             ret = chIQPutI(&SUSBD1.iqueue, usbBulkBuffer[i]);
-            if (ret == Q_FULL)
-                chThdSleepMilliseconds(1);
+            chSysUnlockFromIsr();
+            if (ret == Q_FULL) {
+                chThdYield();
+            }
 
         } while (ret == Q_FULL);
     }
-
-    chSysUnlockFromIsr();
 }
 
 static void onotify(GenericQueue* qp) {
@@ -94,7 +93,7 @@ static void onotify(GenericQueue* qp) {
                 NULL);
 
             if (ret == -1)
-                chThdSleepMilliseconds(1);
+                chThdYield();
 
         } while (ret == -1);
         chSysLock();