移植内核kfifo
代码原理基于内核kfifo的原理移植
rkfifo.c
#include "rkfifo.h"
#define is_power_of_2(x) ((x) != 0 && (((x) & ((x) - 1)) == 0))
static uint32_t CLZ_32(uint32_t n)
{
int ret=0;
uint32_t tmp = ~n;
while(tmp&0x80000000)
{
tmp <<= 1;
ret++;
}
return ret;
}
uint32_t rounddown_pow_of_two(uint32_t n)
{
uint32_t ret;
if(n==0){
return 0;
}
if((n & (n-1)) == 0){
return n;
}
ret = CLZ_32(n);
return 1<<(32-ret);
}
static inline unsigned int kfifo_unused(struct __rkfifo *fifo)
{
return (fifo->mask + 1) - (fifo->in - fifo->out);
}
int rkfifo_alloc(struct __rkfifo *fifo, uint32_t size,
uint32_t esize, int gfp_mask)
{
/*
* round down to the next power of 2, since our 'let the indices
* wrap' technique works only in this case.
*/
size /= esize;
if (!is_power_of_2(size)){
//size = rounddown_pow_of_two(size);
fifo->data = NULL;
fifo->mask = 0;
return -EINVAL;
}
fifo->in = 0;
fifo->out = 0;
fifo->esize = esize;
if (size < 2) {
fifo->data = NULL;
fifo->mask = 0;
return -EINVAL;
}
fifo->data = RK_MALLOC(size * esize);
if (!fifo->data) {
fifo->mask = 0;
return -ENOMEM;
}
fifo->mask = size - 1;
return 0;
}
void rkfifo_free(struct __rkfifo *fifo)
{
RK_FREE(fifo->data);
fifo->in = 0;
fifo->out = 0;
fifo->esize = 0;
fifo->data = NULL;
fifo->mask = 0;
}
/*保证 size/esize 是2的n次幂*/
int rkfifo_init(struct __rkfifo *fifo, void *buffer,
uint32_t size, uint32_t esize)
{
size /= esize;
if (!is_power_of_2(size)){
fifo->mask = 0;
//size = rounddown_pow_of_two(size);
return -EINVAL;
}
fifo->in = 0;
fifo->out = 0;
fifo->esize = esize;
fifo->data = buffer;
if (size < 2) {
fifo->mask = 0;
return -EINVAL;
}
fifo->mask = size - 1;
return 0;
}
static void kfifo_copy_in(struct __rkfifo *fifo, const void *src,
unsigned int len, unsigned int off)
{
unsigned int size = fifo->mask + 1;
unsigned int esize = fifo->esize;
unsigned int l;
off &= fifo->mask;
if (esize != 1) {
off *= esize;
size *= esize;
len *= esize;
}
l = min(len, size - off);
memcpy(fifo->data + off, src, l);
memcpy(fifo->data, src + l, len - l);
/*
* make sure that the data in the fifo is up to date before
* incrementing the fifo->in index counter
*/
//smp_wmb();
}
unsigned int rkfifo_in(struct __rkfifo *fifo,
const void *buf, unsigned int len)
{
unsigned int l;
l = kfifo_unused(fifo);
if (len > l)
len = l;
kfifo_copy_in(fifo, buf, len, fifo->in);
fifo->in += len;
return len;
}
static void kfifo_copy_out(struct __rkfifo *fifo, void *dst,
unsigned int len, unsigned int off)
{
unsigned int size = fifo->mask + 1;
unsigned int esize = fifo->esize;
unsigned int l;
off &= fifo->mask;
if (esize != 1) {
off *= esize;
size *= esize;
len *= esize;
}
l = min(len, size - off);
memcpy(dst, fifo->data + off, l);
memcpy(dst + l, fifo->data, len - l);
/*
* make sure that the data is copied before
* incrementing the fifo->out index counter
*/
//smp_wmb();
}
unsigned int rkfifo_out_peek(struct __rkfifo *fifo,
void *buf, unsigned int len)
{
unsigned int l;
l = fifo->in - fifo->out;
if (len > l)
len = l;
kfifo_copy_out(fifo, buf, len, fifo->out);
return len;
}
unsigned int rkfifo_out(struct __rkfifo *fifo,
void *buf, unsigned int len)
{
len = rkfifo_out_peek(fifo, buf, len);
fifo->out += len;
return len;
}
rkfifo.h
#ifndef __RKFIFO_H
#define __RKFIFO_H
#include <stdlib.h>
#include <stdint.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
struct __rkfifo {
unsigned int in;
unsigned int out;
unsigned int mask;
unsigned int esize;
void *data;
};
#define min(a,b) a>b ? b : a
#define ENOMEM 12 /* Out of Memory */
#define EINVAL 22 /* Invalid argument */
#define ENOSPC 28 /* No space left on device */
#define RK_MALLOC(a) malloc(a)
#define RK_FREE(a) free(a)
#define EXP_2_N(a) do{}while(0);
uint32_t rounddown_pow_of_two(uint32_t n);
int rkfifo_alloc(struct __rkfifo *fifo, uint32_t size,uint32_t esize, int gfp_mask);
void rkfifo_free(struct __rkfifo *fifo);
int rkfifo_init(struct __rkfifo *fifo, void *buffer,uint32_t size, uint32_t esize);
unsigned int rkfifo_in(struct __rkfifo *fifo,const void *buf, unsigned int len);
unsigned int rkfifo_out_peek(struct __rkfifo *fifo,void *buf, unsigned int len);
unsigned int rkfifo_out(struct __rkfifo *fifo,void *buf, unsigned int len);
#endif
main.c
#include <stdlib.h>
#include <stdint.h>
#include <stdio.h>
#include "rkfifo.h"
struct _tt{
int a;
char b[16];
};
struct _tt test []={
{0,"aaa"},
{1,"bbb"},
{2,"ccc"},
{3,"ddd"},
{4,"eee"},
{5,"fff"},
{6,"ggg"},
};
int main()
{
int i=0;
int len=0;
struct _tt temp[4];
struct __rkfifo tfifo;
char buffer[4*sizeof(struct _tt)];
printf("rounddown_pow_of_two = %d\n",rounddown_pow_of_two(5*sizeof(struct _tt)));
//rkfifo_init(&tfifo,buffer, 4*sizeof(struct _tt), sizeof(struct _tt));
rkfifo_alloc(&tfifo, 4*sizeof(struct _tt), sizeof(struct _tt),0);
for(i=0;i<sizeof(test)/sizeof(struct _tt);i++)
{
len = rkfifo_in(&tfifo,&test[i],1);
printf("rkfifo_in len=%d, %s\n",len,test[i].b);
}
len = rkfifo_out_peek(&tfifo,temp,4);
printf("rkfifo_out_peek %s %s %s %s\n",
temp[0].b,temp[1].b,temp[2].b,temp[3].b);
do
{
len = rkfifo_out(&tfifo,&temp[0],1);
if(len)printf("rkfifo_out len=%d, %s\n",len,temp[0].b);
} while (len);
return 0;
}
