pinebuds/utils/kfifo/kfifo.c

145 lines
3.8 KiB
C

#include "kfifo.h"
#include "hal_trace.h"
#include <stdio.h>
#include <string.h>
static int is_power_of_2(unsigned int n) {
int r = 0;
if (n != 0 && ((n & (n - 1)) == 0))
r = 1;
return r;
}
void kfifo_init(struct kfifo *fifo, unsigned char *buffer, unsigned int len) {
ASSERT(is_power_of_2(len), "kfifo_init : len %d not power of 2!!!!!", len);
fifo->size = len;
fifo->buffer = buffer;
fifo->in = fifo->out = 0;
}
unsigned int kfifo_put(struct kfifo *fifo, unsigned char *buffer,
unsigned int len) {
unsigned int l;
len = MIN(len, fifo->size - fifo->in + fifo->out);
__sync_synchronize();
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);
__sync_synchronize();
fifo->in += len;
return len;
}
unsigned int kfifo_get(struct kfifo *fifo, unsigned char *buffer,
unsigned int len) {
unsigned int l;
len = MIN(len, fifo->in - fifo->out);
__sync_synchronize();
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);
__sync_synchronize();
fifo->out += len;
return len;
}
unsigned int kfifo_peek(struct kfifo *fifo, unsigned int len_want,
unsigned char **buff1, unsigned char **buff2,
unsigned int *len1, unsigned int *len2) {
unsigned int l, len;
*buff1 = *buff2 = NULL;
*len1 = *len2 = 0;
len = MIN(len_want, fifo->in - fifo->out);
if (len < len_want) {
return 0;
}
__sync_synchronize();
l = MIN(len, fifo->size - (fifo->out & (fifo->size - 1)));
*buff1 = fifo->buffer + (fifo->out & (fifo->size - 1));
*len1 = l;
if (l < len) {
*buff2 = fifo->buffer;
*len2 = len - l;
}
return len_want;
}
unsigned int kfifo_peek_to_buf(struct kfifo *fifo, unsigned char *buff,
unsigned int len) {
unsigned char *buf1 = NULL, *buf2 = NULL;
unsigned int len1 = 0, len2 = 0;
kfifo_peek(fifo, len, &buf1, &buf2, &len1, &len2);
if (len == (len1 + len2)) {
memcpy(buff, buf1, len1);
memcpy(buff + len1, buf2, len2);
} else {
return 0;
}
return len;
}
unsigned int kfifo_len(struct kfifo *fifo) { return (fifo->in - fifo->out); }
#if 0
struct kfifo test_kfifo;
unsigned char kfifo_buffer[32];
void kfifo_test(void)
{
unsigned char b[10];
kfifo_init(&test_kfifo, kfifo_buffer, 32);
TRACE(1,"init 32 : fifo size %d", test_kfifo.size);
kfifo_put(&test_kfifo, "1234567890", 10);
TRACE(1,"put 10 : fifo len %d", kfifo_len(&test_kfifo));
kfifo_put(&test_kfifo, "abcdefghij", 10);
TRACE(1,"put 10 : fifo len %d", kfifo_len(&test_kfifo));
kfifo_put(&test_kfifo, "!@#$%^&*()", 10);
TRACE(1,"put 10 : fifo len %d", kfifo_len(&test_kfifo));
kfifo_put(&test_kfifo, "VVVV", 4);
TRACE(1,"put 4 : fifo len %d", kfifo_len(&test_kfifo));
kfifo_get(&test_kfifo, b, 2);
TRACE(1,"get 2 : fifo len %d", kfifo_len(&test_kfifo));
TRACE(0,"data:");
for(int i = 0; i < 2; ++i) {
TRACE(1,"%c-", b[i]);
}
TRACE(0," ");
kfifo_get(&test_kfifo, b, 10);
TRACE(1,"get 10 : fifo len %d", kfifo_len(&test_kfifo));
TRACE(0,"data:");
for(int i = 0; i < 10; ++i) {
TRACE(1,"%c-", b[i]);
}
TRACE(0," ");
kfifo_get(&test_kfifo, b, 10);
TRACE(1,"get 10 : fifo len %d", kfifo_len(&test_kfifo));
TRACE(0,"data:");
for(int i = 0; i < 10; ++i) {
TRACE(1,"%c-", b[i]);
}
TRACE(0," ");
kfifo_get(&test_kfifo, b, 10);
TRACE(1,"get 10 : fifo len %d", kfifo_len(&test_kfifo));
TRACE(0,"data:");
for(int i = 0; i < 10; ++i) {
TRACE(1,"%c-", b[i]);
}
TRACE(0," ");
}
#endif