From afb00cd4ebb2dfa8ba4bff6391e5afd6f36ea356 Mon Sep 17 00:00:00 2001
From: gdisirio <gdisirio@35acf78f-673a-0410-8e92-d51de3d6d3f4>
Date: Sat, 16 Jun 2012 15:04:12 +0000
Subject: [PATCH] git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@4279
 35acf78f-673a-0410-8e92-d51de3d6d3f4

---
 os/hal/platforms/STM32/OTGv1/usb_lld.c | 20 +++++++++---------
 os/hal/platforms/STM32/USBv1/usb_lld.c | 29 ++++++++++++++++++++++++++
 2 files changed, 39 insertions(+), 10 deletions(-)

diff --git a/os/hal/platforms/STM32/OTGv1/usb_lld.c b/os/hal/platforms/STM32/OTGv1/usb_lld.c
index 7990fb89a..7987d6a5e 100644
--- a/os/hal/platforms/STM32/OTGv1/usb_lld.c
+++ b/os/hal/platforms/STM32/OTGv1/usb_lld.c
@@ -301,7 +301,7 @@ static void otg_fifo_write_from_queue(usbep_t ep,
 
   ntogo = n;
   while (ntogo > 0) {
-    uint32_t dw, i;
+    uint32_t w, i;
     size_t nw = ntogo / 4;
 
     if (nw > 0) {
@@ -322,10 +322,10 @@ static void otg_fifo_write_from_queue(usbep_t ep,
       break;
 
     /* One byte at time.*/
-    dw = 0;
+    w = 0;
     i = 0;
     while ((ntogo > 0) && (i < 4)) {
-      dw |= (uint32_t)*oqp->q_rdptr++ << (i * 8);
+      w |= (uint32_t)*oqp->q_rdptr++ << (i * 8);
       if (oqp->q_rdptr >= oqp->q_top)
         oqp->q_rdptr = oqp->q_buffer;
       ntogo--;
@@ -357,10 +357,10 @@ static void otg_fifo_write_from_queue(usbep_t ep,
 static uint8_t *otg_do_pop(volatile uint32_t *fifop, uint8_t *buf, size_t n) {
 
   while (n > 0) {
-    uint32_t dw = *fifop;
+    uint32_t w = *fifop;
     /* Note, this line relies on the Cortex-M3/M4 ability to perform
        unaligned word accesses and on the LSB-first memory organization.*/
-    *((uint32_t *)buf) = dw;
+    *((uint32_t *)buf) = w;
     buf += 4;
     n--;
   }
@@ -383,11 +383,11 @@ static void otg_fifo_read_to_buffer(uint8_t *buf, size_t n, size_t max) {
   n = (n + 3) / 4;
   max = (max + 3) / 4;
   while (n) {
-    uint32_t dw = *fifop;
+    uint32_t w = *fifop;
     if (max) {
       /* Note, this line relies on the Cortex-M3/M4 ability to perform
          unaligned word accesses and on the LSB-first memory organization.*/
-      *((uint32_t *)buf) = dw;
+      *((uint32_t *)buf) = w;
       buf += 4;
       max--;
     }
@@ -411,7 +411,7 @@ static void otg_fifo_read_to_queue(InputQueue *iqp, size_t n) {
 
   ntogo = n;
   while (ntogo > 0) {
-    uint32_t dw, i;
+    uint32_t w, i;
     size_t nw = ntogo / 4;
 
     if (nw > 0) {
@@ -432,10 +432,10 @@ static void otg_fifo_read_to_queue(InputQueue *iqp, size_t n) {
       break;
 
     /* One byte at time.*/
-    dw = *fifop;
+    w = *fifop;
     i = 0;
     while ((ntogo > 0) && (i < 4)) {
-      *iqp->q_wrptr++ = (uint8_t)(dw >> (i * 8));
+      *iqp->q_wrptr++ = (uint8_t)(w >> (i * 8));
       if (iqp->q_wrptr >= iqp->q_top)
         iqp->q_wrptr = iqp->q_buffer;
       ntogo--;
diff --git a/os/hal/platforms/STM32/USBv1/usb_lld.c b/os/hal/platforms/STM32/USBv1/usb_lld.c
index b8bf1aa19..f402bed67 100644
--- a/os/hal/platforms/STM32/USBv1/usb_lld.c
+++ b/os/hal/platforms/STM32/USBv1/usb_lld.c
@@ -151,6 +151,35 @@ static void usb_packet_read_to_buffer(stm32_usb_descriptor_t *udp,
  */
 static void usb_packet_read_to_queue(stm32_usb_descriptor_t *udp,
                                      InputQueue *iqp, size_t n) {
+  uint32_t w;
+  size_t nhw;
+  uint32_t *pmap= USB_ADDR2PTR(udp->RXADDR0);
+
+  nhw = n / 2;
+  while (nhw > 0) {
+    w = *pmap++;
+    *iqp->q_wrptr++ = (uint8_t)w;
+    if (iqp->q_wrptr >= iqp->q_top)
+      iqp->q_wrptr = iqp->q_buffer;
+    *iqp->q_wrptr++ = (uint8_t)(w >> 8);
+    if (iqp->q_wrptr >= iqp->q_top)
+      iqp->q_wrptr = iqp->q_buffer;
+    nhw--;
+  }
+  /* Last byte for odd numbers.*/
+  if ((n & 1) != 0) {
+    w = *pmap++;
+    *iqp->q_wrptr++ = (uint8_t)w;
+    if (iqp->q_wrptr >= iqp->q_top)
+      iqp->q_wrptr = iqp->q_buffer;
+  }
+
+  /* Updating queue.*/
+  chSysLockFromIsr();
+  iqp->q_counter += n;
+  while (notempty(&iqp->q_waiting))
+    chSchReadyI(fifo_remove(&iqp->q_waiting))->p_u.rdymsg = Q_OK;
+  chSysUnlockFromIsr();
 }
 
 /**