stm32f4x/lib/fifo.c

#include <stdlib.h>
#include <string.h>
#include <stdint.h>
#include <stdio.h>
#include "fifo.h"

#undef min
#undef is_power_of_2

#define is_power_of_2(x) (((x) != 0) && (((x) & (x - 1)) == 0))
#define min(a, b) (((a) < (b)) ? (a) : (b))

static unsigned int roundup_pow_of_two(unsigned int x)
{
	int i = 0;
	
	if (x != 0) {
		if (is_power_of_2(x)) {
			return x;	
		}
		
		do{
			++i;
			x >>= 1;	
		} while(x > 0);	
	}
	
	return 1UL << i;	
}

int fifo_init(fifo_t fifo, void *buf, unsigned int len)
{
    unsigned int size = len;

    if (!is_power_of_2(len)) { /* round down to 2'n */
        size = roundup_pow_of_two(len);
        size >>= 1;
    }
    fifo->buffer = buf;
    fifo->size   = size;
    fifo->in     = 0;
    fifo->out    = 0;

    return (int) size;
}

unsigned int fifo_put(fifo_t fifo, const unsigned char *buffer, unsigned int len)
{
	unsigned int l;

	len = min(len, fifo->size - fifo->in + fifo->out);
	l = min(len, fifo->size - (fifo->in & (fifo->size - 1)));
	memcpy(fifo->buffer + (fifo->in & (fifo->size - 1)), buffer, l);
	memcpy(fifo->buffer, buffer + l, len - l);
	fifo->in += len;

	return len;
}

unsigned int fifo_get(fifo_t fifo, unsigned char *buffer, unsigned int len)
{
	unsigned int l;

	len = min(len, fifo->in - fifo->out);
	l = min(len, fifo->size - (fifo->out & (fifo->size - 1)));
	memcpy(buffer, fifo->buffer + (fifo->out & (fifo->size - 1)), l);
	memcpy(buffer + l, fifo->buffer, len - l);
	fifo->out += len;

	return len;
}

void fifo_reset(fifo_t fifo)
{
	fifo->in = fifo->out = 0;
}

unsigned int fifo_len(struct fifo *fifo)
{
	unsigned int ret;

	ret =  fifo->in - fifo->out;

	return ret;
}

unsigned int fifo_empty(struct fifo *fifo)
{
	unsigned int ret;
	ret = fifo->size - (fifo->in - fifo->out);
	return ret;	
}