#define _WINSOCK_DEPRECATED_NO_WARNINGS 1

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <Winsock2.h>
#include <ws2tcpip.h>
#include <iphlpapi.h>
#include <process.h>

#include "j2534.h"    // j1699 project j2534 declarations
#include "j1699.h"    // j1699 project general declarations
#include "Sockets.h"  // j1699 project Socket,UDP,TCP declarations
#include "DHCP.h"     // j1699 project DHCP declarations
#include "DoIP.h"     // j1699 project DoIP declarations


// function prototypes
int ReadPacket   ( t_stSocketData *pstSocketData,
                   BYTE           *puStaticBuffer,
                   int             iMaxBuferSize,
                   unsigned long   ulTimeout );
void FindVehicle ( t_stSocketData *pstSocketData );
STATUS RoutingActivation ( void );
long   OpenChannel      ( void );
STATUS StartDoIPChannel ( void );
STATUS ReadDoIPMsg  ( PASSTHRU_MSG *pRxMsg,
                      PASSTHRU_MSG *pTxMsg,
                      WORD *pMsgCnt );
STATUS WriteDoIPMsg ( BYTE *pBuf,
                      WORD BufLen );

STATUS CheckDoIPChannel ( void );


// Local Variables and Constants
static boolean bWindowsSocketsStarted = FALSE;
static boolean bNetworkStarted = FALSE;
static boolean bRoutingActivated = FALSE;
static boolean bSocketDataInitialized = FALSE;
static boolean bForceSocketClose = FALSE;

#define DOIP_TIMEOUT 2000

#define MAX_READ_ATTEMPTS 2   // maximum number of consecutive attempts to read without receiving data
#define MAX_WRITE_ATTEMPTS 3  // maximum number of consecutive attempts to write without response


t_stIPAddressInfo IPAddressInfo[MAX_IP_ADDRESSES];
BYTE              IPAddressInfoCount = 0;

BYTE   EntityCount = 0;

extern unsigned int  BindDelay;
extern unsigned int  BindCountMax;
extern unsigned int  ConnectDelay;
extern unsigned int  ConnectCountMax;

enum
{
	eVIDRequest,
	ePMRequest,
	eStatusRequest,
	eDOIPHandshake
};


unsigned char astDOIPRequests[eDOIPHandshake][sizeof(t_stDOIPHeader)]  =
{
	{0xFF, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00},
	{0x02, 0xFD, 0x40, 0x01, 0x00, 0x00, 0x00, 0x00}
};
unsigned char astDOIPResponses[eDOIPHandshake][sizeof(t_stDOIPHeader)] =
{
	{0x02, 0xFD, 0x00, 0x04, 0x00, 0x00, 0x00, 0x21},
	{0x02, 0xFD, 0x40, 0x02, 0x00, 0x00, 0x00, 0x07}
};
//t_sVIDResponse     stVIDResponse    = { 0x02, 0xFD, 0x00, 0x04, 0x00, 0x00, 0x00, 0x33, "", 0x0C, 0x45, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0 };
//t_stStatusResponse stStatusResponse = { 0x02, 0xFD, 0x40, 0x02, 0x00, 0x00, 0x00, 0x07, 0x00, 0x01, 0x00, htonl(4099) };


// Power Mode defines
#define POWERMODENOTREADY      0x00
#define POWERMODEREADY         0x01
#define POWERMODENOTSUPPORTED  0x02

char *PowerModeStatus[] = 
{
	"Not Ready",
	"Ready",
	"Not Supported"
};


// Entity Status defines
#define DOIPGATEWAY       0x00
#define DOIPNODE          0x01

char *EntityNodeType[] = 
{
	"DoIP Gateway",
	"DoIP Node",
	"Other"
};


// Message Definitions
BYTE NACKResponse[] =
{ 0x02, 0xFD, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x04 };

BYTE VehicleIdentificationRequest[] =
{ 0xFF, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00};
//{ 0x02, 0xFD, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00 };

BYTE VehicleIdentificationRequest_WithEIN[] =
{ 0x02, 0xFD, 0x00, 0x02, 0x00, 0x00, 0x00, 0x06, 0x31, 0x31, 0x31, 0x31, 0x31, 0x31 };

BYTE VehicleIdentificationRequest_WithVIN[] =
{ 0x02, 0xFD, 0x00, 0x03, 0x00, 0x00, 0x00, 0x11, 0x31, 0x31, 0x31, 0x31, 0x31, 0x31, 0x31, 0x31, 0x31, 0x31, 0x31, 0x31, 0x31, 0x31, 0x31, 0x31, 0x31 };

BYTE VehicleIdentificationResponse[] =
{ 0x02, 0xFD, 0x00, 0x04, 0x00, 0x00, 0x00, 0x21, 0x31, 0x31, 0x31, 0x31, 0x31, 0x31, 0x31, 0x31, 0x31, 0x31, 0x31, 0x31, 0x31, 0x31, 0x31, 0x31, 0x31, 0x12, 0x34, 0x31, 0x31, 0x31, 0x31, 0x31, 0x31, 0x32, 0x32, 0x32, 0x32, 0x32, 0x32, 0x00, 0x00 };


BYTE RoutingActivationRequest[] =
{ 0x02, 0xFD, 0x00, 0x05, 0x00, 0x00, 0x00, 0x07, 0x0E, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00 };

BYTE RoutingActivationRequest_WithVM[] =
{ 0x02, 0xFD, 0x00, 0x05, 0x00, 0x00, 0x00, 0x0B, 0x0E, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x02, 0x03, 0x04 };

BYTE RoutingActivationResponse[] =
{ 0x02, 0xFD, 0x00, 0x06, 0x00, 0x00, 0x00, 0x09, 0x0E, 0x00, 0x0E, 0x01, 0x10, 0x00, 0x00, 0x00, 0x00 };

BYTE RoutingActivationResponse_Unsuccessful[] =
{ 0x02, 0xFD, 0x00, 0x06, 0x00, 0x00, 0x00, 0x09, 0x0E, 0x00, 0x00, 0x37, 0x00, 0x00, 0x00, 0x00, 0x00 };

BYTE RoutingActivationResponse_WithVM[] =
{ 0x02, 0xFD, 0x00, 0x06, 0x00, 0x00, 0x00, 0x0D, 0x0E, 0x00, 0x00, 0x37, 0x10, 0x00, 0x00, 0x00, 0x00, 0x04, 0x03, 0x02, 0x01 };


BYTE AliveCheckRequest[] =
{ 0x02, 0xFD, 0x00, 0x07, 0x00, 0x00, 0x00, 0x00 };

BYTE AliveCheckResponse[] =
{ 0x02, 0xFD, 0x00, 0x08, 0x00, 0x00, 0x00, 0x02, 0x0E, 0x00 };


BYTE EntityStatusRequest[] =
{ 0x02, 0xFD, 0x40, 0x01, 0x00, 0x00, 0x00, 0x00 };

BYTE EntityStatusResponse[] =
{ 0x02, 0xFD, 0x40, 0x02, 0x00, 0x00, 0x00, 0x03, 0x01, 0x10, 0x01 };

BYTE EntityStatusResponse_WithMDS[] =
{ 0x02, 0xFD, 0x40, 0x02, 0x00, 0x00, 0x00, 0x07, 0x01, 0x10, 0x01, 0x00, 0x00, 0x10, 0x00 };


BYTE PowerModeRequest[] =
{ 0x02, 0xFD, 0x40, 0x03, 0x00, 0x00, 0x00, 0x00 };

BYTE PowerModeResponse[] =
{ 0x02, 0xFD, 0x40, 0x04, 0x00, 0x00, 0x00, 0x01, 0x01 };


BYTE DiagnosticMsgRequest[] =
{ 0x02, 0xFD, 0x80, 0x01, 0x00, 0x00, 0x00, 0x07, 0x0E, 0x00, 0x00, 0x01, 0x00, 0x01, 0x02 };

BYTE DiagnosticMsgResult[] =
{ 0x02, 0xFD, 0x80, 0x01, 0x00, 0x00, 0x00, 0x08, 0x00, 0xE0, 0x00, 0x55, 0x00, 0x01, 0x02, 0x03 };

BYTE DiagnosticMsg_PositiveResponse[] =
{ 0x02, 0xFD, 0x80, 0x02, 0x00, 0x00, 0x00, 0x05, 0x00, 0x00, 0x00, 0x00, 0x00 };

BYTE DiagnosticMsg_NegativeResponse[] =
{ 0x02, 0xFD, 0x80, 0x03, 0x00, 0x00, 0x00, 0x05, 0x00, 0x00, 0x00, 0x00, 0x05 };




/*******************************************************************************
**
**  Function:  ReadPacket
**
**  Purpose:   Read a packet of data from the socket
**
*******************************************************************************/
int ReadPacket ( t_stSocketData *pstSocketData,
                 BYTE           *puStaticBuffer,
                 int             iMaxBuferSize,
                 unsigned long   ulTimeout )
{
	TIMEVAL         stTimeVal;
	FD_SET          stReadFD;
	FD_SET          stExceptFD;
	INT             iStatus;
	INT             iSize = sizeof ( pstSocketData->remote_sin );
	INT             iBytesRead = 0;

	BYTE            PacketBuffer[MAX_MSG_LEN];
	static BYTE     StorageBuffer[2*MAX_MSG_LEN];
	static INT      StorageBufferSize = 0;
	static boolean  bMsgIncomplete = FALSE;

	INT             MsgLngth = 0;
	WORD            ByteIdx;
	BYTE           *pBuf = NULL;
	boolean         bMsgMove = FALSE;


	// check if socket data is  available
	if ( StorageBufferSize == 0 &&
	     ulTimeout != 0 )
	{
		stTimeVal.tv_sec  = ulTimeout / 1000;
		stTimeVal.tv_usec = (ulTimeout % 1000) * 1000;

		FD_ZERO( &stReadFD );
		FD_SET( pstSocketData->hSock, &stReadFD );

		FD_ZERO( &stExceptFD );
		FD_SET( pstSocketData->hSock, &stExceptFD );

		// Returns when there is data to be read or/and exception or a timeout
		iStatus = select ( 1, &stReadFD, NULL, &stExceptFD, &stTimeVal );

		// If error 
		if ( iStatus == SOCKET_ERROR )
		{
			return -1 * WSAGetLastError ( );
		}

		// Timeout
		if ( iStatus == 0 )
		{
			return 0;
		}
	}


	// if Storage Buffer is empty or incomplete,
	// read in more
	if ( StorageBufferSize == 0 ||
	     bMsgIncomplete == TRUE )
	{
		// if reading from a TCP socket
		if ( pstSocketData->Type == TCP_SOCKET )
		{
			iBytesRead = recv ( pstSocketData->hSock, PacketBuffer, iMaxBuferSize, 0 );
		}
		// else reading from a UDP socket
		else
		{
			iBytesRead = recvfrom ( pstSocketData->hSock, PacketBuffer, iMaxBuferSize, 0,
			                        (struct sockaddr*)&pstSocketData->remote_sin, &iSize );
		}
		if ( iBytesRead == SOCKET_ERROR )
		{
			if ( WSAGetLastError ( ) == WSAETIMEDOUT )
			{
				printf( "Timeout occurred\n" );
			}
			return -1 * WSAGetLastError ( );
		}

		if ( iBytesRead > 0 )
		{
			// if there isn't an incomplete message in the storage buffer
			// prepare to copy the new data to output buffer
			if ( bMsgIncomplete == FALSE )
			{
				if ( iBytesRead < sizeof ( t_stDOIPHeader ) )
				{
					Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
					      "Socket buffer did not contain enough data (%d) for a full DoIP header (%d).\n",
					      iBytesRead,
					      sizeof ( t_stDOIPHeader ) );
					return 0;
				}
				// if this is vehicle identification request loopback,
				// it does not contain the length in the messge,
				// it is 8 bytes long
				if ( PacketBuffer[0] == 0xFF )
				{
					MsgLngth = 8;
				}
				// otherwise get the length from the message 
				else
				{
					MsgLngth = (PacketBuffer[4] << 24) +
					           (PacketBuffer[5] << 16) +
					           (PacketBuffer[6] << 8) +
					            PacketBuffer[7] +
					            8; // include DoIP header length
				}
				// point to packet buffer for message copy
				pBuf = &PacketBuffer[0];
			}

			if ( MsgLngth > iBytesRead )
			{
				bMsgIncomplete = TRUE;
			}

			// if there is more than one message in the packet buffer or
			// the packet buffer contains an incomplete messge,
			// save the remainder in the storage buffer
			if ( MsgLngth < iBytesRead ||
			     bMsgIncomplete == TRUE )
			{
				memcpy ( &StorageBuffer[StorageBufferSize], &PacketBuffer[MsgLngth], (iBytesRead - MsgLngth) );
				StorageBufferSize += iBytesRead - MsgLngth;
			}
		}
	}

	// if there isn't a full message ready to go from packet buffer
	// and there is data in the storage buffer,
	// check if there is a complete message in storage buffer
	if ( !pBuf &&
	     StorageBufferSize > sizeof (t_stDOIPHeader) )
	{
		ByteIdx = 0;
		// check that data is a DoIP message
		while ( (StorageBuffer[ByteIdx]   != 0x02 ||
		         StorageBuffer[ByteIdx+1] != 0xFD) &&
		        ByteIdx < StorageBufferSize )
		{
			ByteIdx++;
		}
		// if the buffer doesn't start with a DoIP message
		// but there is one latter, shuffle out the bad bytes
		if ( ByteIdx != 0 &&
		     ByteIdx < StorageBufferSize )
		{
			BYTE String[2*MAX_MSG_LEN];
			if ( !gbSocketCheck )
			{
				Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
				      "Extraneous Data in RX Buffer: Purging\n" );
			}
			String[0] = (BYTE)NULL;
			for ( WORD i = 0;
			      i < StorageBufferSize;
			      i++ )
			{
				if ( i < ByteIdx )
				{
					sprintf_s ( &String[strlen(String)], (PACKET_SIZE*3) - strlen ( String ),
					            "%02X ",
					            StorageBuffer[i] );
				}

				// shuffle data
				StorageBuffer[i] = StorageBuffer[ByteIdx+i];
			}
			if ( !gbSocketCheck )
			{
				Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
				      "Purged Data: %s\n",
				      String );
			}
			gbSocketCheck = FALSE;
			StorageBufferSize -= ByteIdx;
		}
		else if ( ByteIdx == StorageBufferSize )
		{
			BYTE String[2 * MAX_MSG_LEN];
			Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
			      "Improperly formatted message in Rx Buffer: Purging\n" );
			if ( StorageBufferSize != 0 )
			{
				for ( ByteIdx = 0;
				      ByteIdx < StorageBufferSize;
				      ByteIdx++ )
				{
					sprintf_s ( &String[strlen ( String )], (PACKET_SIZE * 3) - strlen ( String ),
					            "%02X ",
					            StorageBuffer[ByteIdx] );
				}
				Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
				      "Purged Data: %s\n",
				      String );
				memset ( &StorageBuffer[0],
				         0x00,
				         sizeof ( StorageBuffer ) );
				StorageBufferSize = 0;
			}
		}

		MsgLngth = (StorageBuffer[4] << 24) +
		           (StorageBuffer[5] << 16) +
		           (StorageBuffer[6] << 8) +
		            StorageBuffer[7] +
		            8; // include DoIP header length

		if ( MsgLngth <= StorageBufferSize )
		{
			// point to storage buffer for message copy
			pBuf = &StorageBuffer[0];

			// if there is more data in the storage buffer
			// indicate it needs to be moved after copy
			if ( MsgLngth < StorageBufferSize )
			{
				bMsgMove = TRUE;
			}
			else
			{
				StorageBufferSize = 0;
				bMsgIncomplete = FALSE;
			}
		}
	}

	if ( pBuf )
	{
		memcpy ( (char*)puStaticBuffer, pBuf, MsgLngth) ;
	}

	// if storage buffer needs to be shuffled
	if ( bMsgMove )
	{ 
		StorageBufferSize -= MsgLngth;
		for ( WORD i = 0;
		      i < StorageBufferSize;
		      i++ )
		{
			StorageBuffer[i] = StorageBuffer[MsgLngth+i];
		}
	}

	return MsgLngth;
}




/*******************************************************************************
**
**  Function:  FindVehicle
**
**  Purpose:   
**
*******************************************************************************/
void FindVehicle ( t_stSocketData *pstSocketData )
{
	WORD            iBytesWritten;
	WORD            iBytesRead;
	BYTE            aucReadMessage[PACKET_SIZE];
	DWORD           VehicleEndTimestampMsecs;
#define VEHICLE_TIMEOUT  2000  // 2 seconds

	BYTE            DoIPIndex;
	BYTE            IPIndex;
	BYTE            ByteIdx;
	char            DataString[PACKET_SIZE*3];
	char            TextString[200];

	boolean         bRsp = FALSE;
	BYTE            ReadAttempts;
	BYTE            WriteAttempts;

	// In C, as opposed to C++, global static definitions cannot include function calls, so the initialization cannot use htons();
	// Initialization of automatic variables in a function can use htons, so each thread could have a copy of these definitions,
	// using structure initialization, but a single copy used by all threads must be initialized as byte arrays.
	BYTE astDOIPRequests[eDOIPHandshake][sizeof(t_stDOIPHeader)] =
	{
		{0xFF, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00},  // Vehicle Identification Request
		{0x02, 0xFD, 0x40, 0x03, 0x00, 0x00, 0x00, 0x00},  // Power Mode Request
		{0x02, 0xFD, 0x40, 0x01, 0x00, 0x00, 0x00, 0x00}   // DoIP Entity Status Request
	};
	BYTE astDOIPResponses[eDOIPHandshake][sizeof(t_stDOIPHeader)] =
	{
		{0x02, 0xFD, 0x00, 0x04, 0x00, 0x00, 0x00, 0x21},  // Vehicle Announcement Message/Vehicle Identification Response
		{0x02, 0xFD, 0x40, 0x04, 0x00, 0x00, 0x00, 0x01},  // Power Mode Response
		{0x02, 0xFD, 0x40, 0x02, 0x00, 0x00, 0x00, 0x07}   // DoIP Entity Status Response
	};


	// Initialize Entity Count each time we try to connect
	EntityCount = 0;

	// Initialize bNetworkStarted each time we try to connect
	bNetworkStarted = FALSE;

	// purge any previously received data
	PurgeSocketBuffer ( pstSocketData );


	Log ( BLANK, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
	      "\n" );

	for ( DoIPIndex = 0;
	      DoIPIndex < eDOIPHandshake;
	      DoIPIndex++ )
	{
		bRsp = FALSE;
		ReadAttempts = 0;
		WriteAttempts = 0;
		do
		{
			// Setup the address for the the broadcast request
			pstSocketData->remote_sin.sin_family       = AF_INET;
			pstSocketData->remote_sin.sin_addr.s_addr  = INADDR_BROADCAST;
			pstSocketData->remote_sin.sin_port         = htons(TESTERPORT);

			iBytesWritten = sendto ( pstSocketData->hSock, (char*)&astDOIPRequests[DoIPIndex], sizeof ( t_stDOIPHeader ), 0, (SOCKADDR*)&pstSocketData->remote_sin, sizeof ( pstSocketData->remote_sin ) );
			if ( iBytesWritten != sizeof ( t_stDOIPHeader ) )
			{
				pstSocketData->iError  = WSAGetLastError ( );
				char *pErrorString = GetEthernetErrorString ( pstSocketData->iError );
				Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
				      "Socket send Error: %d: %s\n",
				      pstSocketData->iError,
				      pErrorString );
				continue;
			}
			WriteAttempts++;
			VehicleEndTimestampMsecs = GetTickCount ( ) + VEHICLE_TIMEOUT;  // Get the end time for vehicle message check

			if ( DoIPIndex == eVIDRequest )
			{
				sprintf_s ( TextString, 200,
				            "Vehicle Identification Request" );
			}
			else if ( DoIPIndex == ePMRequest )
			{
				sprintf_s ( TextString, 200,
				            "Power Mode Request" );
			}
			else if ( DoIPIndex == eStatusRequest )
			{
				sprintf_s ( TextString, 200,
				            "Entity Status Request" );
			}

			//////////////////////////////////////////////////////////////////////////
			// Log Message Name
			Log ( BLANK, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
			      "\n" );
			Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
			      "%s\n",
			      TextString );

			//////////////////////////////////////////////////////////////////////////
			// Log Tx Message
			DataString[0] = (BYTE)NULL;
			for ( ByteIdx = 0;
			      ByteIdx < iBytesWritten;
			      ByteIdx++ )
			{
				sprintf_s ( &DataString[strlen (DataString)], (PACKET_SIZE*3) - strlen ( DataString ),
				            "%02X ",
				            astDOIPRequests[DoIPIndex][ByteIdx] );
			}
			DataString[3*ByteIdx] = (BYTE)NULL;

			Log ( NETWORK, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
			      "TX MSG: %10luusec %s %s\n",
			      GetTickCount ( ),
			      gstProtocolList[gProtocolIdx].eProtocol == DOIP ? "DOIP" : "DOIP_NDIS",
			      DataString );
			//////////////////////////////////////////////////////////////////////////

			do
			{
				iBytesRead = ReadPacket ( pstSocketData, aucReadMessage, sizeof ( aucReadMessage ), DOIP_TIMEOUT );
				if ( iBytesRead < 0 )
				{
					pstSocketData->iError  = -iBytesRead;
					return;
				}
				else if ( iBytesRead > 0 )
				{
					if ( pstSocketData->remote_sin.sin_addr.S_un.S_addr != pstSocketData->pAddr->ulIPv4Address &&
					     pstSocketData->remote_sin.sin_addr.S_un.S_addr != INADDR_BROADCAST )
					{
						// check if this IP Address is already in the list
						for ( IPIndex = 0;
						      IPIndex < IPAddressInfoCount && IPIndex < MAX_IP_ADDRESSES;
						      IPIndex++ )
						{
							if ( IPAddressInfo[IPIndex].ulIPv4Address == pstSocketData->remote_sin.sin_addr.S_un.S_addr )
							{
								break;
							}
						}
						if ( IPIndex == IPAddressInfoCount )
						{
							IPAddressInfo[IPIndex].ulIPv4Address = pstSocketData->remote_sin.sin_addr.S_un.S_addr;
							IPAddressInfoCount++;
						}
					}

					//////////////////////////////////////////////////////////////////////////
					// Log Rx Message
					DataString[0] = (BYTE)NULL;
					sprintf_s ( &DataString[strlen ( DataString )], (PACKET_SIZE * 3) - strlen ( DataString ),
					            "RX MSG: %10luusec %s ",
					            GetTickCount ( ),
					            gstProtocolList[gProtocolIdx].eProtocol == DOIP ? "DOIP" : "DOIP_NDIS" );
					for ( ByteIdx = 0;
					      ByteIdx < iBytesRead;
					      ByteIdx++ )
					{
						sprintf_s ( &DataString[strlen (DataString)], (PACKET_SIZE*3) - strlen ( DataString ),
						            "%02X ",
						            aucReadMessage[ByteIdx] );
					}
					sprintf_s ( &DataString[strlen ( DataString )], (PACKET_SIZE * 3) - strlen ( DataString ),
					            "  recvd from  %d.%d.%d.%d",
					            pstSocketData->remote_sin.sin_addr.S_un.S_un_b.s_b1,
					            pstSocketData->remote_sin.sin_addr.S_un.S_un_b.s_b2,
					            pstSocketData->remote_sin.sin_addr.S_un.S_un_b.s_b3,
					            pstSocketData->remote_sin.sin_addr.S_un.S_un_b.s_b4 );
					//////////////////////////////////////////////////////////////////////////

					// if loopback, ignore it
					if ( memcmp ( aucReadMessage, astDOIPRequests[DoIPIndex], iBytesRead ) == 0 ||
					     pstSocketData->pAddr->ulIPv4Address == pstSocketData->remote_sin.sin_addr.S_un.S_addr )
					{
						Log ( NETWORK, SCREENOUTPUTOFF, LOGOUTPUTON, NO_PROMPT,
						      "%s\n",
						      DataString );
						Log ( INFORMATION, SCREENOUTPUTOFF, LOGOUTPUTON, NO_PROMPT,
						      "Loopback of Request  rcvd\n" );
						ReadAttempts = 0;
					}

					// if the received msg is Vehicle Identification and already received, ignore it
					else if ( memcmp ( (BYTE*)&astDOIPResponses[0], &aucReadMessage[0], (sizeof ( t_stDOIPHeader ) - 1) ) == 0 &&
					          bNetworkStarted == TRUE )
					{
						Log ( NETWORK, SCREENOUTPUTOFF, LOGOUTPUTON, NO_PROMPT,
						      "%s\n",
						      DataString );
						Log ( INFORMATION, SCREENOUTPUTOFF, LOGOUTPUTON, NO_PROMPT,
						      "Vehicle Identification duplicate  Ignored\n" );
					}

					else
					{
						Log ( NETWORK, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
						      "%s\n",
						      DataString );

						// if the received msg matches the expected response
						if ( memcmp ( (BYTE*)&astDOIPResponses[DoIPIndex], &aucReadMessage[0], (sizeof ( t_stDOIPHeader ) - 1) ) == 0 )
						{
							if ( DoIPIndex == eVIDRequest )
							{
								t_sVIDResponse* psVIDResponse = (t_sVIDResponse*)&aucReadMessage[0];

								memcpy ( gVIDString, psVIDResponse->Info.acVIN, 17 );
								gVIDString[17] = (char)NULL;

								Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
								      "Vehicle Identification Response from %d.%d.%d.%d:\n"
								      "                                VIN: \"%s\"\n"
								      "                                Logical Address: %04X\n"
								      "                                EID: %d.%d.%d.%d.%d.%d\n"
								      "                                GID: %d.%d.%d.%d.%d.%d\n"
								      "                                Action: %02X\n\n",
								      pstSocketData->remote_sin.sin_addr.S_un.S_un_b.s_b1,
								      pstSocketData->remote_sin.sin_addr.S_un.S_un_b.s_b2,
								      pstSocketData->remote_sin.sin_addr.S_un.S_un_b.s_b3,
								      pstSocketData->remote_sin.sin_addr.S_un.S_un_b.s_b4,
								      gVIDString,
								      _byteswap_ushort ( psVIDResponse->Info.usLogicalAddress ),
								      psVIDResponse->Info.aucEID[0], psVIDResponse->Info.aucEID[1],
								      psVIDResponse->Info.aucEID[2], psVIDResponse->Info.aucEID[3],
								      psVIDResponse->Info.aucEID[4], psVIDResponse->Info.aucEID[5],
								      psVIDResponse->Info.aucGID[0], psVIDResponse->Info.aucGID[1],
								      psVIDResponse->Info.aucGID[2], psVIDResponse->Info.aucGID[3],
								      psVIDResponse->Info.aucGID[4], psVIDResponse->Info.aucGID[5],
								      psVIDResponse->Info.ucAction );

								bNetworkStarted = TRUE;
							}

							else if ( DoIPIndex == ePMRequest )
							{
								t_stPMResponse* psPMResponse = (t_stPMResponse*)&aucReadMessage[0];

								Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
								      "Power Mode Response from %d.%d.%d.%d:\n"
								      "                    Mode: %d (%s)\n\n",
								      pstSocketData->remote_sin.sin_addr.S_un.S_un_b.s_b1,
								      pstSocketData->remote_sin.sin_addr.S_un.S_un_b.s_b2,
								      pstSocketData->remote_sin.sin_addr.S_un.S_un_b.s_b3,
								      pstSocketData->remote_sin.sin_addr.S_un.S_un_b.s_b4,
								      psPMResponse->ucMode,
								      PowerModeStatus[psPMResponse->ucMode] );
							}

							else if ( DoIPIndex == eStatusRequest )
							{
								t_stStatusResponse* psStatusResponse = (t_stStatusResponse*)&aucReadMessage[0];

								Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
								      "Entity Status Response from %d.%d.%d.%d:\n"
								      "                       Node Type:     %d (%s)\n"
								      "                       Max Sockets:   %d\n"
								      "                       Open Sockets:  %d\n"
								      "                       Max Data Size: %ld\n\n",
								      pstSocketData->remote_sin.sin_addr.S_un.S_un_b.s_b1,
								      pstSocketData->remote_sin.sin_addr.S_un.S_un_b.s_b2,
								      pstSocketData->remote_sin.sin_addr.S_un.S_un_b.s_b3,
								      pstSocketData->remote_sin.sin_addr.S_un.S_un_b.s_b4,
								      psStatusResponse->Info.ucNodeType,
								      EntityNodeType[psStatusResponse->Info.ucNodeType],
								      psStatusResponse->Info.ucMaxSockets,
								      psStatusResponse->Info.ucOpenSockets,
								      _byteswap_ulong ( psStatusResponse->Info.ulMaxSize ) );

								EntityCount++;

								if ( psStatusResponse->Info.ucMaxSockets == 0 )
								{
									Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
									      "Entity Status Response Error: Must have at least 1 socket available\n\n" );
								}
							}

							bRsp = TRUE;
							ReadAttempts = 0;
							VehicleEndTimestampMsecs = GetTickCount ( ) + VEHICLE_TIMEOUT;  // Get the end time for vehicle message check
						}  // end if ( memcmp )

						//  otherwise, not an expected response
						else
						{
							iBytesRead = 0;
							ReadAttempts++;
						}
					}  // end else (not loopback)
				}  // end if ( iBytesRead > 0 )

				//  otherwise, no data received
				else
				{
					ReadAttempts++;
				}
			}
			while ( (iBytesRead > 0 ||
			        ReadAttempts < MAX_READ_ATTEMPTS ) &&
			        GetTickCount ( ) < VehicleEndTimestampMsecs );
		}
		while ( !bRsp &&
		        WriteAttempts < MAX_WRITE_ATTEMPTS );
	}  // end for ( i < eDOIPHandshake )
}




/*******************************************************************************
**
**  Function:  RoutingActivation
**
**  Purpose:   Write the Routing Activation Request to the socket and read the reply
**
*******************************************************************************/
STATUS RoutingActivation ( void )
{
	int   iBytesRead;
	BYTE  aucReadMessage[PACKET_SIZE];
	BYTE  Responses = 0;
	BYTE  ReadAttempts = 0;
	BYTE  WriteAttempts = 0;

	WORD  ByteIdx;
	char  DataString[PACKET_SIZE*3];


	do
	{
		/////////////////////////////////////////
		// Send Routing Activation Request
		/////////////////////////////////////////
		Log ( BLANK, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
		      "\n" );
		Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
		      "Routing Activation Request\n" );
		if ( WriteDoIPMsg ( (BYTE*)&RoutingActivationRequest, sizeof ( RoutingActivationRequest ) ) == FAIL )
		{
			return FAIL;
		}
		WriteAttempts++;

		/////////////////////////////////////////
		// Log Tx Message
		/////////////////////////////////////////
		DataString[0] = (BYTE)NULL;
		for ( ByteIdx = 0;
		      ByteIdx < sizeof ( RoutingActivationRequest );
		      ByteIdx++ )
		{
			sprintf_s ( &DataString[strlen (DataString)], (PACKET_SIZE*3) - strlen ( DataString ),
			            "%02X ",
			            RoutingActivationRequest[ByteIdx] );
		}
		DataString[3*ByteIdx] = (BYTE)NULL;

		Log ( NETWORK, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
		      "TX MSG: %10luusec %s %s\n",
		      GetTickCount ( ),
		      gstProtocolList[gProtocolIdx].eProtocol == DOIP ? "DOIP" : "DOIP_NDIS",
		      DataString );


		/////////////////////////////////////////
		// Read Routing Activation Resoponse
		/////////////////////////////////////////
		do
		{
			iBytesRead = ReadPacket ( &stTCPSocketData, aucReadMessage, sizeof ( aucReadMessage ), DOIP_TIMEOUT );
			if ( iBytesRead < 0 )
			{
				char *pErrorString = GetEthernetErrorString ( stTCPSocketData.iError );
				Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
				      "Routing Activation Response Error: %d: %s\n",
				      stTCPSocketData.iError,
				      pErrorString );
				return FAIL;
			}
	
			else if ( iBytesRead > 0 )
			{
				/////////////////////////////////
				// Log Rx Message
				/////////////////////////////////
				DataString[0] = (BYTE)NULL;
				sprintf_s ( &DataString[strlen ( DataString )], (PACKET_SIZE * 3) - strlen ( DataString ),
				            "RX MSG: %10luusec %s ",
				            GetTickCount ( ),
				            gstProtocolList[gProtocolIdx].eProtocol == DOIP ? "DOIP" : "DOIP_NDIS" );
				for ( ByteIdx = 0;
				      ByteIdx < iBytesRead;
				      ByteIdx++ )
				{
					sprintf_s ( &DataString[strlen (DataString)], (PACKET_SIZE*3) - strlen ( DataString ),
					            "%02X ",
					            aucReadMessage[ByteIdx] );
				}
				sprintf_s ( &DataString[strlen ( DataString )], (PACKET_SIZE * 3) - strlen ( DataString ),
				            "  recvd from  %d.%d.%d.%d",
				            stTCPSocketData.remote_sin.sin_addr.S_un.S_un_b.s_b1,
				            stTCPSocketData.remote_sin.sin_addr.S_un.S_un_b.s_b2,
				            stTCPSocketData.remote_sin.sin_addr.S_un.S_un_b.s_b3,
				            stTCPSocketData.remote_sin.sin_addr.S_un.S_un_b.s_b4 );

				// if loopback, ignore it
				if ( memcmp ( aucReadMessage, RoutingActivationRequest, iBytesRead ) == 0 ||
				     stTCPSocketData.pAddr->ulIPv4Address == stTCPSocketData.remote_sin.sin_addr.S_un.S_addr )
				{
					Log ( NETWORK, SCREENOUTPUTOFF, LOGOUTPUTON, NO_PROMPT,
					      "%s\n",
					      DataString );
					Log ( NETWORK, SCREENOUTPUTOFF, LOGOUTPUTON, NO_PROMPT,
					      "Loopback of Request  rcvd\n" );
					ReadAttempts = 0;
				}
				else
				{
					Log ( NETWORK, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
					      "%s\n",
					      DataString );

					if ( memcmp ( (BYTE*)&RoutingActivationResponse, aucReadMessage, sizeof ( t_stDOIPHeader ) ) == 0 )
					{
						t_stActivationResponse* psActivationResponse = (t_stActivationResponse*)aucReadMessage;

						Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
						      "Routing Activation Response from %d.%d.%d.%d:\n"
						      "                            DoIP Address:   $%04X\n"
						      "                            Response Code:  $%02X\n\n",
						      stTCPSocketData.remote_sin.sin_addr.S_un.S_un_b.s_b1,
						      stTCPSocketData.remote_sin.sin_addr.S_un.S_un_b.s_b2,
						      stTCPSocketData.remote_sin.sin_addr.S_un.S_un_b.s_b3,
						      stTCPSocketData.remote_sin.sin_addr.S_un.S_un_b.s_b4,
						      _byteswap_ushort ( psActivationResponse->Info.usSourceAddress ),
						      psActivationResponse->Info.ucAtivationResponse );

						Responses++;
						ReadAttempts = 0;
					}
					else
					{
						Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
						      "Improper Response to Routing Activation\n" );
						return FAIL;
					}
				}
			}
			else
			{
				ReadAttempts++;
			}
		}
		while ( iBytesRead > 0 ||
		        (Responses < (EntityCount + 2) &&    // allow for responses from entities which didn't respond to entity status
		         ReadAttempts < MAX_READ_ATTEMPTS) );
	}
	while ( Responses < EntityCount &&
	        WriteAttempts < MAX_WRITE_ATTEMPTS );

	Log ( BLANK, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
	      "\n" );

	bRoutingActivated = TRUE;
	
	return PASS;
}




/*******************************************************************************
**
**  Function:  OpenChannel
**
**  Purpose:   
**
*******************************************************************************/
long OpenChannel ( void )
{
	unsigned int ConnectCount = 0;
	long         result = 0;

	do
	{
		///////////////////////////////////////////////
		// if first time opening the channel,
		// initialize the socket data
		///////////////////////////////////////////////
		if ( bSocketDataInitialized == FALSE )
		{
			///////////////////////////////////////////////
			// initialize the socket data fields
			///////////////////////////////////////////////
			InitSocketData ( &stDHCPSocketData, DHCP_SOCKET );
	
			InitSocketData ( &stUDPSocketData, UDP_SOCKET );
	
			InitSocketData ( &stTCPSocketData, TCP_SOCKET );
	
			// indicate that socket data has been initialized and
			//doesn't need to be done again
			bSocketDataInitialized = TRUE;
		}
	
		///////////////////////////////////////////////
		// if not the first time opening the channel,
		// check if the socket connection still exists,
		// continue, otherwise,
		// close sockets and start again,
		// but, in either case, the socket data fields stay the same
		///////////////////////////////////////////////
		else
		{
			// check that socket connection is still alive
			if ( isSocketConnected ( stTCPSocketData.hSock ) == 1 )
			{
				return 0;
			}
			CloseSockets ( );
			Sleep ( 2000 );
		}
	
	
		////////////////////////////////////////////////
		// start DHCP address search
		////////////////////////////////////////////////
		if ( bDHCP_Server_On == TRUE &&
		     DHCPServer ( ) != PASS )
		{
			return -1;
		}
	
	
		////////////////////////////////////////////////
		// Create a UDP socket
		////////////////////////////////////////////////
		{
		unsigned int BindCount = 0;
		do
		{
			if ( bDHCP_Server_On == FALSE )
			{
				Sleep ( BindDelay );
			}
			if ( OpenSocket ( &stUDPSocketData ) == PASS )
			{
				break;
			}
			else
			{
				CloseSocket ( &stUDPSocketData);
			}
		}
		while ( ++BindCount < BindCountMax );
		if ( stUDPSocketData.iError!= 0 )
		{
			return stUDPSocketData.iError;
		}
		}
	
	
		/////////////////////////////////////////////
		// create a TCP socket
		/////////////////////////////////////////////
		if ( OpenSocket ( &stTCPSocketData ) != 0 )
		{
			return stTCPSocketData.iError;
		}
	
	
		/////////////////////////////////////////////
		// attempt to connect to the vehicle
		/////////////////////////////////////////////
		if ( bNetworkStarted == FALSE )
	
		{
			FindVehicle ( &stUDPSocketData );
		}
	
		// if a vehicle did not successfully respond, exit
		if ( bNetworkStarted == FALSE)
		{
			Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
			      "Vehicle Connection Failed\n" );
	
			// failed to start the known client address socket
			return -1;
		}
	

		Sleep ( ConnectDelay );
	
		/////////////////////////////////////////////
		// if the vehicle is connected,
		// connect the TCP socket to the incoming IP address
		/////////////////////////////////////////////
		memset ( &stTCPSocketData.remote_sin, 0, sizeof ( stTCPSocketData.remote_sin ) );
		stTCPSocketData.remote_sin.sin_family       = AF_INET;
		stTCPSocketData.remote_sin.sin_addr.s_addr  = IPAddressInfo[0].ulIPv4Address;
		stTCPSocketData.remote_sin.sin_port         = htons(TESTERPORT);
	
		if ( connect ( stTCPSocketData.hSock,
		               (struct sockaddr*)&stTCPSocketData.remote_sin,
		               sizeof ( stTCPSocketData.remote_sin ) ) == SOCKET_ERROR )
		{
			stTCPSocketData.iError  = WSAGetLastError ( );
			char *pErrorString = GetEthernetErrorString ( stTCPSocketData.iError );
			Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
			      "TCP Socket connect Error: %d: %s\n",
			      stTCPSocketData.iError,
			      pErrorString );
			result = stTCPSocketData.iError;
		}
		else
		{
			Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
			      "TCP Socket connect successful:  %d.%d.%d.%d\n",
			      stTCPSocketData.remote_sin.sin_addr.S_un.S_un_b.s_b1,
			      stTCPSocketData.remote_sin.sin_addr.S_un.S_un_b.s_b2,
			      stTCPSocketData.remote_sin.sin_addr.S_un.S_un_b.s_b3,
			      stTCPSocketData.remote_sin.sin_addr.S_un.S_un_b.s_b4 );
			result = 0;
		}
	} while ( ++ConnectCount < ConnectCountMax );

	return result;
}




/*******************************************************************************
**
**  Function:  StartDoIPChannel
**
**  Purpose:   
**
*******************************************************************************/
STATUS StartDoIPChannel ( void )
{
	WSADATA       WSAData;
	int           iRetval;
	char         *pErrorString;


	if ( bWindowsSocketsStarted == FALSE )
	{
		/**********************************************************************
		*    start Windows sockets
		**********************************************************************/
		if ( WSAStartup ( MAKEWORD ( 2, 2 ), &WSAData ) )
		{
			iRetval = WSAGetLastError ( );
			pErrorString = GetEthernetErrorString ( iRetval );
			Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
			      "WSAStartup Error: %d: %s\n",
			      iRetval,
			      pErrorString );
			return iRetval;
		}
		bWindowsSocketsStarted = TRUE;
	}


	Log ( BLANK, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
	      "\n\n" );

	if ( OpenChannel ( ) != 0 )
	{
		Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
		      "Open Channel Failed\n" );

		CloseSockets ( );

		WSACleanup ( );
		return FAIL;
	}

	if ( bRoutingActivated == FALSE )
	{
		RoutingActivation ( );
	}

	return PASS;
}




/*******************************************************************************
**
**  Function:  ReadDoIPMsg
**
**  Purpose:   
**
*******************************************************************************/
STATUS ReadDoIPMsg ( PASSTHRU_MSG *pRxMsg, PASSTHRU_MSG *pTxMsg, WORD *pMsgCnt )
{
	WORD  ByteIdx;
	char  DataString[PACKET_SIZE*3];
	char *pErrorString;

	boolean  bRetry;
	BYTE     RetryCnt = 0;


	do
	{
		bRetry = FALSE;

		pRxMsg->DataSize = ReadPacket ( &stTCPSocketData,
		                                &pRxMsg->Data[0],
		                                sizeof ( pRxMsg->Data ),
		                                DOIP_TIMEOUT );
		if ( pRxMsg->DataSize < 0 )
		{
			pErrorString = GetEthernetErrorString ( stTCPSocketData.iError );
			Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
			      "Read Socket Error: %d: %s\n",
			      stTCPSocketData.iError,
			      pErrorString );
			*pMsgCnt = 0;

			// if connectoin reset by peer, reconnect
			if ( stTCPSocketData.iError == 10054 )
			{
				if ( RetryCnt != 0 )
				{
					CloseSocket ( &stTCPSocketData );
				}
				if ( CheckDoIPChannel ( ) == PASS )
				{
					bRetry = TRUE;
					RetryCnt++;
				}
			}
			else
			{
				return FAIL;
			}
		}

		else if ( pRxMsg->DataSize > 0 )
		{
			if ( memcmp ( &pRxMsg->Data[0], &pTxMsg->Data[0], (WORD)pTxMsg->DataSize ) == 0 ||
			     stTCPSocketData.pAddr->ulIPv4Address == stTCPSocketData.remote_sin.sin_addr.S_un.S_addr )
			{
				DataString[0] = (BYTE)NULL;
				for ( ByteIdx = 0;
				      ByteIdx < pRxMsg->DataSize;
				      ByteIdx++ )
				{
					sprintf_s ( &DataString[strlen (DataString)], (PACKET_SIZE*3) - strlen ( DataString ),
					            "%02X ",
					            pRxMsg->Data[ByteIdx] );
				}
				DataString[strlen (DataString)] = (BYTE)NULL;

				Log ( NETWORK, SCREENOUTPUTOFF, LOGOUTPUTON, NO_PROMPT,
				      "RX MSG: %10luusec %s %s  recvd from  %d.%d.%d.%d\n",
				      GetTickCount(),
				      gstProtocolList[gProtocolIdx].eProtocol == DOIP ? "DOIP" : "DOIP_NDIS",
				      DataString,
				      stTCPSocketData.remote_sin.sin_addr.S_un.S_un_b.s_b1,
				      stTCPSocketData.remote_sin.sin_addr.S_un.S_un_b.s_b2,
				      stTCPSocketData.remote_sin.sin_addr.S_un.S_un_b.s_b3,
				      stTCPSocketData.remote_sin.sin_addr.S_un.S_un_b.s_b4 );

				Log ( INFORMATION, SCREENOUTPUTOFF, LOGOUTPUTON, NO_PROMPT,
				      "Loopback of Request  rcvd\n" );

				pRxMsg->RxStatus = TX_MSG_TYPE;
			}
			else
			{
				memcpy ( &RcvdIPAddress, &stTCPSocketData.remote_sin.sin_addr.S_un.S_addr, 4 );
				sprintf_s (  RcvdIPAddressString, sizeof ( RcvdIPAddressString ),
				            "%d.%d.%d.%d",
				            stTCPSocketData.remote_sin.sin_addr.S_un.S_un_b.s_b1,
				            stTCPSocketData.remote_sin.sin_addr.S_un.S_un_b.s_b2,
				            stTCPSocketData.remote_sin.sin_addr.S_un.S_un_b.s_b3,
				            stTCPSocketData.remote_sin.sin_addr.S_un.S_un_b.s_b4 );

				pRxMsg->RxStatus = 0;
			}

			pRxMsg->eProtocolID    = gstProtocolList[gProtocolIdx].eProtocol;
			pRxMsg->TxFlags        = 0;
			pRxMsg->TimestampMsecs = GetTickCount ( );
			pRxMsg->ExtraDataIndex = MAX_MESSAGE_LOG_SIZE;
		}

		else
		{
			*pMsgCnt = 0;
		}
	}
	while ( bRetry == TRUE && 
	        RetryCnt < 3 );

	return PASS;
}




/*******************************************************************************
**
**  Function:  WriteDoIPMsg
**
**  Purpose:   
**
*******************************************************************************/
STATUS WriteDoIPMsg ( BYTE *pBuf, WORD BufLen )
{
	DWORD   ulTimeout = DOIP_TIMEOUT;
	TIMEVAL stTimeVal;
	FD_SET  stWriteFD;
	FD_SET  stExceptFD;
	int     iStatus;
	int     iBytesWritten;
	int     iTotalBytes = 0;

	WORD    ByteIdx;
	char    DataString[PACKET_SIZE*3];

	boolean bRetry;
	BYTE    ErrorCount = 0;
	char   *pErrorString;


	DataString[0] = (BYTE)NULL;
	for ( ByteIdx = 0;
	      ByteIdx < BufLen;
	      ByteIdx++ )
	{
		sprintf_s ( &DataString[strlen (DataString)], (PACKET_SIZE*3) - strlen ( DataString ),
		            "%02X ",
		            pBuf[ByteIdx] );
	}
	DataString[3*ByteIdx] = (BYTE)NULL;

	// It can take several passes to send the messaage if the connection is blocking.
	// If there is a timeout value it will apply to each portition of the message that is sent.
	do
	{
		bRetry = FALSE;
		
		// The connection is setup with a blocking socket.
		// We don't want to get stuck waiting forever to write so we use a select command
		// to see if the connection is clear to send or has an error.
		if ( ulTimeout != 0 )
		{
			// If there is a timeout we must pass the select call first so
			// that we don't get stuck in the send function call. 
			stTimeVal.tv_sec  = ulTimeout / 1000;
			stTimeVal.tv_usec = ulTimeout % 1000;

			FD_ZERO( &stWriteFD );
			FD_SET( stTCPSocketData.hSock, &stWriteFD );

			FD_ZERO( &stExceptFD );
			FD_SET( stTCPSocketData.hSock, &stExceptFD );

			iStatus = select ( 1, NULL, &stWriteFD, &stExceptFD, &stTimeVal );
			// If error
			if ( iStatus == SOCKET_ERROR )
			{
				stTCPSocketData.iError = WSAGetLastError ( );
				pErrorString = GetEthernetErrorString ( stTCPSocketData.iError );
				Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
				      "select Error: %d: %s\n",
				      stTCPSocketData.iError,
				      pErrorString );
				ErrorCount++;
			}
			// If Timeout
			else if ( iStatus == 0 )
			{
				// Any timeout on send closes the socket
				stTCPSocketData.iError = eSocketTimeout;
				Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
				      "select Error: %d: timeout\n",
				      stTCPSocketData.iError );
				ErrorCount++;
			}
		}

		// Setup the address for the the broadcast request
		iBytesWritten = send ( stTCPSocketData.hSock,
		                       (char*)&pBuf[iTotalBytes],
		                       BufLen - iTotalBytes, 0 );
		if ( iBytesWritten == SOCKET_ERROR )
		{
			stTCPSocketData.iError = WSAGetLastError ( );
			pErrorString = GetEthernetErrorString ( stTCPSocketData.iError );
			Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
			      "send Error: %d: %s\n",
			      stTCPSocketData.iError,
			      pErrorString );
			ErrorCount++;

			// if connectoin reset by peer, reconnect
			if ( stTCPSocketData.iError == 10054 )
			{
				if ( ErrorCount > 1 )
				{
					CloseSocket ( &stTCPSocketData );
				}
				if ( CheckDoIPChannel ( ) == PASS )
				{
					bRetry = TRUE;
				}
			}
		}
		else
		{
			ErrorCount = 0;
		}

		// Update the total number of byte sent
		if ( bRetry == FALSE )
		{
			iTotalBytes += iBytesWritten;
		}
	}
	// Continue to send pieces of the message until the entire byte array has been sent.
	while ( BufLen > iTotalBytes &&
	        ErrorCount < 2 );

	if ( ErrorCount != 0 )
	{
		return FAIL;
	}
	else
	{
		return PASS;
	}
}




/*******************************************************************************
**
**  Function:  CheckDoIPChannel
**
**  Purpose:   
**
*******************************************************************************/
STATUS CheckDoIPChannel ( void )
{
	if ( gstProtocolList[gProtocolIdx].eProtocol == DOIP ||
	      gstProtocolList[gProtocolIdx].eProtocol == DOIP_NDIS )
	{
		Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
		      "Checking DoIP Channel (in case vehicle socket closed)..." );
		if ( StartDoIPChannel ( ) != PASS )
		{
			Log ( FAILURE, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
			      "Start DoIP Channel unsuccessful.\n" );
			Log ( SUBSECTION_FAILED_RESULT, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT, "" );
			return FAIL;
		}
	}
	return PASS;
}
