diff --git a/os/hal/include/hal_buffers.h b/os/hal/include/hal_buffers.h
index e5d5fa7b9..d537477f2 100644
--- a/os/hal/include/hal_buffers.h
+++ b/os/hal/include/hal_buffers.h
@@ -101,6 +101,12 @@ struct io_buffers_queue {
    * @brief   Boundary for R/W sequential access.
    */
   uint8_t               *top;
+  /**
+   * @brief   Buffer is being accessed.
+   * @details This flag indicates that the current buffer is being read or
+   *          written by a long, preemptable operation.
+   */
+  bool                  accessed;
   /**
    * @brief   Data notification callback.
    */
@@ -205,7 +211,7 @@ typedef io_buffers_queue_t output_buffers_queue_t;
  *
  * @iclass
  */
-#define obqIsEmptyI(oqp) ((bool)(((obqp)->bwrptr == (obqp)->brdptr) &&       \
+#define obqIsEmptyI(oqp) ((bool)(((obqp)->bwrptr == (obqp)->brdptr) &&      \
                                  ((obqp)->bcounter != 0U)))
 
 /**
@@ -254,6 +260,8 @@ extern "C" {
                       systime_t timeout);
   size_t obqWriteTimeout(output_buffers_queue_t *obqp, const uint8_t *bp,
                          size_t n, systime_t timeout);
+  bool obqTryFlushI(output_buffers_queue_t *obqp);
+  void obqFlush(output_buffers_queue_t *obqp);
 #ifdef __cplusplus
 }
 #endif
diff --git a/os/hal/include/serial_usb.h b/os/hal/include/serial_usb.h
index fca8d56ea..0ca4ba06d 100644
--- a/os/hal/include/serial_usb.h
+++ b/os/hal/include/serial_usb.h
@@ -260,6 +260,7 @@ extern "C" {
   void sduDisconnectI(SerialUSBDriver *sdup);
   void sduConfigureHookI(SerialUSBDriver *sdup);
   bool sduRequestsHook(USBDriver *usbp);
+  void sduSOFHookI(SerialUSBDriver *sdup);
   void sduDataTransmitted(USBDriver *usbp, usbep_t ep);
   void sduDataReceived(USBDriver *usbp, usbep_t ep);
   void sduInterruptTransmitted(USBDriver *usbp, usbep_t ep);
diff --git a/os/hal/lib/streams/chprintf.c b/os/hal/lib/streams/chprintf.c
index 99fea6a09..b2c4478da 100644
--- a/os/hal/lib/streams/chprintf.c
+++ b/os/hal/lib/streams/chprintf.c
@@ -131,7 +131,7 @@ int chvprintf(BaseSequentialStream *chp, const char *fmt, va_list ap) {
   char tmpbuf[MAX_FILLER + 1];
 #endif
 
-  while (TRUE) {
+  while (true) {
     c = *fmt++;
     if (c == 0)
       return n;
diff --git a/os/hal/src/hal_buffers.c b/os/hal/src/hal_buffers.c
index 8a468d9fe..a6787e1e9 100644
--- a/os/hal/src/hal_buffers.c
+++ b/os/hal/src/hal_buffers.c
@@ -74,6 +74,7 @@ void ibqObjectInit(input_buffers_queue_t *ibqp, uint8_t *bp,
   ibqp->buffers  = bp;
   ibqp->ptr      = NULL;
   ibqp->top      = NULL;
+  ibqp->accessed = false;
   ibqp->notify   = infy;
   ibqp->link     = link;
 }
@@ -181,6 +182,8 @@ msg_t ibqGetFullBufferTimeoutS(input_buffers_queue_t *ibqp,
     }
   }
 
+  osalDbgAssert(!ibqIsEmptyI(ibqp), "still empty");
+
   /* Setting up the "current" buffer and its boundary.*/
   ibqp->ptr = ibqp->brdptr + sizeof (size_t);
   ibqp->top = ibqp->ptr + *((size_t *)ibqp->brdptr);
@@ -238,13 +241,13 @@ void ibqReleaseEmptyBufferI(input_buffers_queue_t *ibqp) {
 msg_t ibqGetTimeout(input_buffers_queue_t *ibqp, systime_t timeout) {
   msg_t msg;
 
-  chSysLock();
+  osalSysLock();
 
   /* This condition indicates that a new buffer must be acquired.*/
   if (ibqp->ptr == NULL) {
     msg = ibqGetFullBufferTimeoutS(ibqp, timeout);
     if (msg != MSG_OK) {
-      chSysUnlock();
+      osalSysUnlock();
       return msg;
     }
   }
@@ -259,7 +262,7 @@ msg_t ibqGetTimeout(input_buffers_queue_t *ibqp, systime_t timeout) {
     ibqReleaseEmptyBufferI(ibqp);
   }
 
-  chSysUnlock();
+  osalSysUnlock();
 
   return msg;
 }
@@ -293,6 +296,11 @@ size_t ibqReadTimeout(input_buffers_queue_t *ibqp, uint8_t *bp,
 
   osalSysLock();
 
+  osalDbgAssert(!ibqp->accessed, "queue is being accessed");
+
+  /* Marking the queue as being written by a long operation.*/
+  ibqp->accessed = true;
+
   /* Time window for the whole operation.*/
   deadline = osalOsGetSystemTimeX() + timeout;
 
@@ -307,12 +315,14 @@ size_t ibqReadTimeout(input_buffers_queue_t *ibqp, uint8_t *bp,
          in this case next becomes a very high number because the system
          time is an unsigned type.*/
       if (next_timeout > timeout) {
+        ibqp->accessed = false;
         osalSysUnlock();
         return MSG_TIMEOUT;
       }
 
       msg_t msg = ibqGetFullBufferTimeoutS(ibqp, next_timeout);
       if (msg != MSG_OK) {
+        ibqp->accessed = false;
         osalSysUnlock();
         return msg;
       }
@@ -340,6 +350,7 @@ size_t ibqReadTimeout(input_buffers_queue_t *ibqp, uint8_t *bp,
       ibqReleaseEmptyBufferI(ibqp);
     }
   }
+  ibqp->accessed = false;
   osalSysUnlock();
 
   return r;
@@ -373,6 +384,7 @@ void obqObjectInit(output_buffers_queue_t *obqp, uint8_t *bp,
   obqp->buffers  = bp;
   obqp->ptr      = NULL;
   obqp->top      = NULL;
+  obqp->accessed = false;
   obqp->notify   = onfy;
   obqp->link     = link;
 }
@@ -444,6 +456,9 @@ void obqReleaseEmptyBufferI(output_buffers_queue_t *obqp) {
   if (obqp->brdptr >= obqp->btop) {
     obqp->brdptr = obqp->buffers;
   }
+
+  /* Waking up one waiting thread, if any.*/
+  osalThreadDequeueNextI(&obqp->waiting, MSG_OK);
 }
 
 /**
@@ -471,16 +486,18 @@ msg_t obqGetEmptyBufferTimeoutS(output_buffers_queue_t *obqp,
 
   osalDbgCheckClassS();
 
-  while (obqIsEmptyI(obqp)) {
+  while (obqIsFullI(obqp)) {
     msg_t msg = osalThreadEnqueueTimeoutS(&obqp->waiting, timeout);
     if (msg < MSG_OK) {
       return msg;
     }
   }
 
+  osalDbgAssert(!obqIsFullI(obqp), "still full");
+
   /* Setting up the "current" buffer and its boundary.*/
-  obqp->ptr = obqp->brdptr + sizeof (size_t);
-  obqp->top = obqp->ptr + *((size_t *)obqp->brdptr);
+  obqp->ptr = obqp->bwrptr + sizeof (size_t);
+  obqp->top = obqp->bwrptr + obqp->bsize;
 
   return MSG_OK;
 }
@@ -546,19 +563,18 @@ msg_t obqPutTimeout(output_buffers_queue_t *obqp, uint8_t b,
   if (obqp->ptr == NULL) {
     msg = obqGetEmptyBufferTimeoutS(obqp, timeout);
     if (msg != MSG_OK) {
-      chSysUnlock();
+      osalSysUnlock();
       return msg;
     }
   }
 
   /* Writing the byte to the buffer.*/
-  obqp->bcounter--;
-  *obqp->bwrptr++ = b;
+  *obqp->ptr++ = b;
 
   /* If the current buffer has been fully written then it is posted as
      full in the queue.*/
   if (obqp->ptr >= obqp->top) {
-    obqPostFullBufferI(obqp, obqp->bsize);
+    obqPostFullBufferI(obqp, obqp->bsize - sizeof (size_t));
   }
 
   osalSysUnlock();
@@ -595,6 +611,11 @@ size_t obqWriteTimeout(output_buffers_queue_t *obqp, const uint8_t *bp,
 
   osalSysLock();
 
+  osalDbgAssert(!obqp->accessed, "queue is being accessed");
+
+  /* Marking the queue as being written by a long operation.*/
+  obqp->accessed = true;
+
   /* Time window for the whole operation.*/
   deadline = osalOsGetSystemTimeX() + timeout;
 
@@ -609,12 +630,14 @@ size_t obqWriteTimeout(output_buffers_queue_t *obqp, const uint8_t *bp,
          in this case next becomes a very high number because the system
          time is an unsigned type.*/
       if (next_timeout > timeout) {
+        obqp->accessed = false;
         osalSysUnlock();
         return MSG_TIMEOUT;
       }
 
       msg_t msg = obqGetEmptyBufferTimeoutS(obqp, next_timeout);
       if (msg != MSG_OK) {
+        obqp->accessed = false;
         osalSysUnlock();
         return msg;
       }
@@ -639,12 +662,80 @@ size_t obqWriteTimeout(output_buffers_queue_t *obqp, const uint8_t *bp,
 
     /* Has the current data buffer been finished? if so then release it.*/
     if (obqp->ptr >= obqp->top) {
-      obqPostFullBufferI(obqp, obqp->bsize);
+      obqPostFullBufferI(obqp, obqp->bsize - sizeof (size_t));
     }
   }
+  obqp->accessed = false;
   osalSysUnlock();
 
   return r;
 }
 
+/**
+ * @brief   Flushes the current, partially filled, buffer to the queue.
+ * @note    The notification callback is not invoked because the function
+ *          is meant to be called from ISR context. An operation status is
+ *          returned instead.
+ *
+ * @param[in] obqp      pointer to the @p output_buffers_queue_t object
+ * @return              The operation status.
+ * @retval false        if no new filled buffer has been posted to the queue.
+ * @retval true         if a new filled buffer has been posted to the queue.
+ *
+ * @iclass
+ */
+bool obqTryFlushI(output_buffers_queue_t *obqp) {
+
+  osalDbgCheckClassI();
+
+  /* If queue is empty and there is a buffer partially filled and
+     it is not being written.*/
+  if (obqIsEmptyI(obqp) && (obqp->ptr != NULL) && !obqp->accessed) {
+    size_t size = (size_t)(obqp->ptr - (obqp->bwrptr + sizeof (size_t)));
+
+    if (size > 0U) {
+
+      /* Writing size field in the buffer.*/
+      *((size_t *)obqp->bwrptr) = size;
+
+      /* Posting the buffer in the queue.*/
+      obqp->bcounter--;
+      obqp->bwrptr += obqp->bsize;
+      if (obqp->bwrptr >= obqp->btop) {
+        obqp->bwrptr = obqp->buffers;
+      }
+
+      /* No "current" buffer.*/
+      obqp->ptr = NULL;
+
+      return true;
+    }
+  }
+  return false;
+}
+
+/**
+ * @brief   Flushes the current, partially filled, buffer to the queue.
+ *
+ * @param[in] obqp      pointer to the @p output_buffers_queue_t object
+ *
+ * @api
+ */
+void obqFlush(output_buffers_queue_t *obqp) {
+
+  osalSysLock();
+
+  osalDbgAssert(!obqp->accessed, "queue is being accessed");
+
+  /* If there is a buffer partially filled and not being written.*/
+  if (obqp->ptr != NULL) {
+    size_t size = (size_t)(obqp->ptr - obqp->bwrptr);
+
+    if (size > 0U) {
+      obqPostFullBufferI(obqp, size);
+    }
+  }
+
+  osalSysUnlock();
+}
 /** @} */
diff --git a/os/hal/src/serial_usb.c b/os/hal/src/serial_usb.c
index b22599be2..edf322b1f 100644
--- a/os/hal/src/serial_usb.c
+++ b/os/hal/src/serial_usb.c
@@ -321,8 +321,6 @@ void sduConfigureHookI(SerialUSBDriver *sdup) {
 
   usbPrepareReceive(sdup->config->usbp, sdup->config->bulk_out,
                     buf, SERIAL_USB_BUFFERS_SIZE);
-//  usbPrepareQueuedReceive(usbp, sdup->config->bulk_out, &sdup->iqueue,
-//                          usbp->epc[sdup->config->bulk_out]->out_maxsize);
   (void) usbStartReceiveI(sdup->config->usbp, sdup->config->bulk_out);
 }
 
@@ -362,13 +360,54 @@ bool sduRequestsHook(USBDriver *usbp) {
   return false;
 }
 
+/**
+ * @brief   SOF handler.
+ * @details The SOF interrupt is used for automatic flushing of incomplete
+ *          buffers pending in the output queue.
+ *
+ * @param[in] sdup      pointer to a @p SerialUSBDriver object
+ *
+ * @iclass
+ */
+void sduSOFHookI(SerialUSBDriver *sdup) {
+
+  /* If the USB driver is not in the appropriate state then transactions
+     must not be started.*/
+  if ((usbGetDriverStateI(sdup->config->usbp) != USB_ACTIVE) ||
+      (sdup->state != SDU_READY)) {
+    return;
+  }
+
+  /* If there is already a transaction ongoing then another one cannot be
+     started.*/
+  if (usbGetTransmitStatusI(sdup->config->usbp, sdup->config->bulk_in)) {
+    return;
+  }
+
+  /* Checking if there only a buffer partially filled, if so then it is
+     enforced in the queue and transmitted.*/
+  if (obqTryFlushI(&sdup->obqueue)) {
+    size_t n;
+    uint8_t *buf = obqGetFullBufferI(&sdup->obqueue, &n);
+
+    osalDbgAssert(buf != NULL, "queue is empty");
+
+    osalSysUnlockFromISR();
+
+    usbPrepareTransmit(sdup->config->usbp, sdup->config->bulk_in, buf, n);
+
+    osalSysLockFromISR();
+    (void) usbStartTransmitI(sdup->config->usbp, sdup->config->bulk_in);
+  }
+}
+
 /**
  * @brief   Default data transmitted callback.
  * @details The application must use this function as callback for the IN
  *          data endpoint.
  *
  * @param[in] usbp      pointer to the @p USBDriver object
- * @param[in] ep        endpoint number
+ * @param[in] ep        IN endpoint number
  */
 void sduDataTransmitted(USBDriver *usbp, usbep_t ep) {
   uint8_t *buf;
@@ -384,27 +423,41 @@ void sduDataTransmitted(USBDriver *usbp, usbep_t ep) {
   /* Signaling that space is available in the output queue.*/
   chnAddFlagsI(sdup, CHN_OUTPUT_EMPTY);
 
-  /* Freeing the buffer just transmitted.*/
-  obqReleaseEmptyBufferI(&sdup->obqueue);
+  /* Freeing the buffer just transmitted, if it was not a zero size packet.*/
+  if (usbp->epc[ep]->in_state->txsize > 0U) {
+    obqReleaseEmptyBufferI(&sdup->obqueue);
+  }
 
   /* Checking if there is a buffer ready for transmission.*/
   buf = obqGetFullBufferI(&sdup->obqueue, &n);
+
+  /* Unlocking the critical zone.*/
+  osalSysUnlockFromISR();
+
   if (buf != NULL) {
     /* The endpoint cannot be busy, we are in the context of the callback,
        so it is safe to transmit without a check.*/
-    osalSysUnlockFromISR();
-
     usbPrepareTransmit(usbp, ep, buf, n);
+  }
+  else if ((usbp->epc[ep]->in_state->txsize > 0U) &&
+           ((usbp->epc[ep]->in_state->txsize &
+            ((size_t)usbp->epc[ep]->in_maxsize - 1U)) == 0U)) {
+    /* Transmit zero sized packet in case the last one has maximum allowed
+       size. Otherwise the recipient may expect more data coming soon and
+       not return buffered data to app. See section 5.8.3 Bulk Transfer
+       Packet Size Constraints of the USB Specification document.*/
+    usbPrepareTransmit(usbp, ep, usbp->setup, 0);
 
-    osalSysLockFromISR();
-    (void) usbStartTransmitI(usbp, ep);
-    osalSysUnlockFromISR();
+  }
+  else {
+    /* Nothing to transmit.*/
     return;
   }
 
-  /* There could be a partial buffer being filled, special case.*/
-  /* TODO */
-#error "SERIAL USB UNDERGOING CHANGES, NOT FINISHED YET"
+  /* Locking again and starting transmission.*/
+  osalSysLockFromISR();
+  (void) usbStartTransmitI(usbp, ep);
+  osalSysUnlockFromISR();
 
 #if 0
   /*lint -save -e9013 [15.7] There is no else because it is not needed.*/
@@ -444,7 +497,7 @@ void sduDataTransmitted(USBDriver *usbp, usbep_t ep) {
  *          data endpoint.
  *
  * @param[in] usbp      pointer to the @p USBDriver object
- * @param[in] ep        endpoint number
+ * @param[in] ep        OUT endpoint number
  */
 void sduDataReceived(USBDriver *usbp, usbep_t ep) {
   uint8_t *buf;
@@ -471,10 +524,8 @@ void sduDataReceived(USBDriver *usbp, usbep_t ep) {
   if (buf != NULL) {
     /* Buffer found, starting a new transaction.*/
     osalSysUnlockFromISR();
-
     usbPrepareReceive(sdup->config->usbp, sdup->config->bulk_out,
                       buf, SERIAL_USB_BUFFERS_SIZE);
-
     osalSysLockFromISR();
     (void) usbStartReceiveI(sdup->config->usbp, sdup->config->bulk_out);
   }
diff --git a/testhal/STM32/STM32F4xx/USB_CDC/debug/STM32F4xx-USB_CDC (OpenOCD, Flash and Run).launch b/testhal/STM32/STM32F4xx/USB_CDC/debug/STM32F4xx-USB_CDC (OpenOCD, Flash and Run).launch
index 4a04bbd16..9f4df03ef 100644
--- a/testhal/STM32/STM32F4xx/USB_CDC/debug/STM32F4xx-USB_CDC (OpenOCD, Flash and Run).launch	
+++ b/testhal/STM32/STM32F4xx/USB_CDC/debug/STM32F4xx-USB_CDC (OpenOCD, Flash and Run).launch	
@@ -33,7 +33,7 @@
 <intAttribute key="org.eclipse.cdt.launch.ATTR_BUILD_BEFORE_LAUNCH_ATTR" value="2"/>
 <stringAttribute key="org.eclipse.cdt.launch.COREFILE_PATH" value=""/>
 <stringAttribute key="org.eclipse.cdt.launch.DEBUGGER_REGISTER_GROUPS" value=""/>
-<stringAttribute key="org.eclipse.cdt.launch.FORMAT" value="&lt;?xml version=&quot;1.0&quot; encoding=&quot;UTF-8&quot; standalone=&quot;no&quot;?&gt;&lt;contentList/&gt;"/>
+<stringAttribute key="org.eclipse.cdt.launch.FORMAT" value="&lt;?xml version=&quot;1.0&quot; encoding=&quot;UTF-8&quot; standalone=&quot;no&quot;?&gt;&lt;contentList&gt;&lt;content id=&quot;bcounter-null-obqTryFlushI-(format)&quot; val=&quot;4&quot;/&gt;&lt;content id=&quot;n-sduSOFHookI-(format)&quot; val=&quot;4&quot;/&gt;&lt;/contentList&gt;"/>
 <stringAttribute key="org.eclipse.cdt.launch.GLOBAL_VARIABLES" value="&lt;?xml version=&quot;1.0&quot; encoding=&quot;UTF-8&quot; standalone=&quot;no&quot;?&gt;&#13;&#10;&lt;globalVariableList/&gt;&#13;&#10;"/>
 <stringAttribute key="org.eclipse.cdt.launch.MEMORY_BLOCKS" value="&lt;?xml version=&quot;1.0&quot; encoding=&quot;UTF-8&quot; standalone=&quot;no&quot;?&gt;&#13;&#10;&lt;memoryBlockExpressionList/&gt;&#13;&#10;"/>
 <stringAttribute key="org.eclipse.cdt.launch.PROGRAM_NAME" value="./build/ch.elf"/>
diff --git a/testhal/STM32/STM32F4xx/USB_CDC/main.c b/testhal/STM32/STM32F4xx/USB_CDC/main.c
index 67cfec3da..37c0cfdc1 100644
--- a/testhal/STM32/STM32F4xx/USB_CDC/main.c
+++ b/testhal/STM32/STM32F4xx/USB_CDC/main.c
@@ -26,9 +26,6 @@
 
 #include "usbcfg.h"
 
-/* Virtual serial port over USB.*/
-SerialUSBDriver SDU1;
-
 /*===========================================================================*/
 /* Command line related.                                                     */
 /*===========================================================================*/
diff --git a/testhal/STM32/STM32F4xx/USB_CDC/usbcfg.c b/testhal/STM32/STM32F4xx/USB_CDC/usbcfg.c
index cfe1a7f5c..07a492758 100644
--- a/testhal/STM32/STM32F4xx/USB_CDC/usbcfg.c
+++ b/testhal/STM32/STM32F4xx/USB_CDC/usbcfg.c
@@ -14,9 +14,11 @@
     limitations under the License.
 */
 
-#include "ch.h"
 #include "hal.h"
 
+/* Virtual serial port over USB.*/
+SerialUSBDriver SDU1;
+
 /*
  * Endpoints to be used for USBD1.
  */
@@ -299,6 +301,18 @@ static void usb_event(USBDriver *usbp, usbevent_t event) {
   return;
 }
 
+/*
+ * Handles the USB driver global events.
+ */
+static void sof_handler(USBDriver *usbp) {
+
+  (void)usbp;
+
+  osalSysLockFromISR();
+  sduSOFHookI(&SDU1);
+  osalSysUnlockFromISR();
+}
+
 /*
  * USB driver configuration.
  */
@@ -306,7 +320,7 @@ const USBConfig usbcfg = {
   usb_event,
   get_descriptor,
   sduRequestsHook,
-  NULL
+  sof_handler
 };
 
 /*
diff --git a/testhal/STM32/STM32F4xx/USB_CDC/usbcfg.h b/testhal/STM32/STM32F4xx/USB_CDC/usbcfg.h
index 2ffaa17f9..2da1c40a4 100644
--- a/testhal/STM32/STM32F4xx/USB_CDC/usbcfg.h
+++ b/testhal/STM32/STM32F4xx/USB_CDC/usbcfg.h
@@ -19,6 +19,7 @@
 
 extern const USBConfig usbcfg;
 extern SerialUSBConfig serusbcfg;
+extern SerialUSBDriver SDU1;
 
 #endif  /* _USBCFG_H_ */