[OTGFS] fix readouts from out of buffer

inc/usbd_core.h

@@ -259,6 +259,7 @@ typedef void (*usbd_hw_ep_deconfig)(uint8_t ep);
  * \param buf pointer to read buffer
  * \param blen size of the read buffer in bytes
  * \return size of the actually received data, -1 on error.
+ * \note if data does not fit buffer it will be truncated
  */
 typedef int32_t (*usbd_hw_ep_read)(uint8_t ep, void *buf, uint16_t blen);
 

src/usbd_stm32f105_otgfs.c

@@ -35,15 +35,15 @@ USB_OTG_DeviceTypeDef * const OTGD = (void*)(USB_OTG_FS_PERIPH_BASE + USB_OTG_DE
 volatile uint32_t * const OTGPCTL  = (void*)(USB_OTG_FS_PERIPH_BASE + USB_OTG_PCGCCTL_BASE);
 
 
-inline static volatile uint32_t* EPFIFO(uint8_t ep) {
+inline static uint32_t* EPFIFO(uint32_t ep) {
     return (uint32_t*)(USB_OTG_FS_PERIPH_BASE + USB_OTG_FIFO_BASE + (ep << 12));
 }
 
-inline static USB_OTG_INEndpointTypeDef* EPIN(uint8_t ep) {
+inline static USB_OTG_INEndpointTypeDef* EPIN(uint32_t ep) {
     return (void*)(USB_OTG_FS_PERIPH_BASE + USB_OTG_IN_ENDPOINT_BASE + (ep << 5));
 }
 
-inline static USB_OTG_OUTEndpointTypeDef* EPOUT(uint8_t ep) {
+inline static USB_OTG_OUTEndpointTypeDef* EPOUT(uint32_t ep) {
     return (void*)(USB_OTG_FS_PERIPH_BASE + USB_OTG_OUT_ENDPOINT_BASE + (ep << 5));
 }
 
@@ -321,7 +321,7 @@ void ep_deconfig(uint8_t ep) {
 }
 
 int32_t ep_read(uint8_t ep, void* buf, uint16_t blen) {
-    int32_t len;
+    uint32_t len, tmp;
     volatile uint32_t *fifo = EPFIFO(0);
     /* no data in RX FIFO */
     if (!(OTG->GINTSTS & USB_OTG_GINTSTS_RXFLVL)) return -1;
@@ -329,40 +329,41 @@ int32_t ep_read(uint8_t ep, void* buf, uint16_t blen) {
     if ((OTG->GRXSTSR & USB_OTG_GRXSTSP_EPNUM) != ep) return -1;
     /* pop data from fifo */
     len = _FLD2VAL(USB_OTG_GRXSTSP_BCNT, OTG->GRXSTSP);
-    for (unsigned i = 0; i < len; i +=4) {
-        uint32_t _t = *fifo;
-        if (blen >= 4) {
-            *(__attribute__((packed))uint32_t*)buf = _t;
-            blen -= 4;
-            buf += 4;
-        } else {
-            while (blen){
-                *(uint8_t*)buf++ = 0xFF & _t;
-                _t >>= 8;
-                blen --;
-            }
+    for (int idx = 0; idx < len; idx++) {
+        if ((idx & 0x03) == 0x00) {
+            tmp = *fifo;
+        }
+        if (idx < blen) {
+            ((uint8_t*)buf)[idx] = tmp & 0xFF;
+            tmp >>= 8;
         }
     }
-    return len;
+    return (len < blen) ? len : blen;
 }
 
 int32_t ep_write(uint8_t ep, void *buf, uint16_t blen) {
+    uint32_t len, tmp;
     ep &= 0x7F;
-    volatile uint32_t* _fifo = EPFIFO(ep);
+    volatile uint32_t* fifo = EPFIFO(ep);
     USB_OTG_INEndpointTypeDef* epi = EPIN(ep);
     /* transfer data size in 32-bit words */
-    uint32_t  _len = (blen + 3) >> 2;
+    len = (blen + 3) >> 2;
     /* no enough space in TX fifo */
-    if (_len > epi->DTXFSTS) return -1;
+    if (len > epi->DTXFSTS) return -1;
     if (ep != 0 && epi->DIEPCTL & USB_OTG_DIEPCTL_EPENA) {
         return -1;
     }
     epi->DIEPTSIZ = 0;
     epi->DIEPTSIZ = (1 << USB_OTG_DIEPTSIZ_PKTCNT_Pos) + blen;
     _BMD(epi->DIEPCTL, USB_OTG_DIEPCTL_STALL, USB_OTG_DOEPCTL_EPENA | USB_OTG_DOEPCTL_CNAK);
-    while (_len--) {
-        *_fifo = *(__attribute__((packed)) uint32_t*)buf;
-        buf += 4;
+    /* push data to FIFO */
+    tmp = 0;
+    for (int idx = 0; idx < blen; idx++) {
+        tmp |= (uint32_t)((uint8_t*)buf)[idx] << ((idx & 0x03) << 3);
+        if ((idx & 0x03) == 0x03 || (idx + 1) == blen) {
+            *fifo = tmp;
+            tmp = 0;
+        }
     }
     return blen;
 }

src/usbd_stm32f429_otgfs.c

@@ -34,15 +34,15 @@ USB_OTG_DeviceTypeDef * const OTGD = (void*)(USB_OTG_FS_PERIPH_BASE + USB_OTG_DE
 volatile uint32_t * const OTGPCTL  = (void*)(USB_OTG_FS_PERIPH_BASE + USB_OTG_PCGCCTL_BASE);
 
 
-inline static volatile uint32_t* EPFIFO(uint8_t ep) {
+inline static uint32_t* EPFIFO(uint32_t ep) {
     return (uint32_t*)(USB_OTG_FS_PERIPH_BASE + USB_OTG_FIFO_BASE + (ep << 12));
 }
 
-inline static USB_OTG_INEndpointTypeDef* EPIN(uint8_t ep) {
+inline static USB_OTG_INEndpointTypeDef* EPIN(uint32_t ep) {
     return (void*)(USB_OTG_FS_PERIPH_BASE + USB_OTG_IN_ENDPOINT_BASE + (ep << 5));
 }
 
-inline static USB_OTG_OUTEndpointTypeDef* EPOUT(uint8_t ep) {
+inline static USB_OTG_OUTEndpointTypeDef* EPOUT(uint32_t ep) {
     return (void*)(USB_OTG_FS_PERIPH_BASE + USB_OTG_OUT_ENDPOINT_BASE + (ep << 5));
 }
 
@@ -318,7 +318,7 @@ void ep_deconfig(uint8_t ep) {
 }
 
 int32_t ep_read(uint8_t ep, void* buf, uint16_t blen) {
-    int32_t len;
+    uint32_t len, tmp;
     volatile uint32_t *fifo = EPFIFO(0);
     /* no data in RX FIFO */
     if (!(OTG->GINTSTS & USB_OTG_GINTSTS_RXFLVL)) return -1;
@@ -326,44 +326,41 @@ int32_t ep_read(uint8_t ep, void* buf, uint16_t blen) {
     if ((OTG->GRXSTSR & USB_OTG_GRXSTSP_EPNUM) != ep) return -1;
     /* pop data from fifo */
     len = _FLD2VAL(USB_OTG_GRXSTSP_BCNT, OTG->GRXSTSP);
-    for (unsigned i = 0; i < len; i +=4) {
-        uint32_t _t = *fifo;
-        if (blen >= 4) {
-            /* Cortex M3, M4 supports unaligned access */
-            *(uint32_t*)buf = _t;
-            blen -= 4;
-            buf += 4;
-        } else {
-            while (blen){
-                *(uint8_t*)buf = 0xFF & _t;
-                _t >>= 8;
-                blen --;
-                buf++;
-            }
+    for (int idx = 0; idx < len; idx++) {
+        if ((idx & 0x03) == 0x00) {
+            tmp = *fifo;
+        }
+        if (idx < blen) {
+            ((uint8_t*)buf)[idx] = tmp & 0xFF;
+            tmp >>= 8;
         }
     }
-    return len;
+    return (len < blen) ? len : blen;
 }
 
 int32_t ep_write(uint8_t ep, void *buf, uint16_t blen) {
+    uint32_t len, tmp;
     ep &= 0x7F;
-    volatile uint32_t* _fifo = EPFIFO(ep);
+    volatile uint32_t* fifo = EPFIFO(ep);
     USB_OTG_INEndpointTypeDef* epi = EPIN(ep);
     /* transfer data size in 32-bit words */
-    uint32_t  _len = (blen + 3) >> 2;
+    len = (blen + 3) >> 2;
     /* no enough space in TX fifo */
-    if (_len > epi->DTXFSTS) return -1;
+    if (len > epi->DTXFSTS) return -1;
     if (ep != 0 && epi->DIEPCTL & USB_OTG_DIEPCTL_EPENA) {
         return -1;
     }
     epi->DIEPTSIZ = 0;
     epi->DIEPTSIZ = (1 << 19) + blen;
     _BMD(epi->DIEPCTL, USB_OTG_DIEPCTL_STALL, USB_OTG_DOEPCTL_EPENA | USB_OTG_DOEPCTL_CNAK);
-    while (_len--) {
-        /* Cortex M3, M4 supports unaligned access */
-        uint32_t _t = *(uint32_t*)buf;
-        *_fifo = _t;
-        buf += 4;
+    /* push data to FIFO */
+    tmp = 0;
+    for (int idx = 0; idx < blen; idx++) {
+        tmp |= (uint32_t)((uint8_t*)buf)[idx] << ((idx & 0x03) << 3);
+        if ((idx & 0x03) == 0x03 || (idx + 1) == blen) {
+            *fifo = tmp;
+            tmp = 0;
+        }
     }
     return blen;
 }

src/usbd_stm32f429_otghs.c

@@ -34,15 +34,15 @@ static USB_OTG_DeviceTypeDef * const OTGD = (void*)(USB_OTG_HS_PERIPH_BASE + USB
 static volatile uint32_t * const OTGPCTL  = (void*)(USB_OTG_HS_PERIPH_BASE + USB_OTG_PCGCCTL_BASE);
 
 
-inline static volatile uint32_t* EPFIFO(uint8_t ep) {
+inline static uint32_t* EPFIFO(uint32_t ep) {
     return (uint32_t*)(USB_OTG_HS_PERIPH_BASE + USB_OTG_FIFO_BASE + (ep << 12));
 }
 
-inline static USB_OTG_INEndpointTypeDef* EPIN(uint8_t ep) {
+inline static USB_OTG_INEndpointTypeDef* EPIN(uint32_t ep) {
     return (void*)(USB_OTG_HS_PERIPH_BASE + USB_OTG_IN_ENDPOINT_BASE + (ep << 5));
 }
 
-inline static USB_OTG_OUTEndpointTypeDef* EPOUT(uint8_t ep) {
+inline static USB_OTG_OUTEndpointTypeDef* EPOUT(uint32_t ep) {
     return (void*)(USB_OTG_HS_PERIPH_BASE + USB_OTG_OUT_ENDPOINT_BASE + (ep << 5));
 }
 
@@ -320,7 +320,7 @@ static void ep_deconfig(uint8_t ep) {
 }
 
 static int32_t ep_read(uint8_t ep, void* buf, uint16_t blen) {
-    int32_t len;
+    uint32_t len, tmp;
     volatile uint32_t *fifo = EPFIFO(0);
     /* no data in RX FIFO */
     if (!(OTG->GINTSTS & USB_OTG_GINTSTS_RXFLVL)) return -1;
@@ -328,32 +328,27 @@ static int32_t ep_read(uint8_t ep, void* buf, uint16_t blen) {
     if ((OTG->GRXSTSR & USB_OTG_GRXSTSP_EPNUM) != ep) return -1;
     /* pop data from fifo */
     len = _FLD2VAL(USB_OTG_GRXSTSP_BCNT, OTG->GRXSTSP);
-    for (unsigned i = 0; i < len; i +=4) {
-        uint32_t _t = *fifo;
-        if (blen >= 4) {
-            /* Cortex M3, M4 supports unaligned access */
-            *(uint32_t*)buf = _t;
-            blen -= 4;
-            buf += 4;
-        } else {
-            while (blen){
-                *(uint8_t*)buf++ = 0xFF & _t;
-                _t >>= 8;
-                blen --;
-            }
+    for (int idx = 0; idx < len; idx++) {
+        if ((idx & 0x03) == 0x00) {
+            tmp = *fifo;
+        }
+        if (idx < blen) {
+            ((uint8_t*)buf)[idx] = tmp & 0xFF;
+            tmp >>= 8;
         }
     }
-    return len;
+    return (len < blen) ? len : blen;
 }
 
 static int32_t ep_write(uint8_t ep, void *buf, uint16_t blen) {
+    uint32_t len, tmp;
     ep &= 0x7F;
-    volatile uint32_t* _fifo = EPFIFO(ep);
+    volatile uint32_t* fifo = EPFIFO(ep);
     USB_OTG_INEndpointTypeDef* epi = EPIN(ep);
     /* transfer data size in 32-bit words */
-    uint32_t  _len = (blen + 3) >> 2;
+    len = (blen + 3) >> 2;
     /* no enough space in TX fifo */
-    if (_len > _FLD2VAL(USB_OTG_DTXFSTS_INEPTFSAV, epi->DTXFSTS)) return -1;
+    if (len > _FLD2VAL(USB_OTG_DTXFSTS_INEPTFSAV, epi->DTXFSTS)) return -1;
     if (ep != 0 && epi->DIEPCTL & USB_OTG_DIEPCTL_EPENA) {
         return -1;
     }
@@ -362,11 +357,14 @@ static int32_t ep_write(uint8_t ep, void *buf, uint16_t blen) {
          _VAL2FLD(USB_OTG_DIEPTSIZ_PKTCNT, 1) | _VAL2FLD(USB_OTG_DIEPTSIZ_MULCNT, 1 ) | _VAL2FLD(USB_OTG_DIEPTSIZ_XFRSIZ, blen));
     _BMD(epi->DIEPCTL, USB_OTG_DIEPCTL_STALL, USB_OTG_DOEPCTL_CNAK);
     _BST(epi->DIEPCTL, USB_OTG_DOEPCTL_EPENA);
-    while (_len--) {
-        /* Cortex M3, M4 supports unaligned access */
-        uint32_t _t = *(uint32_t*)buf;
-        *_fifo = _t;
-        buf += 4;
+    /* push data to FIFO */
+    tmp = 0;
+    for (int idx = 0; idx < blen; idx++) {
+        tmp |= (uint32_t)((uint8_t*)buf)[idx] << ((idx & 0x03) << 3);
+        if ((idx & 0x03) == 0x03 || (idx + 1) == blen) {
+            *fifo = tmp;
+            tmp = 0;
+        }
     }
     return blen;
 }
@@ -485,4 +483,4 @@ __attribute__((externally_visible)) const struct usbd_driver usbd_otghs = {
     get_serialno_desc,
 };
 
-#endif //USBD_STM32L476
+#endif //USBD_STM32F429HS

src/usbd_stm32f446_otgfs.c

@@ -34,15 +34,15 @@ USB_OTG_DeviceTypeDef * const OTGD = (void*)(USB_OTG_FS_PERIPH_BASE + USB_OTG_DE
 volatile uint32_t * const OTGPCTL  = (void*)(USB_OTG_FS_PERIPH_BASE + USB_OTG_PCGCCTL_BASE);
 
 
-inline static volatile uint32_t* EPFIFO(uint8_t ep) {
+inline static uint32_t* EPFIFO(uint32_t ep) {
     return (uint32_t*)(USB_OTG_FS_PERIPH_BASE + USB_OTG_FIFO_BASE + (ep << 12));
 }
 
-inline static USB_OTG_INEndpointTypeDef* EPIN(uint8_t ep) {
+inline static USB_OTG_INEndpointTypeDef* EPIN(uint32_t ep) {
     return (void*)(USB_OTG_FS_PERIPH_BASE + USB_OTG_IN_ENDPOINT_BASE + (ep << 5));
 }
 
-inline static USB_OTG_OUTEndpointTypeDef* EPOUT(uint8_t ep) {
+inline static USB_OTG_OUTEndpointTypeDef* EPOUT(uint32_t ep) {
     return (void*)(USB_OTG_FS_PERIPH_BASE + USB_OTG_OUT_ENDPOINT_BASE + (ep << 5));
 }
 
@@ -309,50 +309,51 @@ void ep_deconfig(uint8_t ep) {
 }
 
 int32_t ep_read(uint8_t ep, void* buf, uint16_t blen) {
-    uint32_t len;
+    uint32_t len, tmp;
+    ep &= 0x7F;
     volatile uint32_t *fifo = EPFIFO(0);
     USB_OTG_OUTEndpointTypeDef* epo = EPOUT(ep);
     /* no data in RX FIFO */
     if (!(OTG->GINTSTS & USB_OTG_GINTSTS_RXFLVL)) return -1;
-    ep &= 0x7F;
     if ((OTG->GRXSTSR & USB_OTG_GRXSTSP_EPNUM) != ep) return -1;
     /* pop data from fifo */
     len = _FLD2VAL(USB_OTG_GRXSTSP_BCNT, OTG->GRXSTSP);
-    for (unsigned i = 0; i < len; i +=4) {
-        uint32_t _t = *fifo;
-        if (blen >= 4) {
-            *(uint32_t*)buf = _t;
-            blen -= 4;
-            buf += 4;
-        } else {
-            while (blen){
-                *((uint8_t*)buf++) = 0xFF & _t;
-                _t >>= 8;
-                blen --;
-            }
+    for (int idx = 0; idx < len; idx++) {
+        if ((idx & 0x03) == 0x00) {
+            tmp = *fifo;
+        }
+        if (idx < blen) {
+            ((uint8_t*)buf)[idx] = tmp & 0xFF;
+            tmp >>= 8;
         }
     }
     _BST(epo->DOEPCTL, USB_OTG_DOEPCTL_CNAK | USB_OTG_DOEPCTL_EPENA);
-    return len;
+    return (len < blen) ? len : blen;
 }
 
 int32_t ep_write(uint8_t ep, void *buf, uint16_t blen) {
+    uint32_t len, tmp;
     ep &= 0x7F;
-    volatile uint32_t* _fifo = EPFIFO(ep);
+    volatile uint32_t* fifo = EPFIFO(ep);
     USB_OTG_INEndpointTypeDef* epi = EPIN(ep);
     /* transfer data size in 32-bit words */
-    uint32_t  _len = (blen + 3) >> 2;
+    len = (blen + 3) >> 2;
     /* no enough space in TX fifo */
-    if (_len > epi->DTXFSTS) return -1;
+    if (len > epi->DTXFSTS) return -1;
     if (ep != 0 && epi->DIEPCTL & USB_OTG_DIEPCTL_EPENA) {
         return -1;
     }
     epi->DIEPTSIZ = 0;
     epi->DIEPTSIZ = (1 << 19) + blen;
     _BMD(epi->DIEPCTL, USB_OTG_DIEPCTL_STALL, USB_OTG_DOEPCTL_EPENA | USB_OTG_DOEPCTL_CNAK);
-    while (_len--) {
-        *_fifo = *(uint32_t*)buf;
-        buf += 4;
+    /* push data to FIFO */
+    tmp = 0;
+    for (int idx = 0; idx < blen; idx++) {
+        tmp |= (uint32_t)((uint8_t*)buf)[idx] << ((idx & 0x03) << 3);
+        if ((idx & 0x03) == 0x03 || (idx + 1) == blen) {
+            *fifo = tmp;
+            tmp = 0;
+        }
     }
     return blen;
 }
@@ -463,4 +464,4 @@ uint16_t get_serialno_desc(void *buffer) {
     get_serialno_desc,
 };
 
-#endif //USBD_STM32L476
+#endif //USBD_STM32L446FS

src/usbd_stm32l476_otgfs.c

@@ -34,15 +34,15 @@ USB_OTG_DeviceTypeDef * const OTGD = (void*)(USB_OTG_FS_PERIPH_BASE + USB_OTG_DE
 volatile uint32_t * const OTGPCTL  = (void*)(USB_OTG_FS_PERIPH_BASE + USB_OTG_PCGCCTL_BASE);
 
 
-inline static volatile uint32_t* EPFIFO(uint8_t ep) {
+inline static uint32_t* EPFIFO(uint32_t ep) {
     return (uint32_t*)(USB_OTG_FS_PERIPH_BASE + USB_OTG_FIFO_BASE + (ep << 12));
 }
 
-inline static USB_OTG_INEndpointTypeDef* EPIN(uint8_t ep) {
+inline static USB_OTG_INEndpointTypeDef* EPIN(uint32_t ep) {
     return (void*)(USB_OTG_FS_PERIPH_BASE + USB_OTG_IN_ENDPOINT_BASE + (ep << 5));
 }
 
-inline static USB_OTG_OUTEndpointTypeDef* EPOUT(uint8_t ep) {
+inline static USB_OTG_OUTEndpointTypeDef* EPOUT(uint32_t ep) {
     return (void*)(USB_OTG_FS_PERIPH_BASE + USB_OTG_OUT_ENDPOINT_BASE + (ep << 5));
 }
 
@@ -342,50 +342,51 @@ void ep_deconfig(uint8_t ep) {
 }
 
 int32_t ep_read(uint8_t ep, void* buf, uint16_t blen) {
-    uint32_t len;
+    uint32_t len, tmp;
+    ep &= 0x7F;
     volatile uint32_t *fifo = EPFIFO(0);
     USB_OTG_OUTEndpointTypeDef* epo = EPOUT(ep);
     /* no data in RX FIFO */
     if (!(OTG->GINTSTS & USB_OTG_GINTSTS_RXFLVL)) return -1;
-    ep &= 0x7F;
     if ((OTG->GRXSTSR & USB_OTG_GRXSTSP_EPNUM) != ep) return -1;
     /* pop data from fifo */
     len = _FLD2VAL(USB_OTG_GRXSTSP_BCNT, OTG->GRXSTSP);
-    for (unsigned i = 0; i < len; i +=4) {
-        uint32_t _t = *fifo;
-        if (blen >= 4) {
-            *(uint32_t*)buf = _t;
-            blen -= 4;
-            buf += 4;
-        } else {
-            while (blen){
-                *((uint8_t*)buf++) = 0xFF & _t;
-                _t >>= 8;
-                blen --;
-            }
+    for (int idx = 0; idx < len; idx++) {
+        if ((idx & 0x03) == 0x00) {
+            tmp = *fifo;
+        }
+        if (idx < blen) {
+            ((uint8_t*)buf)[idx] = tmp & 0xFF;
+            tmp >>= 8;
         }
     }
     _BST(epo->DOEPCTL, USB_OTG_DOEPCTL_CNAK | USB_OTG_DOEPCTL_EPENA);
-    return len;
+    return (len < blen) ? len : blen;
 }
 
 int32_t ep_write(uint8_t ep, void *buf, uint16_t blen) {
+    uint32_t len, tmp;
     ep &= 0x7F;
-    volatile uint32_t* _fifo = EPFIFO(ep);
+    volatile uint32_t* fifo = EPFIFO(ep);
     USB_OTG_INEndpointTypeDef* epi = EPIN(ep);
     /* transfer data size in 32-bit words */
-    uint32_t  _len = (blen + 3) >> 2;
+    len = (blen + 3) >> 2;
     /* no enough space in TX fifo */
-    if (_len > epi->DTXFSTS) return -1;
+    if (len > epi->DTXFSTS) return -1;
     if (ep != 0 && epi->DIEPCTL & USB_OTG_DIEPCTL_EPENA) {
         return -1;
     }
     epi->DIEPTSIZ = 0;
     epi->DIEPTSIZ = (1 << 19) + blen;
     _BMD(epi->DIEPCTL, USB_OTG_DIEPCTL_STALL, USB_OTG_DOEPCTL_EPENA | USB_OTG_DOEPCTL_CNAK);
-    while (_len--) {
-        *_fifo = *(uint32_t*)buf;
-        buf += 4;
+    /* push data to FIFO */
+    tmp = 0;
+    for (int idx = 0; idx < blen; idx++) {
+        tmp |= (uint32_t)((uint8_t*)buf)[idx] << ((idx & 0x03) << 3);
+        if ((idx & 0x03) == 0x03 || (idx + 1) == blen) {
+            *fifo = tmp;
+            tmp = 0;
+        }
     }
     return blen;
 }