139 lines
3.8 KiB
C
139 lines
3.8 KiB
C
// dfifo.c
|
|
//
|
|
// Author: J.A. de Jong -ASCEE
|
|
//
|
|
// Description:
|
|
// Implementation of the dFifo queue
|
|
//////////////////////////////////////////////////////////////////////
|
|
|
|
#include "dfifo.h"
|
|
#define DFIFO_QUEUE_MAX_BLOCKS (50)
|
|
|
|
typedef struct dFifo_s {
|
|
dmat queue;
|
|
us start_row;
|
|
us end_row;
|
|
} dFifo;
|
|
us dFifo_size(dFifo* fifo) {
|
|
fsTRACE(15);
|
|
dbgassert(fifo,NULLPTRDEREF);
|
|
dbgassert(fifo->start_row <= fifo->end_row,"BUG");
|
|
feTRACE(15);
|
|
|
|
return fifo->end_row-fifo->start_row;
|
|
}
|
|
dFifo* dFifo_create(const us nchannels,
|
|
const us max_size) {
|
|
|
|
fsTRACE(15);
|
|
dFifo* fifo = a_malloc(sizeof(dFifo));
|
|
fifo->queue = dmat_alloc(max_size,nchannels);
|
|
fifo->start_row = 0;
|
|
fifo->end_row = 0;
|
|
feTRACE(15);
|
|
return fifo;
|
|
}
|
|
void dFifo_free(dFifo* fifo) {
|
|
fsTRACE(15);
|
|
dmat_free(&fifo->queue);
|
|
a_free(fifo);
|
|
feTRACE(15);
|
|
}
|
|
int dFifo_push(dFifo* fifo,const dmat* data) {
|
|
fsTRACE(15);
|
|
dbgassert(fifo && data, NULLPTRDEREF);
|
|
dbgassert(data->n_cols == fifo->queue.n_cols,
|
|
"Invalid number of columns in data");
|
|
|
|
|
|
dmat queue = fifo->queue;
|
|
const us max_size = queue.n_rows;
|
|
const us nchannels = queue.n_cols;
|
|
|
|
us* start_row = &fifo->start_row;
|
|
us* end_row = &fifo->end_row;
|
|
const us added_size = data->n_rows;
|
|
const us size_before = dFifo_size(fifo);
|
|
|
|
if(added_size + dFifo_size(fifo) > max_size) {
|
|
return FIFO_QUEUE_FULL;
|
|
}
|
|
|
|
if(*end_row + added_size > max_size) {
|
|
if(size_before != 0) {
|
|
/* Shift the samples to the front of the queue. TODO: this
|
|
* might not be the most optimal implementation (but it is the
|
|
* most simple). */
|
|
TRACE(15,"Shift samples back in buffer");
|
|
uVARTRACE(15,size_before);
|
|
dmat tmp = dmat_alloc(size_before,nchannels);
|
|
TRACE(15,"SFSG");
|
|
dmat_copy_rows(&tmp,
|
|
&queue,
|
|
0,
|
|
*start_row,
|
|
size_before);
|
|
TRACE(15,"SFSG");
|
|
dmat_copy_rows(&queue,
|
|
&tmp,
|
|
0,0,size_before);
|
|
|
|
*end_row -= *start_row;
|
|
*start_row = 0;
|
|
|
|
dmat_free(&tmp);
|
|
}
|
|
else {
|
|
*start_row = 0;
|
|
*end_row = 0;
|
|
}
|
|
}
|
|
|
|
/* Now, copy samples */
|
|
dmat_copy_rows(&queue, /* to */
|
|
data, /* from */
|
|
*end_row, /* startrow_to */
|
|
0, /* startrow_from */
|
|
added_size); /* n_rows */
|
|
|
|
/* Increase the size */
|
|
*end_row += added_size;
|
|
|
|
feTRACE(15);
|
|
return SUCCESS;
|
|
}
|
|
int dFifo_pop(dFifo* fifo,dmat* data,const us keep) {
|
|
fsTRACE(15);
|
|
dbgassert(fifo && data,NULLPTRDEREF);
|
|
dbgassert(data->n_cols == fifo->queue.n_cols,
|
|
"Invalid number of columns in data");
|
|
dbgassert(keep < data->n_rows, "Number of samples to keep should"
|
|
" be smaller than requested number of samples");
|
|
|
|
us* start_row = &fifo->start_row;
|
|
us* end_row = &fifo->end_row;
|
|
us cur_contents = dFifo_size(fifo);
|
|
us requested = data->n_rows;
|
|
|
|
us obtained = requested > cur_contents ? cur_contents : requested;
|
|
dbgassert(obtained > keep,"Number of samples to keep should be"
|
|
" smaller than requested number of samples");
|
|
|
|
uVARTRACE(15,requested);
|
|
uVARTRACE(15,obtained);
|
|
uVARTRACE(15,*start_row);
|
|
uVARTRACE(15,*end_row);
|
|
|
|
dmat_copy_rows(data,
|
|
&fifo->queue,
|
|
0,
|
|
*start_row,
|
|
obtained);
|
|
|
|
*start_row += obtained - keep;
|
|
|
|
feTRACE(15);
|
|
return (int) obtained;
|
|
}
|
|
//////////////////////////////////////////////////////////////////////
|