public void HandleMessage(ModbusMessage message)
{
for (int i = 0; i < corruptionAttempts; i++)
{
if (random.NextDouble() < corruptionChance / corruptionAttempts)
{
message.address = Utils.FlipRandomBit(message.address);
}
if (random.NextDouble() < corruptionChance / corruptionAttempts)
{
message.function = Utils.FlipRandomBit(message.function);
}
for (int j = 0; j < message.data.Length; j++)
{
if (random.NextDouble() < corruptionChance / corruptionAttempts)
{
message.data[j] = Utils.FlipRandomBit(message.data[j]);
}
}
if (random.NextDouble() < corruptionChance / corruptionAttempts)
{
message.checksum = Utils.FlipRandomBit(message.checksum);
}
}
OnMessage?.Invoke(message);
}
private void Func_WriteMultipleCoils(ModbusMessage message)
{
int firstCoilAddress = message.data[0] * 256 + message.data[1];
int coilsToWrite = message.data[2] * 256 + message.data[3];
byte coilByteCount = message.data[4];
byte[] coilBytes = message.data.Skip(5).Take(message.data.Length - 5).ToArray();
int coilsLeft = coilsToWrite;
for (int i = 0; i < coilByteCount; i++)
{
for (int j = 0; j < 8; j++)
{
coils.Set(i * 8 - 1 - j + firstCoilAddress, (coilBytes[i] & (1 << j)) == 1);
if (--coilsLeft < 0)
{
break;
}
}
}
bus.SendMessage(new ModbusMessage
{
address = id,
function = 15,
data = new byte[4] {
(byte)((firstCoilAddress & 0xFF00) >> 8), (byte)(firstCoilAddress & 0x00FF), (byte)((coilsToWrite & 0xFF00) >> 8), (byte)(coilsToWrite & 0x00FF)
}
});
}
private void Func_ReadCoils(ModbusMessage message)
{
int firstCoilAddress = message.data[0] * 256 + message.data[1];
int coilsToRead = message.data[2] * 256 + message.data[3];
byte[] readData = new byte[coilsToRead / 8 + 1];
int coilsLeft = coilsToRead;
for (int i = 0; i < readData.Length; i++)
{
for (int j = 0; j < 8; j++)
{
readData[i] |= (byte)((coils.Get(i * 8 + j + firstCoilAddress) ? 1 : 0) << j);
if (--coilsLeft < 0)
{
break;
}
}
}
bus.SendMessage(new ModbusMessage
{
address = id,
function = 1,
data = new byte[1] {
(byte)readData.Length
}.Concat(readData).ToArray()
});
}
private void Func_WriteSingleCoil(ModbusMessage message)
{
int coilAddress = message.data[0] * 256 + message.data[1];
coils.Set(coilAddress, message.data[2] * 256 + message.data[3] == 0xFF00);
bus.SendMessage(message);
}
private void Func_WriteHoldingRegister(ModbusMessage message)
{
int registerAddress = message.data[0] * 256 + message.data[1];
int value = message.data[2] * 256 + message.data[3];
holdingRegisters[registerAddress] = (ushort)value;
bus.SendMessage(message);
}
public void SendMessage(ModbusMessage message, bool isFromMaster = false, bool recalcChecksum = true)
{
message.isFromMaster = isFromMaster;
if (recalcChecksum)
{
message.RecalculateChecksum();
}
HandleMessage(message);
}
private void CheckForResponse(ModbusMessage message)
{
Thread.Sleep(timeout);
waitingForResponse.TryGetValue(message.address, out bool waiting);
if (waiting)
{
Logger.Warn($"Device {message.address} has not responded yet, resending message");
SendMessage(message);
}
}
public static ModbusMessage CreateException(ModbusMessage message, ModbusExceptionType type)
{
return(new ModbusMessage
{
address = message.address,
function = (byte)(message.function + 0x80),
data = new byte[1] {
(byte)type
}
});
}
public void SendMessage(ModbusMessage message)
{
if (!message.VerifyChecksum(out ushort actualChecksum))
{
Logger.Warn($"Sending message to device {message.address} with wrong checksum (is {message.checksum.ToString("x2")}, should be {actualChecksum.ToString("x2")})");
}
bus.SendMessage(message, true);
waitingForResponse.TryUpdate(message.address, true, false);
Task.Factory.StartNew(() => CheckForResponse(message));
}
public override void OnMessageReceived(ModbusMessage message)
{
if (message.address == id && message.isFromMaster)
{
if (message.VerifyChecksum(out _))
{
Logger.Log($"(Slave {id}) Received message from master: {message.PrettyPrint()}");
Task.Factory.StartNew(() => HandleMessage(message));
}
else
{
Logger.Warn($"(Slave {id}) Message has invalid checksum, ignoring");
}
}
}
static void Main(string[] args)
{
if (args == null || args.Length == 0)
{
Logger.Error("Command file not specified. Please provide a path to the command file.");
}
ModbusFileParser.LoadFile(args[0]);
ModbusProperties properties = ModbusFileParser.GetProperties();
Bus bus = new Bus(new MasterDevice(0, properties.masterTimeout), properties.corruptionChance, properties.corruptionAttempts);
for (int i = 0; i < properties.slaveCount; i++)
{
bus.Connect(new SlaveDevice((byte)(i + 1), properties.slaveProcessingTime, properties.slaveProcessingTimeJitter));
}
foreach (string[] tokens in ModbusFileParser.GetInstructions())
{
if (tokens.Length >= 2)
{
switch (tokens[0])
{
case "Delay":
Thread.Sleep(int.Parse(tokens[1]));
break;
default:
break;
}
}
else
{
bus.master.SendMessage(ModbusMessage.FromASCII(tokens[0], properties.useChecksum));
}
}
Logger.Log("Executed all instructions");
Console.ReadKey();
}
public override void OnMessageReceived(ModbusMessage message)
{
if (!message.isFromMaster)
{
if (message.VerifyChecksum(out _))
{
Logger.Log($"Received response from slave {message.address}: {message.PrettyPrint()}");
waitingForResponse.TryUpdate(message.address, false, true);
if (message.function >= 0x80)
{
Logger.Warn($"Slave {message.address} threw an exception: {(ModbusExceptionType)message.data[0]}");
}
}
else
{
Logger.Warn("Message has invalid checksum, ignoring");
}
}
}
public static ModbusMessage FromBinary(byte[] input, bool hasChecksum = true)
{
byte[] checksum = input.Skip(input.Length - 2).Take(2).ToArray();
ModbusMessage message = new ModbusMessage
{
address = input[0],
function = input[1],
data = input.Skip(2).Take(input.Length - (hasChecksum ? 4 : 2)).ToArray()
};
if (hasChecksum)
{
message.checksum = (ushort)(checksum[0] << 8 + checksum[1]);
}
else
{
message.RecalculateChecksum();
}
return(message);
}
private void Func_WriteMultipleHoldingRegisters(ModbusMessage message)
{
int firstRegisterAddress = message.data[0] * 256 + message.data[1];
int registersToWrite = message.data[2] * 256 + message.data[3];
byte registerByteCount = message.data[4];
byte[] registerBytes = message.data.Skip(5).Take(registerByteCount).ToArray();
for (int i = 0; i < registersToWrite; i++)
{
holdingRegisters[i + firstRegisterAddress] = (ushort)(registerBytes[i] * 256 + registerBytes[i + 1]);
}
bus.SendMessage(new ModbusMessage
{
address = id,
function = 16,
data = new byte[4] {
(byte)((firstRegisterAddress & 0xFF00) >> 8), (byte)(firstRegisterAddress & 0x00FF), (byte)((registerByteCount & 0xFF00) >> 8), (byte)(registerByteCount & 0x00FF)
}
});
}
private void Func_ReadInputRegisters(ModbusMessage message)
{
int firstRegisterAddress = message.data[0] * 256 + message.data[1];
int registersToRead = message.data[2] * 256 + message.data[3];
ushort[] registers = inputRegisters.Skip(firstRegisterAddress).Take(registersToRead).ToArray();
byte[] registerBytes = new byte[registersToRead * 2];
for (int i = 0; i < registersToRead; i++)
{
registerBytes[i * 2] = (byte)((registers[i] & 0xFF00) >> 8);
registerBytes[i * 2 + 1] = (byte)(registers[i] & 0x00FF);
}
bus.SendMessage(new ModbusMessage
{
address = id,
function = 4,
data = new byte[1] {
(byte)registerBytes.Length
}.Concat(registerBytes).ToArray()
});
}
public abstract void OnMessageReceived(ModbusMessage message);
public virtual void RespondToMessage(ModbusMessage message)
{
throw new NotImplementedException();
}
private void HandleMessage(ModbusMessage message)
{
if (isBusy)
{
bus.SendMessage(ModbusMessage.CreateException(message, ModbusExceptionType.SlaveDeviceBusy));
return;
}
isBusy = true;
Thread.Sleep(Math.Max(processingTime + random.Next(-processingTimeJitter, processingTimeJitter), 0));
try
{
switch (message.function)
{
case 1: // Read coils
Func_ReadCoils(message);
break;
case 2: // Read discrete inputs
Func_ReadDiscreteInputs(message);
break;
case 5: // Write single coil
Func_WriteSingleCoil(message);
break;
case 15: // Write multiple codes
Func_WriteMultipleCoils(message);
break;
case 3: // Read holding registers
Func_ReadHoldingRegisters(message);
break;
case 4: // Read input registers
Func_ReadInputRegisters(message);
break;
case 6: // Write single holding register
Func_WriteHoldingRegister(message);
break;
case 16: // Write multiple holding registers
Func_WriteMultipleHoldingRegisters(message);
break;
default:
RespondToMessage(message);
break;
}
}
catch (NotImplementedException)
{
bus.SendMessage(ModbusMessage.CreateException(message, ModbusExceptionType.IllegalFunction));
}
catch (IndexOutOfRangeException)
{
bus.SendMessage(ModbusMessage.CreateException(message, ModbusExceptionType.IllegalDataAddress));
}
catch (ModbusException e)
{
bus.SendMessage(ModbusMessage.CreateException(message, e.Type));
}
catch (Exception)
{
bus.SendMessage(ModbusMessage.CreateException(message, ModbusExceptionType.SlaveDeviceFailure));
}
isBusy = false;
}
