From a60bd0c8ac374e9d96fd7248503cf822306547d8 Mon Sep 17 00:00:00 2001 From: Reinhard Panhuber Date: Tue, 23 Mar 2021 19:33:04 +0100 Subject: [PATCH] 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 | 201 +++++++++++++------ src/common/tusb_fifo.h | 4 +- src/portable/espressif/esp32s2/dcd_esp32s2.c | 4 +- src/portable/microchip/samg/dcd_samg.c | 4 +- src/portable/nuvoton/nuc505/dcd_nuc505.c | 4 +- src/portable/st/synopsys/dcd_synopsys.c | 6 +- 6 files changed, 149 insertions(+), 74 deletions(-) diff --git a/src/common/tusb_fifo.c b/src/common/tusb_fifo.c index dc74489f5..7d5453850 100644 --- a/src/common/tusb_fifo.c +++ b/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,52 +173,77 @@ 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) -{ - switch (copy_mode) - { - case TU_FIFO_COPY_INC: - memcpy(dst, src, len); - break; - - case TU_FIFO_COPY_CST: - _tu_fifo_write_to_const_dst_ptr(dst, src, len); - 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) + switch (copy_mode) { - // 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; + case TU_FIFO_COPY_INC: + 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 - _ff_push_copy_fct(f->buffer + (wRel * f->item_size), data, nLin*f->item_size, copy_mode); + // Write data to linear part of buffer + memcpy(f->buffer + (wRel * f->item_size), data, nLin*f->item_size); - // 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); + // 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_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; } } @@ -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) + switch (copy_mode) { - // Linear mode only - _ff_pull_copy_fct(p_buffer, f->buffer + (rRel * f->item_size), n*f->item_size, copy_mode); - } - else - { - // Wrap around - uint16_t nLin = f->depth - rRel; + 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 - _ff_pull_copy_fct(p_buffer, f->buffer + (rRel * f->item_size), nLin*f->item_size, copy_mode); + // Read data from linear part of buffer + memcpy(p_buffer, f->buffer + (rRel * f->item_size), nLin*f->item_size); - // 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); + // Read data wrapped part + memcpy((uint8_t*)p_buffer + nLin*f->item_size, f->buffer, (n - nLin) * f->item_size); + } + break; + + 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) { diff --git a/src/common/tusb_fifo.h b/src/common/tusb_fifo.h index 4b4fe92ba..93ed6095d 100644 --- a/src/common/tusb_fifo.h +++ b/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); diff --git a/src/portable/espressif/esp32s2/dcd_esp32s2.c b/src/portable/espressif/esp32s2/dcd_esp32s2.c index eb2c05153..28a3bcd07 100644 --- a/src/portable/espressif/esp32s2/dcd_esp32s2.c +++ b/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 { diff --git a/src/portable/microchip/samg/dcd_samg.c b/src/portable/microchip/samg/dcd_samg.c index 07ab9e83f..8b5bebf85 100644 --- a/src/portable/microchip/samg/dcd_samg.c +++ b/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 { diff --git a/src/portable/nuvoton/nuc505/dcd_nuc505.c b/src/portable/nuvoton/nuc505/dcd_nuc505.c index 5d15ca962..67693e335 100644 --- a/src/portable/nuvoton/nuc505/dcd_nuc505.c +++ b/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 { diff --git a/src/portable/st/synopsys/dcd_synopsys.c b/src/portable/st/synopsys/dcd_synopsys.c index e2390fe57..aa927d5a4 100644 --- a/src/portable/st/synopsys/dcd_synopsys.c +++ b/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 {