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()
This commit is contained in:
Reinhard Panhuber 2021-03-23 19:33:04 +01:00
parent cd491e296e
commit a60bd0c8ac
6 changed files with 149 additions and 74 deletions

View File

@ -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)
{

View File

@ -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);

View File

@ -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
{

View File

@ -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
{

View File

@ -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
{

View File

@ -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
{