Fix bug in writing to constant src/dst address.

Copying has to be conduct in full words (at least for STM32). Renamed
copy function to tu_fifo_write_n_const_addr_full_words()

src/common/tusb_fifo.c

@@ -68,7 +68,7 @@ static void tu_fifo_unlock(tu_fifo_mutex_t mutex)
 typedef enum
 {
   TU_FIFO_COPY_INC,                     ///< Copy from/to an increasing source/destination address - default mode
-  TU_FIFO_COPY_CST,                     ///< Copy from/to a constant source/destination address - required for e.g. STM32 to write into USB hardware FIFO
+  TU_FIFO_COPY_CST_FULL_WORDS,                     ///< Copy from/to a constant source/destination address - required for e.g. STM32 to write into USB hardware FIFO
 } tu_fifo_copy_mode_t;
 
 bool tu_fifo_config(tu_fifo_t *f, void* buffer, uint16_t depth, uint16_t item_size, bool overwritable)
@@ -105,7 +105,7 @@ static inline uint16_t _ff_mod(uint16_t idx, uint16_t depth)
 // Intended to be used to read from hardware USB FIFO in e.g. STM32 where all data is read from a constant address
 // Code adapted from dcd_synopsis.c
 // TODO generalize with configurable 1 byte or 4 byte each read
-static void _tu_fifo_read_from_const_src_ptr(void * dst, const void * src, uint16_t len)
+static void _tu_fifo_read_from_const_src_ptr_in_full_words(void * dst, const void * src, uint16_t len)
 {
   uint8_t * dst_u8 = (uint8_t *)dst;
   volatile uint32_t * rx_fifo = (volatile uint32_t *) src;
@@ -138,7 +138,7 @@ static void _tu_fifo_read_from_const_src_ptr(void * dst, const void * src, uint1
 // Intended to be used to write to hardware USB FIFO in e.g. STM32 where all data is written to a constant address
 // Code adapted from dcd_synopsis.c
 // TODO generalize with configurable 1 byte or 4 byte each write
-static void _tu_fifo_write_to_const_dst_ptr(void * dst, const void * src, uint16_t len)
+static void _tu_fifo_write_to_const_dst_ptr_in_full_words(void * dst, const void * src, uint16_t len)
 {
   volatile uint32_t * tx_fifo = (volatile uint32_t *) dst;
   uint8_t * src_u8 = (uint8_t *)src;
@@ -173,55 +173,80 @@ static inline void _ff_push(tu_fifo_t* f, void const * data, uint16_t wRel)
   memcpy(f->buffer + (wRel * f->item_size), data, f->item_size);
 }
 
-static inline void _ff_push_copy_fct(void * dst, const void * src, uint16_t len, tu_fifo_copy_mode_t copy_mode)
-{
-  switch (copy_mode)
-  {
-    case TU_FIFO_COPY_INC:
-      memcpy(dst, src, len);
-      break;
-
-    case TU_FIFO_COPY_CST:
-      _tu_fifo_read_from_const_src_ptr(dst, src, len);
-      break;
-  }
-}
-
-static inline void _ff_pull_copy_fct(void * dst, const void * src, uint16_t len, tu_fifo_copy_mode_t copy_mode)
+// send n items to FIFO WITHOUT updating write pointer
+static void _ff_push_n(tu_fifo_t* f, void const * data, uint16_t n, uint16_t wRel, tu_fifo_copy_mode_t copy_mode)
 {
   switch (copy_mode)
   {
     case TU_FIFO_COPY_INC:
-      memcpy(dst, src, len);
+      if(n <= f->depth-wRel)
+      {
+        // Linear mode only
+        memcpy(f->buffer + (wRel * f->item_size), data, n*f->item_size);
+      }
+      else
+      {
+        // Wrap around
+        uint16_t nLin = f->depth - wRel;
+
+        // Write data to linear part of buffer
+        memcpy(f->buffer + (wRel * f->item_size), data, nLin*f->item_size);
+
+        // Write data wrapped around
+        memcpy(f->buffer, ((uint8_t const*) data) + nLin*f->item_size, (n - nLin) * f->item_size);
+      }
       break;
 
-    case TU_FIFO_COPY_CST:
-      _tu_fifo_write_to_const_dst_ptr(dst, src, len);
+    case TU_FIFO_COPY_CST_FULL_WORDS:   // Intended for hardware buffers from which it can be read word by word only
+      if(n <= f->depth-wRel)
+      {
+        // Linear mode only
+        _tu_fifo_read_from_const_src_ptr_in_full_words(f->buffer + (wRel * f->item_size), data, n*f->item_size);
+      }
+      else
+      {
+        // Wrap around case
+        uint16_t nLin = (f->depth - wRel) * f->item_size;
+        uint16_t nWrap = (n - nLin) * f->item_size;
+
+        uint8_t * dst_u8 = (uint8_t *)(f->buffer + (wRel * f->item_size));
+        volatile uint32_t * rx_fifo = (volatile uint32_t *) data;
+        CFG_TUSB_MEM_ALIGN uint32_t tmp;
+
+        // Write full words of linear part to buffer
+        uint16_t full_words = nLin >> 2;
+        for(uint16_t i = 0; i < full_words; i++) {
+          tmp = *rx_fifo;
+          memcpy(dst_u8, &tmp, 4);
+          //          dst_u8[0] = tmp & 0x000000FF;
+          //          dst_u8[1] = (tmp & 0x0000FF00) >> 8;
+          //          dst_u8[2] = (tmp & 0x00FF0000) >> 16;
+          //          dst_u8[3] = (tmp & 0xFF000000) >> 24;
+          dst_u8 += 4;
+        }
+
+        // Handle wrap around
+        uint8_t rem = nLin - (full_words << 2);
+        uint8_t remrem = 0;
+        if (rem > 0)
+        {
+          tmp = *rx_fifo;
+          memcpy(dst_u8, &tmp, rem);
+          remrem = tu_min16(nWrap, 4-rem);
+          memcpy(f->buffer, ((uint8_t *) &tmp) + rem, remrem);
+          nWrap -= remrem;
+        }
+
+        // Final part
+        if (nWrap > 0)
+        {
+          _tu_fifo_read_from_const_src_ptr_in_full_words(f->buffer + remrem, data, nWrap);
+        }
+      }
       break;
   }
 }
 
-// send n items to FIFO WITHOUT updating write pointer
-static void _ff_push_n(tu_fifo_t* f, void const * data, uint16_t n, uint16_t wRel, tu_fifo_copy_mode_t copy_mode)
-{
-  if(wRel + n <= f->depth)
-  {
-    // Linear mode only
-    _ff_push_copy_fct(f->buffer + (wRel * f->item_size), data, n*f->item_size, copy_mode);
-  }
-  else
-  {
-    // Wrap around
-    uint16_t nLin = f->depth - wRel;
-
-    // Write data to linear part of buffer
-    _ff_push_copy_fct(f->buffer + (wRel * f->item_size), data, nLin*f->item_size, copy_mode);
-
-    // Write data wrapped around
-    _ff_push_copy_fct(f->buffer, ((uint8_t const*) data) + nLin*f->item_size, (n - nLin) * f->item_size, copy_mode);
-  }
-}
-
 // get one item from FIFO WITHOUT updating read pointer
 static inline void _ff_pull(tu_fifo_t* f, void * p_buffer, uint16_t rRel)
 {
@@ -231,24 +256,74 @@ static inline void _ff_pull(tu_fifo_t* f, void * p_buffer, uint16_t rRel)
 // get n items from FIFO WITHOUT updating read pointer
 static void _ff_pull_n(tu_fifo_t* f, void * p_buffer, uint16_t n, uint16_t rRel, tu_fifo_copy_mode_t copy_mode)
 {
-  if(rRel + n <= f->depth)
-  {
-    // Linear mode only
-    _ff_pull_copy_fct(p_buffer, f->buffer + (rRel * f->item_size), n*f->item_size, copy_mode);
-  }
-  else
+  switch (copy_mode)
   {
-    // Wrap around
-    uint16_t nLin = f->depth - rRel;
-
-    // Read data from linear part of buffer
-    _ff_pull_copy_fct(p_buffer, f->buffer + (rRel * f->item_size), nLin*f->item_size, copy_mode);
+    case TU_FIFO_COPY_INC:
+      if(n <= f->depth-rRel)
+      {
+        // Linear mode only
+        memcpy(p_buffer, f->buffer + (rRel * f->item_size), n*f->item_size);
+      }
+      else
+      {
+        // Wrap around
+        uint16_t nLin = f->depth - rRel;
+
+        // Read data from linear part of buffer
+        memcpy(p_buffer, f->buffer + (rRel * f->item_size), nLin*f->item_size);
+
+        // Read data wrapped part
+        memcpy((uint8_t*)p_buffer + nLin*f->item_size, f->buffer, (n - nLin) * f->item_size);
+      }
+      break;
 
-    // Read data wrapped part
-    _ff_pull_copy_fct((uint8_t*)p_buffer + nLin*f->item_size, f->buffer, (n - nLin) * f->item_size, copy_mode);
+    case TU_FIFO_COPY_CST_FULL_WORDS:
+
+      if(n <= f->depth-rRel)
+      {
+        // Linear mode only
+        _tu_fifo_write_to_const_dst_ptr_in_full_words(p_buffer, f->buffer + (rRel * f->item_size), n*f->item_size);
+      }
+      else
+      {
+        // Wrap around case
+        uint16_t nLin = (f->depth - rRel) * f->item_size;
+        uint16_t nWrap = (n - nLin) * f->item_size;
+
+        volatile uint32_t * tx_fifo = (volatile uint32_t *) p_buffer;
+        uint8_t * src_u8 = f->buffer + (rRel * f->item_size);
+        CFG_TUSB_MEM_ALIGN uint32_t tmp;
+
+        // Pushing full available 32 bit words to FIFO
+        uint16_t const full_words = nLin >> 2;
+        for(uint16_t i = 0; i < full_words; i++){
+          memcpy(&tmp, src_u8, 4);
+          *tx_fifo = tmp;
+          src_u8 += 4;
+        }
+
+        // Handle wrap around
+        uint8_t rem = nLin - (full_words << 2);
+        uint8_t remrem = 0;
+        if (rem > 0)
+        {
+          tmp = 0;
+          memcpy(&tmp, src_u8, rem);
+          remrem = tu_min16(nWrap, 4-rem);
+          memcpy(((uint8_t *) &tmp) + rem, f->buffer, remrem);
+          nWrap -= remrem;
+          *tx_fifo = tmp;
+        }
+
+        // Final part
+        if (nWrap > 0)
+        {
+          _tu_fifo_write_to_const_dst_ptr_in_full_words(p_buffer, f->buffer + remrem, nWrap);
+        }
+      }
+      break;
   }
 }
-
 // Advance an absolute pointer
 static uint16_t advance_pointer(tu_fifo_t* f, uint16_t p, uint16_t offset)
 {
@@ -599,9 +674,9 @@ uint16_t tu_fifo_read_n(tu_fifo_t* f, void * buffer, uint16_t n)
   return _tu_fifo_read_n(f, buffer, n, TU_FIFO_COPY_INC);
 }
 
-uint16_t tu_fifo_read_n_const_addr(tu_fifo_t* f, void * buffer, uint16_t n)
+uint16_t tu_fifo_read_n_const_addr_full_words(tu_fifo_t* f, void * buffer, uint16_t n)
 {
-  return _tu_fifo_read_n(f, buffer, n, TU_FIFO_COPY_CST);
+  return _tu_fifo_read_n(f, buffer, n, TU_FIFO_COPY_CST_FULL_WORDS);
 }
 
 /******************************************************************************/
@@ -723,9 +798,9 @@ uint16_t tu_fifo_write_n(tu_fifo_t* f, const void * data, uint16_t n)
     @return Number of written elements
  */
 /******************************************************************************/
-uint16_t tu_fifo_write_n_const_addr(tu_fifo_t* f, const void * data, uint16_t n)
+uint16_t tu_fifo_write_n_const_addr_full_words(tu_fifo_t* f, const void * data, uint16_t n)
 {
-  return _tu_fifo_write_n(f, data, n, TU_FIFO_COPY_CST);
+  return _tu_fifo_write_n(f, data, n, TU_FIFO_COPY_CST_FULL_WORDS);
 }
 
 /******************************************************************************/
@@ -756,7 +831,7 @@ bool tu_fifo_clear(tu_fifo_t *f)
                 Pointer to the FIFO buffer to manipulate
     @param[in]  overwritable
                 Overwritable mode the fifo is set to
-*/
+ */
 /******************************************************************************/
 bool tu_fifo_set_overwritable(tu_fifo_t *f, bool overwritable)
 {
@@ -877,7 +952,7 @@ void tu_fifo_backward_read_pointer(tu_fifo_t *f, uint16_t n)
                     Number of ITEMS to read from buffer
    @return          len
                     Length of linear part IN ITEMS, if zero corresponding pointer ptr is invalid
-*/
+ */
 /******************************************************************************/
 uint16_t tu_fifo_get_linear_read_info(tu_fifo_t *f, uint16_t offset, void **ptr, uint16_t n)
 {
@@ -948,7 +1023,7 @@ uint16_t tu_fifo_get_linear_read_info(tu_fifo_t *f, uint16_t offset, void **ptr,
                     Number of ITEMS to write into buffer
    @return          len
                     Length of linear part IN ITEMS, if zero corresponding pointer ptr is invalid
-*/
+ */
 /******************************************************************************/
 uint16_t tu_fifo_get_linear_write_info(tu_fifo_t *f, uint16_t offset, void **ptr, uint16_t n)
 {

src/common/tusb_fifo.h

@@ -109,11 +109,11 @@ static inline void tu_fifo_config_mutex(tu_fifo_t *f, tu_fifo_mutex_t write_mute
 
 bool     tu_fifo_write                  (tu_fifo_t* f, void const * p_data);
 uint16_t tu_fifo_write_n                (tu_fifo_t* f, void const * p_data, uint16_t n);
-uint16_t tu_fifo_write_n_const_addr     (tu_fifo_t* f, const void * data, uint16_t n);
+uint16_t tu_fifo_write_n_const_addr_full_words    (tu_fifo_t* f, const void * data, uint16_t n);
 
 bool     tu_fifo_read                   (tu_fifo_t* f, void * p_buffer);
 uint16_t tu_fifo_read_n                 (tu_fifo_t* f, void * p_buffer, uint16_t n);
-uint16_t tu_fifo_read_n_const_addr      (tu_fifo_t* f, void * buffer, uint16_t n);
+uint16_t tu_fifo_read_n_const_addr_full_words     (tu_fifo_t* f, void * buffer, uint16_t n);
 
 bool     tu_fifo_peek_at                (tu_fifo_t* f, uint16_t pos, void * p_buffer);
 uint16_t tu_fifo_peek_at_n              (tu_fifo_t* f, uint16_t pos, void * p_buffer, uint16_t n);

src/portable/espressif/esp32s2/dcd_esp32s2.c

@@ -517,7 +517,7 @@ static void receive_packet(xfer_ctl_t *xfer, /* usb_out_endpoint_t * out_ep, */
   if (xfer->ff)
   {
     // Ring buffer
-    tu_fifo_write_n_const_addr(xfer->ff, (const void *) rx_fifo, to_recv_size);
+    tu_fifo_write_n_const_addr_full_words(xfer->ff, (const void *) rx_fifo, to_recv_size);
   }
   else
   {
@@ -573,7 +573,7 @@ static void transmit_packet(xfer_ctl_t *xfer, volatile usb_in_endpoint_t *in_ep,
 
   if (xfer->ff)
   {
-    tu_fifo_read_n_const_addr(xfer->ff, (void *) tx_fifo, to_xfer_size);
+    tu_fifo_read_n_const_addr_full_words(xfer->ff, (void *) tx_fifo, to_xfer_size);
   }
   else
   {

src/portable/microchip/samg/dcd_samg.c

@@ -437,7 +437,7 @@ void dcd_int_handler(uint8_t rhport)
           // write to EP fifo
           if (xfer->ff)
           {
-            tu_fifo_read_n_const_addr(xfer->ff, (void *) &UDP->UDP_FDR[epnum], xact_len);
+            tu_fifo_read_n_const_addr_full_words(xfer->ff, (void *) &UDP->UDP_FDR[epnum], xact_len);
           }
           else
           {
@@ -470,7 +470,7 @@ void dcd_int_handler(uint8_t rhport)
         // Read from EP fifo
         if (xfer->ff)
         {
-          tu_fifo_write_n_const_addr(xfer->ff, (const void *) &UDP->UDP_FDR[epnum], xact_len);
+          tu_fifo_write_n_const_addr_full_words(xfer->ff, (const void *) &UDP->UDP_FDR[epnum], xact_len);
         }
         else
         {

src/portable/nuvoton/nuc505/dcd_nuc505.c

@@ -183,7 +183,7 @@ static void dcd_userEP_in_xfer(struct xfer_ctl_t *xfer, USBD_EP_T *ep)
   /* provided buffers are thankfully 32-bit aligned, allowing most data to be transfered as 32-bit */
   if (xfer->ff)
   {
-    tu_fifo_read_n_const_addr(xfer->ff, (void *) (&ep->EPDAT_BYTE), bytes_now);
+    tu_fifo_read_n_const_addr_full_words(xfer->ff, (void *) (&ep->EPDAT_BYTE), bytes_now);
   }
   else
   {
@@ -657,7 +657,7 @@ void dcd_int_handler(uint8_t rhport)
           /* copy the data from the PC to the previously provided buffer */
           if (xfer->ff)
           {
-            tu_fifo_write_n_const_addr(xfer->ff, (const void *) &ep->EPDAT_BYTE, tu_min16(available_bytes, xfer->total_bytes - xfer->out_bytes_so_far));
+            tu_fifo_write_n_const_addr_full_words(xfer->ff, (const void *) &ep->EPDAT_BYTE, tu_min16(available_bytes, xfer->total_bytes - xfer->out_bytes_so_far));
           }
           else
           {

src/portable/st/synopsys/dcd_synopsys.c

@@ -902,7 +902,7 @@ static void handle_rxflvl_ints(uint8_t rhport, USB_OTG_OUTEndpointTypeDef * out_
       if (xfer->ff)
       {
         // Ring buffer
-        tu_fifo_write_n_const_addr(xfer->ff, (const void *) rx_fifo, bcnt);
+        tu_fifo_write_n_const_addr_full_words(xfer->ff, (const void *) rx_fifo, bcnt);
       }
       else
       {
@@ -1008,7 +1008,7 @@ static void handle_epin_ints(uint8_t rhport, USB_OTG_DeviceTypeDef * dev, USB_OT
 
         // Process every single packet (only whole packets can be written to fifo)
         for(uint16_t i = 0; i < remaining_packets; i++){
-          uint16_t remaining_bytes = (in_ep[n].DIEPTSIZ & USB_OTG_DIEPTSIZ_XFRSIZ_Msk) >> USB_OTG_DIEPTSIZ_XFRSIZ_Pos;
+          volatile uint16_t remaining_bytes = (in_ep[n].DIEPTSIZ & USB_OTG_DIEPTSIZ_XFRSIZ_Msk) >> USB_OTG_DIEPTSIZ_XFRSIZ_Pos;
           // Packet can not be larger than ep max size
           uint16_t packet_size = tu_min16(remaining_bytes, xfer->max_size);
 
@@ -1022,7 +1022,7 @@ static void handle_epin_ints(uint8_t rhport, USB_OTG_DeviceTypeDef * dev, USB_OT
           if (xfer->ff)
           {
             usb_fifo_t tx_fifo = FIFO_BASE(rhport, n);
-            tu_fifo_read_n_const_addr(xfer->ff, (void *) tx_fifo, packet_size);
+            tu_fifo_read_n_const_addr_full_words(xfer->ff, (void *) tx_fifo, packet_size);
           }
           else
           {