clean up _ff_push_n
This commit is contained in:
parent
d6737fb5fe
commit
9042e973d3
|
@ -168,18 +168,20 @@ static inline void _ff_push(tu_fifo_t* f, void const * data, uint16_t wRel)
|
||||||
// send n items to FIFO WITHOUT updating write pointer
|
// 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)
|
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)
|
||||||
{
|
{
|
||||||
|
uint16_t const nLin = f->depth - wRel;
|
||||||
|
uint16_t const nWrap = n - nLin; // only used if wrapped
|
||||||
|
|
||||||
switch (copy_mode)
|
switch (copy_mode)
|
||||||
{
|
{
|
||||||
case TU_FIFO_COPY_INC:
|
case TU_FIFO_COPY_INC:
|
||||||
if(n <= f->depth-wRel)
|
if(n <= nLin)
|
||||||
{
|
{
|
||||||
// Linear mode only
|
// Linear only
|
||||||
memcpy(f->buffer + (wRel * f->item_size), data, n*f->item_size);
|
memcpy(f->buffer + (wRel * f->item_size), data, n*f->item_size);
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
// Wrap around
|
// Wrap around
|
||||||
uint16_t nLin = f->depth - wRel;
|
|
||||||
|
|
||||||
// Write data to linear part of buffer
|
// Write data to linear part of buffer
|
||||||
memcpy(f->buffer + (wRel * f->item_size), data, nLin*f->item_size);
|
memcpy(f->buffer + (wRel * f->item_size), data, nLin*f->item_size);
|
||||||
|
@ -189,61 +191,51 @@ static void _ff_push_n(tu_fifo_t* f, void const * data, uint16_t n, uint16_t wRe
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case TU_FIFO_COPY_CST_FULL_WORDS: // Intended for hardware buffers from which it can be read word by word only
|
case TU_FIFO_COPY_CST_FULL_WORDS:
|
||||||
if(n <= f->depth-wRel)
|
// Intended for hardware buffers from which it can be read word by word only
|
||||||
|
if(n <= nLin)
|
||||||
{
|
{
|
||||||
// Linear mode only
|
// Linear only
|
||||||
_ff_push_const_addr(f->buffer + (wRel * f->item_size), data, n*f->item_size);
|
_ff_push_const_addr(f->buffer + (wRel * f->item_size), data, n*f->item_size);
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
// Wrap around case
|
// Wrap around case
|
||||||
uint16_t nLin = (f->depth - wRel) * f->item_size;
|
uint16_t nLin_bytes = nLin * f->item_size;
|
||||||
uint16_t nWrap = (n - nLin) * f->item_size;
|
uint16_t nWrap_bytes = nWrap * f->item_size;
|
||||||
|
|
||||||
// Optimize for fast word copies
|
uint8_t* dst = f->buffer + (wRel * f->item_size);
|
||||||
typedef struct{
|
|
||||||
uint32_t val;
|
|
||||||
} __attribute((__packed__)) unaligned_uint32_t;
|
|
||||||
|
|
||||||
unaligned_uint32_t* dst = (unaligned_uint32_t*)(f->buffer + (wRel * f->item_size));
|
// Write full words to linear part of buffer
|
||||||
|
uint16_t nLin_4n_bytes = nLin_bytes & 0xFFFC;
|
||||||
|
_ff_push_const_addr(dst, data, nLin_4n_bytes);
|
||||||
|
dst += nLin_4n_bytes;
|
||||||
|
|
||||||
|
// There could be odd 1-3 bytes before the wrap-around boundary
|
||||||
volatile uint32_t * rx_fifo = (volatile uint32_t *) data;
|
volatile uint32_t * rx_fifo = (volatile uint32_t *) data;
|
||||||
|
uint8_t rem = nLin_bytes & 0x03;
|
||||||
// Write full words of linear part to buffer
|
|
||||||
uint16_t full_words = nLin >> 2;
|
|
||||||
while(full_words--)
|
|
||||||
{
|
|
||||||
dst->val = *rx_fifo;
|
|
||||||
dst++;
|
|
||||||
}
|
|
||||||
|
|
||||||
uint8_t * dst_u8;
|
|
||||||
uint8_t rem = nLin & 0x03;
|
|
||||||
// Handle wrap around
|
|
||||||
if (rem > 0)
|
if (rem > 0)
|
||||||
{
|
{
|
||||||
dst_u8 = (uint8_t *)dst;
|
uint8_t remrem = tu_min16(nWrap_bytes, 4-rem);
|
||||||
uint8_t remrem = tu_min16(nWrap, 4-rem);
|
nWrap_bytes -= remrem;
|
||||||
nWrap -= remrem;
|
|
||||||
uint32_t tmp = *rx_fifo;
|
uint32_t tmp32 = *rx_fifo;
|
||||||
uint8_t * src_u8 = ((uint8_t *) &tmp);
|
uint8_t * src_u8 = ((uint8_t *) &tmp32);
|
||||||
while(rem--)
|
|
||||||
{
|
// Write 1-3 bytes before wrapped boundary
|
||||||
*dst_u8++ = *src_u8++;
|
while(rem--) *dst++ = *src_u8++;
|
||||||
}
|
|
||||||
dst_u8 = f->buffer;
|
// Read more bytes to beginning to complete a word
|
||||||
while(remrem--)
|
dst = f->buffer;
|
||||||
{
|
while(remrem--) *dst++ = *src_u8++;
|
||||||
*dst_u8++ = *src_u8++;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
dst_u8 = f->buffer;
|
dst = f->buffer; // wrap around to beginning
|
||||||
}
|
}
|
||||||
|
|
||||||
// Final part
|
// Write data wrapped part
|
||||||
if (nWrap > 0) _ff_push_const_addr(dst_u8, data, nWrap);
|
if (nWrap_bytes > 0) _ff_push_const_addr(dst, data, nWrap_bytes);
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
@ -284,7 +276,7 @@ static void _ff_pull_n(tu_fifo_t* f, void* p_buffer, uint16_t n, uint16_t rRel,
|
||||||
case TU_FIFO_COPY_CST_FULL_WORDS:
|
case TU_FIFO_COPY_CST_FULL_WORDS:
|
||||||
if ( n <= nLin )
|
if ( n <= nLin )
|
||||||
{
|
{
|
||||||
// Linear mode only
|
// Linear only
|
||||||
_ff_pull_const_addr(p_buffer, f->buffer + (rRel * f->item_size), n*f->item_size);
|
_ff_pull_const_addr(p_buffer, f->buffer + (rRel * f->item_size), n*f->item_size);
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
|
@ -296,13 +288,12 @@ static void _ff_pull_n(tu_fifo_t* f, void* p_buffer, uint16_t n, uint16_t rRel,
|
||||||
|
|
||||||
uint8_t* src = f->buffer + (rRel * f->item_size);
|
uint8_t* src = f->buffer + (rRel * f->item_size);
|
||||||
|
|
||||||
// Read data from linear part of buffer
|
// Read full words from linear part of buffer
|
||||||
uint16_t nLin_4n_bytes = nLin_bytes & 0xFFFC;
|
uint16_t nLin_4n_bytes = nLin_bytes & 0xFFFC;
|
||||||
_ff_pull_const_addr(p_buffer, src, nLin_4n_bytes);
|
_ff_pull_const_addr(p_buffer, src, nLin_4n_bytes);
|
||||||
src += nLin_4n_bytes;
|
src += nLin_4n_bytes;
|
||||||
|
|
||||||
// There could be odd 1-3 bytes before the wrap-around boundary
|
// There could be odd 1-3 bytes before the wrap-around boundary
|
||||||
// Handle wrap around - do it manually as these are only 4 bytes and its faster without memcpy
|
|
||||||
volatile uint32_t * tx_fifo = (volatile uint32_t *) p_buffer;
|
volatile uint32_t * tx_fifo = (volatile uint32_t *) p_buffer;
|
||||||
uint8_t rem = nLin_bytes & 0x03;
|
uint8_t rem = nLin_bytes & 0x03;
|
||||||
if (rem > 0)
|
if (rem > 0)
|
||||||
|
@ -310,17 +301,17 @@ static void _ff_pull_n(tu_fifo_t* f, void* p_buffer, uint16_t n, uint16_t rRel,
|
||||||
uint8_t remrem = tu_min16(nWrap_bytes, 4-rem);
|
uint8_t remrem = tu_min16(nWrap_bytes, 4-rem);
|
||||||
nWrap_bytes -= remrem;
|
nWrap_bytes -= remrem;
|
||||||
|
|
||||||
uint32_t tmp;
|
uint32_t tmp32;
|
||||||
uint8_t * dst_u8 = (uint8_t *)&tmp;
|
uint8_t * dst_u8 = (uint8_t *)&tmp32;
|
||||||
|
|
||||||
// Get 1-3 bytes before wrapped boundary
|
// Read 1-3 bytes before wrapped boundary
|
||||||
while(rem--) *dst_u8++ = *src++;
|
while(rem--) *dst_u8++ = *src++;
|
||||||
|
|
||||||
// Get more bytes from beginning to form a complete word
|
// Read more bytes from beginning to complete a word
|
||||||
src = f->buffer;
|
src = f->buffer;
|
||||||
while(remrem--) *dst_u8++ = *src++;
|
while(remrem--) *dst_u8++ = *src++;
|
||||||
|
|
||||||
*tx_fifo = tmp;
|
*tx_fifo = tmp32;
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
|
|
Loading…
Reference in New Issue