mirror of
https://github.com/servalproject/serval-dna.git
synced 2024-12-27 16:38:53 +00:00
184 lines
3.9 KiB
C
184 lines
3.9 KiB
C
/*
|
|
* This is a simple FIFO implementation using a circular buffer.
|
|
*
|
|
* Heavily inspired by http://lwn.net/Articles/101808/
|
|
*
|
|
* Could probably generalise in a similar fashion to sys/queue.h
|
|
*
|
|
*/
|
|
|
|
#include <assert.h>
|
|
#include <pthread.h>
|
|
#include <stdlib.h>
|
|
#include <stdint.h>
|
|
#include <string.h>
|
|
|
|
#define min(a, b) \
|
|
({ __typeof__ (a) _a = (a); \
|
|
__typeof__ (b) _b = (b); \
|
|
_a < _b ? _a : _b; })
|
|
|
|
struct fifo {
|
|
unsigned int rdidx;
|
|
unsigned int wridx;
|
|
unsigned int size;
|
|
unsigned int len;
|
|
uint8_t buffer[0];
|
|
};
|
|
|
|
/*
|
|
* fifo_alloc - allocates a new FIFO
|
|
* @size: the size of the internal buffer.
|
|
*
|
|
*/
|
|
struct fifo *
|
|
fifo_alloc(unsigned int size) {
|
|
struct fifo *fifo;
|
|
|
|
if ((fifo = malloc(sizeof(struct fifo) + size)) == NULL)
|
|
return NULL;
|
|
|
|
fifo->rdidx = fifo->wridx = 0;
|
|
fifo->size = size;
|
|
fifo->len = 0;
|
|
|
|
return fifo;
|
|
}
|
|
|
|
/*
|
|
* fifo_free - frees the FIFO
|
|
* @fifo: the fifo to be freed.
|
|
*/
|
|
void
|
|
fifo_free(struct fifo *fifo) {
|
|
free(fifo);
|
|
}
|
|
|
|
/*
|
|
* fifo_reset - removes the entire FIFO contents
|
|
* @fifo: the fifo to be emptied.
|
|
*/
|
|
void
|
|
fifo_reset(struct fifo *fifo) {
|
|
|
|
fifo->rdidx = fifo->wridx = 0;
|
|
fifo->len = 0;
|
|
}
|
|
|
|
/*
|
|
* fifo_put - puts some data into the FIFO
|
|
* @fifo: the fifo to be used.
|
|
* @buffer: the data to be added.
|
|
* @len: the length of the data to be added.
|
|
*
|
|
* This function copies at most 'len' bytes from the 'buffer' into
|
|
* the FIFO depending on the free space, and returns the number of
|
|
* bytes copied.
|
|
*/
|
|
unsigned int
|
|
fifo_put(struct fifo *fifo, uint8_t *buffer, unsigned int len) {
|
|
unsigned int total, remaining;
|
|
|
|
total = remaining = min(len, fifo->size - fifo->len);
|
|
while (remaining > 0) {
|
|
unsigned int l = min(remaining, fifo->size - fifo->wridx);
|
|
memcpy(fifo->buffer + fifo->wridx, buffer, l);
|
|
fifo->wridx += l;
|
|
fifo->wridx %= fifo->size;
|
|
fifo->len += l;
|
|
buffer += l;
|
|
remaining -= l;
|
|
}
|
|
|
|
return total;
|
|
}
|
|
|
|
/*
|
|
* fifo_get - gets some data from the FIFO
|
|
* @fifo: the fifo to be used.
|
|
* @buffer: where the data must be copied.
|
|
* @len: the size of the destination buffer.
|
|
*
|
|
* This function copies at most 'len' bytes from the FIFO into the
|
|
* 'buffer' and returns the number of copied bytes.
|
|
*/
|
|
unsigned int
|
|
fifo_get(struct fifo *fifo, uint8_t *buffer, unsigned int len) {
|
|
unsigned int total, remaining;
|
|
|
|
total = remaining = min(len, fifo->len);
|
|
while (remaining > 0) {
|
|
unsigned int l = min(remaining, fifo->size - fifo->rdidx);
|
|
memcpy(buffer, fifo->buffer + fifo->rdidx, l);
|
|
fifo->rdidx += l;
|
|
fifo->rdidx %= fifo->size;
|
|
fifo->len -= l;
|
|
buffer += l;
|
|
remaining -= l;
|
|
}
|
|
|
|
return total;
|
|
}
|
|
|
|
/*
|
|
* fifo_unget - puts some data into the FIFO head
|
|
* @fifo: the fifo to be used.
|
|
* @buffer: the data to be added.
|
|
* @len: the length of the data to be added.
|
|
*
|
|
* This function copies at most 'len' bytes from the 'buffer' into
|
|
* the FIFO depending on the free space, and returns the number of
|
|
* bytes copied.
|
|
*/
|
|
unsigned int
|
|
fifo_unget(struct fifo *fifo, uint8_t *buffer, unsigned int len) {
|
|
unsigned int total, remaining, l;
|
|
int dst;
|
|
|
|
total = remaining = min(len, fifo->size - fifo->len);
|
|
|
|
/* Index to start putting data back */
|
|
dst = fifo->rdidx - len;
|
|
while (dst < 0)
|
|
dst += fifo->size;
|
|
|
|
while (remaining > 0) {
|
|
l = min(remaining, fifo->size - dst);
|
|
memcpy(fifo->buffer + dst, buffer, l);
|
|
|
|
fifo->len += l;
|
|
buffer += l;
|
|
remaining -= l;
|
|
}
|
|
|
|
fifo->rdidx = dst;
|
|
|
|
return total;
|
|
}
|
|
|
|
/*
|
|
* fifo_avail - returns the number of bytes available for reading in the FIFO
|
|
* @fifo: the fifo to be used.
|
|
*/
|
|
unsigned int
|
|
fifo_avail(struct fifo *fifo) {
|
|
unsigned int result;
|
|
|
|
result = fifo->len;
|
|
|
|
return result;
|
|
}
|
|
|
|
/*
|
|
* fifo_space - returns the number of bytes available for writing in the FIFO
|
|
* @fifo: the fifo to be used.
|
|
*/
|
|
unsigned int
|
|
fifo_space(struct fifo *fifo) {
|
|
unsigned int result;
|
|
|
|
result = fifo->size - fifo->len;
|
|
|
|
return result;
|
|
}
|