icmp.cpp

Go to the documentation of this file.

// icmp.cpp

#include "icmp.h"

const char* ICMPDATATEMPLATE  = "abcdefghijklmnopqrstuvw";

void FillICMPData(char* pData, int size)
{
  while (size)
  {
    const char* pszTemplate = ICMPDATATEMPLATE;
    while (*pszTemplate && size)
    {
      *pData = *pszTemplate;
      size--;
      pszTemplate++;
      pData++;
    }
  }
}

USHORT IPChecksum(USHORT* pBuffer, int nSize)
{
  unsigned long cksum = 0;

  while (nSize > 1) 
  {
    cksum += *pBuffer++;
    nSize -= sizeof(USHORT);
  }
  
  if (nSize) 
    cksum += *(UCHAR*)pBuffer;

  cksum = (cksum >> 16) + (cksum & 0xffff);
  cksum += (cksum >> 16);
  return (USHORT)(~cksum);
}

void EncodePacket(char* pPacket, int iDataSize, USHORT uSequence)
{
  ICMP_HEADER* pIcmp = (ICMP_HEADER*)pPacket;
  pIcmp->type = ICMP_ECHO_REQUEST;
  pIcmp->code = 0;
  pIcmp->id = (USHORT)GetCurrentProcessId();
  pIcmp->seq = uSequence;
  char* pData = ((char*)pIcmp) + sizeof(ICMP_HEADER);
  FillICMPData(pData+sizeof(LONGLONG), iDataSize-sizeof(LONGLONG));
  pIcmp->cksum = 0;
  // Embed timer tick in first bytes
  LARGE_INTEGER starttick;
  QueryPerformanceCounter(&starttick);
  *(LONGLONG*)pData = starttick.QuadPart;
  pIcmp->cksum = IPChecksum((USHORT*)pIcmp, sizeof(ICMP_HEADER) + iDataSize);
}

bool DecodePacket(char* pPacket, int iSize, USHORT& uSequence, LONGLONG& llTick)
{
  IP_HEADER* pIPHeader = (IP_HEADER*)pPacket;
  int iIpHdrlen = pIPHeader->h_len * 4; //Number of 32-bit words*4 = bytes
  if (iSize < iIpHdrlen + (int)sizeof(ICMP_HEADER))
    return false;

  ICMP_HEADER* pIcmpHdr = (ICMP_HEADER*) (((char*) pIPHeader) + iIpHdrlen);
  char* pData = pPacket + iIpHdrlen + sizeof(ICMP_HEADER);
  //int iDataSize = iSize - iIpHdrlen - sizeof(ICMP_HEADER);
  USHORT uCksumPacket = pIcmpHdr->cksum;
  pIcmpHdr->cksum = 0;
  USHORT cksum = IPChecksum((USHORT*)pPacket, iSize);
  pIcmpHdr->cksum = uCksumPacket;
  if (cksum != uCksumPacket)
    return false;

  switch(pIcmpHdr->code)
  {
    case ICMP_ECHO_REPLY:
      if (pIcmpHdr->id == (USHORT)GetCurrentProcessId())
      {
        uSequence = pIcmpHdr->seq;
        // First data bytes are send tick
        llTick = *(LONGLONG*)pData;
        return true;
      }
      break;
    case ICMP_DESTINATION_UNREACHABLE:
      // Destination is unreachable and no response is gathered
      break;
    case ICMP_TIME_EXCEEDED:
      // No response is gathered
      break;
  }
  return false;
}

---