kingkevin
/
esp32-s2_dfu
1
0
Fork 0
Code Issues Releases Activity

refactor copy to and from fifo

This commit is contained in:
hathach 2020-05-14 14:24:55 +07:00
parent f445274634
commit b0d49e55de
1 changed files with 46 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

85
src/common/tusb_fifo.c
View File

@ -71,33 +71,38 @@ bool tu_fifo_config(tu_fifo_t *f, void* buffer, uint16_t depth, uint16_t item_si
return true;
}
static inline uint16_t _ff_mod(uint16_t idx, uint16_t depth)
{
return (idx < depth) ? idx : (idx-depth);
}
// retrieve data from fifo
static void _tu_ff_pull(tu_fifo_t* f, void * buffer)
static inline void _ff_pull(tu_fifo_t* f, void * buffer, uint16_t n)
{
memcpy(buffer,
f->buffer + (f->rd_idx * f->item_size),
f->item_size);
f->item_size*n);
f->rd_idx = (f->rd_idx + 1) % f->depth;
f->count--;
f->rd_idx = _ff_mod(f->rd_idx + n, f->depth);
f->count -= n;
}
// send data to fifo
static void _tu_ff_push(tu_fifo_t* f, void const * data)
static inline void _ff_push(tu_fifo_t* f, void const * data, uint16_t n)
{
memcpy( f->buffer + (f->wr_idx * f->item_size),
data,
f->item_size);
memcpy(f->buffer + (f->wr_idx * f->item_size),
data,
f->item_size*n);
f->wr_idx = (f->wr_idx + 1) % f->depth;
f->wr_idx = _ff_mod(f->wr_idx + n, f->depth);
if (tu_fifo_full(f))
{
f->rd_idx = f->wr_idx; // keep the full state (rd == wr && len = size)
f->rd_idx = f->wr_idx; // keep the full state (rd == wr && count = depth)
}
else
{
f->count++;
f->count += n;
}
}
@ -123,7 +128,7 @@ bool tu_fifo_read(tu_fifo_t* f, void * buffer)
tu_fifo_lock(f);
_tu_ff_pull(f, buffer);
_ff_pull(f, buffer, 1);
tu_fifo_unlock(f);
@ -152,26 +157,24 @@ uint16_t tu_fifo_read_n (tu_fifo_t* f, void * buffer, uint16_t count)
tu_fifo_lock(f);
/* Limit up to fifo's count */
if(count > f->count)
count = f->count;
// Limit up to fifo's count
if(count > f->count) count = f->count;
if(count + f->rd_idx <= f->depth)
{
memcpy(buffer, f->buffer + f->rd_idx * f->item_size, count * f->item_size);
_ff_pull(f, buffer, count);
}
else
{
uint16_t part1 = (f->depth - f->rd_idx) * f->item_size;
memcpy(buffer, f->buffer + f->rd_idx * f->item_size, part1);
memcpy((uint8_t*)buffer + part1, f->buffer, count * f->item_size - part1);
uint16_t const part1 = f->depth - f->rd_idx;
// Part 1: from rd_idx to end
_ff_pull(f, buffer, part1);
buffer = ((uint8_t*) buffer) + part1*f->item_size;
// Part 2: start to remaining
_ff_pull(f, buffer, count-part1);
}
f->rd_idx += count;
if (f->rd_idx >= f->depth)
f->rd_idx -= f->depth;
f->count -= count;
tu_fifo_unlock(f);
@ -196,12 +199,16 @@ bool tu_fifo_peek_at(tu_fifo_t* f, uint16_t pos, void * p_buffer)
{
if ( pos >= f->count ) return false;
tu_fifo_lock(f);
// rd_idx is pos=0
uint16_t index = (f->rd_idx + pos) % f->depth;
uint16_t index = _ff_mod(f->rd_idx + pos, f->depth);
memcpy(p_buffer,
f->buffer + (index * f->item_size),
f->item_size);
tu_fifo_unlock(f);
return true;
}
@ -228,7 +235,7 @@ bool tu_fifo_write (tu_fifo_t* f, const void * data)
tu_fifo_lock(f);
_tu_ff_push(f, data);
_ff_push(f, data, 1);
tu_fifo_unlock(f);
@ -257,14 +264,15 @@ uint16_t tu_fifo_write_n (tu_fifo_t* f, const void * data, uint16_t count)
tu_fifo_lock(f);
uint8_t const* buf8 = (uint8_t const*) data;
// Not overwritable limit up to full
if (!f->overwritable)
{
// Not overwritable limit up to full
count = tu_min16(count, tu_fifo_remaining(f));
}
// Only copy last part
else if (count > f->depth)
{
// Only copy last part
buf8 = buf8 + (count - f->depth) * f->item_size;
count = f->depth;
f->wr_idx = 0;
@ -274,21 +282,20 @@ uint16_t tu_fifo_write_n (tu_fifo_t* f, const void * data, uint16_t count)
if (count + f->wr_idx <= f->depth )
{
memcpy(f->buffer + f->wr_idx * f->item_size, buf8, count * f->item_size);
_ff_push(f, buf8, count);
}
else
{
uint16_t part1 = (f->depth - f->wr_idx) * f->item_size;
memcpy(f->buffer + f->wr_idx * f->item_size, buf8, part1);
memcpy(f->buffer, buf8 + part1, count * f->item_size - part1);
uint16_t const part1 = f->depth - f->wr_idx;
// Part 1: from wr_idx to end
_ff_push(f, buf8, part1);
buf8 += part1*f->item_size;
// Part 2: start to remaining
_ff_push(f, buf8, count-part1);
}
f->wr_idx += count;
if (f->wr_idx >= f->depth)
f->wr_idx -= f->depth;
f->count += count;
tu_fifo_unlock(f);
return count;