Core/HDLC.cpp

/*===========================================================================
FILE:
   HDLC.cpp

DESCRIPTION:
   Encode and decode asynchronous HDLC protocol packets as described 
   by both the QUALCOMM download & SDIC (diagnostic) protocol documents

PUBLIC CLASSES AND METHODS:
   HDLCDecode()
   HDLCEncode()

Copyright (c) 2011, Code Aurora Forum. All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
    * Redistributions of source code must retain the above copyright
      notice, this list of conditions and the following disclaimer.
    * Redistributions in binary form must reproduce the above copyright
      notice, this list of conditions and the following disclaimer in the
      documentation and/or other materials provided with the distribution.
    * Neither the name of Code Aurora Forum nor
      the names of its contributors may be used to endorse or promote
      products derived from this software without specific prior written
      permission.


THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE.
===========================================================================*/

//-----------------------------------------------------------------------------
// Include Files
//-----------------------------------------------------------------------------
#include "StdAfx.h"
#include "HDLC.h"
#include "CRC.h"
#include "SharedBuffer.h"
#include "ProtocolServer.h"

//---------------------------------------------------------------------------
// Definitions
//---------------------------------------------------------------------------

/* Async HDLC defines */
const BYTE AHDLC_FLAG    = 0x7e;
const BYTE AHDLC_ESCAPE  = 0x7d;
const BYTE AHDLC_ESC_M   = 0x20;

/*=========================================================================*/
// Free Methods
/*=========================================================================*/

/*===========================================================================
METHOD:
   HDLCDecode (Free Method)

DESCRIPTION:
   HDLC decode the given buffer returning the results in an allocated buffer
  
PARAMETERS:
   pBuf        [ I ] - The data buffer to decode

RETURN VALUE:
   sSharedBuffer * : The decoded buffer (allocated), 0 on error
===========================================================================*/
sSharedBuffer * HDLCDecode( sSharedBuffer * pBuf )
{  
   // The return buffer
   sSharedBuffer * pRet = 0;

   // The has to be something to decode
   if (pBuf == 0 || pBuf->IsValid() == false)
   {
      return pRet;
   }

   // Grab raw data from shared buffer
   const BYTE * pData = pBuf->GetBuffer();
   UINT sz = pBuf->GetSize();

   // Is the first character a leading flag?
   if (pData[0] == AHDLC_FLAG)
   {
      pData++;
      sz--;
   }

   // There must be at least four bytes (data, CRC, trailing flag)
   if (sz < 4)
   {
      return pRet;
   }

   // The last character must be the trailing flag
   if (pData[sz - 1] == AHDLC_FLAG)
   {
      sz--;
   }
   else 
   {
      return pRet;      
   }

   // Allocate the decode buffer
   PBYTE pDecoded = new BYTE[sz];
   if (pDecoded == 0)
   {
      return pRet;
   }

   // Handle escaped characters and copy into decode buffer
   UINT encodeIndex = 0;
   UINT decodeIndex = 0;
   while (encodeIndex < sz)
   {
      BYTE b = pData[encodeIndex++];
      if (b == AHDLC_ESCAPE && encodeIndex < sz)
      {
         b = pData[encodeIndex++];
         b ^= AHDLC_ESC_M;
      }

      pDecoded[decodeIndex++] = b;
   }

   // Check CRC value
   if (CheckCRC( pDecoded, decodeIndex ) == false)
   {
      delete [] pDecoded;
      return pRet;
   }
      
   // Adjust decode length down for CRC
   decodeIndex -= 2;

   // ... and wrap up in a shared buffer
   pRet = new sSharedBuffer( decodeIndex, pDecoded, pBuf->GetType() );
   return pRet;
}

/*===========================================================================
METHOD:
   HDLCEncode (Free Method)

DESCRIPTION:
   HDLC encode the given buffer returning the results in an allocated buffer

PARAMETERS:
   pBuf        [ I ] - The data buffer to decode

RETURN VALUE:
   sSharedBuffer * : The decoded buffer (allocated), 0 on error
===========================================================================*/
sSharedBuffer * HDLCEncode( sSharedBuffer * pBuf )
{
   // The return buffer
   sSharedBuffer * pRet = 0;

   // The has to be something to decode
   if (pBuf == 0 || pBuf->IsValid() == false)
   {
      return pRet;
   }

   // Grab raw data from shared buffer
   const BYTE * pData = pBuf->GetBuffer();
   UINT sz = pBuf->GetSize();

   // Compute CRC
   USHORT CRC = CalculateCRC( pData, sz * 8 );

   // Allocate the encode buffer
   PBYTE pEncoded = new BYTE[sz * 2 + 4];
   if (pEncoded == 0)
   {
      return pRet;
   }

   // Add leading flag
   UINT encodeIndex = 0; 
   pEncoded[encodeIndex++] = AHDLC_FLAG;

   // Add data, escaping when necessary
   UINT decodeIndex = 0;
   while (decodeIndex < sz)
   {
      BYTE value = pData[decodeIndex++];  
      if (value == AHDLC_FLAG || value == AHDLC_ESCAPE)
      {
         value ^= AHDLC_ESC_M;
         pEncoded[encodeIndex++] = AHDLC_ESCAPE;
      }

      pEncoded[encodeIndex++] = value;
   }

   // Byte order CRC
   BYTE byteOrderedCRC[2];
   byteOrderedCRC[0] = (BYTE)(CRC & 0x00ff);
   byteOrderedCRC[1] = (BYTE)(CRC >> 8);

   // Add CRC
   UINT c = 0;
   while (c < 2)
   {
      BYTE value = byteOrderedCRC[c++];  
      if (value == AHDLC_FLAG || value == AHDLC_ESCAPE)
      {
         value ^= AHDLC_ESC_M;
         pEncoded[encodeIndex++] = AHDLC_ESCAPE;
      }

      pEncoded[encodeIndex++] = value;
   }

   // Add trailing flag
   pEncoded[encodeIndex++] = AHDLC_FLAG;

   // Wrap up in a shared buffer
   pRet = new sSharedBuffer( encodeIndex, pEncoded, pBuf->GetType() );
   return pRet;
}

/*===========================================================================
METHOD:
   HDLCUnitTest (Free Method)

DESCRIPTION:
   Simple in = out testing of HDLCEncode/HDLCDecode

RETURN VALUE:
   bool: 
      true  - Test succeeded
      false - Test failed
===========================================================================*/
#ifdef DEBUG
#include <cstdlib>

bool HDLCUnitTest()
{
   // Assume failure
   bool bRC = false;

   const UINT MAX_LEN = 2048;
   BYTE testBuf[MAX_LEN];

   srand( GetTickCount() );

   UINT len = (((UINT)rand()) % MAX_LEN) + 1;
   for (UINT i = 0; i < len; i++)
   {
      testBuf[i] = (BYTE)((UINT)rand() % 256);
   }

   sSharedBuffer * pOrig = new sSharedBuffer( testBuf, len, 0 );
   if (pOrig != 0)
   {
      // Encode buffer
      sSharedBuffer * pEnc = HDLCEncode( pOrig );
      if (pEnc != 0)
      {
         // Now decode buffer encoded above
         sSharedBuffer * pDec = HDLCDecode( pEnc );
         if (pDec != 0) 
         {
            if (pOrig->IsValid() == true && pDec->IsValid() == true)
            {
               // Compare decoded to original
               const BYTE * pOrigData = pOrig->GetBuffer();
               const BYTE * pDecData = pDec->GetBuffer();

               if (len == pDec->GetSize())
               {
                  int cmp = memcmp( (const void *)pOrigData,
                                    (const void *)pDecData,
                                    (size_t)len );

                  bRC = (cmp == 0);
               }
            }

            delete [] pDec;
         }

         delete [] pEnc;
      }

      delete [] pOrig;
   }

   return bRC;
}

#endif