Ha Thach
GitHub
commit
be9f938bd3
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -54,6 +54,90 @@ void test_normal(void)
|
|||
}
|
||||
}
|
||||
|
||||
void test_item_size(void)
|
||||
{
|
||||
TU_FIFO_DEF(ff4, FIFO_SIZE, uint32_t, false);
|
||||
tu_fifo_clear(&ff4);
|
||||
|
||||
uint32_t data[20];
|
||||
for(uint32_t i=0; i<sizeof(data)/4; i++) data[i] = i;
|
||||
|
||||
tu_fifo_write_n(&ff4, data, 10);
|
||||
|
||||
uint32_t rd[10];
|
||||
uint16_t rd_count;
|
||||
|
||||
// read 0 -> 4
|
||||
rd_count = tu_fifo_read_n(&ff4, rd, 5);
|
||||
TEST_ASSERT_EQUAL( 5, rd_count );
|
||||
TEST_ASSERT_EQUAL_UINT32_ARRAY( data, rd, rd_count ); // 0 -> 4
|
||||
|
||||
tu_fifo_write_n(&ff4, data+10, 5);
|
||||
|
||||
// read 5 -> 14
|
||||
rd_count = tu_fifo_read_n(&ff4, rd, 10);
|
||||
TEST_ASSERT_EQUAL( 10, rd_count );
|
||||
TEST_ASSERT_EQUAL_UINT32_ARRAY( data+5, rd, rd_count ); // 5 -> 14
|
||||
}
|
||||
|
||||
void test_read_n(void)
|
||||
{
|
||||
// prepare data
|
||||
uint8_t data[20];
|
||||
for(int i=0; i<sizeof(data); i++) data[i] = i;
|
||||
|
||||
for(uint8_t i=0; i < FIFO_SIZE; i++) tu_fifo_write(&ff, data+i);
|
||||
|
||||
uint8_t rd[10];
|
||||
uint16_t rd_count;
|
||||
|
||||
// case 1: Read index + count < depth
|
||||
// read 0 -> 4
|
||||
rd_count = tu_fifo_read_n(&ff, rd, 5);
|
||||
TEST_ASSERT_EQUAL( 5, rd_count );
|
||||
TEST_ASSERT_EQUAL_MEMORY( data, rd, rd_count ); // 0 -> 4
|
||||
|
||||
// case 2: Read index + count > depth
|
||||
// write 10, 11, 12
|
||||
tu_fifo_write(&ff, data+10);
|
||||
tu_fifo_write(&ff, data+11);
|
||||
tu_fifo_write(&ff, data+12);
|
||||
|
||||
rd_count = tu_fifo_read_n(&ff, rd, 7);
|
||||
TEST_ASSERT_EQUAL( 7, rd_count );
|
||||
|
||||
TEST_ASSERT_EQUAL_MEMORY( data+5, rd, rd_count ); // 5 -> 11
|
||||
|
||||
// Should only read until empty
|
||||
TEST_ASSERT_EQUAL( 1, tu_fifo_read_n(&ff, rd, 100) );
|
||||
}
|
||||
|
||||
void test_write_n(void)
|
||||
{
|
||||
// prepare data
|
||||
uint8_t data[20];
|
||||
for(int i=0; i<sizeof(data); i++) data[i] = i;
|
||||
|
||||
// case 1: wr + count < depth
|
||||
tu_fifo_write_n(&ff, data, 8); // wr = 8, count = 8
|
||||
|
||||
uint8_t rd[10];
|
||||
uint16_t rd_count;
|
||||
|
||||
rd_count = tu_fifo_read_n(&ff, rd, 5); // wr = 8, count = 3
|
||||
TEST_ASSERT_EQUAL( 5, rd_count );
|
||||
TEST_ASSERT_EQUAL_MEMORY( data, rd, rd_count ); // 0 -> 4
|
||||
|
||||
// case 2: wr + count > depth
|
||||
tu_fifo_write_n(&ff, data+8, 6); // wr = 3, count = 9
|
||||
|
||||
for(rd_count=0; rd_count<7; rd_count++) tu_fifo_read(&ff, rd+rd_count); // wr = 3, count = 2
|
||||
|
||||
TEST_ASSERT_EQUAL_MEMORY( data+5, rd, rd_count); // 5 -> 11
|
||||
|
||||
TEST_ASSERT_EQUAL(2, tu_fifo_count(&ff));
|
||||
}
|
||||
|
||||
void test_peek(void)
|
||||
{
|
||||
uint8_t temp;
|
||||
|
|
|
|||
Loading…
Reference in New Issue