streambuffer.c

ARM嵌入式应用开发实例USB项目控制器的实现源代码

#include <linux/config.h>
#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/poll.h>
#include <linux/usb.h>
#include <linux/slab.h>

#include "streambuffer.h"

void dump_bin(char *Title,unsigned char *buf, int len);

#define	debug 0

#undef	dbg
#define dbg(format, arg...) \
	do { if (debug) printk(KERN_DEBUG __FILE__ ": [" __FUNCTION__ "] " format "\n" , ## arg); } while (0)

#define msg(format, arg...) \
	do { printk(KERN_DEBUG __FILE__ ": [" __FUNCTION__ "] " format "\n" , ## arg); } while (0)

/////////////////////////////////////////////////////////////
// private function

static inline unsigned int * sync_scan(unsigned int *addr, unsigned int syncword, int size)
{
	if(!size)
		return addr;
	
	__asm__ (
		"cld\n"
		"repnz; scasl\n\t"
		"jz 1f\n\t"
		"mov $0, %%edi\n"
		"1:"
		: "=D" (addr), "=c" (size)
		: "0" (addr), "1" (size), "a" (syncword));
	
	if(addr != NULL)
		addr--;

	return addr;
}

static int bufblk_alloc(PSTREAM_BUFFER psb)
{
	int i;

	dbg("Enter");

	dbg("bufblk_ready is %d", psb->bufblk_ready);
	if(psb->bufblk_ready) return 0;

/*
	psb->bufblk_cache = kmem_cache_create("Stream Buffer",
										  STREAMBUF_BLKSIZE + 96, 0,
										  0, NULL, NULL);
*/

	psb->bufblk_cache = kmem_cache_create(psb->cachename,
										  STREAMBUF_BLKSIZE + 96, 0,
										  0, NULL, NULL);

	if(psb->bufblk_cache == NULL)
	{
		dbg("ERROR: could not create cache");
		return -1;
	}

	for(i = 0; i < STREAMBUF_TOTAL_BLKS; i++)
	{
		psb->bufblk_pool[i]	= kmem_cache_alloc(psb->bufblk_cache, GFP_KERNEL);
		if(psb->bufblk_pool[i] == NULL)
		{
			dbg("ERROR: could not allocate block buffer for stream buffer");
			return -1;
		}
		else
		{
			dbg("SUCCESS: pointer is %p", psb->bufblk_pool[i]);
		}
	}

	psb->bufblk_ready = 1;

	dbg("Leave");

	return 0;
}

static void bufblk_free(PSTREAM_BUFFER psb)
{
	int i;

	psb->bufblk_ready = 0;

	if(psb->bufblk_cache == NULL)
	{
		dbg("ERROR: bufblk_cache is NULL");
		return;
	}

	for(i = 0; i < STREAMBUF_TOTAL_BLKS; i++)
	{
		if(psb->bufblk_pool[i] != NULL)
		{
			kmem_cache_free(psb->bufblk_cache, psb->bufblk_pool[i]);
			psb->bufblk_pool[i] = NULL;
		}
	}

	kmem_cache_destroy(psb->bufblk_cache);
	psb->bufblk_cache = NULL;
}

static int bufblk_calc_freespace(PSTREAM_BUFFER psb)
{
	int rv;

	rv = psb->buf_tail - psb->buf_head;
	if(rv < 0)
	{
		rv += STREAMBUF_TOTAL_SIZE;
	}

	rv = STREAMBUF_TOTAL_SIZE - rv;

	dbg("bufblk: freespace = %d (%d/%d)", rv,
		psb->buf_head, psb->buf_tail);

	return rv;
}

static int bufblk_copy_in(PSTREAM_BUFFER psb, void *buf, int len)
{
	int cur_off, count, total_len;
	int blknum;

	char *pblk, *pdata;

	count = 0;
	total_len = len;

	blknum	= (psb->buf_tail + 1) / STREAMBUF_BLKSIZE;
	cur_off	= psb->buf_tail % STREAMBUF_BLKSIZE;

	pdata = (char*) buf;

	dbg("bufblk_copy_in: blknum = %d, cur_off = %d, len = %d", blknum, cur_off, len);

	while(len > 0)
	{

		pblk = (char*) (psb->bufblk_pool[blknum]);
		dbg("pointer is %p", pblk);

		if((cur_off + len) <= STREAMBUF_BLKSIZE)
		{
			dbg("not cross block");

			if(cur_off < 0 || (cur_off + len) > STREAMBUF_BLKSIZE)
			{
				err("out of range: offset = %d, len = %d", cur_off, len);
			}

			memcpy(pblk + cur_off, pdata, len);

			dbg("Block number: 0x%X, offset: 0x%X, len: 0x%X", blknum, cur_off, len);

			pdata += len; count += len; len = 0;
		}
		else
		{
			int n;

			dbg("cross block, count = %d, len = %d, cur_off = %d", count, len, cur_off);

			n = STREAMBUF_BLKSIZE - cur_off;

			if(cur_off < 0 || (cur_off + n) > STREAMBUF_BLKSIZE)
			{
				err("out of range: offset = %d, len = %d", cur_off, len);
			}

			memcpy(pblk + cur_off, pdata, n);

			dbg("Block number: 0x%X, offset: 0x%X, len: 0x%X", blknum, cur_off, n);

			pdata += n; count += n; len -= n;
			cur_off = 0; blknum = (blknum + 1) % STREAMBUF_TOTAL_BLKS;
		}
	}

	psb->buf_tail = (psb->buf_tail + count) % STREAMBUF_TOTAL_SIZE;

	if(count != total_len)
	{
		err("ERROR: count != length (%d/%d)", count, total_len);
	}
	
	return count;
}

static int bufblk_copy_out(PSTREAM_BUFFER psb, int offset, void *buf, int len)
{
	int cur_off, count;
	int blknum;

	char *pblk, *pdst;

	dbg("Enter offset = 0x%X, len = 0x%X", offset, len);

	blknum	= (offset + 1) / STREAMBUF_BLKSIZE;
	cur_off	= offset % STREAMBUF_BLKSIZE;

	pdst = (char*) buf;
	count = 0;

	while(len > 0)
	{
		pblk = (char*) (psb->bufblk_pool[blknum]);
		if((cur_off + len) <= STREAMBUF_BLKSIZE)
		{
			copy_to_user(pdst, pblk + cur_off, len);

			dbg("Block number: 0x%X, offset: 0x%X, len: 0x%X", blknum, cur_off, len);

			pdst += len; count += len; len = 0;
		}
		else
		{
			int n;

			n = STREAMBUF_BLKSIZE - cur_off;
			copy_to_user(pdst, pblk + cur_off, n);

			dbg("Block number: 0x%X, offset: 0x%X, len: 0x%X", blknum, cur_off, n);

			pdst += n; count += n; len -= n;
			cur_off = 0; blknum = (blknum + 1) % STREAMBUF_TOTAL_BLKS;
		}
	}

	dbg("Leave");

	return count;
}

static int finfo_init(PSTREAM_BUFFER psb)
{
	psb->finfo_head = 0;
	psb->finfo_tail = 0;

	psb->buf_head	= 0;
	psb->buf_tail	= 0;

	psb->least_freespace = STREAMBUF_TOTAL_SIZE;

	return 0;
}

inline static int finfo_is_empty(PSTREAM_BUFFER psb)
{
	return (psb->finfo_head == psb->finfo_tail);
}

static void finfo_add_tail(PSTREAM_BUFFER psb, int offset)
{
	int new_tail;

	new_tail = (psb->finfo_tail + 1) % MAX_FRAMES;

	if(new_tail == psb->finfo_head)
	{
		err("ERROR: try to add tail when list is full");
		return;
	}

	psb->finfo_pool[psb->finfo_tail].offset = offset;
	psb->finfo_pool[psb->finfo_tail].len	 = 0;

	if(psb->finfo_tail != psb->finfo_head)
	{
		int frame_len;
		int prev;

		/* Calculate previous node position */
		prev = psb->finfo_tail - 1;

		if(prev < 0)
			prev += MAX_FRAMES;

		/* Calculate frame length */
		frame_len = offset - psb->finfo_pool[prev].offset;

		if(frame_len < 0)
			frame_len += STREAMBUF_TOTAL_SIZE;

		/* Assign frame length */
		psb->finfo_pool[prev].len = frame_len;
	}

	psb->finfo_tail = new_tail;
}

static void finfo_remove_head(PSTREAM_BUFFER psb)
{
	int new_head;

	/* List is empty*/
	if(psb->finfo_head == psb->finfo_tail)
		return;
	
	new_head 	= (psb->finfo_head + 1) % MAX_FRAMES;

	psb->buf_head	= psb->finfo_pool[new_head].offset;
	psb->finfo_head	= new_head;
}

/*
 * Original local functions
 */

static void InitLock(PSTREAM_BUFFER psb)
{
	spin_lock_init(&(psb->spinlock));
	// init_MUTEX(&(psb->mutex));
}

static void LockBuffer(PSTREAM_BUFFER psb)
{
	spin_lock_irqsave(&(psb->spinlock), psb->lock_flags);
	// down(&(psb->mutex));
}

static void UnlockBuffer(PSTREAM_BUFFER psb)
{
	spin_unlock_irqrestore(&(psb->spinlock), psb->lock_flags);
	// up(&(psb->mutex));
}

/////////////////////////////////////////////////////////////
// public function

int StreamBuffer_Init(PSTREAM_BUFFER psb)
{
	dbg("Enter");

#if 1
	if(bufblk_alloc(psb))
	{
		bufblk_free(psb);
		return -1;
	}
#endif

	finfo_init(psb);

	InitLock(psb);
	
	dbg("Leave Successfully");
	return 0;
}

int StreamBuffer_Reset(PSTREAM_BUFFER psb)
{
	dbg("Enter");

	finfo_init(psb);

	dbg("Leave Successfully");

	return 0;
}

void StreamBuffer_Uninit(PSTREAM_BUFFER psb)
{
	dbg("Enter");

#if 1
	bufblk_free(psb);
#endif

	dbg("Leave");
}

int StreamBuffer_AddBlock(PSTREAM_BUFFER psb, unsigned char *pblock, int size)
{
	int rv, cur_offset;
	int freespace;
	int cnt;

	dbg("Enter");

	if(size > 200000)
	{
		err("Strange things: size = %d", size);
	}

	if(!psb->bufblk_ready)
	{
		dbg("[" __FUNCTION__ "] " "Buffer block not initialized");
		return 0;
		return -1;
	}

	LockBuffer(psb);

	dbg("Add block at %p, size = %d", pblock, size);

	/* Drop frame until getting enough space */
	cnt = 10;
	do
	{
		freespace = bufblk_calc_freespace(psb);

		if(freespace < psb->least_freespace)
		{
			psb->least_freespace = freespace;
			msg("Least freespace for %p: %d (%d/%d)", psb, psb->least_freespace,
				 psb->buf_head, psb->buf_tail);
		}
		
		if(freespace > (size + 8))
			break;

		/* Discard first frame */
		finfo_remove_head(psb);
		msg("Drop frames! size required: %d", size);
		cnt--;
	}while(cnt>0);

	if(freespace <= (size + 8))
	{
		err("free space is not enough in stream buffer");
		err("finfo: (%d/%d)", psb->finfo_head, psb->finfo_tail);
		goto exit;
	}

	/* Add data to frame buffer */
	cur_offset = psb->buf_tail;
	rv = bufblk_copy_in(psb, pblock, size);

#if 0
	/* Search frame header */
	{
		int i;

		for(i = 0; i < rv; i++)
		{
			if(*((unsigned int *)(pblock+i)) == SYNCWORD)
			{
				int sync_offset = (cur_offset + i) % (STREAMBUF_TOTAL_SIZE);
				finfo_add_tail(psb, sync_offset);
				dbg("SYNCWORD at 0x%06X", sync_offset);
			}
		}
	}
#else
	/* Search frame header */
	{
		int offset, words;
		unsigned int *pcur, *pend;
		unsigned int *pdata = (unsigned int *) pblock;

		offset	= 0;
		words	= rv / 4;

		pcur	= pdata;
		pend	= (unsigned int*) (pblock + rv);

		// msg("bufstart/cur_offset: %p/0x%08x", pdata, cur_offset);
		while(pcur < pend) {
			pcur = sync_scan(pcur, SYNCWORD, pend - pcur);
			if(pcur == NULL)
				break;

			// msg("sync_scan: %p", pcur);
			// msg("data[-1, 0, 1]: 0x%08x, 0x%08x, 0x%08x", *(pcur-1), *pcur, *(pcur+1));

			if(*pcur == SYNCWORD) {
				int sync_offset;
				
				sync_offset = (cur_offset + (((char*)pcur) - ((char*)pdata))) % (STREAMBUF_TOTAL_SIZE);
				finfo_add_tail(psb, sync_offset);
				// msg("SYNCWORD at 0x%06X", sync_offset);
				pcur++;
			}
			else {
				break;
			}
		}
	}
#endif

exit:
	UnlockBuffer(psb);

	dbg("Leave Normally");

	return 0;
}

////////////////////////////////////////////////////////////////////////////////////////
//
// return:
//		0  -- success
//		1 -- buffer underflow
//		2 -- dest buffer size too small

int StreamBuffer_GetFrame(PSTREAM_BUFFER psb, unsigned char *pDstBuf, int *DstBufLen)
{
	int frame_len, frame_offset, rv;
	rv = 1;

	dbg("Enter");

	if(finfo_is_empty(psb))
	{
		dbg("Stream buffer is empty");
		rv = 1;
		goto empty;
	}
	
	if(spin_is_locked(&(psb->spinlock))) {
		rv = 1;
		goto empty;
	}

	LockBuffer(psb);

	frame_offset	= psb->finfo_pool[psb->finfo_head].offset;
	frame_len		= psb->finfo_pool[psb->finfo_head].len;

	dbg("[GetFrame] Current fame: offset %d, len %d", frame_offset, frame_len);

	if(frame_len <= 0)
	{
		rv = 1;
		goto exit;
	}

	if(*DstBufLen < frame_len)
	{
		dbg("[" __FUNCTION__ "] " "Dest buffer too small %d/%d", frame_len, *DstBufLen);
		rv = 2;
		goto exit;
	}

	bufblk_copy_out(psb, frame_offset, pDstBuf, frame_len);
	*DstBufLen = frame_len;

	finfo_remove_head(psb);

	rv = 0;

exit:
	UnlockBuffer(psb);

empty:
	dbg("Leave");

	return rv;
}