internal ModbusResponse Deserialize(ResponseBuffer buffer, ModbusRequest request, int timeout)
{
ModbusResponse result;
try
{
result = DeserializeResponse(buffer, request, timeout);
}
catch (TimeoutException ex)
{
throw new ModbusCommException(ModbusCommErrorCode.ResponseTimeout, buffer, ex, request);
}
catch (ModbusCommException ex)
{
throw new ModbusCommException(ex.Code, buffer, ex.InnerException, request);
}
catch (Exception ex)
{
throw new ModbusCommException(buffer, ex, request);
}
if (result is ModbusCommErrorResponse commErrorResponse)
{
throw new ModbusCommException(commErrorResponse.ErrorCode, commErrorResponse.ReceivedBytes, commErrorResponse.Request);
}
return(result);
}
internal override ModbusResponse DeserializeReadRegisterResponse(ResponseBuffer buffer, ModbusReadRequest request, int timeout)
{
if (IsException(buffer, request, timeout, out var responseMessage))
{
return(responseMessage);
}
byte byteLength = Read(buffer, 2, timeout);
if (IsErrorCRC(buffer, 3 + byteLength, request, timeout))
{
throw new ModbusCommException(ModbusCommErrorCode.ErrorCRC, buffer, request);
}
if (Read(buffer, 0, timeout) != request.SlaveAddress)
{
throw new ModbusCommException(ModbusCommErrorCode.ResponseSlaveAddressDoNotMatch, buffer, request);
}
if ((Read(buffer, 1, timeout) & 0x7f) != (byte)request.Function)
{
throw new ModbusCommException(ModbusCommErrorCode.ResponseFunctionDoNotMatch, buffer, request);
}
if (byteLength != (byte)(request.Length * 2))
{
throw new ModbusCommException(ModbusCommErrorCode.ResponseLengthDoNotMatch, buffer, request);
}
return(new ModbusReadRegisterResponse(Read(buffer, 3, byteLength, timeout).ToArray(), request));
}
private bool IsException(ResponseBuffer buffer, ModbusRequest request, int timeout, out ModbusResponse responseMessage)
{
if ((Read(buffer, 1, timeout) & 0x80) == 0x80)
{
var codeValue = Read(buffer, 2, timeout);
if (IsErrorCRC(buffer, 3, request, timeout))
{
throw new ModbusCommException(ModbusCommErrorCode.ErrorCRC, buffer, request);
}
ModbusExceptionCode exceptionCode = ModbusExceptionCode.NotDefined;
if (Enum.IsDefined(typeof(ModbusExceptionCode), codeValue))
{
exceptionCode = (ModbusExceptionCode)codeValue;
}
responseMessage = new ModbusExceptionResponse(exceptionCode, request);
return(true);
}
else
{
responseMessage = null;
return(false);
}
}
internal override ModbusResponse DeserializeReadBooleanResponse(ResponseBuffer buffer, ModbusReadRequest request, int timeout)
{
if (IsException(buffer, request, timeout, out var responseMessage))
{
return(responseMessage);
}
byte byteLength = Read(buffer, 2, timeout);
if (IsErrorLRC(buffer, 3 + byteLength, request, timeout, out responseMessage))
{
return(responseMessage);
}
if (Read(buffer, 0, timeout) != request.SlaveAddress)
{
return(new ModbusCommErrorResponse(ModbusCommErrorCode.ResponseSlaveAddressDoNotMatch, buffer, request));
}
if ((Read(buffer, 1, timeout) & 0x7f) != (byte)request.Function)
{
return(new ModbusCommErrorResponse(ModbusCommErrorCode.ResponseFunctionDoNotMatch, buffer, request));
}
if (byteLength != (byte)Math.Ceiling(request.Length / 8d))
{
return(new ModbusCommErrorResponse(ModbusCommErrorCode.ResponseLengthDoNotMatch, buffer, request));
}
return(new ModbusReadBooleanResponse(Read(buffer, 3, byteLength, timeout).SelectMany(b => ByteToBooleanArray(b)).Take(request.Length).ToArray(), request));
}
internal virtual ModbusResponse DeserializeResponse(ResponseBuffer buffer, ModbusRequest request, int timeout)
{
if (request is ModbusReadRequest readMessage)
{
switch (readMessage.ObjectType)
{
case ModbusObjectType.DiscreteInput:
case ModbusObjectType.Coil:
return(DeserializeReadBooleanResponse(buffer, readMessage, timeout));
case ModbusObjectType.InputRegister:
case ModbusObjectType.HoldingRegister:
return(DeserializeReadRegisterResponse(buffer, readMessage, timeout));
}
}
else if (request is ModbusWriteCoilRequest writeCoilMessage)
{
return(DeserializeWriteResponse(buffer, writeCoilMessage, timeout));
}
else if (request is ModbusWriteHoldingRegisterRequest writeHoldingRegisterMessage)
{
return(DeserializeWriteResponse(buffer, writeHoldingRegisterMessage, timeout));
}
throw new ArgumentOutOfRangeException(nameof(request));
}
internal virtual byte Read(ResponseBuffer buffer, int index, int timeout)
{
if (index >= buffer.Count)
{
buffer.Read((uint)(index - buffer.Count + 1), timeout);
}
return(buffer[index]);
}
internal virtual IEnumerable <byte> Read(ResponseBuffer buffer, int index, int count, int timeout)
{
if (index + count > buffer.Count)
{
buffer.Read((uint)(index + count - buffer.Count), timeout);
}
return(buffer.Skip(index).Take(count));
}
internal override byte Read(ResponseBuffer buffer, int index, int timeout)
{
if (index * 2 >= buffer.Count - 2)
{
buffer.Read((uint)(index * 2 - buffer.Count + 3), timeout);
}
return(byte.Parse(Encoding.ASCII.GetString(buffer.Skip(index * 2 + 1).Take(2).ToArray()), System.Globalization.NumberStyles.HexNumber, null));
}
internal override ModbusResponse DeserializeResponse(ResponseBuffer buffer, ModbusRequest request, int timeout)
{
ModbusResponse result = null;
while (result == null ||
result is ModbusCommErrorResponse responseCommErrorMessage &&
responseCommErrorMessage.ErrorCode != ModbusCommErrorCode.ResponseTimeout)
{
if (buffer.Count > 0)
{
errorBuffer.Add(buffer[0]);
buffer.RemoveAt(0);
}
buffer.Read(timeout);
try
{
while (buffer[0] != 0x3a)
{
errorBuffer.Add(buffer[0]);
buffer.RemoveAt(0);
buffer.Read(timeout);
}
}
catch
{
return(new ModbusCommErrorResponse(ModbusCommErrorCode.ResponseAsciiStartError, errorBuffer.Concat(buffer), request));
}
result = base.DeserializeResponse(buffer, request, timeout);
}
if (result is ModbusCommErrorResponse responseCommError)
{
result = new ModbusCommErrorResponse(responseCommError.ErrorCode, errorBuffer.Concat(responseCommError.ReceivedBytes), request);
}
else
{
var asciiEnd = buffer.Read(2, timeout);
if (!asciiEnd.SequenceEqual(new byte[] { 13, 10 }))
{
return(new ModbusCommErrorResponse(ModbusCommErrorCode.ResponseAsciiEndError, buffer, request));
}
if (errorBuffer.Count > 0)
{
RaiseUnrecognized(buffer.Channel, errorBuffer.ToArray());
errorBuffer.Clear();
}
}
return(result);
}
internal override IEnumerable <byte> Read(ResponseBuffer buffer, int index, int count, int timeout)
{
if ((index + count) * 2 > buffer.Count - 1)
{
buffer.Read((uint)((index + count) * 2 - buffer.Count + 1), timeout);
}
var bytes = buffer.Skip(index * 2 + 1).Take(count * 2).ToArray();
for (int i = 0; i < count; i++)
{
yield return(byte.Parse(Encoding.ASCII.GetString(bytes, i * 2, 2), System.Globalization.NumberStyles.HexNumber, null));
}
}
internal override ModbusResponse DeserializeWriteResponse(ResponseBuffer buffer, ModbusWriteHoldingRegisterRequest request, int timeout)
{
if (IsException(buffer, request, timeout, out var responseMessage))
{
return(responseMessage);
}
if (IsErrorCRC(buffer, 6, request, timeout))
{
throw new ModbusCommException(ModbusCommErrorCode.ErrorCRC, buffer, request);
}
if (Read(buffer, 0, timeout) != request.SlaveAddress)
{
throw new ModbusCommException(ModbusCommErrorCode.ResponseSlaveAddressDoNotMatch, buffer, request);
}
if ((Read(buffer, 1, timeout) & 0x7f) != (byte)request.Function)
{
throw new ModbusCommException(ModbusCommErrorCode.ResponseFunctionDoNotMatch, buffer, request);
}
if (ToUInt16(buffer, 2) != request.Address)
{
throw new ModbusCommException(ModbusCommErrorCode.ResponseAddressDoNotMatch, buffer, request);
}
ushort value = ToUInt16(buffer, 4);
switch (request.Function)
{
case ModbusFunction.WriteSingleHoldingRegister:
if (value != request.SingleRegisterValue)
{
throw new ModbusCommException(ModbusCommErrorCode.ResponseWritedValueDoNotMatch, buffer, request);
}
break;
case ModbusFunction.WriteMultipleHoldingRegisters:
if (value != request.Length)
{
throw new ModbusCommException(ModbusCommErrorCode.ResponseWritedLengthDoNotMatch, buffer, request);
}
break;
}
return(new ModbusWriteResponse(request));
}
internal override ModbusResponse DeserializeWriteResponse(ResponseBuffer buffer, ModbusWriteCoilRequest request, int timeout)
{
if (IsException(buffer, request, timeout, out var responseMessage))
{
return(responseMessage);
}
if (IsErrorCRC(buffer, 6, request, timeout))
{
throw new ModbusCommException(ModbusCommErrorCode.ErrorCRC, buffer, request);
}
if (Read(buffer, 0, timeout) != request.SlaveAddress)
{
throw new ModbusCommException(ModbusCommErrorCode.ResponseSlaveAddressDoNotMatch, buffer, request);
}
if ((Read(buffer, 1, timeout) & 0x7f) != (byte)request.Function)
{
throw new ModbusCommException(ModbusCommErrorCode.ResponseFunctionDoNotMatch, buffer, request);
}
if (ToUInt16(buffer, 2) != request.Address)
{
throw new ModbusCommException(ModbusCommErrorCode.ResponseAddressDoNotMatch, buffer, request);
}
switch (request.Function)
{
case ModbusFunction.WriteSingleCoil:
if (Read(buffer, 4, timeout) != (request.SingleBooleanValue ? 0xff : 0x00) ||
Read(buffer, 5, timeout) != 0x00)
{
throw new ModbusCommException(ModbusCommErrorCode.ResponseWritedValueDoNotMatch, buffer, request);
}
break;
case ModbusFunction.WriteMultipleCoils:
if (ToUInt16(buffer, 4) != request.Length)
{
throw new ModbusCommException(ModbusCommErrorCode.ResponseWritedLengthDoNotMatch, buffer, request);
}
break;
}
return(new ModbusWriteResponse(request));
}
internal override ModbusResponse DeserializeResponse(ResponseBuffer buffer, ModbusRequest request, int timeout)
{
var responseWaitHandle = this.responseWaitHandles[request.TransactionID] = new ResponseWaitHandle(buffer, request, timeout);
Task.Run(() => RunReceive(buffer.Channel));
responseWaitHandle.WaitOne(timeout);
var result = responseWaitHandle.Response;
if (result == null)
{
this.responseWaitHandles.Remove(request.TransactionID);
return(new ModbusCommErrorResponse(ModbusCommErrorCode.ResponseTimeout, new byte[0], request));
}
return(result);
}
private bool IsErrorLRC(ResponseBuffer buffer, int messageLength, ModbusRequest request, int timeout, out ModbusResponse responseMessage)
{
byte lrc = 0;
foreach (var b in Read(buffer, 0, messageLength, timeout))
{
lrc += b;
}
lrc = (byte)(-lrc & 0xff);
if (lrc != Read(buffer, messageLength, timeout))
{
responseMessage = new ModbusCommErrorResponse(ModbusCommErrorCode.ErrorLRC, buffer, request);
return(true);
}
else
{
responseMessage = null;
return(false);
}
}
internal override ModbusResponse DeserializeResponse(ResponseBuffer buffer, ModbusRequest request, int timeout)
{
ModbusResponse result = base.DeserializeResponse(buffer, request, timeout);
while (result is ModbusCommErrorResponse responseCommErrorMessage &&
responseCommErrorMessage.ErrorCode != ModbusCommErrorCode.ResponseTimeout)
{
errorBuffer.Add(buffer[0]);
buffer.RemoveAt(0);
result = base.DeserializeResponse(buffer, request, timeout);
}
if (result is ModbusCommErrorResponse responseCommError)
{
result = new ModbusCommErrorResponse(responseCommError.ErrorCode, errorBuffer.Concat(responseCommError.ReceivedBytes), request);
}
else if (errorBuffer.Count > 0)
{
RaiseUnrecognized(buffer.Channel, errorBuffer.ToArray());
errorBuffer.Clear();
}
return(result);
}
internal ushort ToUInt16(ResponseBuffer buffer, int index, int timeout)
{
return(ToUInt16(Read(buffer, index, 2, timeout).ToArray(), 0));
}
public ResponseWaitHandle(ResponseBuffer buffer, ModbusRequest request, int timeout) : base(false, EventResetMode.ManualReset)
{
ResponseBuffer = buffer;
Request = request;
Timeout = timeout;
}
internal abstract ModbusResponse DeserializeWriteResponse(ResponseBuffer buffer, ModbusWriteHoldingRegisterRequest request, int timeout);
internal abstract ModbusResponse DeserializeWriteResponse(ResponseBuffer buffer, ModbusWriteCoilRequest request, int timeout);
internal abstract ModbusResponse DeserializeReadRegisterResponse(ResponseBuffer buffer, ModbusReadRequest request, int timeout);
private bool IsErrorCRC(ResponseBuffer buffer, int messageLength, ModbusRequest request, int timeout)
{
var crc = Read(buffer, messageLength, 2, timeout).ToArray();
return(!CalculateCrc(buffer.Take(messageLength)).SequenceEqual(crc));
}
private void RunReceive(ModbusChannel channel)
{
lock (responseWaitHandles)
{
if (!isReceiving)
{
isReceiving = true;
try
{
var readBuffer = new ResponseBuffer(channel);
while (responseWaitHandles.Count > 0)
{
if (errorBuffer.Count >= 256)
{
RaiseUnrecognized(channel, errorBuffer.ToArray());
errorBuffer.Clear();
}
readBuffer.Read(2, 0);
ushort transactionID = (ushort)((readBuffer[0] << 8) | readBuffer[1]);
if (!responseWaitHandles.TryGetValue(transactionID, out var responseWaitHandle))
{
errorBuffer.AddRange(readBuffer);
readBuffer.Clear();
continue;
}
readBuffer.Read(2, responseWaitHandle.Timeout);
if (readBuffer[2] != 0 ||
readBuffer[3] != 0)
{
errorBuffer.AddRange(readBuffer);
readBuffer.Clear();
responseWaitHandle.Response = new ModbusCommErrorResponse(ModbusCommErrorCode.ModbusTcpSymbolError, errorBuffer, responseWaitHandle.Request);
continue;
}
var result = base.DeserializeResponse(readBuffer, responseWaitHandle.Request, responseWaitHandle.Timeout);
if (result is ModbusCommErrorResponse responseCommErrorMessage)
{
errorBuffer.AddRange(readBuffer.Take(4));
readBuffer.RemoveRange(0, 4);
responseWaitHandle.Response = result;
continue;
}
if (readBuffer.Count - 6 != (ushort)((readBuffer[4] << 8) | readBuffer[5]))
{
errorBuffer.AddRange(readBuffer.Take(4));
readBuffer.RemoveRange(0, 4);
responseWaitHandle.Response = new ModbusCommErrorResponse(ModbusCommErrorCode.ResponseTcpLengthDoNotMatch, readBuffer, responseWaitHandle.Request);
continue;
}
if (errorBuffer.Count > 0)
{
RaiseUnrecognized(channel, errorBuffer.ToArray());
errorBuffer.Clear();
}
responseWaitHandle.ResponseBuffer.AddRange(readBuffer);
readBuffer.Clear();
responseWaitHandle.Response = result;
responseWaitHandle.Set();
this.responseWaitHandles.Remove(transactionID);
}
}
catch
{
}
isReceiving = false;
}
}
}
