ringbuffer.c

ucos2.85在vs2005上模拟环境

#include "ringbuffer.h"

#define PKT_READY 0
#define PKT_DISPOSED 1

static int eof=0;

void rb_init(struct ringbuffer_s *rbuf, void *data, int len)
{
	int i;

	rbuf->data = data;
	rbuf->size = len;
	rbuf->ptr_r = rbuf->ptr_w = 0;
	rbuf->error = 0;
	rbuf->pre_r = rbuf->ptr_r;
	rbuf->need_filter = 0;
	rbuf->fp	= 0;

	rbuf->sem = RTOS_SemaphoreCreate(1);

	for(i=0;i<MAX_PID_FILTER;i++)
		rbuf->pid[i] = 0x2000;
}

//填充初始化,将一个缓冲初始化为一个循环缓冲
void rb_fill_init(struct ringbuffer_s *rbuf, void *data, int len)
{
	int i;

	rbuf->data = data;
	rbuf->size = len;
	rbuf->ptr_r = 0;
	rbuf->ptr_w = len-1;
	rbuf->error = 0;
	rbuf->pre_r = rbuf->ptr_r;
	rbuf->need_filter = 0;
	rbuf->fp	= 0;

	rbuf->sem = RTOS_SemaphoreCreate(1);

	for(i=0;i<MAX_PID_FILTER;i++)
		rbuf->pid[i] = 0x2000;
}

int rb_add_filter(struct ringbuffer_s *rbuf, u16 pid)
{
	int i;

	if(pid > 0x1fff) return 0;

	for(i=0;i<MAX_PID_FILTER;i++)
	{
		if(rbuf->pid[i] == pid) return 1;
	}

	for(i=0;i<MAX_PID_FILTER;i++)
	{
		if(rbuf->pid[i] == 0x2000)
		{
			rbuf->pid[i] = pid;
			return 1;
		}
	}

	return 0;
}

int rb_clr_filter(struct ringbuffer_s *rbuf, u16 pid)
{
	int i;

	if(pid > 0x1fff) return 0;

	for(i=0;i<MAX_PID_FILTER;i++)
	{
		if(rbuf->pid[i] == pid) 
		{
			rbuf->pid[i] = 0x2000;
			return 1;
		}
	}

	return 0;
}

void rb_set_filter(struct ringbuffer_s *rbuf, u8 yes)
{
	rbuf->need_filter = yes;
}

void rb_clrall_filter(struct ringbuffer_s *rbuf)
{
	int i;

	for(i=0;i<MAX_PID_FILTER;i++)
		rbuf->pid[i] = 0x2000;
}

int rb_empty(struct ringbuffer_s *rbuf)
{
	return (rbuf->ptr_r == rbuf->ptr_w);
}

int	rb_free(struct ringbuffer_s *rbuf)
{
	int free;

	free = rbuf->ptr_r - rbuf->ptr_w;
	if(free <= 0) free += rbuf->size;
	
	return free - 1;
}

int rb_avail(struct ringbuffer_s *rbuf)
{
	int avail;

	avail = rbuf->ptr_w - rbuf->ptr_r;
	if(avail < 0) avail += rbuf->size;
	
	return avail;
}

void rb_flush(struct ringbuffer_s *rbuf)
{
	int i;

	for(i=0;i<MAX_PID_FILTER;i++)
		rbuf->pid[i] = 0x2000;

	rbuf->ptr_r = rbuf->ptr_w;
	rbuf->pre_r = rbuf->ptr_r;
	rbuf->error = 0;
}

int rb_read(struct ringbuffer_s *rbuf, u8 *buf, int len)
{
	int todo = len;
	int split;
	
	if(!buf) return 0;

	sem_lock(rbuf->sem);

	//记录前一次读指针
	rbuf->pre_r = rbuf->ptr_r;
	
	split = (rbuf->ptr_r + len > rbuf->size) ? rbuf->size - rbuf->ptr_r : 0;
	if(split > 0)
	{
		memcpy(buf, rbuf->data+rbuf->ptr_r, split);
		buf += split;
		todo -= split;
		rbuf->ptr_r = 0;	
	}
	memcpy(buf, rbuf->data+rbuf->ptr_r, todo);
	rbuf->ptr_r = (rbuf->ptr_r + todo) % rbuf->size;
	
	sem_unlock(rbuf->sem);

	return len;
}

void rb_seek(struct ringbuffer_s *rbuf, int len)
{
	int todo = len;
	int split;
	
	//记录前一次读指针
	rbuf->pre_r = rbuf->ptr_r;
	
	split = (rbuf->ptr_r + len > rbuf->size) ? rbuf->size - rbuf->ptr_r : 0;
	if(split > 0)
	{
		todo -= split;
		rbuf->ptr_r = 0;	
	}

	rbuf->ptr_r = (rbuf->ptr_r + todo) % rbuf->size;

}


//填充，测试用，从文件读
void rb_refill(struct ringbuffer_s *rbuf)
{
	int free;

	free = rb_free(rbuf);
	rb_fill(rbuf, rbuf->fp, free);

	OSTimeDly(1);
}

//针对过滤器对数据以len大小为单位过滤
int rb_filter(struct ringbuffer_s *rbuf, u8 *buf, int len)
{
	int i;
	u16 pid;
	int todo = len;
	int split=0;
	u8 *sync;
	
	
	if(!buf) return -1;
	if(eof) return -1;

	//获取dma里的写指针
	//rbuf->ptr_w = get_dma_wptr();
begin:

	if(len > rb_avail(rbuf))
	{
		rb_refill(rbuf);
		if(eof) return -1;
	}

	//记录前一次读指针
//	rbuf->pre_r = rbuf->ptr_r;

	split = (rbuf->ptr_r + len) > rbuf->size ? rbuf->size - rbuf->ptr_r : 0;
//	printf("split = %d \n", split);
	if(split > 0)
	{
		sync = rbuf->data+rbuf->ptr_r;
		for(i=0;i<split;i++)
		{
			if(sync[i] == 0x47) 
			{
				rbuf->ptr_r += i;
//				printf("ptr_r = %d \n", rbuf->ptr_r);

				//if(rbuf->ptr_r == rbuf->size - 1)
				//	hex_dump(rbuf->data, rbuf->size);

				goto pid_filter;
			}
		}
		todo -= split;
		rbuf->ptr_r = 0;	
	}

	sync = rbuf->data+rbuf->ptr_r;

	for(i=0;i<todo;i++)
	{
		if(sync[i] == 0x47) 
		{
			rbuf->ptr_r = (rbuf->ptr_r + i) % rbuf->size;
			goto pid_filter;
		}
	}

	return -1;

pid_filter:
	
	if(len > rb_avail(rbuf))
	{
		rb_refill(rbuf);
		if(eof) return -1;
	}
	
	if(!rbuf->need_filter)
	{
		rb_read(rbuf, buf, len);
		return 0xffff;
	}

	rb_read(rbuf, buf, 3);

	pid = ((u16)(buf[1] & 0x1f) << 8) + buf[2];
//	printf("pid = %d \n", pid);

	for(i=0;i<MAX_PID_FILTER;i++)
	{
		if(rbuf->pid[i] == pid)
		{
			//rb_backspace(rbuf, -3);
			rb_read(rbuf, buf+3, len-3);
			return pid;
		}
	}
	//调整指针
	rb_seek(rbuf, len-3);
	goto begin;

	return -1;
}


int rb_write(struct ringbuffer_s *rbuf, const u8 *buf, int len)
{
	int todo = len;
	int split;

	if(!buf) return 0;
	
	sem_lock(rbuf->sem);

	split = (rbuf->ptr_w + len > rbuf->size) ? rbuf->size - rbuf->ptr_w : 0;

	if (split > 0) 
	{
		memcpy(rbuf->data+rbuf->ptr_w, buf, split);
		buf += split;
		todo -= split;
		rbuf->ptr_w = 0;
	}
	memcpy(rbuf->data+rbuf->ptr_w, buf, todo);
	rbuf->ptr_w = (rbuf->ptr_w + todo) % rbuf->size;

	sem_unlock(rbuf->sem);

	return len;
}

//从文件读数据填充到rb里
int rb_fill(struct ringbuffer_s *rbuf, FILE *fp, int len)
{
	int todo = len;
	int split;
	int size;

	split = (rbuf->ptr_w + len > rbuf->size) ? rbuf->size - rbuf->ptr_w : 0;

	if (split > 0) 
	{
		size = fread(rbuf->data+rbuf->ptr_w, split, 1, fp);
		if(size <= 0) 
		{
			printf("file eof\n");
			eof = 1;
			return 0;
		}

		todo -= split;
		rbuf->ptr_w = 0;
	}
	size = fread(rbuf->data+rbuf->ptr_w, todo, 1, fp);
	if(size <= 0) 
	{
		printf("file eof\n");
		eof = 1;
		return 0;
	}
	rbuf->ptr_w = (rbuf->ptr_w + todo) % rbuf->size;

	return len;
}

//读指针回退
void rb_backspace(struct ringbuffer_s *s, int offset)
{
	s->ptr_r = s->pre_r;
}

int rb_pkt_write(struct ringbuffer_s *rbuf, u8* buf, int len)
{
	int status;
	int oldptr_w = rbuf->ptr_w;

	RB_WRITE_BYTE(rbuf, len >> 8);
	RB_WRITE_BYTE(rbuf, len & 0xff);
	RB_WRITE_BYTE(rbuf, PKT_READY);
	status = rb_write(rbuf, buf, len);

	if (status < 0) rbuf->ptr_w = oldptr_w;
	return status;
}

int rb_pkt_read(struct ringbuffer_s *rbuf, int idx,
				int offset, u8* buf, int len)
{
	int todo;
	int split;
	int pktlen;

	pktlen = rbuf->data[idx] << 8;
	pktlen |= rbuf->data[(idx + 1) % rbuf->size];
	if (offset > pktlen) return -1;
	if ((offset + len) > pktlen) len = pktlen - offset;

	idx = (idx + RB_PKTHDRSIZE + offset) % rbuf->size;
	todo = len;
	split = ((idx + len) > rbuf->size) ? rbuf->size - idx : 0;
	if (split > 0) {
		memcpy(buf, rbuf->data+idx, split);
		
		buf += split;
		todo -= split;
		idx = 0;
	}

	memcpy(buf, rbuf->data+idx, todo);

	return len;
}

void rb_pkt_dispose(struct ringbuffer_s *rbuf, int idx)
{
	int pktlen;

	rbuf->data[(idx + 2) % rbuf->size] = PKT_DISPOSED;

	// clean up disposed packets
	while(rb_avail(rbuf) > RB_PKTHDRSIZE) {
		if (RB_PEEK(rbuf, 2) == PKT_DISPOSED) {
			pktlen = RB_PEEK(rbuf, 0) << 8;
			pktlen |= RB_PEEK(rbuf, 1);
			RB_SKIP(rbuf, pktlen + RB_PKTHDRSIZE);
		} else {
			// first packet is not disposed, so we stop cleaning now
			break;
		}
	}
}

int rb_pkt_next(struct ringbuffer_s *rbuf, int idx, int* pktlen)
{
	int consumed;
	int curpktlen;
	int curpktstatus;

	if (idx == -1) {
	       idx = rbuf->ptr_r;
	} else {
		curpktlen = rbuf->data[idx] << 8;
		curpktlen |= rbuf->data[(idx + 1) % rbuf->size];
		idx = (idx + curpktlen + RB_PKTHDRSIZE) % rbuf->size;
	}

	consumed = (idx - rbuf->ptr_r) % rbuf->size;

	while((rb_avail(rbuf) - consumed) > RB_PKTHDRSIZE) {

		curpktlen = rbuf->data[idx] << 8;
		curpktlen |= rbuf->data[(idx + 1) % rbuf->size];
		curpktstatus = rbuf->data[(idx + 2) % rbuf->size];

		if (curpktstatus == PKT_READY) {
			*pktlen = curpktlen;
			return idx;
		}

		consumed += curpktlen + RB_PKTHDRSIZE;
		idx = (idx + curpktlen + RB_PKTHDRSIZE) % rbuf->size;
	}

	// no packets available
	return -1;
}