149 lines
2.7 KiB
C
149 lines
2.7 KiB
C
/*
|
|
SFIFO 1.3 Simple portable lock-free FIFO
|
|
|
|
(c) 2000-2002, David Olofson - free software under the terms of the LGPL 2.1
|
|
*/
|
|
|
|
|
|
/*
|
|
-----------------------------------------------------------
|
|
TODO:
|
|
* Is there a way to avoid losing one byte of buffer
|
|
space to avoid extra variables or locking?
|
|
|
|
* Test more compilers and environments.
|
|
-----------------------------------------------------------
|
|
*/
|
|
|
|
#include <string.h>
|
|
#include <stdlib.h>
|
|
|
|
#include "sfifo.h"
|
|
#include "../common/debug.h"
|
|
|
|
/*
|
|
* Alloc buffer, init FIFO etc...
|
|
*/
|
|
SFIFO_SCOPE int sfifo_init(sfifo_t *f, int size)
|
|
{
|
|
memset(f, 0, sizeof(sfifo_t));
|
|
|
|
if(size > SFIFO_MAX_BUFFER_SIZE)
|
|
return -EINVAL;
|
|
|
|
/*
|
|
* Set sufficient power-of-2 size.
|
|
*
|
|
* No, there's no bug. If you need
|
|
* room for N bytes, the buffer must
|
|
* be at least N+1 bytes. (The fifo
|
|
* can't tell 'empty' from 'full'
|
|
* without unsafe index manipulations
|
|
* otherwise.)
|
|
*/
|
|
f->size = 1;
|
|
for(; f->size <= size; f->size <<= 1)
|
|
;
|
|
|
|
/* Get buffer */
|
|
if( 0 == (f->buffer = (void *)malloc(f->size)) )
|
|
return -ENOMEM;
|
|
|
|
return 0;
|
|
}
|
|
|
|
/*
|
|
* Dealloc buffer etc...
|
|
*/
|
|
SFIFO_SCOPE void sfifo_close(sfifo_t *f)
|
|
{
|
|
if(f->buffer) {
|
|
free(f->buffer);
|
|
f->buffer = NULL; /* Prevent double free */
|
|
}
|
|
}
|
|
|
|
/*
|
|
* Empty FIFO buffer
|
|
*/
|
|
SFIFO_SCOPE void sfifo_flush(sfifo_t *f)
|
|
{
|
|
debug("sfifo_flush()");
|
|
/* Reset positions */
|
|
f->readpos = 0;
|
|
f->writepos = 0;
|
|
}
|
|
|
|
/*
|
|
* Write bytes to a FIFO
|
|
* Return number of bytes written, or an error code
|
|
*/
|
|
SFIFO_SCOPE int sfifo_write(sfifo_t *f, const void *_buf, int len)
|
|
{
|
|
int total;
|
|
int i;
|
|
const char *buf = (const char *)_buf;
|
|
|
|
if(!f->buffer)
|
|
return -ENODEV; /* No buffer! */
|
|
|
|
/* total = len = min(space, len) */
|
|
total = sfifo_space(f);
|
|
debug1("sfifo_space() = %d",total);
|
|
if(len > total)
|
|
len = total;
|
|
else
|
|
total = len;
|
|
debug1("sfifo_write() = %d", total);
|
|
|
|
i = f->writepos;
|
|
if(i + len > f->size)
|
|
{
|
|
memcpy(f->buffer + i, buf, f->size - i);
|
|
buf += f->size - i;
|
|
len -= f->size - i;
|
|
i = 0;
|
|
}
|
|
memcpy(f->buffer + i, buf, len);
|
|
f->writepos = i + len;
|
|
|
|
return total;
|
|
}
|
|
|
|
|
|
/*
|
|
* Read bytes from a FIFO
|
|
* Return number of bytes read, or an error code
|
|
*/
|
|
SFIFO_SCOPE int sfifo_read(sfifo_t *f, void *_buf, int len)
|
|
{
|
|
int total;
|
|
int i;
|
|
char *buf = (char *)_buf;
|
|
|
|
if(!f->buffer)
|
|
return -ENODEV; /* No buffer! */
|
|
|
|
/* total = len = min(used, len) */
|
|
total = sfifo_used(f);
|
|
debug1("sfifo_used() = %d",total);
|
|
if(len > total)
|
|
len = total;
|
|
else
|
|
total = len;
|
|
debug1("sfifo_read() = %d", total);
|
|
|
|
i = f->readpos;
|
|
if(i + len > f->size)
|
|
{
|
|
memcpy(buf, f->buffer + i, f->size - i);
|
|
buf += f->size - i;
|
|
len -= f->size - i;
|
|
i = 0;
|
|
}
|
|
memcpy(buf, f->buffer + i, len);
|
|
f->readpos = i + len;
|
|
|
|
return total;
|
|
}
|
|
|