diff --git a/os/hal/platforms/STM32/USBv1/usb_lld.c b/os/hal/platforms/STM32/USBv1/usb_lld.c
index 1041f69da..0d61b68c7 100644
--- a/os/hal/platforms/STM32/USBv1/usb_lld.c
+++ b/os/hal/platforms/STM32/USBv1/usb_lld.c
@@ -174,8 +174,12 @@ static void usb_packet_read_to_queue(stm32_usb_descriptor_t *udp,
       iqp->q_wrptr = iqp->q_buffer;
   }
 
-  /* Updating queue counter.*/
+  /* Updating queue.*/
+  chSysLockFromIsr();
   iqp->q_counter += n;
+  while (notempty(&iqp->q_waiting))
+    chSchReadyI(fifo_remove(&iqp->q_waiting))->p_u.rdymsg = Q_OK;
+  chSysUnlockFromIsr();
 }
 
 /**
@@ -240,6 +244,19 @@ static void usb_packet_write_from_queue(stm32_usb_descriptor_t *udp,
     if (oqp->q_rdptr >= oqp->q_top)
       oqp->q_rdptr = oqp->q_buffer;
   }
+
+  /* Updating queue. Note, the lock is done in this unusual way because this
+     function can be called from both ISR and thread context so the kind
+     of lock function to be invoked cannot be decided beforehand.*/
+  port_lock();
+  dbg_enter_lock();
+
+  oqp->q_counter += n;
+  while (notempty(&oqp->q_waiting))
+    chSchReadyI(fifo_remove(&oqp->q_waiting))->p_u.rdymsg = Q_OK;
+
+  dbg_leave_lock();
+  port_unlock();
 }
 
 /**
@@ -266,31 +283,6 @@ static void usb_prepare_receive(USBDriver *usbp, usbep_t ep, size_t n) {
                              usbp->epc[ep]->out_maxsize);
 }
 
-/**
- * @brief   Prepares for a transmit transaction on an IN endpoint.
- * @post    The endpoint is ready for @p usbStartTransmitI().
- * @note    The transmit transaction size is equal to the data contained
- *          in the queue.
- *
- * @param[in] usbp      pointer to the @p USBDriver object
- * @param[in] ep        endpoint number
- * @param[in] n         maximum number of bytes to copy
- *
- * @special
- */
-static void usb_prepare_transmit(USBDriver *usbp, usbep_t ep, size_t n) {
-  USBInEndpointState *isp = usbp->epc[ep]->in_state;
-
-  isp->txsize = n;
-  isp->txcnt  = 0;
-
-  /* Transfer initialization.*/
-  if (n > (size_t)usbp->epc[ep]->in_maxsize)
-    n = (size_t)usbp->epc[ep]->in_maxsize;
-  usb_packet_write_from_buffer(USB_GET_DESCRIPTOR(ep),
-                               isp->mode.linear.txbuf, n);
-}
-
 /*===========================================================================*/
 /* Driver interrupt handlers.                                                */
 /*===========================================================================*/
@@ -382,9 +374,15 @@ CH_IRQ_HANDLER(Vector90) {
           n = epcp->in_maxsize;
         else
           n = epcp->in_state->txsize;
-        usb_packet_write_from_buffer(USB_GET_DESCRIPTOR(ep),
-                                     epcp->in_state->mode.linear.txbuf,
-                                     n);
+
+        if (epcp->in_state->txqueued)
+          usb_packet_write_from_queue(USB_GET_DESCRIPTOR(ep),
+                                      epcp->in_state->mode.queue.txqueue,
+                                      n);
+        else
+          usb_packet_write_from_buffer(USB_GET_DESCRIPTOR(ep),
+                                       epcp->in_state->mode.linear.txbuf,
+                                       n);
         chSysLockFromIsr();
         usb_lld_start_in(usbp, ep);
         chSysUnlockFromIsr();
@@ -406,8 +404,15 @@ CH_IRQ_HANDLER(Vector90) {
         stm32_usb_descriptor_t *udp = USB_GET_DESCRIPTOR(ep);
         n = (size_t)udp->RXCOUNT0 & RXCOUNT_COUNT_MASK;
 
-        /* Reads the packet into the linear buffer.*/
-        usb_packet_read_to_buffer(udp, epcp->out_state->mode.linear.rxbuf, n);
+        /* Reads the packet into the defined buffer.*/
+        if (epcp->out_state->rxqueued)
+          usb_packet_read_to_queue(udp,
+                                   epcp->out_state->mode.queue.rxqueue,
+                                   n);
+        else
+          usb_packet_read_to_buffer(udp,
+                                    epcp->out_state->mode.linear.rxbuf,
+                                    n);
 
         /* Transaction data updated.*/
         epcp->out_state->mode.linear.rxbuf  += n;
@@ -735,6 +740,7 @@ void usb_lld_prepare_transmit(USBDriver *usbp, usbep_t ep,
   /* Transfer initialization.*/
   if (n > (size_t)usbp->epc[ep]->in_maxsize)
     n = (size_t)usbp->epc[ep]->in_maxsize;
+
   usb_packet_write_from_buffer(USB_GET_DESCRIPTOR(ep),
                                isp->mode.linear.txbuf, n);
 }
@@ -788,8 +794,9 @@ void usb_lld_prepare_queued_transmit(USBDriver *usbp, usbep_t ep,
   /* Transfer initialization.*/
   if (n > (size_t)usbp->epc[ep]->in_maxsize)
     n = (size_t)usbp->epc[ep]->in_maxsize;
-  usb_packet_write_from_buffer(USB_GET_DESCRIPTOR(ep),
-                               isp->mode.linear.txbuf, n);
+
+  usb_packet_write_from_queue(USB_GET_DESCRIPTOR(ep),
+                              isp->mode.queue.txqueue, n);
 }
 
 /**
@@ -801,13 +808,8 @@ void usb_lld_prepare_queued_transmit(USBDriver *usbp, usbep_t ep,
  * @notapi
  */
 void usb_lld_start_out(USBDriver *usbp, usbep_t ep) {
-  USBOutEndpointState *osp = usbp->epc[ep]->out_state;
 
-  if (osp->rxqueued) {
-    InputQueue *iqp = osp->mode.queue.rxqueue;
-    while (notempty(&iqp->q_waiting))
-      chSchReadyI(fifo_remove(&iqp->q_waiting))->p_u.rdymsg = Q_OK;
-  }
+  (void)usbp;
 
   EPR_SET_STAT_RX(ep, EPR_STAT_RX_VALID);
 }
@@ -821,13 +823,8 @@ void usb_lld_start_out(USBDriver *usbp, usbep_t ep) {
  * @notapi
  */
 void usb_lld_start_in(USBDriver *usbp, usbep_t ep) {
-  USBInEndpointState *isp = usbp->epc[ep]->in_state;
 
-  if (isp->txqueued) {
-    OutputQueue *oqp = isp->mode.queue.txqueue;
-    while (notempty(&oqp->q_waiting))
-      chSchReadyI(fifo_remove(&oqp->q_waiting))->p_u.rdymsg = Q_OK;
-  }
+  (void)usbp;
 
   EPR_SET_STAT_TX(ep, EPR_STAT_TX_VALID);
 }
diff --git a/os/hal/src/usb.c b/os/hal/src/usb.c
index ceae24770..fa79ea0c1 100644
--- a/os/hal/src/usb.c
+++ b/os/hal/src/usb.c
@@ -313,10 +313,11 @@ void usbInitEndpointI(USBDriver *usbp, usbep_t ep,
               "usbEnableEndpointI(), #2", "already initialized");
 
   /* Logically enabling the endpoint in the USBDriver structure.*/
-//  if (!(epcp->ep_mode & USB_EP_MODE_PACKET)) {
+  if (epcp->in_state != NULL)
     memset(epcp->in_state, 0, sizeof(USBInEndpointState));
+  if (epcp->out_state != NULL)
     memset(epcp->out_state, 0, sizeof(USBOutEndpointState));
-//  }
+
   usbp->epc[ep] = epcp;
 
   /* Low level endpoint activation.*/
diff --git a/os/kernel/include/chdebug.h b/os/kernel/include/chdebug.h
index b899233e4..24dce243e 100644
--- a/os/kernel/include/chdebug.h
+++ b/os/kernel/include/chdebug.h
@@ -77,6 +77,8 @@
 /*===========================================================================*/
 
 #if !CH_DBG_SYSTEM_STATE_CHECK
+#define dbg_enter_lock()
+#define dbg_leave_lock()
 #define dbg_check_disable()
 #define dbg_check_suspend()
 #define dbg_check_enable()
@@ -88,6 +90,9 @@
 #define dbg_check_leave_isr()
 #define chDbgCheckClassI();
 #define chDbgCheckClassS();
+#else
+#define dbg_enter_lock() (dbg_lock_cnt = 1)
+#define dbg_leave_lock() (dbg_lock_cnt = 0)
 #endif
 
 /*===========================================================================*/
@@ -213,6 +218,8 @@ extern char *dbg_panic_msg;
 extern "C" {
 #endif
 #if CH_DBG_SYSTEM_STATE_CHECK
+  extern cnt_t dbg_isr_cnt;
+  extern cnt_t dbg_lock_cnt;
   void dbg_check_disable(void);
   void dbg_check_suspend(void);
   void dbg_check_enable(void);
diff --git a/os/kernel/src/chdebug.c b/os/kernel/src/chdebug.c
index 8d38dcd6e..0bee1b4ea 100644
--- a/os/kernel/src/chdebug.c
+++ b/os/kernel/src/chdebug.c
@@ -108,7 +108,7 @@ void dbg_check_lock(void) {
 
   if ((dbg_isr_cnt != 0) || (dbg_lock_cnt != 0))
     chDbgPanic("SV#4");
-  dbg_lock_cnt = 1;
+  dbg_enter_lock();
 }
 
 /**
@@ -120,7 +120,7 @@ void dbg_check_unlock(void) {
 
   if ((dbg_isr_cnt != 0) || (dbg_lock_cnt <= 0))
     chDbgPanic("SV#5");
-  dbg_lock_cnt = 0;
+  dbg_leave_lock();
 }
 
 /**
@@ -132,7 +132,7 @@ void dbg_check_lock_from_isr(void) {
 
   if ((dbg_isr_cnt <= 0) || (dbg_lock_cnt != 0))
     chDbgPanic("SV#6");
-  dbg_lock_cnt = 1;
+  dbg_enter_lock();
 }
 
 /**
@@ -144,7 +144,7 @@ void dbg_check_unlock_from_isr(void) {
 
   if ((dbg_isr_cnt <= 0) || (dbg_lock_cnt <= 0))
     chDbgPanic("SV#7");
-  dbg_lock_cnt = 0;
+  dbg_leave_lock();
 }
 
 /**
diff --git a/readme.txt b/readme.txt
index 588b39d15..0c98ab9d6 100644
--- a/readme.txt
+++ b/readme.txt
@@ -145,7 +145,6 @@
 - FIX: Fixed various minor documentation errors (bug 3484942)(backported
   to 2.4.1).
 - NEW: USB implementation for STM32F105/F107/2xx/F4xx devices.
-  TODO: Modify the F103 USB driver for the new model.
 - NEW: Improved SerialUSB driver using the new queued mode, much smaller
   than the previous driver.
 - NEW: Improved USB driver model supporting also queues for endpoint I/O,
diff --git a/testhal/STM32F1xx/USB_CDC/main.c b/testhal/STM32F1xx/USB_CDC/main.c
index d4625066f..b1af1b1bd 100644
--- a/testhal/STM32F1xx/USB_CDC/main.c
+++ b/testhal/STM32F1xx/USB_CDC/main.c
@@ -437,6 +437,8 @@ int main(void) {
   /*
    * Activates the USB driver and then the USB bus pull-up on D+.
    */
+  usbDisconnectBus(serusbcfg.usbp);
+  chThdSleepMilliseconds(1000);
   sduObjectInit(&SDU1);
   sduStart(&SDU1, &serusbcfg);
   usbConnectBus(serusbcfg.usbp);