Innovenergy_trunk/csharp/Lib/Protocols/Modbus/Clients/ModbusRtuClient.cs

using System.Diagnostics;
using System.Runtime.CompilerServices;
using InnovEnergy.Lib.Protocols.Modbus.Connections;
using InnovEnergy.Lib.Protocols.Modbus.Conversions;
using InnovEnergy.Lib.Protocols.Modbus.Protocol;
using InnovEnergy.Lib.Protocols.Modbus.Protocol.Frames;
using InnovEnergy.Lib.Protocols.Modbus.Protocol.Frames.Commands;
using InnovEnergy.Lib.Protocols.Modbus.Protocol.Frames.Replies;
using InnovEnergy.Lib.Protocols.Modbus.Util;
using InnovEnergy.Lib.Utils;
using Bytes = System.Collections.Generic.IReadOnlyList<System.Byte>;
using static System.Runtime.CompilerServices.MethodImplOptions;

namespace InnovEnergy.Lib.Protocols.Modbus.Clients;

using UInt16s = IReadOnlyList<UInt16>;

public class ModbusRtuClient : ModbusClient
{
    private const Int32 CrcSize = 2;
   
    public ModbusRtuClient(ModbusConnection connection, Byte slaveId) : base(connection, slaveId)
    {
    }

    public override IReadOnlyList<Boolean> ReadDiscreteInputs(UInt16 readAddress, UInt16 nValues)
    {
        throw new NotImplementedException();
    }

    public override ModbusRegisters ReadInputRegisters(UInt16 readAddress, UInt16 nValues)
    {
        var wireReadAddress = LogicToWire(readAddress);
        
        var cmd = new ReadInputRegistersCommandFrame(SlaveId, wireReadAddress, nValues);
        var crc = CalcCrc(cmd);

        // TX
        cmd.Data.Concat(crc).Apply(Connection.Transmit);

        var nToRead = cmd.ExpectedResponseSize + CrcSize;

        // RX
        var response = nToRead
                      .ConvertTo<UInt16>()
                      .Apply(Connection.Receive)
                      .ToArraySegment()
                      .Apply(AssertCrc)
                      .SkipLast(CrcSize)
                      .Apply(ReadInputRegistersResponseFrame.Parse)
                      .Apply(cmd.VerifyResponse);

        return new ModbusRegisters(readAddress, response.RegistersRead.ToArray());
    }


    public override ModbusRegisters ReadHoldingRegisters(UInt16 readAddress, UInt16 nValues)
    {
        var wireReadAddress = LogicToWire(readAddress);
        
        var cmd = new ReadHoldingRegistersCommandFrame(SlaveId, wireReadAddress, nValues);
        var crc = CalcCrc(cmd.Data);

        // TX
        cmd.Data.Concat(crc).Apply(Connection.Transmit);

        // RX
        var nToRead = cmd.ExpectedResponseSize + CrcSize;
        var response = nToRead
                      .ConvertTo<UInt16>()
                      .Apply(Connection.Receive)
                      .ToArraySegment()
                      .Apply(AssertCrc)
                      .SkipLast(CrcSize)
                      .Apply(ReadHoldingRegistersResponseFrame.Parse)
                      .Apply(cmd.VerifyResponse);

        return new ModbusRegisters(readAddress, response.RegistersRead.ToArray());
    }

    public override UInt16 WriteMultipleCoils(UInt16 writeAddress, IReadOnlyList<Boolean> coils)
    {
        throw new NotImplementedException();
    }


    public override UInt16 WriteRegisters(UInt16 writeAddress, UInt16s values)
    {
        var wireWriteAddress = LogicToWire(writeAddress);
        
        var cmd = new WriteRegistersCommandFrame(SlaveId, wireWriteAddress, values);
        var crc = CalcCrc(cmd);
        var nToRead = cmd.ExpectedResponseSize + CrcSize;

        // TX        
        cmd.Data.Concat(crc).Apply(Connection.Transmit);
        

        // RX
        var response = nToRead
                      .ConvertTo<UInt16>()
                      .Apply(Connection.Receive)
                      .ToArraySegment()
                      .Apply(AssertCrc)
                      .SkipLast(CrcSize)
                      .Apply(WriteRegistersResponseFrame.Parse)
                      .Apply(cmd.VerifyResponse);

        return response.NbWritten;
    }

    public override ModbusRegisters ReadWriteRegisters(UInt16 readAddress, UInt16 nbToRead, UInt16 writeAddress, UInt16s registersToWrite)
    {
        var wireReadAddress = LogicToWire(readAddress);
        var wireWriteAddress = LogicToWire(writeAddress);
        
        var cmd = new ReadWriteRegistersCommandFrame(SlaveId, 
                                                     wireReadAddress,
                                                     nbToRead, 
                                                     wireWriteAddress, 
                                                     registersToWrite);
        var crc = CalcCrc(cmd);

        // TX
        cmd.Data.Concat(crc).Apply(Connection.Transmit); 

        // RX
        var nToRead = cmd.ExpectedResponseSize + CrcSize;
        var response = nToRead
                      .ConvertTo<UInt16>()
                      .Apply(Connection.Receive)
                      .ToArraySegment()
                      .SkipLast(CrcSize)
                      .Apply(ReadWriteRegistersResponseFrame.Parse)
                      .Apply(cmd.VerifyResponse);

        return new ModbusRegisters(readAddress, response.RegistersRead.ToArray());
    }

    public static ArraySegment<Byte> AssertCrc(ArraySegment<Byte> data)
    {
        var expectedCrc = data.SkipLast(CrcSize).Apply(CalcCrc);
        var actualCrc = data.TakeLast(CrcSize);

        if (!actualCrc.SequenceEqual(expectedCrc))
            throw new CrcException(expectedCrc, actualCrc);

        return data;
    }


    // not used ATM, see AssertCrc
    public static Boolean CheckCrc(Bytes data)
    {
        var expectedCrc = data.SkipLast(CrcSize).Apply(CalcCrc);
        var actualCrc = data.TakeLast(CrcSize);

        return actualCrc.SequenceEqual(expectedCrc);
    }

    [DebuggerStepThrough][MethodImpl(AggressiveInlining)]
    private static Bytes CalcCrc(ModbusFrame frame) => CalcCrc(frame.Data);

    [DebuggerStepThrough][MethodImpl(AggressiveInlining)]
    private static Bytes CalcCrc(ArraySegment<Byte> data) => CalcCrc((IEnumerable<Byte>)data);

    private static Bytes CalcCrc(IEnumerable<Byte> data)
    {
        UInt16 crc = 0xFFFF;

        foreach (var b in data)
        {
            crc ^= b;

            for (var bit = 0; bit < 8; bit++)
            {
                var bit0 = (crc & 0x0001) != 0;
                crc >>= 1;
                if (bit0) crc ^= 0xA001;
            }
        }

        var hi = 0xFF & crc;
        var lo = (crc >> 8) & 0xFF;

        return new[] { (Byte)hi, (Byte)lo }; // big endian
    }
}