/// <summary>
/// Parse the replies checking for a valid response, if we have a valid response extract the payload data
/// </summary>
/// <param name="rxMsgs"></param>
/// <param name="txMode"></param>
/// <param name="payload"></param>
/// <returns></returns>
UDSPacket ParseUDSResponse(PassThruMsg rxMsg, UDScmd.Mode txMode)
{
var rxMsgBytes = rxMsg.GetBytes();
UDSPacket rxPacket = new UDSPacket();
//Iterate the reply bytes to find the echod ECU index, response code, function response and payload data if there is any
//If we could use some kind of HEX regex this would be a bit neater
int stateMachine = 0;
for (int i = 0; i < rxMsgBytes.Length; i++)
{
switch (stateMachine)
{
case 0:
if (rxMsgBytes[i] == 0x07)
{
stateMachine = 1;
}
else if (rxMsgBytes[i] != 0)
{
return(rxPacket);
}
break;
case 1:
if (rxMsgBytes[i] == 0xE8)
{
stateMachine = 2;
}
else
{
return(rxPacket);
}
break;
case 2:
var payload = new byte[rxMsgBytes.Length - i];
int payloadLength = rxMsgBytes.Length - i;
if (payloadLength > 0)
{
payload = new byte[payloadLength];
Array.Copy(rxMsgBytes, i, payload, 0, payloadLength);
rxPacket = new UDSPacket(payload, txMode);
;
}
return(rxPacket);
case 3:
default:
return(rxPacket);
}
}
return(rxPacket);
}
public UDSPacket(byte [] data, UDScmd.Mode mode)
{
if (data == null)
{
return;
}
if (data.Length < 1)
{
return;
}
if ((UDScmd.Response)data[0] != UDScmd.Response.NEGATIVE_RESPONSE)
{
Response = (UDScmd.Response)(data[0] - (byte)mode);
}
else
{
Response = UDScmd.Response.NEGATIVE_RESPONSE;
}
if (data.Length < 2)
{
return;
}
Mode = (UDScmd.Mode)data[1];
int payloadLength = 0;
switch (mode)
{
case UDScmd.Mode.TRANSFER_DATA:
case UDScmd.Mode.DIAGNOSTIC_COMMAND:
case UDScmd.Mode.READ_MEMORY_BY_ADDRESS:
if (Response == UDScmd.Response.NEGATIVE_RESPONSE)
{
NegativeResponse = (UDScmd.NegativeResponse)data[2];
}
payloadLength = data.Length - 1;
if (payloadLength < 1)
{
return;
}
Payload = new byte[payloadLength];
Buffer.BlockCopy(data, 1, Payload, 0, payloadLength);
break;
case UDScmd.Mode.REQUEST_DOWNLOAD:
if (Response == UDScmd.Response.NEGATIVE_RESPONSE)
{
NegativeResponse = (UDScmd.NegativeResponse)data[2];
return;
}
if (data.Length < 3)
{
return;
}
LengthFormatIdentifier = data[1];
MaxNumberOfBlockLength = data[2];
break;
case UDScmd.Mode.SECURITY_ACCESS:
int offset;
if (Response == UDScmd.Response.NEGATIVE_RESPONSE)
{
offset = 3;
if (data.Length < 3)
{
return;
}
NegativeResponse = (UDScmd.NegativeResponse)data[2];
}
else
{
offset = 2;
SubFunction = data[1];
}
payloadLength = data.Length - offset;
if (payloadLength < 1)
{
return;
}
Payload = new byte[payloadLength];
Buffer.BlockCopy(data, offset, Payload, 0, payloadLength);
break;
default:
break;
}
}