ssp_mbuf.c

abstract rtos

#include "syscfg.h"

#ifdef __cplusplus
#if __cplusplus
extern "C"{
#endif 
#endif 



#include "sys/sys_pub.h"
#include "aos.h"








MBUF_DATABLOCKDESCRIPTOR_S * mbuf_create_dbdes(U32 ulDataBlockLength, U32 ulType, U32 mpe);
U32 mbuf_compress(MBUF_S * pstMBuf, U32 ulDataBlockNumber, U32 mpe);




MBUF_S * mbuf_create_and_copy_buf(U32 ulReserveHeadSpace, U32 ulLength, 
								 U8 * pucBuffer, U32 ulType, U32 mpe)
{
	MBUF_S * pstMBuf;
	U32 ulResult;

	if(pucBuffer == NULL)
	{
		return NULL;
	}
	if(ulReserveHeadSpace > MBUF_DATA_BLOCK2048_SIZE)
	{
		return NULL;
	}

	pstMBuf = aos_dmem_alloc(mpe, SID_MBUF, MBUF_MBUF_SIZE);
	if(pstMBuf == NULL)
	{
		return (NULL);
	}
	pstMBuf->ulMbufMagic = MBUF_MAGIC_NUMBER;
	pstMBuf->stDataBlockDescriptor.ulDBMagic = DBES_MAGIC_NUMBER;
	pstMBuf->stDataBlockDescriptor.pucDataBlock \
		= aos_dmem_alloc(mpe, SID_U8, MBUF_DATA_BLOCK2048_SIZE);
	if(pstMBuf->stDataBlockDescriptor.pucDataBlock == NULL)
	{
		pstMBuf->ulMbufMagic = 0;
		(VOID)aos_dmem_free(pstMBuf);
		return (NULL);
	}

	
	pstMBuf->stDataBlockDescriptor.ulType = ulType;
	pstMBuf->stDataBlockDescriptor.ulOption = 0;
	pstMBuf->stDataBlockDescriptor.pstNextDataBlockDescriptor = NULL;
	pstMBuf->stDataBlockDescriptor.pucData = pstMBuf->stDataBlockDescriptor.pucDataBlock;
	pstMBuf->stDataBlockDescriptor.ulDataLength = MBUF_MIN(MBUF_DATA_BLOCK2048_SIZE,  ulReserveHeadSpace + ulLength);
	pstMBuf->stDataBlockDescriptor.ulDataBlockLength = MBUF_DATA_BLOCK2048_SIZE;
	
	pstMBuf->ulTotalDataLength = pstMBuf->stDataBlockDescriptor.ulDataLength;
	pstMBuf->ulDataBlockNumber = 1;
	pstMBuf->pstNextMBuf = NULL;
	(VOID)aos_memset( (VOID *) (& pstMBuf->stUserTagData), 0 , sizeof(MBUF_USERTAGDATA_S));

	
	ulResult = mbuf_copy_from_buff(pstMBuf, ulReserveHeadSpace, ulLength, pucBuffer, mpe);
	if(ulResult == MBUF_OK)
	{
		MBUF_CUT_HEAD(pstMBuf, ulReserveHeadSpace);
		return pstMBuf;
	}
	else
	{
		(VOID)mbuf_destroy(pstMBuf);
		return NULL;
	}
}


MBUF_S * mbuf_create(U32 ulReserveHeadSpace, U32 ulLength, U32 ulType, U32 mpe)
{
	MBUF_S * pstMBuf;

	if(ulReserveHeadSpace + ulLength > MBUF_DATA_BLOCK2048_SIZE)
	{
		return NULL;
	}

	
	pstMBuf = aos_dmem_alloc(mpe, SID_MBUF, MBUF_MBUF_SIZE);
	if(pstMBuf == NULL)
	{
		return (NULL);
	}
	pstMBuf->ulMbufMagic = MBUF_MAGIC_NUMBER;
	pstMBuf->stDataBlockDescriptor.ulDBMagic = DBES_MAGIC_NUMBER;
	pstMBuf->stDataBlockDescriptor.pucDataBlock \
		= aos_dmem_alloc(mpe, SID_U8, MBUF_DATA_BLOCK2048_SIZE);
	if(pstMBuf->stDataBlockDescriptor.pucDataBlock == NULL)
	{
		pstMBuf->ulMbufMagic = 0;
		(VOID)aos_dmem_free(pstMBuf);
		return (NULL);
	}

	
	pstMBuf->stDataBlockDescriptor.ulType = ulType;
	pstMBuf->stDataBlockDescriptor.ulOption = 0;
	pstMBuf->stDataBlockDescriptor.pstNextDataBlockDescriptor = NULL;
	pstMBuf->stDataBlockDescriptor.pucData = pstMBuf->stDataBlockDescriptor.pucDataBlock + ulReserveHeadSpace;
	pstMBuf->stDataBlockDescriptor.ulDataLength = ulLength;
	pstMBuf->stDataBlockDescriptor.ulDataBlockLength = MBUF_DATA_BLOCK2048_SIZE;
	
	pstMBuf->ulTotalDataLength = ulLength;
	pstMBuf->ulDataBlockNumber = 1;
	pstMBuf->pstNextMBuf = NULL;
	(VOID)aos_memset( (VOID *) (& pstMBuf->stUserTagData), 0 , sizeof(MBUF_USERTAGDATA_S));
	return pstMBuf;
}

U32 mbuf_destroy(MBUF_S * pstMBuf)
{
	MBUF_DATABLOCKDESCRIPTOR_S * pstNextDataBlockDescriptor;
	MBUF_DATABLOCKDESCRIPTOR_S * pstDataBlockDescriptor;
	U32 ulResult;
	U32 ulFail;

	
	if (MBUF_IS_BAD(pstMBuf))
	{
		AOS_ASSERT(0);
		return MBUF_FAIL;
	}
	ulFail = 0;
	
	for(pstDataBlockDescriptor = pstMBuf->stDataBlockDescriptor.pstNextDataBlockDescriptor;\
		pstDataBlockDescriptor != NULL;\
		pstDataBlockDescriptor = pstNextDataBlockDescriptor)
	{
		if (DBES_IS_BAD(pstDataBlockDescriptor))
		{
			AOS_ASSERT(0);
			return MBUF_FAIL;
		}
		if(pstDataBlockDescriptor->pucDataBlock != NULL)
		{
			ulResult = aos_dmem_free(pstDataBlockDescriptor->pucDataBlock);
			if(ulResult != 0)
			{
				ulFail = 1;
			}
		}
		
		pstNextDataBlockDescriptor = pstDataBlockDescriptor->pstNextDataBlockDescriptor;
		pstDataBlockDescriptor->ulDBMagic = 0;
		ulResult = aos_dmem_free(pstDataBlockDescriptor);
		if(ulResult != 0)
		{
			ulFail = 1;
		}
	}
	
	
	if(pstMBuf->stDataBlockDescriptor.pucDataBlock != NULL)
	{
		ulResult = aos_dmem_free(pstMBuf->stDataBlockDescriptor.pucDataBlock);
		if(ulResult != 0)
		{
			ulFail = 1;
		}
	}
	pstMBuf->ulMbufMagic = 0;
	ulResult = aos_dmem_free(pstMBuf);
	if(ulResult != 0)
	{
		ulFail = 1;
	}
	
	if(ulFail == 1)
	{
		return MBUF_FAIL;
	}
	else
	{
		return MBUF_OK;
	}
}


U32 mbuf_copy_from_buff(MBUF_S * pstMBuf, U32 ulOffset, U32 ulLength, \
									U8 * pucBuffer, U32 mpe)
{
	MBUF_DATABLOCKDESCRIPTOR_S * pstDataBlockDescriptor;
	MBUF_DATABLOCKDESCRIPTOR_S * pstNextDataBlockDescriptor;
	U32 ulCopyLength;
	U32 ulTotalDataLength;
	U32 ulResult;
	U32 ulReferenceCount;
	MBUF_DATABLOCKDESCRIPTOR_S * pstDataBlockDescriptorHead;
	MBUF_DATABLOCKDESCRIPTOR_S * pstDataBlockDescriptorTemp;
	MBUF_DATABLOCKDESCRIPTOR_S * pstNextDataBlockDescriptorTemp;

	U32 ulFail;
	U32 ulFreedDataLength;
	U32 ulFreedDataBlockNumber;
	U32 ulType;

	if (MBUF_IS_BAD(pstMBuf))
	{
		AOS_ASSERT(0);
		return MBUF_FAIL;
	}
	if(ulOffset > pstMBuf->ulTotalDataLength)
	{
		return MBUF_FAIL;
	}
	if(pucBuffer == NULL)
	{
		return MBUF_FAIL;
	}

	ulTotalDataLength = 0;
	ulFail = 0;
	ulType = pstMBuf->stDataBlockDescriptor.ulType;
	pstDataBlockDescriptorHead = NULL;
	
	for(pstDataBlockDescriptor = &(pstMBuf->stDataBlockDescriptor);\
		ulOffset > pstDataBlockDescriptor->ulDataLength;\
		pstDataBlockDescriptor = pstDataBlockDescriptor->pstNextDataBlockDescriptor)
	{
		if (DBES_IS_BAD(pstDataBlockDescriptor))
		{
			AOS_ASSERT(0);
			return MBUF_FAIL;
		}

		ulOffset -= pstDataBlockDescriptor->ulDataLength;
		ulTotalDataLength += pstDataBlockDescriptor->ulDataLength;
#if 0 
		if(pstDataBlockDescriptor->pstNextDataBlockDescriptor == NULL)
		{
			
			pstDataBlockDescriptor->pstNextDataBlockDescriptor\
				=pstNextDataBlockDescriptor = mbuf_create_dbdes(mpe, MBUF_DATA_BLOCK2048_SIZE);
			if(pstNextDataBlockDescriptor == NULL)
			{
				pstMBuf->ulTotalDataLength = ulTotalDataLength;
				return MBUF_FAIL;
			}
			pstNextDataBlockDescriptor->pucData = pstNextDataBlockDescriptor->pucDataBlock;
			pstNextDataBlockDescriptor->ulDataLength = MBUF_MIN(MBUF_DATA_BLOCK2048_SIZE, ulLength + ulOffset);
			pstMBuf->ulDataBlockNumber ++;
		}
#endif 
	}

	
	
	while(ulLength > 0)
	{

		if (DBES_IS_BAD(pstDataBlockDescriptor))
		{
			AOS_ASSERT(0);
			return MBUF_FAIL;
		}
		
		ulResult = aos_dmem_ref_get(pstDataBlockDescriptor->pucDataBlock, & ulReferenceCount);
		if(ulResult != 0)
		{
			
			ulReferenceCount = 2;
		}
		if(pstDataBlockDescriptor->pstNextDataBlockDescriptor == NULL && ulReferenceCount == 1)
		{
			
			U32 ulUsableLength;
			
			
    		ulUsableLength = (U32)(pstDataBlockDescriptor->pucDataBlock 
                    			    + pstDataBlockDescriptor->ulDataBlockLength
                    				- pstDataBlockDescriptor->pucData 
                    				- ulOffset);
            
			ulCopyLength = MBUF_MIN(ulUsableLength,	ulLength);
			if(pstDataBlockDescriptor->ulDataLength < ulOffset + ulCopyLength)
			{
				
				pstDataBlockDescriptor->ulDataLength = ulOffset + ulCopyLength;
			}
		}
		else
		{
			ulCopyLength = MBUF_MIN(pstDataBlockDescriptor->ulDataLength - ulOffset, ulLength);
		}
		

		if( NULL == aos_memcpy((VOID *)(pstDataBlockDescriptor->pucData + ulOffset), (VOID*)pucBuffer, ulCopyLength) )
		{
			ulFail = 1;
		}
		pucBuffer += ulCopyLength;
		ulLength -= ulCopyLength;
		ulTotalDataLength += (ulCopyLength + ulOffset);
		ulOffset = 0;
	
		if(ulLength == 0)
		{
			break;
		}
		if(pstDataBlockDescriptor->pstNextDataBlockDescriptor == NULL)
		{
			if(pstDataBlockDescriptorHead == NULL)
			{
				pstDataBlockDescriptorHead = pstDataBlockDescriptor;
			}
			pstDataBlockDescriptor->pstNextDataBlockDescriptor\
				= pstNextDataBlockDescriptor = mbuf_create_dbdes(MBUF_DATA_BLOCK2048_SIZE, ulType, mpe);
			if(pstNextDataBlockDescriptor == NULL)
			{
				
				pstMBuf->ulTotalDataLength = ulTotalDataLength;
				
				ulFreedDataLength = 0;
				ulFreedDataBlockNumber = 0;

				pstDataBlockDescriptorTemp = pstDataBlockDescriptorHead->pstNextDataBlockDescriptor;
				pstDataBlockDescriptorHead->pstNextDataBlockDescriptor = NULL;
				while(pstDataBlockDescriptorTemp != NULL)
				{
					if (DBES_IS_BAD(pstDataBlockDescriptorTemp))
					{
						AOS_ASSERT(0);
						return MBUF_FAIL;
					}
					ulFreedDataLength += pstDataBlockDescriptorTemp->ulDataLength;
					ulFreedDataBlockNumber ++;

					(VOID)aos_dmem_free(pstDataBlockDescriptorTemp->pucDataBlock);
					pstNextDataBlockDescriptorTemp = pstDataBlockDescriptorTemp->pstNextDataBlockDescriptor;
					pstDataBlockDescriptorTemp->ulDBMagic = 0;
					(VOID)aos_dmem_free(pstDataBlockDescriptorTemp);
					pstDataBlockDescriptorTemp = pstNextDataBlockDescriptorTemp;
				}
				pstMBuf->ulTotalDataLength -= ulFreedDataLength;
				pstMBuf->ulDataBlockNumber -= ulFreedDataBlockNumber;
				
				return MBUF_FAIL;
			}
			pstNextDataBlockDescriptor->ulDataLength = MBUF_MIN(MBUF_DATA_BLOCK2048_SIZE, ulLength);
			pstNextDataBlockDescriptor->pucData = pstNextDataBlockDescriptor->pucDataBlock;
			pstMBuf->ulDataBlockNumber ++;
		}
		pstDataBlockDescriptor = pstDataBlockDescriptor->pstNextDataBlockDescriptor;
	}

	
	if(pstMBuf->ulTotalDataLength < ulTotalDataLength)
	{
		pstMBuf->ulTotalDataLength = ulTotalDataLength;
	}
	if(ulFail == 1)
	{
		return MBUF_FAIL;
	}
	else
	{
		return MBUF_OK;
	}
}

U32 mbuf_copy_to_buff(MBUF_S * pstMBuf, U32 ulOffset, U32 ulLength, 
									U8 * pucBuffer)
{
	MBUF_DATABLOCKDESCRIPTOR_S * pstDataBlockDescriptor;
	U32 ulCopyLength;

	if (MBUF_IS_BAD(pstMBuf))
	{
		AOS_ASSERT(0);
		return MBUF_FAIL;
	}
	if(pucBuffer == NULL)
	{
		return MBUF_FAIL;
	}
	if(ulOffset > pstMBuf->ulTotalDataLength)
	{
		return MBUF_FAIL;
	}
	
	if(ulLength > pstMBuf->ulTotalDataLength - ulOffset)
	{
		return MBUF_FAIL;
	}
	if(ulLength == 0)
	{
		return MBUF_OK;
	}

	
	for(pstDataBlockDescriptor = &(pstMBuf->stDataBlockDescriptor);\
		ulOffset >= pstDataBlockDescriptor->ulDataLength;\
		pstDataBlockDescriptor = pstDataBlockDescriptor->pstNextDataBlockDescriptor)
	{
		if (DBES_IS_BAD(pstDataBlockDescriptor))
		{
			AOS_ASSERT(0);
			return MBUF_FAIL;
		}
		ulOffset -= pstDataBlockDescriptor->ulDataLength;
	}

	
	
	while(ulLength > 0 )
	{
		if (DBES_IS_BAD(pstDataBlockDescriptor))
		{
			AOS_ASSERT(0);
			return MBUF_FAIL;
		}
		ulCopyLength = MBUF_MIN(pstDataBlockDescriptor->ulDataLength - ulOffset, ulLength);
		if( NULL == aos_memcpy((VOID*)pucBuffer, pstDataBlockDescriptor->pucData + ulOffset, ulCopyLength) )
		{
			return MBUF_FAIL;
		}
		ulLength -= ulCopyLength;
		pucBuffer += ulCopyLength;
		ulOffset = 0;
		pstDataBlockDescriptor = pstDataBlockDescriptor->pstNextDataBlockDescriptor;
	}
	return MBUF_OK;
}

MBUF_S * mbuf_reference_copy(MBUF_S * pstMBuf, U32 ulOffset, U32 ulLength, U32 mpe)
{
	MBUF_DATABLOCKDESCRIPTOR_S * pstDataBlockDescriptor = NULL;
	MBUF_S * pstMBufCopy;
	MBUF_DATABLOCKDESCRIPTOR_S * pstDataBlockDescriptorCopy = NULL;
	MBUF_DATABLOCKDESCRIPTOR_S * pstDataBlockDescriptorCopyHead;
	MBUF_DATABLOCKDESCRIPTOR_S ** ppstDataBlockDescriptorCopy;
	MBUF_DATABLOCKDESCRIPTOR_S * pstTempDataBlockDescriptor;
	MBUF_DATABLOCKDESCRIPTOR_S * pstTempNextDataBlockDescriptor;
	U32 ulDataBlockNumber;
	U32 ulResult;

	if (MBUF_IS_BAD(pstMBuf))
	{
		AOS_ASSERT(0);
		return NULL;
	}
	if(ulOffset > pstMBuf->ulTotalDataLength)
	{
		return NULL;
	}
	if(ulLength > pstMBuf->ulTotalDataLength - ulOffset)
	{