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.
88 lines
1.8 KiB
88 lines
1.8 KiB
2 years ago
|
#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;
|
||
|
|
}
|
||
|
|
|
||
|
|
|