tpam_hdlc.c

移植到2410开发板上的源代码

		
/*-----------------------------
 | go back here to treat the
 | carry when its length
 | is over 7 and for the first
 | byte (with flag)
 +-----------------------------*/
carry:

		dwNb ++;

		/*-----------------------------
		 | shift the byte to get the
		 | byte to encode (without
		 | taking into account the
		 | initial shift)
		 +-----------------------------*/
		if (dwShiftNb)
		{
			dwShifter = byCharIn << dwShiftNb;
			byNewCarry = dwShifter >> 8;
			byCharIn = dwShifter | byCarry;
			byCarry = byNewCarry;
		}

		/*-----------------------------
		 | get the data from the arrays
		 | and take into account the
		 | initial shift for the byte
		 | to encode
		 +-----------------------------*/
		woInfo = stuffs[dwState][byCharIn];
		woDecal |= (woInfo & 0x00FF) << 8;
		* pbyBuffOut ++ = woDecal >> *pdwInitialShift;
		woDecal = woInfo & 0x00FF;
		dwState = woInfo >> 12;

		/*-----------------------------
		 | treat the lost bit if we had
		 | a carry overflow
		 +-----------------------------*/
		if (byCarryMSB != 0xFF)
		{
			if (!dwShiftNb)
			{
				if(byCarryMSB)
					byCarry = 1;
				dwShiftNb = 1;
			}
			byCarryMSB = 0xFF;
		}

		/*-----------------------------
		 | if one '0' get inserted, we
		 | have to calculate the new
		 | carry and the new shift
		 +-----------------------------*/
		if (woInfo & 0x0F00)
		{
			byCarryMSB = byCarry & 0x40;
			byCarry <<= (woInfo & 0x0C00) >> 10;
			byCarry |= (woInfo & 0x0300) >> 8;
			dwShiftNb += (woInfo & 0x0C00) >> 10;
		}

		/*-----------------------------
		 | if the carry is a whole byte
		 | we use it as a byte to encode
		 +-----------------------------*/
		if (dwShiftNb > 7)
		{
			if (dwShiftNb == 8)
				byCarryMSB = 0xFF;
			dwShiftNb = 0;
			byCharIn = byCarry;
			byCarry = 0;
			goto carry;
		}

		/*-----------------------------
		 | at the end of the array
		 +-----------------------------*/
		if (!dwLength)
		{
			/*-----------------------------
			 | take into account the bits
			 | set in the carry
			 +-----------------------------*/
			if (bContinue)
			{
				bContinue = FALSE;
				byCharIn = 0;
				goto carry;
			}
			
			/*-----------------------------
			 | treat the last byte if we
			 | had a carry overflow
			 +-----------------------------*/
			if (bContinue2 && ((8 - *pdwInitialShift) + dwShiftNb) > 7)
			{
				bContinue2 = FALSE;
				byCharIn = 0;
				goto carry;
			}

			/*-----------------------------
			 | Calculate the new shift
			 +-----------------------------*/
			*pdwInitialShift = ((8 - *pdwInitialShift) + dwShiftNb)%8;

			/*-----------------------------
			 | Add a flag at the end of the
			 | carry and a full flag
			 +-----------------------------*/
			pbyBuffOut--;
			*pbyBuffOut++ |= 0x7E << *pdwInitialShift;
			byCarry = 0x7E7E >> (8 - *pdwInitialShift);
			*pbyBuffOut++ = byCarry;
			*pbyBuffOut++ = byCarry;
			dwNb += 2;
		}
	}

	/*-------------------------------
	 | Pad the array to a multiple
	 | of 64 bytes.
	 +-------------------------------*/
	for(;dwNb%64;dwNb++)
		*pbyBuffOut ++ = byCarry;

	*pdwInitialShift = (8 - *pdwInitialShift)%8;

	return dwNb;
}



/*- AuverTech Telecom -------------------------------------------------------+
 |                                                                           |
 | @Function  : tpam_hdlc_decode                                             |
 | @Author    : Cyrille Boudon                                               |
 |                                                                           |
 +---------------------------------------------------------------------------+
 |                                                                           |
 | @Param     : BYTE * pbyBuffIn  IN,  array of bytes to decode              |
 | @Param     : BYTE * pbyBuffOut OUT, array of decoded bytes                |
 | @Param     : DWORD dwLength    IN,  count of bytes to decode              |
 |                                                                           |
 | @Return    : DWORD             count of decoded bytes                     |
 |                                                                           |
 +------------------------------- @Abstract ---------------------------------+
 |                                                                           |
 | Bit de-stuffing of the array pbyBuffIn. There has to be at least 1 full   |
 | flag at the beginning. At the end there has to be a flag or an abort (more|
 | than 6 consecutive '1').                                                  |
 | If an abort is encountered, the returned count is '0'.                    |
 |                                                                           |
 +---------------------------------------------------------------------------*/
DWORD tpam_hdlc_decode(BYTE * pbyBuffIn, BYTE * pbyBuffOut, DWORD dwLength)
{
	BYTE	byCharIn;    // byte being decoded
	BYTE	byCarry;     // current carry
	WORD	woInfo;      // data read in the arrays
	WORD	woNb1;       // count of '1' at the end of the previous byte
	BYTE	byShift;     // shift of the first flag
	DWORD	dwInit;      // temporary variable
	DWORD	dwLengthOut; // count of the decoded bytes
	BYTE	byLgCarry;   // count of used bits in the carry
	BYTE	byLgByte;    // count of used bits in the decoded byte

	/*-----------------------------
	 | Find the 1st flag. At the end
	 | of the loop dwShift is the count
	 | of bits to reach the 1st bit
	 | of the 1st flag.
	 +-----------------------------*/
	dwInit = *pbyBuffIn | (*(pbyBuffIn+1)<<8) | (*(pbyBuffIn+2)<<16);
	for (byShift=0;byShift<17;byShift++)
	{
		if (!(((dwInit>>byShift)&0xFF)^0x7E))
		{
			break;
		}
	}

	/*-----------------------------
	 | If at the end of the previous
	 | loop dwShift = 17, it means
	 | that no flag was found in the
	 | first 3 bytes (normally
	 | impossible impossible with the
	 | DSP algorithm)
	 +-----------------------------*/
	if (byShift == 17)
		return 0;

	/*-----------------------------
	 | Plase the array pointer after
	 | the first flag. Update the
	 | shift.
	 +-----------------------------*/
	pbyBuffIn += byShift/8 + 1;
	dwLength -= byShift/8 + 1;
	byShift %= 8;
	
	/*-----------------------------
	 | Walk through the frame to
	 | find the first data byte
	 +-----------------------------*/
	dwInit = *pbyBuffIn | (*(pbyBuffIn+1)<<8);
	while (!(((dwInit>>byShift)&0xFF)^0x7E))
	{
		pbyBuffIn ++;
		dwLength --;
		dwInit = *pbyBuffIn | (*(pbyBuffIn+1)<<8);
	}

	dwLengthOut = 0;
	byCarry = 0;
	byLgCarry = 0;
	byLgByte = 0;

	/*-------------------------------
	 | Treat the first byte
	 +-------------------------------*/	
	byCharIn = (*pbyBuffIn >> byShift) << byShift;
	woInfo = destuffs[0][byCharIn];
	byLgByte = ((woInfo & 0x7000) >> 12) + 1;
	woNb1 = (woInfo & 0x0F00) >> 8;
	dwLength --;
	pbyBuffIn++;

	if (woNb1 > 5)
		return 0;

	if (byLgByte - byShift == 8)
	{
		*pbyBuffOut ++ = woInfo;
		dwLengthOut ++;
	}
	else
	{
		byCarry = woInfo << (8 - byLgByte);
		byLgCarry = byLgByte - byShift;
	}

	/*-------------------------------
	 | main loop
	 +-------------------------------*/
	while(dwLength --)
	{
		byCharIn = *pbyBuffIn ++;

		woInfo = destuffs[woNb1][byCharIn];
		byLgByte = ((woInfo & 0x7000) >> 12) + 1;
		
		/*-------------------------------
		 | if the used bits in the carry
		 | and the current byte makes
		 | possible to output a full byte
		 +-------------------------------*/
		if (byLgByte + byLgCarry >= 8)
		{
			*pbyBuffOut ++ = ( (woInfo << 8) | byCarry) >> (8 - byLgCarry);
			dwLengthOut ++;
			byLgCarry += byLgByte - 8;
			byCarry = woInfo << (8-byLgByte);
		}
		/*-------------------------------
		 | if the used bits in the carry
		 | and the current byte doesn't
		 | make possible to output a full 
		 | byte
		 +-------------------------------*/
		else
		{
			dwInit = (woInfo << 8) | byCarry;
			byLgCarry += byLgByte;
			byCarry = dwInit >> byLgByte;
		}

		woNb1 = (woInfo & 0x0F00) >> 8;

		/*-------------------------------
		 | if the current byte contains
		 | six or more consecutive '1'
		 +-------------------------------*/
		if (woInfo & 0x8000)
		{
			byCarry = ((byCharIn << 8) | *(pbyBuffIn-2)) >> (8 - byLgCarry);
			if (byCarry == 0x7E)
				return dwLengthOut-1;
			else
				if (woNb1 > 6)
					return 0;
				else
					if ((!(*pbyBuffIn & 1)) && (byLgCarry == 7))
						return dwLengthOut;
					else
						return 0;
		}
	}

	return dwLengthOut;
}