ssp_mbuf.c

abstract rtos

	while(pstDataBlockDescriptor != NULL)
	{
	    if (DBES_IS_BAD(pstDataBlockDescriptor))
		{
			AOS_ASSERT(0);
			return MBUF_FAIL;
		}
	    if(pstDataBlockDescriptor->ulDataLength == 0)
	    {
			pstLastDataBlockDescriptor->pstNextDataBlockDescriptor 
				= pstDataBlockDescriptor->pstNextDataBlockDescriptor;
			pstMBuf->ulDataBlockNumber --;
			(VOID)aos_dmem_free(pstDataBlockDescriptor->pucDataBlock);
			pstDataBlockDescriptor->ulDBMagic = 0;
			(VOID)aos_dmem_free(pstDataBlockDescriptor);
			pstDataBlockDescriptor = pstLastDataBlockDescriptor->pstNextDataBlockDescriptor;
		}
		else
		{
			pstLastDataBlockDescriptor = pstDataBlockDescriptor;
			pstDataBlockDescriptor = pstDataBlockDescriptor->pstNextDataBlockDescriptor;
		}
	}

	if( (pstMBuf->stDataBlockDescriptor).pstNextDataBlockDescriptor == NULL )
	{
		return MBUF_OK;
	}

	
	if( (pstMBuf->stDataBlockDescriptor).ulDataLength == 0 )
	{
		pstMBuf->ulDataBlockNumber --;
		(VOID)aos_dmem_free( (pstMBuf->stDataBlockDescriptor).pucDataBlock );
		
		pstDataBlockDescriptor = pstMBuf->stDataBlockDescriptor.pstNextDataBlockDescriptor;
		pstMBuf->stDataBlockDescriptor = 
			* (pstMBuf->stDataBlockDescriptor.pstNextDataBlockDescriptor);
		pstDataBlockDescriptor->ulDBMagic = 0;
		(VOID)aos_dmem_free(pstDataBlockDescriptor);
	}
	return MBUF_OK;

}









MBUF_DATABLOCKDESCRIPTOR_S * mbuf_create_dbdes(U32 ulDataBlockLength, 
														  U32 ulType, U32 mpe)
{
	MBUF_DATABLOCKDESCRIPTOR_S * pstDataBlockDescriptor;

	pstDataBlockDescriptor = aos_dmem_alloc(mpe, SID_MBUF_DBDES,
		MBUF_DATA_BLOCK_DESC_SIZE);
	if(pstDataBlockDescriptor == NULL)
	{
		return NULL;
	}
	pstDataBlockDescriptor->ulDBMagic = DBES_MAGIC_NUMBER;
	pstDataBlockDescriptor->pucDataBlock = aos_dmem_alloc(mpe, SID_U8, ulDataBlockLength);
	if(pstDataBlockDescriptor->pucDataBlock == NULL)
	{
		pstDataBlockDescriptor->ulDBMagic = 0;
		(VOID)aos_dmem_free(pstDataBlockDescriptor);
		return NULL;
	}
	pstDataBlockDescriptor->pstNextDataBlockDescriptor = NULL;
	pstDataBlockDescriptor->ulDataBlockLength = ulDataBlockLength;
	pstDataBlockDescriptor->ulType = ulType;
	pstDataBlockDescriptor->ulOption = 0;
	return pstDataBlockDescriptor;
	
}

U32 mbuf_pullup(MBUF_S * pstMBuf, U32 ulLength, U32 mpe)
{
	MBUF_DATABLOCKDESCRIPTOR_S * pstDataBlockDescriptor;
	MBUF_DATABLOCKDESCRIPTOR_S  stDataBlockDescriptor;
	U32 ulResult;

	if (MBUF_IS_BAD(pstMBuf))
	{
		AOS_ASSERT(0);
		return MBUF_FAIL;
	}

	if(ulLength > pstMBuf->ulTotalDataLength || ulLength > MBUF_DATA_BLOCK2048_SIZE)
	{
		return MBUF_FAIL;
	}
	
	pstDataBlockDescriptor = mbuf_create_dbdes(MBUF_DATA_BLOCK2048_SIZE, \
		pstMBuf->stDataBlockDescriptor.ulType, mpe);
	if(pstDataBlockDescriptor == NULL)
	{
		return MBUF_FAIL;
	}
	pstDataBlockDescriptor->pucData = pstDataBlockDescriptor->pucDataBlock + MBUF_DATA_BLOCK2048_SIZE - ulLength;

	pstDataBlockDescriptor->ulDataLength = ulLength;
	
	ulResult = mbuf_copy_to_buff(pstMBuf, 0, ulLength, pstDataBlockDescriptor->pucData);
	if(ulResult != 0)
	{
		(VOID)aos_dmem_free(pstDataBlockDescriptor->pucDataBlock);
		pstDataBlockDescriptor->ulDBMagic = 0;
		(VOID)aos_dmem_free(pstDataBlockDescriptor);
		return MBUF_FAIL;
	}

	
	(VOID)mbuf_cut_and_free_head(pstMBuf, ulLength);
	pstMBuf->ulTotalDataLength += ulLength;
	
	
	stDataBlockDescriptor = pstMBuf->stDataBlockDescriptor;
	pstMBuf->stDataBlockDescriptor = * (pstDataBlockDescriptor);
	* (pstDataBlockDescriptor) = stDataBlockDescriptor;
	pstMBuf->stDataBlockDescriptor.pstNextDataBlockDescriptor = pstDataBlockDescriptor;
	
	pstMBuf->ulDataBlockNumber ++;
	return MBUF_OK;
}

U32 mbuf_prepend_db(MBUF_S * pstMBuf, U32 ulLength, U32 mpe)
{
	MBUF_DATABLOCKDESCRIPTOR_S * pstDataBlockDescriptor;
	MBUF_DATABLOCKDESCRIPTOR_S stDataBlockDescriptor;

	if (MBUF_IS_BAD(pstMBuf))
	{
		AOS_ASSERT(0);
		return MBUF_FAIL;
	}

	if(ulLength > MBUF_DATA_BLOCK2048_SIZE)
	{
		return MBUF_FAIL;
	}
	if(ulLength == 0)
	{
		return MBUF_OK;
	}

	pstDataBlockDescriptor = mbuf_create_dbdes(MBUF_DATA_BLOCK2048_SIZE,\
		pstMBuf->stDataBlockDescriptor.ulType, mpe);
	if(pstDataBlockDescriptor == NULL)
	{
		return MBUF_FAIL;
	}
	pstDataBlockDescriptor->pucData = pstDataBlockDescriptor->pucDataBlock + MBUF_DATA_BLOCK2048_SIZE;
	pstDataBlockDescriptor->ulDataLength = 0;
	
	
	
	stDataBlockDescriptor = pstMBuf->stDataBlockDescriptor;
	pstMBuf->stDataBlockDescriptor = * (pstDataBlockDescriptor);
	* (pstDataBlockDescriptor) = stDataBlockDescriptor;
	pstMBuf->stDataBlockDescriptor.pstNextDataBlockDescriptor = pstDataBlockDescriptor;
	pstMBuf->ulDataBlockNumber ++;

	pstMBuf->stDataBlockDescriptor.pucData -= ulLength;
	pstMBuf->stDataBlockDescriptor.ulDataLength += ulLength;
	pstMBuf->ulTotalDataLength += ulLength;

	return MBUF_OK;
}

VOID mbuf_cut_head(MBUF_S * pstMBuf, U32 ulLength)
{
	U32 ulMyLength;
	MBUF_DATABLOCKDESCRIPTOR_S * pstDataBlockDescriptor;

	
	
	if (MBUF_IS_BAD(pstMBuf))
	{
		AOS_ASSERT(0);
		return ;
	}

	ulMyLength = (ulLength);
	pstDataBlockDescriptor = &((pstMBuf)->stDataBlockDescriptor);
	while(pstDataBlockDescriptor != NULL)
	{
		 if (DBES_IS_BAD(pstDataBlockDescriptor))
		{
			AOS_ASSERT(0);
			return ;
		}
		if(pstDataBlockDescriptor->ulDataLength <= ulMyLength)
		{
			ulMyLength -= pstDataBlockDescriptor->ulDataLength;
			pstDataBlockDescriptor->pucData += pstDataBlockDescriptor->ulDataLength;
			pstDataBlockDescriptor->ulDataLength = 0;

			
			pstDataBlockDescriptor = pstDataBlockDescriptor->pstNextDataBlockDescriptor;
		}
		else
		{
			pstDataBlockDescriptor->ulDataLength -= ulMyLength;
			pstDataBlockDescriptor->pucData += ulMyLength;
			ulMyLength = 0;
			break;
		}
	}
	
	pstMBuf->ulTotalDataLength -= ((ulLength) - ulMyLength);
	return ;
}

U32 mbuf_compress(MBUF_S * pstMBuf, U32 ulDataBlockNumber, U32 mpe)
{
	U32 ulLength;
	U32 ulType;
	U32 ulIndex;
	MBUF_DATABLOCKDESCRIPTOR_S * pstDataBlockDescriptor;
	U32 ulSurplusLength;
	MBUF_DATABLOCKDESCRIPTOR_S * pstDataBlockDescriptorNew;
	MBUF_DATABLOCKDESCRIPTOR_S * pstDataBlockDescriptorTemp;
	MBUF_DATABLOCKDESCRIPTOR_S * pstNextDataBlockDescriptorTemp;
	U32 ulResult;
	U8 * pucDataBlock;
	U32 ulDataBlockLength;
	
	if(ulDataBlockNumber == 0)
	{
		return MBUF_FAIL;
	}
	if (MBUF_IS_BAD(pstMBuf))
	{
		AOS_ASSERT(0);
		return MBUF_FAIL;
	}
	if(ulDataBlockNumber == 1)
	{

		
		if(pstMBuf->ulTotalDataLength <= 2048)
		{
			pucDataBlock = aos_dmem_alloc(mpe, SID_U8, 2048);
			ulDataBlockLength = 2048;
		}
		else if(pstMBuf->ulTotalDataLength <= 4096)
		{
			pucDataBlock = aos_dmem_alloc(mpe, SID_U8, 4096);
			ulDataBlockLength = 4096;
		}
		else
		{
			pucDataBlock = aos_dmem_alloc(mpe, SID_U8, pstMBuf->ulTotalDataLength);
			ulDataBlockLength = pstMBuf->ulTotalDataLength;
		}

		if(pucDataBlock == NULL)
		{
			return MBUF_FAIL;
		}
		
		
		ulResult = mbuf_copy_to_buff(pstMBuf, 0, pstMBuf->ulTotalDataLength, pucDataBlock);
		if(ulResult != 0)
		{
			(VOID)aos_dmem_free(pucDataBlock);
			return MBUF_FAIL;
		}
		
		
		for(pstDataBlockDescriptor = pstMBuf->stDataBlockDescriptor.pstNextDataBlockDescriptor;\
			pstDataBlockDescriptor != NULL;\
			pstDataBlockDescriptor = pstNextDataBlockDescriptorTemp)
		{
			if (DBES_IS_BAD(pstDataBlockDescriptor))
			{
				AOS_ASSERT(0);
				return MBUF_FAIL;
			}
			(VOID)aos_dmem_free(pstDataBlockDescriptor->pucDataBlock);
			
			pstNextDataBlockDescriptorTemp = pstDataBlockDescriptor->pstNextDataBlockDescriptor;
			pstDataBlockDescriptor->ulDBMagic = 0;
			(VOID)aos_dmem_free(pstDataBlockDescriptor);
		}
		(VOID)aos_dmem_free(pstMBuf->stDataBlockDescriptor.pucDataBlock);
		
		
		pstMBuf->stDataBlockDescriptor.pstNextDataBlockDescriptor = NULL;
		pstMBuf->stDataBlockDescriptor.pucDataBlock = pucDataBlock;
		pstMBuf->stDataBlockDescriptor.ulDataBlockLength = ulDataBlockLength;
		pstMBuf->stDataBlockDescriptor.pucData = pucDataBlock;
		pstMBuf->stDataBlockDescriptor.ulDataLength = pstMBuf->ulTotalDataLength;
		
		pstMBuf->ulDataBlockNumber = 1;
		return MBUF_OK;
	}
	else
	{
		ulLength = 0;
		ulType = pstMBuf->stDataBlockDescriptor.ulType;
		ulIndex = 0;
		pstDataBlockDescriptor = & pstMBuf->stDataBlockDescriptor;
		for(;;)
		{
			if (DBES_IS_BAD(pstDataBlockDescriptor))
			{
				AOS_ASSERT(0);
				return MBUF_FAIL;
			}
			
			ulLength += pstDataBlockDescriptor->ulDataLength;
			ulIndex ++;
			if(ulIndex == ulDataBlockNumber - 1)
			{
				break;
			}
			pstDataBlockDescriptor = pstDataBlockDescriptor->pstNextDataBlockDescriptor;
		}
		
		ulSurplusLength = pstMBuf->ulTotalDataLength - ulLength;
		if(ulSurplusLength <= 2048)
		{
			pstDataBlockDescriptorNew = mbuf_create_dbdes(2048, ulType, mpe);
			if(pstDataBlockDescriptorNew == NULL)
			{
				return MBUF_FAIL;
			}
		}
		else if(ulSurplusLength <= 4096)
		{
			pstDataBlockDescriptorNew = mbuf_create_dbdes(4096, ulType, mpe);
			if(pstDataBlockDescriptorNew == NULL)
			{
				return MBUF_FAIL;
			}
		}
		else
		{
			
			pstDataBlockDescriptorNew = aos_dmem_alloc(mpe, SID_MBUF_DBDES, MBUF_DATA_BLOCK_DESC_SIZE);
			if(pstDataBlockDescriptorNew == NULL)
			{
				return MBUF_FAIL;
			}
			pstDataBlockDescriptorNew->ulDBMagic = DBES_MAGIC_NUMBER;
			pstDataBlockDescriptorNew->pucDataBlock = aos_dmem_alloc(mpe, SID_U8, ulSurplusLength);
			if(pstDataBlockDescriptorNew->pucDataBlock == NULL)
			{
				pstDataBlockDescriptorNew->ulDBMagic = 0;
				(VOID)aos_dmem_free(pstDataBlockDescriptorNew);
				return MBUF_FAIL;
			}
			pstDataBlockDescriptorNew->pstNextDataBlockDescriptor = NULL;
			pstDataBlockDescriptorNew->ulDataBlockLength = ulSurplusLength;
			pstDataBlockDescriptorNew->ulType = ulType;
			pstDataBlockDescriptorNew->ulOption = 0;
		}
		pstDataBlockDescriptorNew->ulDataLength = ulSurplusLength;
		pstDataBlockDescriptorNew->pucData = pstDataBlockDescriptorNew->pucDataBlock;
		
		
		pstDataBlockDescriptorTemp = pstDataBlockDescriptor->pstNextDataBlockDescriptor;
		ulLength = 0;
		while(pstDataBlockDescriptorTemp != NULL)
		{
			if (DBES_IS_BAD(pstDataBlockDescriptorTemp))
			{
				AOS_ASSERT(0);
				return MBUF_FAIL;
			}
			if( NULL == aos_memcpy(pstDataBlockDescriptorNew->pucData + ulLength, \
				pstDataBlockDescriptorTemp->pucData, pstDataBlockDescriptorTemp->ulDataLength) )
			{
				(VOID)aos_dmem_free(pstDataBlockDescriptorNew->pucDataBlock);
				pstDataBlockDescriptorNew->ulDBMagic = 0;
				(VOID)aos_dmem_free(pstDataBlockDescriptorNew);
				return MBUF_FAIL;
			}
			
			ulLength += pstDataBlockDescriptorTemp->ulDataLength;
			pstDataBlockDescriptorTemp = pstDataBlockDescriptorTemp->pstNextDataBlockDescriptor;
		}
		
		pstDataBlockDescriptorTemp = pstDataBlockDescriptor->pstNextDataBlockDescriptor;
		while(pstDataBlockDescriptorTemp != NULL)
		{
			if (DBES_IS_BAD(pstDataBlockDescriptorTemp))
			{
				AOS_ASSERT(0);
				return MBUF_FAIL;
			}
			(VOID)aos_dmem_free(pstDataBlockDescriptorTemp->pucDataBlock);
			pstNextDataBlockDescriptorTemp = pstDataBlockDescriptorTemp->pstNextDataBlockDescriptor;
			pstDataBlockDescriptorTemp->ulDBMagic = 0;
			(VOID)aos_dmem_free(pstDataBlockDescriptorTemp);
			pstDataBlockDescriptorTemp = pstNextDataBlockDescriptorTemp;
		}
		
		pstDataBlockDescriptor->pstNextDataBlockDescriptor = pstDataBlockDescriptorNew;
		pstMBuf->ulDataBlockNumber = ulDataBlockNumber;
		return MBUF_OK;
	}
}

#if ( CONFIG_MANUAL_ALIGN == TRUE )
U32 mbuf_align_dword(MBUF_S *pMBuf)
{
    U8  *pByte;
    U16 *pShort;
    int off;
    U32 i;

    pByte = pMBuf->stDataBlockDescriptor .pucData;
    if((U32)pByte & 0x1)   
    {
       if( (U32)pByte & 0x2 )
       {  
          pByte=pByte-3;
          pMBuf->stDataBlockDescriptor .pucData -=3;
          off = 3;
       } 
       else
       { 
          pByte=pByte-1;
          pMBuf->stDataBlockDescriptor .pucData -=1;
          off = 1;
       }

       for( i= 0; i< pMBuf->stDataBlockDescriptor .ulDataLength; i++,pByte++ )
       {
           *pByte = *(pByte+off);
       }
      
    }
    else if( (U32)pByte & 0x2 )
    {  
        pShort=(U16*)(pMBuf->stDataBlockDescriptor .pucData-2);
        pMBuf->stDataBlockDescriptor .pucData -= 2;

        for( i= 0 ; i <(pMBuf->stDataBlockDescriptor .ulDataLength+1)/2; i++,pShort++)
        {
            *pShort = *(pShort+1);
        }

    } 
  
    return AOS_SUCC;
}    
#endif
#ifdef	__cplusplus
#if __cplusplus
}
#endif
#endif