www.pudn.com > uCOSII上实现的tcpip.rar > zbuffer.c

#include "include/zbuffer.h"
#include "include/zstats.h"

#define ZBUFFER_LEN	128
#define ZBUFFER_NUM 256

/*this two point as the free zbuffer chain's head and tail*/
static volatile zbuffer_t *zbuffer_head = NULL; 
static volatile zbuffer_t *zbuffer_tail = NULL;

/*free buffer memory's pool , we use a huge arrary*/

static u8_t zbuffer_memory[ ZBUFFER_NUM * MEM_ALIGN_SIZE( sizeof(zbuffer_t) + ZBUFFER_LEN )];

static zbuffer_t * zbuffer_alloc(void)
{
	zbuffer_t *pzbuffer;
	sys_enter_critical();
	pzbuffer = (zbuffer_t *)zbuffer_head;
	if ( pzbuffer )
		zbuffer_head = (volatile zbuffer_t    *)pzbuffer->next;

#ifdef ZSTATS
	if ( pzbuffer != NULL)
	zstats.memory_used --;	
#endif

	sys_exit_critical();
	return pzbuffer;
}

static u8_t zbuffer_free(zbuffer_t *pzbuffer)
{
	if ( ( ((u8_t *)pzbuffer - zbuffer_memory) %  MEM_ALIGN_SIZE( sizeof(zbuffer_t) + ZBUFFER_LEN) ) == 0)
	{
		pzbuffer->tot_len = 0;
		pzbuffer->len = 0;
		pzbuffer->next = NULL;
		pzbuffer->pdata = (void *)( (u8_t *)pzbuffer + MEM_ALIGN_SIZE( sizeof(zbuffer_t) ));
		sys_enter_critical();
		zbuffer_tail->next = pzbuffer;
		zbuffer_tail =(volatile zbuffer_t *)pzbuffer;

#ifdef	ZSTATS		
		zstats.memory_used ++;	
#endif
		sys_exit_critical();
		return 0;
	}	
	return -1;
}

/****************************************************
 * 
 * Init zbuffer's memory and free buffer chain
 * set zbuffer head's pointer and tail's pointer
 *
 ****************************************************/

void zbuffer_init(void)
{
	zbuffer_t	*pzbuffer, *p;
	u16_t	i;
	u16_t	align_len;

	align_len = MEM_ALIGN_SIZE( sizeof(zbuffer_t) + ZBUFFER_LEN );
	
	zbuffer_head = (zbuffer_t *)zbuffer_memory;
	pzbuffer = (zbuffer_t *)zbuffer_head;
	
	for ( i = 0 ; i < ZBUFFER_NUM; i ++)
	{
		pzbuffer->next = (zbuffer_t *)( (u8_t *)pzbuffer + align_len );
		pzbuffer->len = 0;			/*because this zbuffer is in free chain*/
		pzbuffer->tot_len = 0;
		pzbuffer->pdata = (void *)( (u8_t *)pzbuffer + MEM_ALIGN_SIZE( sizeof(zbuffer_t) ));
		p = pzbuffer;
		pzbuffer = p->next;
	}
	p->next = NULL;

#ifdef	ZSTATS		
		zstats.memory_used = ZBUFFER_NUM;
#endif

	zbuffer_tail = p;
}

/*******************************************
 *
 * alloc a zbuffer chain 
 * called by INT and TASK
 *******************************************/

zbuffer_t* zbuffer_new(u16_t tot_len)
{
	zbuffer_t *pzbuffer;
	zbuffer_t *p, *q;
	s32_t	rsize;	
   
   	pzbuffer = zbuffer_alloc();
   	if ( !pzbuffer )
		return NULL;

	q = pzbuffer;
	
	pzbuffer->tot_len = tot_len;
 	pzbuffer->len = tot_len > ZBUFFER_LEN? ZBUFFER_LEN: tot_len;
	pzbuffer->next = NULL;
	
	rsize = tot_len - ZBUFFER_LEN;
	
	while( rsize > 0)
	{
		p = zbuffer_alloc();
		if (!p)
		{
			zbuffer_delete(pzbuffer);
			return NULL;
		}
	
		p->len = rsize > ZBUFFER_LEN? ZBUFFER_LEN: rsize;
		p->next = NULL;
		p->tot_len = 0;
		pzbuffer->next = p;
		pzbuffer = p;
		rsize -= ZBUFFER_LEN;
	}

	return q;
}

/*******************************************
 *
 * free a zbuffer chain to main free chain
 * called by INT and TASK
 *******************************************/

void zbuffer_delete(zbuffer_t *pzbuffer)
{
	zbuffer_t *p;	
	while( pzbuffer != NULL)
	{
		p = pzbuffer->next;
		if ( zbuffer_free(pzbuffer) != 0)
			return;
		pzbuffer = p;
	}
}

/*******************************************
 *
 * addjust a buffer chain's head or tail
 * called by INT
 *******************************************/


zbuffer_t * zbuffer_adjust(zbuffer_t *pzbuffer, s16_t flen, s16_t blen)
{
	zbuffer_t *new_head, *new_tail;
	zbuffer_t *p,*q;

	if ( flen == 0)
	{
		new_head = pzbuffer;
	}
	else if ( flen > 0)
	{
		new_head = zbuffer_new(flen);
		p = new_head;
		if (!p)
		{
			zbuffer_delete(pzbuffer);
			return NULL;
		}
		while( p->next != NULL) p = p->next;
		p->next = pzbuffer;
		new_head->tot_len += pzbuffer->tot_len;
		pzbuffer->tot_len = 0;	/*bacause this is not head of chain*/
	
	}
	else if ( flen < 0)
	{
		s32_t rsize;
		
		if ( ((-1)*flen) >= pzbuffer->tot_len )
		{
			zbuffer_delete(pzbuffer);
			return NULL;		
		}

		rsize = (-1)*flen;
		rsize -= pzbuffer->len;
		p = pzbuffer;
		q = NULL;
		
		while( rsize > 0 )
		{
			q = p;
			p = p->next;
			rsize -= p->len;
		}
		rsize += p->len;
		p->len -= rsize;
		p->pdata = (void *)((u8_t *)p->pdata + rsize);
		p->tot_len = pzbuffer->tot_len + flen; /*this is new head of chain*/
		if ( q != NULL)
		{
			q->next = NULL;
			zbuffer_delete(pzbuffer);
		}
		new_head = p;
	}

	if ( blen == 0)
	{
		return new_head;
	}
	else if ( blen > 0)
	{
		new_tail = zbuffer_new(blen);
		if ( new_tail == NULL )
		{
			zbuffer_delete( new_head );
			return NULL;
		}
		new_head->tot_len += blen;
		p = new_head;
		while( p->next != NULL) p = p->next;
		p->next = new_tail;
		new_tail->tot_len = 0; /*bacause this is not head*/
		return new_head;		
	}
	else if ( blen < 0)
	{
		s32_t	rsize = new_head->tot_len + blen;

		p = new_head;
		if ( rsize < 0)
		{
			zbuffer_delete(new_head);
			return NULL;
		}

		rsize -= p->len;
		while( rsize > 0)
		{
			p = p->next;
			rsize -= p->len;
		}
		rsize += p->len;
		if ( p->next != NULL)
		{
			zbuffer_delete(p->next);
		}
		p->len = rsize;
		p->next = NULL;
		new_head->tot_len += blen;
		return new_head;
	}	
	
	return NULL;	
}


/************************************
 * Read from zbuffer's pdata and length
 * is min of zbuffer's and len
 ***********************************/

void zbuffer_read(zbuffer_t *pzbuffer, u8_t *pdata, u16_t *len)
{
	zbuffer_t	*p;
	u16_t	i, count;
	
	if ( pzbuffer == NULL)
	{
		*len = 0;		
	}
	
	if ( *len >= pzbuffer->tot_len)
	{
		*len = pzbuffer->tot_len;
		p = pzbuffer;
		while( p != NULL)
		{
			for ( i = 0; i < p->len; i++)
			{
				*pdata = *(u8_t *)((u8_t *)p->pdata + i);
				pdata ++;
			}
			p = p->next;
		}
		return;	
	}

	count = 0;
	p = pzbuffer;
	while( p != NULL)
	{
		for ( i = 0; i < p->len; i++)
		{
			*pdata = *(u8_t *)((u8_t *)p->pdata + i);
			pdata ++;
			count ++;
			if ( count >=  *len)
			{
				return;
			}
		}
		p = p->next;
	}

	return;	

}


void zbuffer_write(zbuffer_t *pzbuffer, u8_t *pdata, u16_t *len)
{
	zbuffer_t	*p;
	u16_t	i, count;
	
	if ( pzbuffer == NULL)
	{
		*len = 0;		
	}
	
	if ( *len >= pzbuffer->tot_len)
	{
		*len = pzbuffer->tot_len;
		p = pzbuffer;
		while( p != NULL)
		{
			for ( i = 0; i < p->len; i++)
			{
			    *(u8_t *)((u8_t *)p->pdata + i) = *pdata;
				pdata ++;
			}
			p = p->next;
		}
		return;	
	}

	count = 0;
	p = pzbuffer;
	while( p != NULL)
	{
		for ( i = 0; i < p->len; i++)
		{
			*(u8_t *)((u8_t *)p->pdata + i) = *pdata;
			pdata ++;
			count ++;
			if ( count >=  *len)
			{
				return;
			}
		}
		p = p->next;
	}

	return;	

}

zbuffer_t *zbuffer_add(zbuffer_t *pzbuffer1, zbuffer_t *pzbuffer2)
{
	zbuffer_t *p;
	
	if ( pzbuffer1 == NULL || pzbuffer2 == NULL )
		return NULL;
	
	p = pzbuffer1;
	while(p->next != NULL) p = p->next;
	p->next = pzbuffer2;
	pzbuffer1->tot_len += pzbuffer2->tot_len;
	pzbuffer2->tot_len = 0;

	return pzbuffer1;
}

zbuffer_t * zbuffer_copy(zbuffer_t *pzbuffer1, zbuffer_t *pzbuffer2)
{
	u16_t i, j;

	zbuffer_t *pzbuffer, *p, *p2;
	
	p = pzbuffer = zbuffer_new( pzbuffer2->tot_len);

	p2 = pzbuffer1;
	
	if ( pzbuffer == NULL)
	{
		return NULL;
	}

	i = 0; j = 0;

	while(1)
	{
		if ( pzbuffer2 == NULL || pzbuffer == NULL)
		{
				break;
		}
		
		*((u8_t *)pzbuffer->pdata + i) = *((u8_t *)pzbuffer2->pdata + j);
		i++;
		j++;
		
		if ( i >= pzbuffer->len)
		{
			i = 0;
			pzbuffer = pzbuffer->next;
		}
		if ( j >= pzbuffer2->len)
		{
			j = 0;
			pzbuffer2 = pzbuffer2->next;
		}
	}

	if	( p2 != NULL )
		return zbuffer_add(p2, p);
	else
		return p;

	return NULL;
	
}
u16_t zbuffer_chain(zbuffer_t *pzbuffer)
{
	u16_t i;
	zbuffer_t *p;
	
	if ( pzbuffer == NULL)
			return 0;
	p = pzbuffer;
	i = 1;
	while( p->next != NULL) 
	{
		p = p->next;
		i++;
	}

	return i;
}

u8_t zbuffer_head_adjust(zbuffer_t *pbuffer, s16_t offset)
{
	void *p;
	
	
	p = (void *)( (u8_t *)pbuffer + MEM_ALIGN_SIZE( sizeof(zbuffer_t) ));
	if ( offset > 0)
	{
		if ( ((u8_t *)(pbuffer->pdata) - (u8_t *)p) < offset )
		{
			return -1;
		}
		pbuffer->pdata = (void *)( (u8_t *)(pbuffer->pdata) - offset);
		pbuffer->len += offset;
		pbuffer->tot_len += offset;
		return 0;
	}
	else
	{
		p = (void *)((u8_t *)p + pbuffer->len); 
		offset *= -1;
		if ( ((u8_t *)p - (u8_t *)pbuffer->pdata) < offset ) 
			return -1;
		pbuffer->pdata = (void *)((u8_t *)pbuffer->pdata + offset);
		pbuffer->len -= offset;
		pbuffer->tot_len -= offset;
		return 0;
	}
	return -1;
}

void zbuffer_dump(zbuffer_t *pbuffer)
{
	u16_t i;
	u16_t count = 0;
	zbuffer_t *p;
	
	
	p = pbuffer;
	printf("\n");
	while( p != NULL)
	{
		for ( i = 0; i < p->len; i++)
		{
		    printf("%2x ", *(u8_t *)((u8_t *)p->pdata + i));
			count ++;
			if ( count % 20 == 0)
				printf("\n");
		}
		p = p->next;
	}
	printf("\n");
	return;	
	
}