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: git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@4279
 35acf78f-673a-0410-8e92-d51de3d6d3f4

---
 os/hal/platforms/STM32/USBv1/usb_lld.c | 29 +++++++++++++++++++++++++++++
 1 file changed, 29 insertions(+)

(limited to 'os/hal/platforms/STM32/USBv1')

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();
 }
 
 /**
-- 
cgit v1.2.3