/*******************************************************************************
********************************************************************************
**
**  Copyright(c) 2022, Alliance for Automotive Innovation
**  Used only under license from the Alliance for Automotive Innovation. All Rights Reserved.
**
**  Project:  J1699-5
**  FileName: RequestSID.c
**  Author:   EnGenius
**  Date:     2/25/2022
**  Email:    <support@autosinnovate.org>
**
**  Purpose:  SAE J1699-5 Vehicle OBD II Compliance Test Cases Source Code.
**            This source code is intended to run the tests described in
**            the SAE J1699-5 document in an automated manner, when compiled
**            and used with an SAE J2534-compatible pass-thru device.
**
**            File j1699.c contains information on building and running this test.
**
**  Description:
**
**  Modifications:  03/13/2023  Initial Version
**
********************************************************************************
*******************************************************************************/

#include <stdio.h>    // C Library input and output declarations
#include <stdlib.h>   // C Library general function declarations
#include <string.h>   // C Library character array declarations
#include <time.h>     // C Library time and date declarations
#include <windows.h>  // Windows API declarations
#include "j2534.h"    // j1699 project j2534 declarations
#include "j1699.h"    // j1699 project general declarations
#include "DoIP.h"     // j1699 project DoIP declarations

//  Funtion prototypes
STATUS SetupRequestMsg    ( REQ_MSG *,
                            PASSTHRU_MSG * );
STATUS ProcessISO15765Msg ( REQ_MSG *,
                            PASSTHRU_MSG *,
                            unsigned long *,
                            BYTE *,
                            BYTE *,
                            unsigned long *,
                            unsigned long * );
STATUS ProcessDoIPMsg ( REQ_MSG *,
                            PASSTHRU_MSG *,
                            unsigned long *,
                            BYTE *,
                            BYTE *,
                            unsigned long *,
                            unsigned long * );

extern STATUS ResetSIDResponseData ( PASSTHRU_MSG *,
                                     unsigned long );
extern STATUS SaveSIDResponseData  ( PASSTHRU_MSG *,
                                     REQ_MSG *,
                                     BYTE * );
extern STATUS LookupEcuIndex       ( PASSTHRU_MSG *,
                                     BYTE * );
extern void   SaveTransactionStart ( void );        // marks the start of a new transaction in the ring buffer

extern STATUS ReadDoIPMsg  ( PASSTHRU_MSG *, PASSTHRU_MSG *, WORD * );

//  Variable prototypes
//  Wait / Pending data
static unsigned long EcuWaitFlags          = 0;            // up to 32 ECUs
static unsigned long ResponsePendingDelayTimeMsecs  = 0;
static BOOL          bPadErrorPermanent    = FALSE;

extern unsigned long gMaxResponseTimeMsecs;
extern unsigned long gMinResponseTimeMsecs;     // min response time

extern ECU_DATA *gstResponse;
extern BYTE      RcvdIPAddress[4];


/*******************************************************************************
**
**  Function:  RequestSID
**
**  Purpose:   Request a service ID
**
**  Returns:   RETRY  - NRC=$21 durning initialization of ISO 15765
**             ERRORS - One or more correct early/late responses
**             FAIL   - No response, wrong response, or catestrophic error
**             PASS   - One or more correct responses, all on time
**
*******************************************************************************/
STATUS RequestSID ( REQ_MSG       *pstReqMsg,
                    unsigned long  Flags )
{
	PASSTHRU_MSG  RxMsg;
	PASSTHRU_MSG  TxMsg;
	unsigned long MsgCnt;
	STATUS        eRetVal;
	STATUS        eRetCode = PASS;    // saves the return code from function calls
	unsigned long StartTimestampMsecs;
	unsigned long ResponseTimeoutMsecs;
	unsigned long ResponseTimeExtensionMsecs;
	unsigned long TxTimestampMsecs;
	unsigned long SOMTimestampMsecs;
	BYTE          NumResponses;
	BYTE          NumFirstFrames;
	BOOL          bFirstResponse;
	char          String[MAX_LOG_STRING_SIZE];
	BYTE          EcuTimingIdx;


	// Initialize local variables
	ResponseTimeExtensionMsecs = 0;
	TxTimestampMsecs           = 0;
	SOMTimestampMsecs          = 0;

	// Reset wait variables
	EcuWaitFlags                  = 0;
	ResponsePendingDelayTimeMsecs = 0;

	// Initialize ECU variables
	for ( EcuTimingIdx = 0;
	      EcuTimingIdx < gNumOfECUsForAlloc;
	      EcuTimingIdx++ )
	{
		gstResponse[EcuTimingIdx].ResponseTimeExtensionMsecs    = 0;
		gstResponse[EcuTimingIdx].ResponsePendingDelayTimeMsecs = 0;
		gstResponse[EcuTimingIdx].bFFReceived                   = FALSE;
		gstResponse[EcuTimingIdx].bNAKReceived                  = FALSE;
	}

	// if gbSuspendLogOutput is true, then clear buffer
	if ( gbSuspendLogOutput == TRUE )
	{
		SaveTransactionStart ( );
	}

	// If not burst test, stop tester present message and delay
	// before each request to avoid exceeding minimum OBD request timing
	if ( (Flags & REQ_MSG_NO_PERIODIC_DISABLE) == 0)
	{
		// Stop the tester present message before each request
		if ( StopPeriodicMsg ( TRUE ) == FAIL )
		{
			return FAIL;
		}

		gstProtocolList[gProtocolIdx].TesterPresentID = -1;

		Sleep ( gRequestDelayTimeMsecs );
	}

	// Setup request message based on the protocol
	if ( Flags & REQ_MSG_PHYSICALLY_ADDRESSED )
	{
		pstReqMsg->bPhysicallyAddressed = TRUE;
	}
	else
	{
		pstReqMsg->bPhysicallyAddressed = FALSE;
	}

	if ( SetupRequestMsg ( pstReqMsg, &TxMsg ) != PASS )
	{
		return FAIL;
	}


	if ( gstProtocolList[gProtocolIdx].eProtocol == ISO15765 )
	{
		// Clear the transmit queue before sending request
		if ( (eRetVal = PassThruIoctl ( gstProtocolList[gProtocolIdx].ChannelID,
		                                CLEAR_TX_BUFFER, NULL, NULL )) != STATUS_NOERROR )
		{
			Log ( J2534_FAILURE, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
			      "%s returned %ld",
			      "PassThruIoctl(CLEAR_TX_BUFFER)",
			      eRetVal );
			return FAIL;
		}
	
		// Clear the receive queue before sending request
		if ( (eRetVal = PassThruIoctl ( gstProtocolList[gProtocolIdx].ChannelID,
		                                CLEAR_RX_BUFFER, NULL, NULL )) != STATUS_NOERROR )
		{
			Log ( J2534_FAILURE, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
			      "%s returned %ld",
			      "PassThruIoctl(CLEAR_RX_BUFFER)",
			      eRetVal );
			return FAIL;
		}
	
		// Send the request
		MsgCnt = 1;
		if ( (eRetVal  = PassThruWriteMsgs ( gstProtocolList[gProtocolIdx].ChannelID,
		                                     &TxMsg,
		                                     &MsgCnt,
		                                     500 )) != STATUS_NOERROR )
		{
			//  don't log timeouts during DetermineOBDProtocol
			if ( !(gbDetermineProtocol == TRUE &&
			       eRetVal == ERR_TIMEOUT) )
			{
				Log ( J2534_FAILURE, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
				      "%s returned %ld",
				      "PassThruWriteMsgs",
				      eRetVal );
				return FAIL;
			}
		}

		// Log the request message to compare to what is sent
		LogMsg ( &TxMsg, LOG_REQ_MSG );
	}

	else if ( gstProtocolList[gProtocolIdx].eProtocol == DOIP ||
	          gstProtocolList[gProtocolIdx].eProtocol == DOIP_NDIS )
	{
		// Clear the transmit queue before sending request
	
		// Clear the receive queue before sending request
	
		// Send the request
		if ( WriteDoIPMsg ( &TxMsg.Data[0], (WORD)TxMsg.DataSize ) == FAIL )
		{
			return FAIL;
		}
		TxMsg.TimestampMsecs = TxTimestampMsecs = GetTickCount ( );

		// Log the request message to compare to what is sent
		LogMsg ( &TxMsg, LOG_NORMAL_MSG );
	}

	// Reset the response data buffers
	if ( ResetSIDResponseData ( &TxMsg, Flags ) != PASS )
	{
		Log ( FAILURE, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
		      "Cannot reset SID $%02 response data\n",
		      TxMsg.Data[gstProtocolList[gProtocolIdx].HeaderSize] );
		return FAIL;
	}

	// Read the response(s) with a timeout of twice what is allowed
	// so we can see late responses.
	NumResponses    = 0;
	NumFirstFrames  = 0;
	bFirstResponse  = TRUE;
	StartTimestampMsecs = GetTickCount ( );

	do
	{
		// Process response based on protocol
		switch ( gstProtocolList[gProtocolIdx].eProtocol )
		{
			case ISO15765:
			{
				if ( bFirstResponse == TRUE )
				{
					bFirstResponse = FALSE;
					ResponseTimeoutMsecs =  5 * gMaxResponseTimeMsecs;
					sprintf_s ( String, MAX_LOG_STRING_SIZE,
					            "In RequestSID - Initial PassThruReadMsgs" );
				}
				else
				{
					ResponseTimeoutMsecs =  (5 * gMaxResponseTimeMsecs) + ResponseTimeExtensionMsecs;
					sprintf_s ( String, MAX_LOG_STRING_SIZE,
					            "In RequestSID - Loop PassThruReadMsgs" );
				}
		
				// Read the next response
				MsgCnt = 1;
				eRetVal = PassThruReadMsgs( gstProtocolList[gProtocolIdx].ChannelID,
				                            &RxMsg,
				                            &MsgCnt,
				                            ResponseTimeoutMsecs );
		
				if ( eRetVal != STATUS_NOERROR &&
				     eRetVal != ERR_BUFFER_EMPTY &&
				     eRetVal != ERR_NO_FLOW_CONTROL )
				{
					// Log undesirable returns
					Log ( J2534_FAILURE, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
					      "%s returned %ld",
					      String,
					      eRetVal );
					eRetCode |= FAIL;
				}
		
				// If a message was received, process it
				if ( MsgCnt == 1 )
				{
					// Save all read messages in the log file
					LogMsg ( &RxMsg, LOG_NORMAL_MSG );

					eRetCode |= ProcessISO15765Msg ( pstReqMsg,
					                                 &RxMsg,
					                                 &StartTimestampMsecs,
					                                 &NumResponses,
					                                 &NumFirstFrames,
					                                 &TxTimestampMsecs,
					                                 &ResponseTimeExtensionMsecs );
				}
			}
			break;  // end case ISO15765

			case DOIP:
			case DOIP_NDIS:
			{
				// Read the next response
				MsgCnt = 1;
				RxMsg.DataSize = MAX_MSG_LEN;
				eRetVal = ReadDoIPMsg ( &RxMsg, &TxMsg, (WORD *)&MsgCnt );

				if ( eRetVal != STATUS_NOERROR &&
				     eRetVal != ERR_BUFFER_EMPTY &&
				     eRetVal != ERR_NO_FLOW_CONTROL )
				{
					// Log undesirable returns
					Log ( FAILURE, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
					      "ReadDoIPMsg returned %ld\n",
					      eRetVal );
					eRetCode |= FAIL;
				}
		
				// If a message was received, process it
				if ( MsgCnt == 1 &&
				     (RxMsg.RxStatus & TX_MSG_TYPE) == 0 )
				{
					// Save all read messages in the log file
					LogMsg ( &RxMsg, LOG_NORMAL_MSG );

					eRetCode |= ProcessDoIPMsg ( pstReqMsg,
					                             &RxMsg,
					                             &StartTimestampMsecs,
					                             &NumResponses,
					                             &NumFirstFrames,
					                             &TxTimestampMsecs,
					                             &ResponseTimeExtensionMsecs );
				}
			}
			break;

			default:
			{
				Log ( FAILURE, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
				      "Invalid protocol specified for response.\n" );
				return FAIL;
			}
		}

		// If all expected ECUs responded and flag is set, don't wait for timeout
		// NOTE: This mechanism is only good for single message response per ECU
		if ( NumResponses >= gNumOfECUs &&
		     (Flags & REQ_MSG_RETURN_AFTER_ALL_RESPONSES) )
		{
			break;  // leave do/while loop
		}
	}
	while ( MsgCnt == 1 &&
	        ((gstProtocolList[gProtocolIdx].eProtocol == DOIP || gstProtocolList[gProtocolIdx].eProtocol == DOIP_NDIS) &&
	          (GetTickCount ( ) - StartTimestampMsecs) < 2000) ||  // 2sec
	        (gstProtocolList[gProtocolIdx].eProtocol == ISO15765 &&
	         (GetTickCount ( ) - StartTimestampMsecs) < ((5 * gMaxResponseTimeMsecs) + ResponseTimeExtensionMsecs)) );  //extend response time

	// Restart the periodic message if protocol determined and not in burst test
	if ( gbProtocolDetermined == TRUE &&
	     (Flags & REQ_MSG_NO_PERIODIC_DISABLE) == 0 )
	{
		if ( gstProtocolList[gProtocolIdx].TesterPresentID == -1 )
		{
			if ( StartPeriodicMsg ( ) != PASS )
			{
				Log ( FAILURE, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
				      "Problems starting periodic messages.\n" );
				eRetCode |= FAIL;
			}
		}
	}

	// Preserve the total number of ECUs responding and
	// allow calling routine to access information.
	if ( Flags & REQ_MSG_PHYSICALLY_ADDRESSED )
	{
		gNumOfECUsResp += NumResponses;
	}
	else
	{
		gNumOfECUsResp = NumResponses;
	}

	// Return code based on whether this protocol supports OBD
	if ( NumResponses > 0 )
	{
		if ( gbProtocolDetermined == FALSE &&
		     (eRetCode & RETRY) != RETRY )
		{
			Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
			      "%d OBD ECU(s) found\n\n",
			      gNumOfECUsResp);

			// IF this is the first ECU check save the number of responses
			if ( geTestPhase == eTestNoDTC && gTestSubsection == 2 ||
			     geTestPhase == eTestZEV   && gTestSubsection == 1 )
			{
				if ( gNumOfECUsResp > gNumOfECUs )
				{
					gNumOfECUs = gNumOfECUsResp;
				}
			}

			// Check if number of OBD-II ECUs entered by user matches the number detected
			if ( gNumOfECUsResp != gUserNumOfECUs )
			{
				Log ( FAILURE, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
				      "Number of OBD-II ECUs currently detected (%d) does not match the number previously entered (%d).\n\n",
				      gNumOfECUsResp,
				      gUserNumOfECUs );
				if ( gstProtocolList[gProtocolIdx].eProtocol == ISO15765 )
				{
				eRetCode |= FAIL;
			}
			}

		}

		// If there weren't any errors since sending the request, pass
		if ( (eRetCode & FAIL) != FAIL )
		{
			if ( (eRetCode & RETRY) == RETRY )
			{
				return RETRY;
			}
			else if ( (eRetCode & ERRORS) == ERRORS )
			{
				return ERRORS;
			}
			else
			{
				return PASS;
			}
		}
	}

	// If there was at least one failure since sending the request, return FAIL)
	if ( (eRetCode & FAIL) == FAIL )
	{
		return FAIL;
	}

	if ( Flags & REQ_MSG_ALLOW_NO_RESPONSE )
	{
		// calling function must determine any actual responses
		return PASS;
	}

	if ( gbProtocolDetermined == TRUE &&
	     (Flags & REQ_MSG_IGNORE_NO_RESPONSE) != REQ_MSG_IGNORE_NO_RESPONSE )
	{
		Log ( WARNING, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
		      "No response to OBD request\n" );
	}
	return FAIL;
}


/*******************************************************************************
**
**  Function:  ProcessISO15765Msg
**
**  Purpose:   Process response based on connection to
**             ISO15765.
**
**  Returns:   RETRY, ERRORS, FAIL, or PASS
**
*******************************************************************************/
STATUS ProcessISO15765Msg ( REQ_MSG       *pstReqMsg,
                            PASSTHRU_MSG  *pRxMsg,
                            unsigned long *pStartTimestampMsecs,
                            BYTE          *pNumResponses,
                            BYTE          *pNumFirstFrames,
                            unsigned long *pTxTimestampMsecs,
                            unsigned long *pExtendResponseTimeMsecs )
{
	unsigned long   ResponseTimeMsecs;
	unsigned long   EcuTimingIdx;
	BYTE            EcuIdx;
	unsigned long   EcuId = 0;
	unsigned short  IDIndex;
	STATUS          eRetCode = PASS;    // saves the return code from function calls
	BOOL            bRequestMsgLpbk = FALSE;


	// create the ECU ID for later use
	for ( IDIndex = 0;
	      IDIndex < 4;
	      IDIndex++ )
	{
		EcuId = (EcuId << 8) + (pRxMsg->Data[IDIndex]);
	}

	// if this is an echo of the request, don't track it's timing data
	if ( pRxMsg->RxStatus & TX_MSG_TYPE )
	{
		bRequestMsgLpbk = TRUE;
	}
	else
	{
		// Find this ECU's Timing Structure
		for ( EcuTimingIdx = 0;
		      EcuTimingIdx < gNumOfECUsForAlloc;
		      EcuTimingIdx++ )
		{
			if ( gstResponse[EcuTimingIdx].RespId == EcuId ||
			     gstResponse[EcuTimingIdx].RespId == 0x00 )
			{
				if ( gstResponse[EcuTimingIdx].RespId == 0x00 )
				{
					gstResponse[EcuTimingIdx].RespId = EcuId;

					// calculate this ECU's ID for requests
					if ( gstProtocolList[gProtocolIdx].eProtocolTag == ISO15765_29_BIT_I )
					{
						// swap MSB and LSB of LSW
						gstResponse[EcuTimingIdx].ReqId =  (gstResponse[EcuTimingIdx].RespId & 0xFFFF0000) +
						                                  ((gstResponse[EcuTimingIdx].RespId & 0x0000FF00) >> 8) +
						                                  ((gstResponse[EcuTimingIdx].RespId & 0x000000FF) << 8);
					}
					else
					{
						// request ID is 8 less than response ID
						gstResponse[EcuTimingIdx].ReqId = gstResponse[EcuTimingIdx].RespId - 0x08;
					}
				}
				break;  // leave ECU Timing for loop
			}
		}
	}

	// FAIL for padding error only once
	if ( bPadErrorPermanent == FALSE )
	{
		if ( pRxMsg->RxStatus & ISO15765_PADDING_ERROR )
		{
			bPadErrorPermanent = TRUE;

			Log ( ERROR_FAILURE, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
			      "ECU %X  ISO15765 message padding error.\n",
			      EcuId );
			eRetCode = ERRORS;
		}
	}

	ResponseTimeMsecs = (pRxMsg->TimestampMsecs - *pTxTimestampMsecs) / 1000;

	// Check for FirstFrame indication
	if ( pRxMsg->RxStatus & ISO15765_FIRST_FRAME )
	{
		// Check if response was late (all first frame indications due in P2_MAX)
		if ( (pRxMsg->TimestampMsecs - *pTxTimestampMsecs) > ((gMaxResponseTimeMsecs + gstResponse[EcuTimingIdx].ResponseTimeExtensionMsecs) * 1000) )
		{
			// Exceeded maximum response time
			Log ( ERROR_FAILURE, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
			      "ECU %X  OBD First Frame Indication was later than allowed (> %dmsec)\n",
			      EcuId,
			      ((gMaxResponseTimeMsecs + gstResponse[EcuTimingIdx].ResponseTimeExtensionMsecs) * 1000) );
			eRetCode = ERRORS;

			if ( !bRequestMsgLpbk )
			{
				gstResponse[EcuTimingIdx].RespTimeOutofRange++;
				gstResponse[EcuTimingIdx].RespTimeTooLate++;

				// Tally up the response statistics
				gstResponse[EcuTimingIdx].AggregateResponseTimeMsecs += ResponseTimeMsecs;
				gstResponse[EcuTimingIdx].AggregateResponses += ( ( ( pRxMsg->DataSize - 4 ) / ISO15765_MAX_BYTES_PER_FRAME ) + 1 );
				if ( ResponseTimeMsecs > gstResponse[EcuTimingIdx].LongestResponsesTimeMsecs )
				{
					gstResponse[EcuTimingIdx].LongestResponsesTimeMsecs = ResponseTimeMsecs;
				}
			}
			Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
			      "Calculated OBD Response time: %2.4f msec\n",
			      ( (float)(pRxMsg->TimestampMsecs - *pTxTimestampMsecs) / 1000) );
		}

		// Extend the response time to the worst case for segmented responses
		gstResponse[EcuTimingIdx].ResponseTimeExtensionMsecs = 30000;
		if ( gstResponse[EcuTimingIdx].ResponseTimeExtensionMsecs > *pExtendResponseTimeMsecs )
		{
			*pExtendResponseTimeMsecs = gstResponse[EcuTimingIdx].ResponseTimeExtensionMsecs;
		}
		(*pNumFirstFrames)++;
		gstResponse[EcuTimingIdx].bFFReceived = TRUE;
		Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
		      "Receiving segmented responses, please wait...\n" );
	}

	// Check for echoed request message
	else if ( pRxMsg->RxStatus & TX_MSG_TYPE )
	{
		if ( pRxMsg->DataSize >= 5 &&
		     pRxMsg->Data[4] == pstReqMsg->SID )
		{
			// Save the timestamp
			*pTxTimestampMsecs = pRxMsg->TimestampMsecs;
		}
	}

	// Check for NAK response message
	else if ( pRxMsg->DataSize >= 7    &&
	          pRxMsg->Data[4]  == NAK  &&
	          pRxMsg->Data[5]  == pstReqMsg->SID )
	{
		if ( gstResponse[EcuTimingIdx].bNAKReceived == FALSE )
		{
			// Tally up the response statistics
			gstResponse[EcuTimingIdx].AggregateResponseTimeMsecs += ResponseTimeMsecs;
			gstResponse[EcuTimingIdx].AggregateResponses += ( ( ( pRxMsg->DataSize - 4 ) / ISO15765_MAX_BYTES_PER_FRAME ) + 1 );
			if ( ResponseTimeMsecs > gstResponse[EcuTimingIdx].LongestResponsesTimeMsecs )
			{
				gstResponse[EcuTimingIdx].LongestResponsesTimeMsecs = ResponseTimeMsecs;
			}

			gstResponse[EcuTimingIdx].bNAKReceived = TRUE;
			gstResponse[EcuTimingIdx].NAKCode = pRxMsg->Data[6];
		}

		// Check if response was late (all negative respones due in P2_MAX)
		if ( (pRxMsg->TimestampMsecs - *pTxTimestampMsecs) > ((gMaxResponseTimeMsecs +  gstResponse[EcuTimingIdx].ResponsePendingDelayTimeMsecs) * 1000) )
		{
			// Exceeded maximum response time
			Log ( ERROR_FAILURE, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
			      "ECU %X  OBD Negative Response was later than allowed (> %dmsec)\n",
			      EcuId,
			      gMaxResponseTimeMsecs + gstResponse[EcuTimingIdx].ResponsePendingDelayTimeMsecs );
			eRetCode = ERRORS;

			gstResponse[EcuTimingIdx].RespTimeOutofRange++;
			gstResponse[EcuTimingIdx].RespTimeTooLate++;
			Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
			      "Calculated OBD Response time: %2.4f msec\n",
			      ((float)(pRxMsg->TimestampMsecs - *pTxTimestampMsecs) / 1000) );
		}

		// Save the response information
		if ( SaveSIDResponseData ( pRxMsg,
		                           pstReqMsg,
		                           pNumResponses ) != PASS )
		{
			Log ( FAILURE, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
			      "Cannot save SID response data\n" );
			return FAIL;
		}

		// check the kind of response received for the vehicle
		switch ( pRxMsg->Data[6] )
		{
			case NAK_GENERAL_REJECT: // 0x10
			{
				Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
				      "ECU %X  General reject to SID $%02X.\n",
				      EcuId,
				      pRxMsg->Data[5] );
			}
			break;

			case NAK_SERVICE_NOT_SUPPORTED: // 0x11
			{
				Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
				      "ECU %X  SID $%02X not supported.\n",
				      EcuId,
				      pRxMsg->Data[5] );
			}
			break;

			case NAK_SUBFUNCTION_NOT_SUPPORTED: // 0x12
			{
				Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
				      "ECU %X  SID $%02X supported. Unsupported subfunction request.\n",
				      EcuId,
				      pRxMsg->Data[5] );
			}
			break;

			case NAK_INVALID_FORMAT: // 0x13
			{
				Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
				      "ECU %X  SID $%02X supported. Invalid format in request.\n",
				      EcuId,
				      pRxMsg->Data[5] );
			}
			break;

			case NAK_REPEAT_REQUEST: // 0x21
			{
				Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
				      "ECU %X  SID $%02X supported. Busy.\n",
				      EcuId,
				      pRxMsg->Data[5] );
			}
			break;

			case NAK_CONDITIONS_NOT_CORRECT: // 0x22
			{
				Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
				      "ECU %X  SID $%02X supported. Conditions not correct.\n",
				      EcuId,
				      pRxMsg->Data[5] );
			}
			break;

			case NAK_ID_NOT_SUPPORTED: // 0x31
			{
				Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
				      "ECU %X  SID $%02X supported. Unsupported DID request\n",
				      EcuId,
				      pRxMsg->Data[5] );
			}
			break;

			case NAK_GENERAL_PROGRAMMING_FAILURE: // 0x72
			{
				Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
				      "ECU %X  SID $%02X supported. General programming failure.\n",
				      EcuId,
				      pRxMsg->Data[5] );
			}
			break;

			case NAK_RESPONSE_PENDING: // 0x78
			{
				Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
				      "ECU %X  SID $%02X supported. Response pending.\n",
				      EcuId,
				      pRxMsg->Data[5] );
			}
			break;

			default:
			{
				Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
				      "ECU %X  Unknown negative response to SID $%02X.\n",
				      EcuId,
				      pRxMsg->Data[5] );
			}
			break;
		}

		// NAK Truth table
		switch( pstReqMsg->SID )
		{
			case 0x22:
			{
				if ( pRxMsg->Data[6] == NAK_ID_NOT_SUPPORTED )
				{
					break;  // handle in calling function
				}
				else if ( gbDetermineProtocol == TRUE &&
				          pRxMsg->Data[6] == NAK_REPEAT_REQUEST &&
				          (pstReqMsg->u.DID[0] == 0xF400 ||
				           pstReqMsg->u.DID[0] == 0xF810) )
				{
					return RETRY;
				}
				else if ( pRxMsg->Data[6] == NAK_RESPONSE_PENDING &&
				          pstReqMsg->u.DID[0] >= 0xF800 &&
				          pstReqMsg->u.DID[0] >= 0xF8FF )
				{
					// If response pending, extend the wait time
					gstResponse[EcuTimingIdx].ResponseTimeExtensionMsecs = 30000;
					if ( gstResponse[EcuTimingIdx].ResponseTimeExtensionMsecs > *pExtendResponseTimeMsecs )
					{
						*pExtendResponseTimeMsecs = gstResponse[EcuTimingIdx].ResponseTimeExtensionMsecs;
					}
					gstResponse[EcuTimingIdx].ResponsePendingDelayTimeMsecs = 5000;
					if ( gstResponse[EcuTimingIdx].ResponsePendingDelayTimeMsecs > ResponsePendingDelayTimeMsecs )
					{
						ResponsePendingDelayTimeMsecs = gstResponse[EcuTimingIdx].ResponsePendingDelayTimeMsecs;
					}

					// set wait flag
					if ( LookupEcuIndex ( pRxMsg, &EcuIdx ) == PASS )
					{
						EcuWaitFlags |= (1<<EcuIdx);
						gstResponse[EcuIdx].bResponseReceived = FALSE;  // allow ECU to respond again
					}
					else
					{
						return FAIL;
					}
				}
				// IF EOBD/IOBD/OBDBr and CVN
				else if ( pstReqMsg->u.DID[0] == 0xF806 &&
				          (gstUserInput.eComplianceType != US_OBDII &&
				           gstUserInput.eComplianceType != HD_OBD) )
				{
					break;
				}
				else
				{
					Log ( FAILURE, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
					      "ECU %X  SID $22 NAK response error!\n",
					      EcuId );
					return FAIL;
				}
			}
			break;

			case 0x19:
			{
				if ( pRxMsg->Data[6] == NAK_SERVICE_NOT_SUPPORTED ||
				     pRxMsg->Data[6] == NAK_SUBFUNCTION_NOT_SUPPORTED ||
				     pRxMsg->Data[6] == NAK_CONDITIONS_NOT_CORRECT ||
				     pRxMsg->Data[6] == NAK_ID_NOT_SUPPORTED )
				{
					eRetCode = ERRORS;
				}
				else if ( pRxMsg->Data[6] == NAK_RESPONSE_PENDING )
				{
					// If response pending, extend the wait time
					gstResponse[EcuTimingIdx].ResponseTimeExtensionMsecs = 30000;
					if ( gstResponse[EcuTimingIdx].ResponseTimeExtensionMsecs > *pExtendResponseTimeMsecs )
					{
						*pExtendResponseTimeMsecs = gstResponse[EcuTimingIdx].ResponseTimeExtensionMsecs;
					}
					gstResponse[EcuTimingIdx].ResponsePendingDelayTimeMsecs = 5000;
					if ( gstResponse[EcuTimingIdx].ResponsePendingDelayTimeMsecs > ResponsePendingDelayTimeMsecs )
					{
						ResponsePendingDelayTimeMsecs = gstResponse[EcuTimingIdx].ResponsePendingDelayTimeMsecs;
					}

					// set wait flag
					if ( LookupEcuIndex ( pRxMsg, &EcuIdx ) == PASS )
					{
						EcuWaitFlags |= (1<<EcuIdx);
						gstResponse[EcuIdx].bResponseReceived = FALSE;  // allow ECU to respond again
					}
					else
					{
						return FAIL;
					}
				}
				else
				{
					Log ( FAILURE, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
					      "ECU %X  SID $19 NAK response error!\n",
					      EcuId );
					return FAIL;
				}
			}
			break;

			case 0x14:
			{
				if ( pRxMsg->Data[6] == NAK_SERVICE_NOT_SUPPORTED ||
				     pRxMsg->Data[6] == NAK_SUBFUNCTION_NOT_SUPPORTED ||
				     pRxMsg->Data[6] == NAK_CONDITIONS_NOT_CORRECT ||
				     (pRxMsg->Data[6] == NAK_ID_NOT_SUPPORTED && gbPhysicalAddressing == FALSE) ||
				     pRxMsg->Data[6] == NAK_GENERAL_PROGRAMMING_FAILURE )
				{
					return ERRORS;
				}
				else if ( pRxMsg->Data[6] == NAK_RESPONSE_PENDING )
				{
					// If response pending, extend the wait time
					gstResponse[EcuTimingIdx].ResponseTimeExtensionMsecs = 30000;
					if ( gstResponse[EcuTimingIdx].ResponseTimeExtensionMsecs > *pExtendResponseTimeMsecs )
					{
						*pExtendResponseTimeMsecs = gstResponse[EcuTimingIdx].ResponseTimeExtensionMsecs;
					}
					gstResponse[EcuTimingIdx].ResponsePendingDelayTimeMsecs = 5000;
					if ( gstResponse[EcuTimingIdx].ResponsePendingDelayTimeMsecs > ResponsePendingDelayTimeMsecs )
					{
						ResponsePendingDelayTimeMsecs = gstResponse[EcuTimingIdx].ResponsePendingDelayTimeMsecs;
					}

					// set wait flag
					if ( LookupEcuIndex ( pRxMsg, &EcuIdx ) == PASS )
					{
						EcuWaitFlags |= (1<<EcuIdx);
						gstResponse[EcuIdx].bResponseReceived = FALSE;  // allow ECU to respond again
					}
					else
					{
						return FAIL;
					}
				}
				else if ( pRxMsg->Data[6] != NAK_ID_NOT_SUPPORTED || gbPhysicalAddressing != TRUE )
				{
					Log ( FAILURE, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
					      "ECU %X  SID $14 NAK response error!\n",
					      EcuId );
					return FAIL;
				}
			}
			break;

			case 0x31:
			{
				if ( pRxMsg->Data[6] == NAK_SERVICE_NOT_SUPPORTED )
				{
					break;
				}
				else if ( pRxMsg->Data[6] == NAK_CONDITIONS_NOT_CORRECT )
				{
					eRetCode = ERRORS;
				}
				else
				{
					Log ( FAILURE, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
					      "ECU %X  SID $31 NAK response error!\n",
					      EcuId,
					      pstReqMsg->SID );
				}
			}
			break;

			default :
			{
				Log ( FAILURE, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
				      "ECU %X  SID $%02X NAK response error!\n",
				      EcuId,
				      pstReqMsg->SID );
				return FAIL;
			}
			break;
		}
	}

	// Check for response message
	else if ( pRxMsg->DataSize >= 5 )
	{
		if ( gstResponse[EcuTimingIdx].bNAKReceived == FALSE &&
		     gstResponse[EcuTimingIdx].bFFReceived == FALSE )
		{
			// Tally up the response statistics
			gstResponse[EcuTimingIdx].AggregateResponseTimeMsecs += ResponseTimeMsecs;
			gstResponse[EcuTimingIdx].AggregateResponses += ( ( ( pRxMsg->DataSize - 4 ) / ISO15765_MAX_BYTES_PER_FRAME ) + 1 );
			if ( ResponseTimeMsecs > gstResponse[EcuTimingIdx].LongestResponsesTimeMsecs )
			{
				gstResponse[EcuTimingIdx].LongestResponsesTimeMsecs = ResponseTimeMsecs;
			}
		}
		else
		{
			gstResponse[EcuTimingIdx].bNAKReceived = FALSE;
		}

		// Check if response was late (compensate for segmented responses)
		if ( (pRxMsg->TimestampMsecs - *pTxTimestampMsecs) >
		     ((gMaxResponseTimeMsecs + gstResponse[EcuTimingIdx].ResponseTimeExtensionMsecs) * 1000) )
		{
			// Exceeded maximum response time
			Log ( ERROR_FAILURE, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
			      "ECU %X  OBD Response was later than allowed (> %dmsec)\n",
			      EcuId,
			      (gMaxResponseTimeMsecs + gstResponse[EcuTimingIdx].ResponseTimeExtensionMsecs) );
			eRetCode = ERRORS;

			gstResponse[EcuTimingIdx].RespTimeOutofRange++;
			gstResponse[EcuTimingIdx].RespTimeTooLate++;
			Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
			      "Calculated OBD Response time: %2.4f msec\n",
			      ((float)(pRxMsg->TimestampMsecs - *pTxTimestampMsecs) / 1000) );
		}

		// clear wait flag
		if ( LookupEcuIndex ( pRxMsg, &EcuIdx ) == PASS )
		{
			EcuWaitFlags &= ~(1<<EcuIdx);
			gstResponse[EcuTimingIdx].ResponsePendingDelayTimeMsecs = 0;        // ECU response no longer pending
		}
		else
		{
			return FAIL;
		}

		// Check if all ECUs with response pending have responded
		if ( EcuWaitFlags == 0 )
		{
			ResponsePendingDelayTimeMsecs = 0;         // no ECU response is pending

			if ( *pNumFirstFrames == 0 )
			{
				*pExtendResponseTimeMsecs = 0;  // no First Frames, clear extended response time
			}
		}

		// Check if we can turn off the time extension for segmented frames
		if ( pRxMsg->DataSize > ( 4 + ISO15765_MAX_BYTES_PER_FRAME ) )
		{
			if ( *pNumFirstFrames > 0 )
			{
				// If we have received as many segmented frames as FirstFrame indications,
				// restore 'response pending' time extension
				if ( --(*pNumFirstFrames) == 0 )
				{
					*pExtendResponseTimeMsecs = ResponsePendingDelayTimeMsecs;
				}
			}
		}

		// Check for proper SID response
		if ( pRxMsg->Data[4] != ( pstReqMsg->SID + OBD_RESPONSE_BIT ) )
		{
			Log ( FAILURE, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
			      "ECU %X  Invalid SID response to request\n",
			      EcuId );
			return FAIL;
		}

		// If Service $19 was requested
		if ( pstReqMsg->SID == 0x19 && 
		     (pstReqMsg->u.ID[0] == 0x03 ||  // ReportSnapshotIdentification
		      pstReqMsg->u.ID[0] == 0x04 ||  // ReportDTCSnapshotRecordByDTCNumber
		      pstReqMsg->u.ID[0] == 0x06 ||  // ReportDTCExtDataRecordByDTCNumber
		      pstReqMsg->u.ID[0] == 0x1A ||  // ReportDTCExtDataIdentification
 		      pstReqMsg->u.ID[0] == 0x42 ||  // ReportWWHOBDDTCByMaskRecord (Pending (0x04) or Confirmed (0x08))
		      pstReqMsg->u.ID[0] == 0x55 ||  // ReportWWHOBDDTCWithPermanentStatus
		      pstReqMsg->u.ID[0] == 0x56) )  // ReportDTCInformationByDTCReadinessGroupIdentifier
		{

		}
		// If a Service other than $19 was requested
		else if ( pstReqMsg->NumIds != 0 )
		{
			// Check for proper PID/MID/TID/INF response
			for ( IDIndex = 0;
			      IDIndex < (pstReqMsg->SID == 0x22 ? pstReqMsg->NumIds*2 : pstReqMsg->NumIds);
			      IDIndex++ )
			{
				if ( pRxMsg->Data[6] == pstReqMsg->u.ID[IDIndex] )
				{
					break;  // leave ID for loop
				}
				// if this is a SID $14 request, no DID in response
				if ( pstReqMsg->SID == 0x14 )
				{
					IDIndex++;
				}
			}

			if ( (pstReqMsg->SID == 0x22 && IDIndex == pstReqMsg->NumIds*2) ||
			     (pstReqMsg->SID != 0x22 && IDIndex == pstReqMsg->NumIds) )
			{
				Log ( FAILURE, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
				      "ECU %X  Invalid PID response to request\n",
				      EcuId );
				return FAIL;
			}
		}

		// If this is any response to a Functionally Addressed request or
		// a response from the correct ECU to a Physically Addressed request,
		// save the data
		if ( pstReqMsg->bPhysicallyAddressed == FALSE ||
		     (pstReqMsg->bPhysicallyAddressed == TRUE &&
		      EcuId == pstReqMsg->RespEcuID ) )
		{
			// Save the response information
			if ( SaveSIDResponseData ( pRxMsg,
			                           pstReqMsg,
			                           pNumResponses ) != PASS )
			{
				Log ( FAILURE, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
				      "Cannot save SID response data\n" );
				eRetCode = FAIL;
			}
		}

		// Otherwise this response to a Physically Address request
		// is from the wrong ECU
		else
		{
			Log ( FAILURE, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
			      "ECU %X  Responded to a Physically Addressed Request (expected a response from ECU %X)\n" ,
			      EcuId,
			      pstReqMsg->RespEcuID );
			eRetCode = FAIL;
		}
	}

	// Ignore, invalid message

	return eRetCode;
}


/*******************************************************************************
**
**  Function:  ProcessDoIPMsg
**
**  Purpose:   Process response based on connection to
**             DoIP.
**
**  Returns:   RETRY, ERRORS, FAIL, or PASS
**
*******************************************************************************/
STATUS ProcessDoIPMsg ( REQ_MSG       *pstReqMsg,
                        PASSTHRU_MSG  *pRxMsg,
                        unsigned long *pStartTimestampMsecs,
                        BYTE          *pNumResponses,
                        BYTE          *pNumFirstFrames,
                        unsigned long *pTxTimestampMsecs,
                        unsigned long *pExtendResponseTimeMsecs )
{
	unsigned long   ResponseTimeMsecs;
	unsigned long   EcuTimingIdx;
	BYTE            EcuIdx;
	unsigned long   EcuId = 0;
	unsigned short  IDIndex;
	STATUS          eRetCode = PASS;    // saves the return code from function calls
	BOOL            bRequestMsgLpbk = FALSE;


	// create the reponding ECU ID for later use
	EcuId = (pRxMsg->Data[8] << 8) + (pRxMsg->Data[9]);

	if ( EcuId != FUNCTIONALADDRESS )
	{
	// Find this ECU's Timing Structure
	for ( EcuTimingIdx = 0;
	      EcuTimingIdx < gNumOfECUsForAlloc;
	      EcuTimingIdx++ )
	{
		if ( gstResponse[EcuTimingIdx].RespId == EcuId ||
		     gstResponse[EcuTimingIdx].RespId == 0x00 )
		{
			if ( gstResponse[EcuTimingIdx].RespId == 0x00 )
			{
				// save the reponding ECU ID for later use
				gstResponse[EcuTimingIdx].RespId = EcuId;

				// for DoIP RespID and ReqId are the same
				gstResponse[EcuTimingIdx].ReqId = EcuId;

				// save the IP address of the responding ECU
				memcpy ( &gstResponse[EcuTimingIdx].IPv4Address, &RcvdIPAddress, 4 );
			}
			break;  // leave ECU Timing for loop
		}
	}
	}

	ResponseTimeMsecs = (pRxMsg->TimestampMsecs - *pTxTimestampMsecs) / 1000;


	// Check for valid message size
	if ( pRxMsg->DataSize > 12 )
	{
		// Check for NAK response message
		if ( pRxMsg->Data[2] == 0x80  &&
		     pRxMsg->Data[3] == 0x03 )
		{
			if ( gstResponse[EcuTimingIdx].bNAKReceived == FALSE )
			{
				// Tally up the response statistics
				gstResponse[EcuTimingIdx].AggregateResponseTimeMsecs += ResponseTimeMsecs;
				gstResponse[EcuTimingIdx].AggregateResponses++;
				if ( ResponseTimeMsecs > gstResponse[EcuTimingIdx].LongestResponsesTimeMsecs )
				{
					gstResponse[EcuTimingIdx].LongestResponsesTimeMsecs = ResponseTimeMsecs;
				}
	
				gstResponse[EcuTimingIdx].bNAKReceived = TRUE;
			}
	
			// Check if response was late (all negative respones due in P2_MAX)
			if ( (pRxMsg->TimestampMsecs - *pTxTimestampMsecs) > ((gMaxResponseTimeMsecs +  gstResponse[EcuTimingIdx].ResponsePendingDelayTimeMsecs) * 1000) )
			{
				// Exceeded maximum response time
				Log ( ERROR_FAILURE, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
				      "ECU %X  OBD Negative Response was later than allowed (> %dmsec)\n",
				      EcuId,
				      gMaxResponseTimeMsecs + gstResponse[EcuTimingIdx].ResponsePendingDelayTimeMsecs );
				eRetCode = ERRORS;
	
				gstResponse[EcuTimingIdx].RespTimeOutofRange++;
				gstResponse[EcuTimingIdx].RespTimeTooLate++;
				Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
				      "Calculated OBD Response time: %2.4f msec\n",
				      ((float)(pRxMsg->TimestampMsecs - *pTxTimestampMsecs) / 1000) );
			}
	
			// check the kind of response received for the vehicle
			switch ( pRxMsg->Data[12] )
			{
				case INVALIDSOURCEADDRESS: // 0x02
				{
					Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
					      "ECU %X  NAK Invalid source address.\n",
					      EcuId );
				}
				break;
	
				case UNKNOWNTARGETADDRESS: // 0x03
				{
					Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
					      "ECU %X  NAK Unknown target address.\n",
					      EcuId );
				}
				break;
	
				case MESSAGETOOLONG: // 0x04
				{
					Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
					      "ECU %X  NAK Diagnostic message too large.\n",
					      EcuId );
				}
				break;
	
				case OUTOFMEMORY: // 0x05
				{
					Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
					      "ECU %X  NAK Out of memory.\n",
					      EcuId );
				}
				break;
	
				case TARGETUNREACHABLE: // 0x06
				{
					Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
					      "ECU %X  NAK Target unreachable.\n",
					      EcuId );
				}
				break;
	
				case UNKNOWNNETWORK: // 0x07
				{
					Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
					      "ECU %X  NAK Unknown network.\n",
					      EcuId );
				}
				break;
	
				case TRANSPORTPROTOCOLERROR: // 0x08
				{
					Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
					      "ECU %X  NAK Transport protocol error.\n",
					      EcuId );
				}
				break;
	
				default:
				{
					Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
					      "ECU %X  NAK Unknown negative response.\n",
					      EcuId );
				}
				break;
			}
	
			// NAK Truth table
			switch( pstReqMsg->SID )
			{
				case 0x22:
				{
					if ( gbDetermineProtocol == TRUE &&
						pstReqMsg->u.DID[0] == 0xF400 &&
						pRxMsg->Data[6] == NAK_REPEAT_REQUEST )
					{
						return RETRY;
					}
					// IF EOBD/IOBD/OBDBr and CVN
					if ( pstReqMsg->u.DID[0] == 0xF806 &&
					     ( gstUserInput.eComplianceType != US_OBDII &&
					       gstUserInput.eComplianceType != HD_OBD ) )
					{
						break;
					}
					else
					{
						Log ( FAILURE, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
						      "ECU %X  SID $22 NAK response error!\n",
						      EcuId );
						return FAIL;
					}
				}
				break;
	
				case 0x14:
				{
					if ( pRxMsg->Data[6] == NAK_RESPONSE_PENDING )
					{
						// If response pending, extend the wait time
						gstResponse[EcuTimingIdx].ResponseTimeExtensionMsecs = 30000;
						if ( gstResponse[EcuTimingIdx].ResponseTimeExtensionMsecs > *pExtendResponseTimeMsecs )
						{
							*pExtendResponseTimeMsecs = gstResponse[EcuTimingIdx].ResponseTimeExtensionMsecs;
						}
						gstResponse[EcuTimingIdx].ResponsePendingDelayTimeMsecs = 5000;
						if ( gstResponse[EcuTimingIdx].ResponsePendingDelayTimeMsecs > ResponsePendingDelayTimeMsecs )
						{
							ResponsePendingDelayTimeMsecs = gstResponse[EcuTimingIdx].ResponsePendingDelayTimeMsecs;
						}
	
						// set wait flag
						if ( LookupEcuIndex ( pRxMsg, &EcuIdx ) == PASS )
						{
							EcuWaitFlags |= (1<<EcuIdx);
							gstResponse[EcuIdx].bResponseReceived = FALSE;  // allow ECU to respond again
						}
						else
						{
							return FAIL;
						}
					}
					else if ( pRxMsg->Data[6] != NAK_SEQUENCE_ERROR )
					{
						Log ( FAILURE, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
						      "ECU %X  SID $14 NAK response error!\n",
						      EcuId );
						return FAIL;
					}
				}
				break;
	
				case 0x31:
				{
					if ( pRxMsg->Data[6] == NAK_SERVICE_NOT_SUPPORTED )
					{
						break;
					}
					else
					{
						Log ( FAILURE, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
						      "ECU %X  SID $31 NAK response error!\n",
						      EcuId,
						      pstReqMsg->SID );
					}
				}
				break;
	
				default :
				{
					Log ( FAILURE, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
					      "ECU %X  SID $%02X NAK response error!\n",
					      EcuId,
					      pstReqMsg->SID );
					return FAIL;
				}
				break;
			}
		}  // end if ( negative response )

		// Check for POS response message
		else if ( pRxMsg->Data[2] == 0x80  &&
		          pRxMsg->Data[3] == 0x02 )
		{
			// check the kind of response received for the vehicle
			switch ( pRxMsg->Data[12] )
			{
				case 0x00: // 0x00
				{
					Log ( INFORMATION, SCREENOUTPUTOFF, LOGOUTPUTON, NO_PROMPT,
					      "ECU %X  Positive ACK message received.\n",
					      EcuId );
				}
				break;
	
				default:
				{
					Log ( INFORMATION, SCREENOUTPUTOFF, LOGOUTPUTON, NO_PROMPT,
					      "ECU %X  ACK Unknown positive response.\n",
					      EcuId );
				}
				break;
			}
		}  // end if ( positve response )

		// Check diagnostic message
		else if ( pRxMsg->Data[2] == 0x80  &&
		          pRxMsg->Data[3] == 0x01 )
		{
			// Check for echoed request message
			if ( pRxMsg->RxStatus & TX_MSG_TYPE )
			{
				if ( pRxMsg->DataSize >= 13 &&
				     pRxMsg->Data[12] == pstReqMsg->SID )
				{
					// Save the timestamp
					*pTxTimestampMsecs = pRxMsg->TimestampMsecs;
				}
			}
		
			// Check for NAK response message
			else if ( pRxMsg->DataSize >= 15    &&
			          pRxMsg->Data[12]  == NAK  &&
			          pRxMsg->Data[13]  == pstReqMsg->SID )
			{
				if ( gstResponse[EcuTimingIdx].bNAKReceived == FALSE )
				{
					// Tally up the response statistics
					gstResponse[EcuTimingIdx].AggregateResponseTimeMsecs += ResponseTimeMsecs;
					gstResponse[EcuTimingIdx].AggregateResponses += ( ( ( pRxMsg->DataSize - 4 ) / ISO15765_MAX_BYTES_PER_FRAME ) + 1 );
					if ( ResponseTimeMsecs > gstResponse[EcuTimingIdx].LongestResponsesTimeMsecs )
					{
						gstResponse[EcuTimingIdx].LongestResponsesTimeMsecs = ResponseTimeMsecs;
					}
		
					gstResponse[EcuTimingIdx].bNAKReceived = TRUE;
					gstResponse[EcuTimingIdx].NAKCode = pRxMsg->Data[14];
				}
		
				// Check if response was late (all negative respones due in P2_MAX)
				if ( (pRxMsg->TimestampMsecs - *pTxTimestampMsecs) > ((gMaxResponseTimeMsecs +  gstResponse[EcuTimingIdx].ResponsePendingDelayTimeMsecs) * 1000) )
				{
					// Exceeded maximum response time
					Log ( ERROR_FAILURE, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
					      "ECU %X  OBD Negative Response was later than allowed (> %dmsec)\n",
					      EcuId,
					      gMaxResponseTimeMsecs + gstResponse[EcuTimingIdx].ResponsePendingDelayTimeMsecs );
					eRetCode = ERRORS;
		
					gstResponse[EcuTimingIdx].RespTimeOutofRange++;
					gstResponse[EcuTimingIdx].RespTimeTooLate++;
					Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
					      "Calculated OBD Response time: %2.4f msec\n",
					      ((float)(pRxMsg->TimestampMsecs - *pTxTimestampMsecs) / 1000) );
				}
		
				// Save the response information
				if ( SaveSIDResponseData ( pRxMsg,
				                           pstReqMsg,
				                           pNumResponses ) != PASS )
				{
					Log ( FAILURE, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
					      "Cannot save SID response data\n" );
					return FAIL;
				}
		
				// check the kind of response received for the vehicle
				switch ( pRxMsg->Data[14] )
				{
					case NAK_GENERAL_REJECT: // 0x10
					{
						Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
						      "ECU %X  General reject to SID $%02X.\n",
						      EcuId,
						      pRxMsg->Data[13] );
					}
					break;
		
					case NAK_SERVICE_NOT_SUPPORTED: // 0x11
					{
						Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
						      "ECU %X  SID $%02X not supported.\n",
						      EcuId,
						      pRxMsg->Data[13] );
					}
					break;
		
					case NAK_SUBFUNCTION_NOT_SUPPORTED: // 0x12
					{
						Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
						      "ECU %X  SID $%02X supported. Unsupported subfunction request.\n",
						      EcuId,
						      pRxMsg->Data[13] );
					}
					break;
		
					case NAK_INVALID_FORMAT: // 0x13
					{
						Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
						      "ECU %X  SID $%02X supported. Invalid format in request.\n",
						      EcuId,
						      pRxMsg->Data[13] );
					}
					break;
		
					case NAK_REPEAT_REQUEST: // 0x21
					{
						Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
						      "ECU %X  SID $%02X supported. Busy.\n",
						      EcuId,
						      pRxMsg->Data[13] );
					}
					break;
		
					case NAK_CONDITIONS_NOT_CORRECT: // 0x22
					{
						Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
						      "ECU %X  SID $%02X supported. Conditions not correct.\n",
						      EcuId,
						      pRxMsg->Data[13] );
					}
					break;
		
					case NAK_ID_NOT_SUPPORTED: // 0x31
					{
						Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
						      "ECU %X  SID $%02X supported. Unsupported DID request\n",
						      EcuId,
						      pRxMsg->Data[13] );
					}
					break;
		
					case NAK_GENERAL_PROGRAMMING_FAILURE: // 0x72
					{
						Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
						      "ECU %X  SID $%02X supported. General programming failure.\n",
						      EcuId,
						      pRxMsg->Data[13] );
					}
					break;
		
					case NAK_RESPONSE_PENDING: // 0x78
					{
						Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
						      "ECU %X  SID $%02X supported. Response pending.\n",
						      EcuId,
						      pRxMsg->Data[13] );
					}
					break;
		
					default:
					{
						Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
						      "ECU %X  Unknown negative response to SID $%02X.\n",
						      EcuId,
						      pRxMsg->Data[13] );
					}
					break;
				}
		
				// NAK Truth table
				switch( pstReqMsg->SID )
				{
					case 0x22:
					{
						if ( pRxMsg->Data[14] == NAK_ID_NOT_SUPPORTED )
						{
							break;  // handle in calling function
						}
						else if ( gbDetermineProtocol == TRUE &&
						          pRxMsg->Data[14] == NAK_REPEAT_REQUEST &&
						          (pstReqMsg->u.DID[0] == 0xF400 ||
						           pstReqMsg->u.DID[0] == 0xF810) )
						{
							return RETRY;
						}
						else if ( pRxMsg->Data[14] == NAK_RESPONSE_PENDING &&
						          pstReqMsg->u.DID[0] >= 0xF800 &&
						          pstReqMsg->u.DID[0] >= 0xF8FF )
						{
							// If response pending, extend the wait time
							gstResponse[EcuTimingIdx].ResponseTimeExtensionMsecs = 30000;
							if ( gstResponse[EcuTimingIdx].ResponseTimeExtensionMsecs > *pExtendResponseTimeMsecs )
							{
								*pExtendResponseTimeMsecs = gstResponse[EcuTimingIdx].ResponseTimeExtensionMsecs;
							}
							gstResponse[EcuTimingIdx].ResponsePendingDelayTimeMsecs = 5000;
							if ( gstResponse[EcuTimingIdx].ResponsePendingDelayTimeMsecs > ResponsePendingDelayTimeMsecs )
							{
								ResponsePendingDelayTimeMsecs = gstResponse[EcuTimingIdx].ResponsePendingDelayTimeMsecs;
							}
		
							// set wait flag
							if ( LookupEcuIndex ( pRxMsg, &EcuIdx ) == PASS )
							{
								EcuWaitFlags |= (1<<EcuIdx);
								gstResponse[EcuIdx].bResponseReceived = FALSE;  // allow ECU to respond again
							}
							else
							{
								return FAIL;
							}
						}
						// IF EOBD/IOBD/OBDBr and CVN
						else if ( pstReqMsg->u.DID[0] == 0xF806 &&
						          (gstUserInput.eComplianceType != US_OBDII &&
						           gstUserInput.eComplianceType != HD_OBD) )
						{
							break;
						}
						else
						{
							Log ( FAILURE, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
							      "ECU %X  SID $22 NAK response error!\n",
							      EcuId );
							return FAIL;
						}
					}
					break;
		
					case 0x19:
					{
						if ( pRxMsg->Data[14] == NAK_SERVICE_NOT_SUPPORTED ||
						     pRxMsg->Data[14] == NAK_SUBFUNCTION_NOT_SUPPORTED ||
						     pRxMsg->Data[14] == NAK_CONDITIONS_NOT_CORRECT ||
						     pRxMsg->Data[14] == NAK_ID_NOT_SUPPORTED )
						{
							eRetCode = ERRORS;
						}
						else if ( pRxMsg->Data[14] == NAK_RESPONSE_PENDING )
						{
							// If response pending, extend the wait time
							gstResponse[EcuTimingIdx].ResponseTimeExtensionMsecs = 30000;
							if ( gstResponse[EcuTimingIdx].ResponseTimeExtensionMsecs > *pExtendResponseTimeMsecs )
							{
								*pExtendResponseTimeMsecs = gstResponse[EcuTimingIdx].ResponseTimeExtensionMsecs;
							}
							gstResponse[EcuTimingIdx].ResponsePendingDelayTimeMsecs = 5000;
							if ( gstResponse[EcuTimingIdx].ResponsePendingDelayTimeMsecs > ResponsePendingDelayTimeMsecs )
							{
								ResponsePendingDelayTimeMsecs = gstResponse[EcuTimingIdx].ResponsePendingDelayTimeMsecs;
							}
		
							// set wait flag
							if ( LookupEcuIndex ( pRxMsg, &EcuIdx ) == PASS )
							{
								EcuWaitFlags |= (1<<EcuIdx);
								gstResponse[EcuIdx].bResponseReceived = FALSE;  // allow ECU to respond again
							}
							else
							{
								return FAIL;
							}
						}
						else
						{
							Log ( FAILURE, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
							      "ECU %X  SID $19 NAK response error!\n",
							      EcuId );
							return FAIL;
						}
					}
					break;
		
					case 0x14:
					{
						if (  pRxMsg->Data[14] == NAK_SERVICE_NOT_SUPPORTED ||
						      pRxMsg->Data[14] == NAK_SUBFUNCTION_NOT_SUPPORTED ||
						      pRxMsg->Data[14] == NAK_CONDITIONS_NOT_CORRECT ||
						     (pRxMsg->Data[14] == NAK_ID_NOT_SUPPORTED && gbPhysicalAddressing == FALSE) ||
						      pRxMsg->Data[14] == NAK_GENERAL_PROGRAMMING_FAILURE )
						{
							return ERRORS;
						}
						else if ( pRxMsg->Data[14] == NAK_RESPONSE_PENDING )
						{
							// If response pending, extend the wait time
							gstResponse[EcuTimingIdx].ResponseTimeExtensionMsecs = 30000;
							if ( gstResponse[EcuTimingIdx].ResponseTimeExtensionMsecs > *pExtendResponseTimeMsecs )
							{
								*pExtendResponseTimeMsecs = gstResponse[EcuTimingIdx].ResponseTimeExtensionMsecs;
							}
							gstResponse[EcuTimingIdx].ResponsePendingDelayTimeMsecs = 5000;
							if ( gstResponse[EcuTimingIdx].ResponsePendingDelayTimeMsecs > ResponsePendingDelayTimeMsecs )
							{
								ResponsePendingDelayTimeMsecs = gstResponse[EcuTimingIdx].ResponsePendingDelayTimeMsecs;
							}
		
							// set wait flag
							if ( LookupEcuIndex ( pRxMsg, &EcuIdx ) == PASS )
							{
								EcuWaitFlags |= (1<<EcuIdx);
								gstResponse[EcuIdx].bResponseReceived = FALSE;  // allow ECU to respond again
							}
							else
							{
								return FAIL;
							}
						}
						else if ( pRxMsg->Data[14] != NAK_ID_NOT_SUPPORTED || gbPhysicalAddressing != TRUE )
						{
							Log ( FAILURE, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
							      "ECU %X  SID $14 NAK response error!\n",
							      EcuId );
							return FAIL;
						}
					}
					break;
		
					case 0x31:
					{
						if ( pRxMsg->Data[14] == NAK_SERVICE_NOT_SUPPORTED )
						{
							break;
						}
						else if ( pRxMsg->Data[14] == NAK_CONDITIONS_NOT_CORRECT )
						{
							eRetCode = ERRORS;
						}
						else
						{
							Log ( FAILURE, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
							      "ECU %X  SID $31 NAK response error!\n",
							      EcuId,
							      pstReqMsg->SID );
						}
					}
					break;
		
					default :
					{
						Log ( FAILURE, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
						      "ECU %X  SID $%02X NAK response error!\n",
						      EcuId,
						      pstReqMsg->SID );
						return FAIL;
					}
					break;
				}
			}
		
			// Check for response message
			else
			{
				if ( gstResponse[EcuTimingIdx].bNAKReceived == FALSE )
				{
					// Tally up the response statistics
					gstResponse[EcuTimingIdx].AggregateResponseTimeMsecs += ResponseTimeMsecs;
					gstResponse[EcuTimingIdx].AggregateResponses++;
					if ( ResponseTimeMsecs > gstResponse[EcuTimingIdx].LongestResponsesTimeMsecs )
					{
						gstResponse[EcuTimingIdx].LongestResponsesTimeMsecs = ResponseTimeMsecs;
					}
				}
				else
				{
					gstResponse[EcuTimingIdx].bNAKReceived = FALSE;
				}
	
				// Check if response was late (compensate for segmented responses)
				if ( (pRxMsg->TimestampMsecs - *pTxTimestampMsecs) >
				     ((gMaxResponseTimeMsecs + gstResponse[EcuTimingIdx].ResponseTimeExtensionMsecs) * 1000) )
				{
					// Exceeded maximum response time
					Log ( ERROR_FAILURE, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
					      "ECU %X  OBD Response was later than allowed (> %dmsec)\n",
					      EcuId,
					      (gMaxResponseTimeMsecs + gstResponse[EcuTimingIdx].ResponseTimeExtensionMsecs) );
					eRetCode = ERRORS;
	
					gstResponse[EcuTimingIdx].RespTimeOutofRange++;
					gstResponse[EcuTimingIdx].RespTimeTooLate++;
					Log ( INFORMATION, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
					      "Calculated OBD Response time: %2.4f msec\n",
					      ((float)(pRxMsg->TimestampMsecs - *pTxTimestampMsecs) / 1000) );
				}
	
				// clear wait flag
				if ( LookupEcuIndex ( pRxMsg, &EcuIdx ) == PASS )
				{
					EcuWaitFlags &= ~(1<<EcuIdx);
					gstResponse[EcuTimingIdx].ResponsePendingDelayTimeMsecs = 0;        // ECU response no longer pending
				}
				else
				{
					return FAIL;
				}
	
				// Check if all ECUs with response pending have responded
				if ( EcuWaitFlags == 0 )
				{
					ResponsePendingDelayTimeMsecs = 0;         // no ECU response is pending
	
					if ( *pNumFirstFrames == 0 )
					{
						*pExtendResponseTimeMsecs = 0;  // no First Frames, clear extended response time
					}
				}
	
				// Check for proper SID response
				if ( pRxMsg->Data[12] != ( pstReqMsg->SID + OBD_RESPONSE_BIT ) )
				{
					Log ( FAILURE, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
					      "ECU %X  Invalid SID response to request\n",
					      EcuId );
					return FAIL;
				}
	
				// If a DTC was requested
				if ( pstReqMsg->SID == 0x19 && 
				     (pstReqMsg->u.ID[0] == 0x03 ||
	 			      ((pstReqMsg->u.ID[0] == 0x42 || pstReqMsg->u.ID[0] == 0x55) &&
				       pstReqMsg->u.ID[0] == pRxMsg->Data[5] &&
				       pstReqMsg->u.ID[1] == pRxMsg->Data[6])) )
				{
	
				}
				// If a PID was requested
				else if ( pstReqMsg->NumIds != 0 )
				{
					// Check for proper PID/MID/TID/INF response
					for ( IDIndex = 0;
					      IDIndex < (pstReqMsg->SID == 0x22 ? pstReqMsg->NumIds*2 : pstReqMsg->NumIds);
					      IDIndex++ )
					{
						if ( pRxMsg->Data[14] == pstReqMsg->u.ID[IDIndex] )
						{
							break;  // leave ID for loop
						}
						// if this is a SID $14 request, no DID in response
						if ( pstReqMsg->SID == 0x14 )
						{
							IDIndex++;
						}
					}
	
					if ( IDIndex == pstReqMsg->NumIds )
					{
						Log ( FAILURE, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
						      "ECU %X  Invalid PID response to request\n",
						      EcuId );
						return FAIL;
					}
				}
	
				// If this is any response to a Functionally Addressed request or
				// a response from the correct ECU to a Physically Addressed request,
				// save the data
				if ( pstReqMsg->bPhysicallyAddressed == FALSE ||
				     (pstReqMsg->bPhysicallyAddressed == TRUE &&
				      EcuId == pstReqMsg->RespEcuID ) )
				{
					// Save the response information
					if ( SaveSIDResponseData ( pRxMsg,
					                           pstReqMsg,
					                           pNumResponses ) != PASS )
					{
						Log ( FAILURE, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
						      "Cannot save SID response data\n" );
						eRetCode = FAIL;
					}
				}
	
				// Otherwise this response to a Physically Address request
				// is from the wrong ECU
				else
				{
					Log ( FAILURE, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
					      "ECU %X  Responded to a Physically Addressed Request (expecting a response from ECU %X\n" ,
					      EcuId,
					      pstReqMsg->RespEcuID );
					eRetCode = FAIL;
				}
			}  // end check for response message
		}
	}  // end if ( pRxMsg->DataSize > 12 )

	// if datasize !> 12, Ignore, invalid message

	return eRetCode;
}


/*******************************************************************************
**
**  Function:  SetupRequestMsg
**
**  Purpose:   Setup request message based on the protocol.
**             Routine will normalize the message data handling between
**             the regulated OBD protocol message structures.
**
*******************************************************************************/
STATUS SetupRequestMsg ( REQ_MSG       *pstReqMsg,
                         PASSTHRU_MSG  *pTxMsg )
{
	unsigned short TxIndex;
	unsigned short IdIdx;


	// Set message data information common to all protocols
	pTxMsg->eProtocolID = gstProtocolList[gProtocolIdx].eProtocol;
	pTxMsg->RxStatus    = TX_MSG_TYPE;
	pTxMsg->TxFlags     = 0x00;  // Change back to non-blocking

	// Configure protocol specific message data.
	switch ( gstProtocolList[gProtocolIdx].eProtocol )
	{
		case ISO15765:
		{
			pTxMsg->TxFlags |= ISO15765_FRAME_PAD;

			if ( gstProtocolList[gProtocolIdx].eInitFlags & CAN_29BIT_ID )
			{
				pTxMsg->TxFlags |= CAN_29BIT_ID;

				if ( pstReqMsg->bPhysicallyAddressed )
				{
					pTxMsg->Data[0] = (BYTE)((pstReqMsg->ReqEcuID & 0xFF000000) >> 24);
					pTxMsg->Data[1] = (BYTE)((pstReqMsg->ReqEcuID & 0x00FF0000) >> 16);
					pTxMsg->Data[2] = (BYTE)((pstReqMsg->ReqEcuID & 0x0000FF00) >> 8);
					pTxMsg->Data[3] = (BYTE)(pstReqMsg->ReqEcuID & 0x000000FF);

					pstReqMsg->RespEcuID = (pstReqMsg->ReqEcuID & 0xFFFF0000) +
					                       ((pstReqMsg->ReqEcuID & 0x0000FF00) >> 8) +
					                       ((pstReqMsg->ReqEcuID & 0x000000FF) << 8);
				}
				else
				{
					pTxMsg->Data[0] = 0x18;
					pTxMsg->Data[1] = 0xDB;
					pTxMsg->Data[2] = 0x33;
					pTxMsg->Data[3] = TESTER_NODE_ADDRESS;
				}
			}
			else
			{
				pTxMsg->Data[0] = 0x00;
				pTxMsg->Data[1] = 0x00;
				if ( pstReqMsg->bPhysicallyAddressed )
				{
					pTxMsg->Data[2] = (pstReqMsg->ReqEcuID & 0xFF00) >> 8;
					pTxMsg->Data[3] =  pstReqMsg->ReqEcuID & 0x00FF;

					pstReqMsg->RespEcuID = (pstReqMsg->ReqEcuID & 0xFFFF) + 0x08;
				}
				else
				{
					pTxMsg->Data[2] = 0x07;
					pTxMsg->Data[3] = 0xDF;
				}
			}
			pTxMsg->Data[4] = pstReqMsg->SID;

			// if this is check for J1979 classic support (SID $1-$9)
			if ( pstReqMsg->SID < 0x0A )
			{
				pTxMsg->Data[5]  = 0x00;
				pTxMsg->DataSize = 6;
			}
			// if this is check for J1979 classic support (SID $A)
			else if ( pstReqMsg->SID == 0x0A )
			{
				pTxMsg->DataSize = 5;
			}
			// if this is a DTC or Control request, the data is in bytes
			else if ( pstReqMsg->SID == 0x14 ||
			          pstReqMsg->SID == 0x19 ||
			          pstReqMsg->SID == 0x31 )
			{
				for ( TxIndex = 5, IdIdx = 0;
				      IdIdx < pstReqMsg->NumIds;
				      TxIndex++, IdIdx++ )
				{
					pTxMsg->Data[TxIndex] = pstReqMsg->u.ID[IdIdx];
				}

				pTxMsg->DataSize = 5 + pstReqMsg->NumIds;
			}
			// if this is a PID, MID, INF request, data is in words (2 bytes)
			else
			{
				for ( TxIndex = 5, IdIdx = 0;
				      IdIdx < pstReqMsg->NumIds;
				      IdIdx++ )
				{
					pTxMsg->Data[TxIndex++] = (unsigned char)((pstReqMsg->u.DID[IdIdx] & 0xFF00) >> 8);
					pTxMsg->Data[TxIndex++] = (unsigned char)(pstReqMsg->u.DID[IdIdx]  & 0x00FF);
				}
				pTxMsg->DataSize = 5 + ((pstReqMsg->NumIds) *2);
			}
		}
		break;

		case DOIP:
		case DOIP_NDIS:
		{
			unsigned long DataSize;

			#define  MIN_DOIP_PAYLOAD_LEN  5
			#define  DOIP_HEADER_LEN       8
			

			// DoIP header
			pTxMsg->Data[0] = (BYTE)(0x02);  // DoIP Protocol Version
			pTxMsg->Data[1] = (BYTE)(0xFD);  // Inverted Doip Protocol Version
			pTxMsg->Data[2] = (BYTE)(0x80);  // DoIp Payload Type (diagnostic message 0x8001) MSB
			pTxMsg->Data[3] = (BYTE)(0x01);  // DoIp Payload Type (diagnostic message 0x8001) LSB

			// DoIP Payload Length determination
			// if this is check for J1979 classic support (SID $1-$9)
			if (pstReqMsg->SID < 0x0A)
			{
				pTxMsg->Data[13] = 0x00;
				DataSize = MIN_DOIP_PAYLOAD_LEN +1;
			}
			// if this is check for J1979 classic support (SID $A)
			else if (pstReqMsg->SID == 0x0A)
			{
				// No Data
				DataSize = MIN_DOIP_PAYLOAD_LEN;
			}
			// if this is a DTC or Control request, the data is in bytes
			else if ( pstReqMsg->SID == 0x14 ||
			          pstReqMsg->SID == 0x19 ||
			          pstReqMsg->SID == 0x31 )
			{
				for ( TxIndex = 13, IdIdx = 0;
				      IdIdx < pstReqMsg->NumIds;
				      TxIndex++, IdIdx++ )
				{
					pTxMsg->Data[TxIndex] = pstReqMsg->u.ID[IdIdx];
				}
				DataSize = MIN_DOIP_PAYLOAD_LEN + pstReqMsg->NumIds;
			}
			// if this is a PID, MID, INF request, data is in words (2 bytes)
			else
			{
				for ( TxIndex = 13, IdIdx = 0;
				      IdIdx < pstReqMsg->NumIds;
				      IdIdx++ )
				{
					pTxMsg->Data[TxIndex++] = (unsigned char)((pstReqMsg->u.DID[IdIdx] & 0xFF00) >> 8);
					pTxMsg->Data[TxIndex++] =  (unsigned char)(pstReqMsg->u.DID[IdIdx] & 0x00FF);
				}
				DataSize = MIN_DOIP_PAYLOAD_LEN + ((pstReqMsg->NumIds) * 2);
			}

			pTxMsg->Data[4] = (BYTE)((DataSize & 0xFF000000) >> 24);  // DoIP Payload length MSB
			pTxMsg->Data[5] = (BYTE)((DataSize & 0x00FF0000) >> 16);  // .
			pTxMsg->Data[6] = (BYTE)((DataSize & 0x0000FF00) >> 8);   // .
			pTxMsg->Data[7] =  (BYTE)(DataSize & 0x000000FF);         // DoIp Payload lenght LSB

			// DoIP Source Address
			pTxMsg->Data[8] = (BYTE)(0x0E);  // DoIP Source Address MSB
//			pTxMsg->Data[9] = (BYTE)(0x80);  // DoIP Source Address LSB
			pTxMsg->Data[9] = (BYTE)(0x00);  // DoIP Source Address LSB

			// DoIP Target Address
			if ( pstReqMsg->bPhysicallyAddressed )
			{
				pTxMsg->Data[10] = (pstReqMsg->ReqEcuID & 0xFF00) >> 8;
				pTxMsg->Data[11] =  pstReqMsg->ReqEcuID & 0x00FF;

				pstReqMsg->RespEcuID = pstReqMsg->ReqEcuID;
			}
			else
			{
				pTxMsg->Data[10] = 0xE0;
				pTxMsg->Data[11] = 0x00;
//				pTxMsg->Data[10] = 0x0C;
//				pTxMsg->Data[11] = 0x45;
			}

			// DoIP UDS Service
			pTxMsg->Data[12] = pstReqMsg->SID;

			// message data size is DoIP Header Length + DoIP Payload Length
			pTxMsg->DataSize = DOIP_HEADER_LEN + DataSize;
			pTxMsg->ExtraDataIndex = MAX_MESSAGE_LOG_SIZE;
		}
		break;

		default:
		{
			Log ( FAILURE, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
			      "Invalid protocol specified in SetupRequestMsg\n" );
			return FAIL;
		}
	}

	return PASS;
}


STATUS RequestSID_PhysicallyAddressed_All ( REQ_MSG  *pstReqMsg, unsigned long Flags )
{
	unsigned int   EcuIdx;

	STATUS         eRetVal = PASS;


	gNumOfECUsResp = 0;

	// Determine the range of ECU addresses to request
	for ( EcuIdx = 0;
	      EcuIdx < gNumOfECUs;
	      EcuIdx++ )
	{
		if ( RequestSID_PhysicallyAddressed_Single ( pstReqMsg, Flags, gstResponse[EcuIdx].ReqId ) == FAIL )
		{
			eRetVal = FAIL;
		}
	}

	return eRetVal;
}


STATUS RequestSID_PhysicallyAddressed_Single ( REQ_MSG *pstReqMsg, unsigned long Flags, unsigned long EcuId )
{
	STATUS         eRetVal = PASS;


	pstReqMsg->ReqEcuID = EcuId;
	if ( RequestSID ( pstReqMsg, Flags|REQ_MSG_PHYSICALLY_ADDRESSED ) == FAIL )
	{
		eRetVal = FAIL;
		Log ( FAILURE, SCREENOUTPUTON, LOGOUTPUTON, NO_PROMPT,
		     "No Response from ECU %X to Physically Addressed SID $%02X request\n",
		     pstReqMsg->RespEcuID,
		     pstReqMsg->SID);
	}
	return eRetVal;
}