You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
93 lines
1.7 KiB
93 lines
1.7 KiB
#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;
|
|
}
|
|
|
|
|