serval-dna/fifo.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;
}