#include "kfifo.h"
 
 
void *memcpy(void *__dest, void *__src, int __n)
{
	int i = 0;
	uint8_t *d = (uint8_t *)__dest, *s = (uint8_t *)__src;
 
	for (i = __n >> 3; i > 0; i--) {
		*d++ = *s++;
		*d++ = *s++;
		*d++ = *s++;
		*d++ = *s++;
		*d++ = *s++;
		*d++ = *s++;
		*d++ = *s++;
		*d++ = *s++;
	}
 
	if (__n & 1 << 2) {
		*d++ = *s++;
		*d++ = *s++;
		*d++ = *s++;
		*d++ = *s++;
	}
 
	if (__n & 1 << 1) {
		*d++ = *s++;
		*d++ = *s++;
	}
 
	if (__n & 1)
		*d++ = *s++;
 
	return __dest;
}
 
void kfifo_init(kfifo_t *fifo, uint8_t *pFifoBuffer, uint16_t fifoSize)
{
	fifo->buffer = pFifoBuffer;
	fifo->size = fifoSize;
	fifo->in = fifo->out = 0;
}
 
uint16_t __kfifo_put(kfifo_t *fifo, uint8_t *buffer, uint16_t len)
{
	unsigned int l;
 
	len = min(len, fifo->size - fifo->in + fifo->out);
 
	/* first put the data starting from fifo->in to buffer end */
	l = min(len, fifo->size - (fifo->in & (fifo->size - 1)));
	memcpy(fifo->buffer + (fifo->in & (fifo->size - 1)), buffer, l);
 
	/* then put the rest (if any) at the beginning of the buffer */
	memcpy(fifo->buffer, buffer + l, len - l);

	printf( "buf 11  12  %02x %02x ", fifo->buffer[11], fifo->buffer[12]);
 
	fifo->in += len;
 
	return len;
}
 
void __kfifo_put_singlebyte(kfifo_t *fifo, uint8_t data)
{
	fifo->buffer[(fifo->in++) & (fifo->size - 1)] = data;
}
 
uint16_t __kfifo_get(kfifo_t *fifo, uint8_t *buffer, uint16_t len)//uint8_t
{
	unsigned int l;
 
	len = min(len, fifo->in - fifo->out);
 
	/* first get the data from fifo->out until the end of the buffer */
	l = min(len, fifo->size - (fifo->out & (fifo->size - 1)));
	memcpy(buffer, fifo->buffer + (fifo->out & (fifo->size - 1)), l);
 
	/* then get the rest (if any) from the beginning of the buffer */
	memcpy(buffer + l, fifo->buffer, len - l);
 
	fifo->out += len;
 
	return len;
}