diff --git a/os/hal/src/serial_usb.c b/os/hal/src/serial_usb.c
index b17669c04..430218edb 100644
--- a/os/hal/src/serial_usb.c
+++ b/os/hal/src/serial_usb.c
@@ -124,7 +124,7 @@ static void inotify(GenericQueue *qp) {
     if (n != USB_ENDPOINT_BUSY) {
       sdup->iqueue.q_rdptr = sdup->iqueue.q_buffer;
       chSemSetCounterI(&sdup->iqueue.q_sem, n);
-//      chIOAddFlagsI(sdup, IO_INPUT_AVAILABLE);
+      chIOAddFlagsI(sdup, IO_INPUT_AVAILABLE);
     }
   }
 }
@@ -143,7 +143,7 @@ static void onotify(GenericQueue *qp) {
   if (n != USB_ENDPOINT_BUSY) {
     sdup->oqueue.q_wrptr = sdup->oqueue.q_buffer;
     chSemSetCounterI(&sdup->oqueue.q_sem, SERIAL_USB_BUFFERS_SIZE);
-//    chIOAddFlagsI(sdup, IO_OUTPUT_EMPTY);
+    chIOAddFlagsI(sdup, IO_OUTPUT_EMPTY);
   }
 }
 
diff --git a/testhal/STM32/USB_CDC/main.c b/testhal/STM32/USB_CDC/main.c
index 0c2a3ac9d..6af497542 100644
--- a/testhal/STM32/USB_CDC/main.c
+++ b/testhal/STM32/USB_CDC/main.c
@@ -367,7 +367,8 @@ static msg_t Thread2(void *arg) {
       uint8_t buffer[0x40];
       size_t n = chIQReadTimeout(&sdup->iqueue, buffer,
                                  sizeof(buffer), TIME_IMMEDIATE);
-      chOQWriteTimeout(&sdup->oqueue, buffer, n, TIME_IMMEDIATE);
+      if (n > 0)
+        chOQWriteTimeout(&sdup->oqueue, buffer, n, TIME_IMMEDIATE);
     }
   }
 }