wirenboard / wb-mqtt-serial / 6

Committed 13 Jan 2026 06:31AM UTC coverage: 76.817% (+4.0%) from 72.836%

Build # 6

Build Type

Pull #1049

github

Specific Base 8b2ffc

Committed by

Ilia1S

Commit Message

Remove fw from groups

Pull Request Pull Request #1049: WB-MR templates: Add delays

Run Details

6873 of 9161 branches covered (75.02%)

12966 of 16879 relevant lines covered (76.82%)

830.18 hits per line

Source File
Press 'n' to go to next uncovered line, 'b' for previous

87.3

/src/modbus_base.cpp

#include "modbus_base.h"

#include "bin_utils.h"
#include "crc16.h"
#include "serial_exc.h"

using namespace std;
using namespace BinUtils;

namespace
{
    const size_t CRC_SIZE = 2;

    std::string GetModbusExceptionMessage(uint8_t code)
    {
        if (code == 0) {
            return std::string();
        }
        // clang-format off
        const char* errs[] =
            { "illegal function",                         // 0x01
              "illegal data address",                     // 0x02
              "illegal data value",                       // 0x03
              "server device failure",                    // 0x04
              "long operation (acknowledge)",             // 0x05
              "server device is busy",                    // 0x06
              "",                                         // 0x07
              "memory parity error",                      // 0x08
              "",                                         // 0x09
              "gateway path is unavailable",              // 0x0A
              "gateway target device failed to respond"   // 0x0B
            };
        // clang-format on
        --code;
        if (code < sizeof(errs) / sizeof(char*)) {
            return errs[code];
        }
        return "invalid modbus error code (" + std::to_string(code + 1) + ")";
    }

    // throws C++ exception on modbus error code
    void ThrowIfModbusException(uint8_t code)
    {
        if (code == 0) {
            return;
        }
        throw Modbus::TModbusExceptionError(code);
    }

    bool IsWriteFunction(Modbus::EFunction function)
    {
        return function == Modbus::EFunction::FN_WRITE_MULTIPLE_COILS ||
               function == Modbus::EFunction::FN_WRITE_MULTIPLE_REGISTERS ||
               function == Modbus::EFunction::FN_WRITE_SINGLE_COIL ||
               function == Modbus::EFunction::FN_WRITE_SINGLE_REGISTER;
    }

    bool IsReadFunction(Modbus::EFunction function)
    {
        return function == Modbus::EFunction::FN_READ_COILS || function == Modbus::EFunction::FN_READ_DISCRETE ||
               function == Modbus::EFunction::FN_READ_HOLDING || function == Modbus::EFunction::FN_READ_INPUT;
    }

    bool IsReadWriteFunction(Modbus::EFunction function)
    {
        return function == Modbus::EFunction::FN_READ_WRITE_MULTIPLE_REGISTERS;
    }

    bool IsSingleBitFunction(Modbus::EFunction function)
    {
        return function == Modbus::EFunction::FN_READ_COILS || function == Modbus::EFunction::FN_READ_DISCRETE ||
               function == Modbus::EFunction::FN_WRITE_SINGLE_COIL ||
               function == Modbus::EFunction::FN_WRITE_MULTIPLE_COILS;
    }

    uint16_t GetCoilsByteSize(uint16_t count)
    {
        return count / 8 + (count % 8 != 0 ? 1 : 0);
    }

    std::vector<uint8_t> MakeReadRequestPDU(Modbus::EFunction function, uint16_t address, uint16_t count)
    {
        std::vector<uint8_t> res(5);
        res[0] = function;
        WriteAs2Bytes(res.data() + 1, address);
        WriteAs2Bytes(res.data() + 3, count);
        return res;
    }

    std::vector<uint8_t> MakeWriteSingleRequestPDU(Modbus::EFunction function,
                                                   uint16_t address,
                                                   const std::vector<uint8_t>& data)
    {
        if (data.size() != 2) {
            throw Modbus::TMalformedRequestError("data size " + std::to_string(data.size()) +
                                                 " doesn't match function code " + std::to_string(function));
        }
        std::vector<uint8_t> res(5);
        res[0] = function;
        WriteAs2Bytes(res.data() + 1, address);
        res[3] = data[0];
        res[4] = data[1];
        return res;
    }

    std::vector<uint8_t> MakeWriteMultipleRequestPDU(Modbus::EFunction function,
                                                     uint16_t address,
                                                     uint16_t count,
                                                     const std::vector<uint8_t>& data)
    {
        if ((function == Modbus::EFunction::FN_WRITE_MULTIPLE_COILS && data.size() != GetCoilsByteSize(count)) ||
            (function == Modbus::EFunction::FN_WRITE_MULTIPLE_REGISTERS && data.size() != count * 2))
        {
            throw Modbus::TMalformedRequestError("data size " + std::to_string(data.size()) +
                                                 " doesn't match function code " + std::to_string(function));
        }
        std::vector<uint8_t> res(6 + data.size());
        res[0] = function;
        WriteAs2Bytes(res.data() + 1, address);
        WriteAs2Bytes(res.data() + 3, count);
        res[5] = data.size();
        std::copy(data.begin(), data.end(), res.begin() + 6);
        return res;
    }

    // get actual function code even if exception
    uint8_t GetFunctionCode(uint8_t functionCodeByte)
    {
        return functionCodeByte & 127;
    }

    Modbus::EFunction GetFunction(uint8_t functionCode)
    {
        if (Modbus::IsSupportedFunction(functionCode)) {
            return static_cast<Modbus::EFunction>(functionCode);
        }
        throw Modbus::TUnexpectedResponseError("unknown modbus function code: " + to_string(functionCode));
    }
}

Modbus::IModbusTraits::IModbusTraits(bool forceFrameTimeout): ForceFrameTimeout(forceFrameTimeout)
{}

bool Modbus::IModbusTraits::GetForceFrameTimeout()
{
    return ForceFrameTimeout;
}

// TModbusRTUTraits

Modbus::TModbusRTUTraits::TModbusRTUTraits(bool forceFrameTimeout): IModbusTraits(forceFrameTimeout)
{}

TPort::TFrameCompletePred Modbus::TModbusRTUTraits::ExpectNBytes(size_t n) const
{
    return [=](uint8_t* buf, size_t size) {
        if (size < 2)
            return false;
        if (IsException(buf[1])) // GetPDU
            return size >= EXCEPTION_RESPONSE_PDU_SIZE + DATA_SIZE;
        return size >= n;
    };
}

size_t Modbus::TModbusRTUTraits::GetPacketSize(size_t pduSize) const
{
    return DATA_SIZE + pduSize;
}

void Modbus::TModbusRTUTraits::FinalizeRequest(std::vector<uint8_t>& request, uint8_t slaveId)
{
    request[0] = slaveId;
    WriteAs2Bytes(&request[request.size() - 2], CRC16::CalculateCRC16(request.data(), request.size() - 2));
}

TReadFrameResult Modbus::TModbusRTUTraits::ReadFrame(TPort& port,
                                                     uint8_t slaveId,
                                                     const std::chrono::milliseconds& responseTimeout,
                                                     const std::chrono::milliseconds& frameTimeout,
                                                     std::vector<uint8_t>& response,
                                                     bool matchSlaveId) const
{
    auto rc =
        port.ReadFrame(response.data(), response.size(), responseTimeout, frameTimeout, ExpectNBytes(response.size()));
    // RTU response should be at least 3 bytes: 1 byte slave_id, 2 bytes CRC
    if (rc.Count < DATA_SIZE) {
        throw Modbus::TMalformedResponseError("invalid data size");
    }

    uint16_t crc = (response[rc.Count - 2] << 8) + response[rc.Count - 1];
    if (crc != CRC16::CalculateCRC16(response.data(), rc.Count - 2)) {
        throw Modbus::TMalformedResponseError("invalid crc");
    }

    if (ForceFrameTimeout) {
        std::array<uint8_t, 256> buf;
        try {
            port.ReadFrame(buf.data(), buf.size(), frameTimeout, frameTimeout);
        } catch (const TResponseTimeoutException& e) {
            // No extra data
        }
    }

    if (matchSlaveId && slaveId != response[0]) {
        throw Modbus::TUnexpectedResponseError("request and response slave id mismatch");
    }
    return rc;
}

Modbus::TReadResult Modbus::TModbusRTUTraits::Transaction(TPort& port,
                                                          uint8_t slaveId,
                                                          const std::vector<uint8_t>& requestPdu,
                                                          size_t expectedResponsePduSize,
                                                          const std::chrono::milliseconds& responseTimeout,
                                                          const std::chrono::milliseconds& frameTimeout,
                                                          bool matchSlaveId)
{
    std::vector<uint8_t> request(GetPacketSize(requestPdu.size()));
    std::copy(requestPdu.begin(), requestPdu.end(), request.begin() + 1);
    FinalizeRequest(request, slaveId);

    port.WriteBytes(request.data(), request.size());

    std::vector<uint8_t> response(GetPacketSize(expectedResponsePduSize));

    auto readRes = ReadFrame(port, slaveId, responseTimeout, frameTimeout, response, matchSlaveId);

    TReadResult res;
    res.ResponseTime = readRes.ResponseTime;
    res.SlaveId = slaveId;
    res.Pdu.assign(response.begin() + 1, response.begin() + (readRes.Count - CRC_SIZE));
    return res;
}

// TModbusTCPTraits

std::mutex Modbus::TModbusTCPTraits::TransactionIdMutex;
std::unordered_map<std::string, uint16_t> Modbus::TModbusTCPTraits::TransactionIds;

uint16_t Modbus::TModbusTCPTraits::GetTransactionId(TPort& port)
{
    auto portDescription = port.GetDescription(false);
    std::unique_lock lk(TransactionIdMutex);
    try {
        return ++TransactionIds.at(portDescription);
    } catch (const std::out_of_range&) {
        TransactionIds.emplace(portDescription, 1);
        return 1;
    }
}

void Modbus::TModbusTCPTraits::ResetTransactionId(TPort& port)
{
    auto portDescription = port.GetDescription(false);
    std::unique_lock lk(TransactionIdMutex);
    TransactionIds.erase(portDescription);
}

Modbus::TModbusTCPTraits::TModbusTCPTraits()
{}

void Modbus::TModbusTCPTraits::SetMBAP(std::vector<uint8_t>& req,
                                       uint16_t transactionId,
                                       size_t pduSize,
                                       uint8_t slaveId) const
{
    req[0] = ((transactionId >> 8) & 0xFF);
    req[1] = (transactionId & 0xFF);
    req[2] = 0; // MODBUS
    req[3] = 0;
    ++pduSize; // length includes additional byte of unit identifier
    req[4] = ((pduSize >> 8) & 0xFF);
    req[5] = (pduSize & 0xFF);
    req[6] = slaveId;
}

uint16_t Modbus::TModbusTCPTraits::GetLengthFromMBAP(const std::vector<uint8_t>& buf) const
{
    uint16_t l = buf[4];
    l <<= 8;
    l += buf[5];
    return l;
}

size_t Modbus::TModbusTCPTraits::GetPacketSize(size_t pduSize) const
{
    return MBAP_SIZE + pduSize;
}

void Modbus::TModbusTCPTraits::FinalizeRequest(std::vector<uint8_t>& request, uint8_t slaveId, uint16_t transactionId)
{
    SetMBAP(request, transactionId, request.size() - MBAP_SIZE, slaveId);
}

TReadFrameResult Modbus::TModbusTCPTraits::ReadFrame(TPort& port,
                                                     uint8_t slaveId,
                                                     uint16_t transactionId,
                                                     const std::chrono::milliseconds& responseTimeout,
                                                     const std::chrono::milliseconds& frameTimeout,
                                                     std::vector<uint8_t>& response,
                                                     bool matchSlaveId) const
{
    auto startTime = chrono::steady_clock::now();
    // Timeout for reading packet with expected transaction ID
    auto packetTimeout = port.CalcResponseTimeout(responseTimeout + frameTimeout);
    while (chrono::duration_cast<chrono::microseconds>(chrono::steady_clock::now() - startTime) < packetTimeout) {
        if (response.size() < MBAP_SIZE) {
            response.resize(MBAP_SIZE);
        }
        auto rc = port.ReadFrame(response.data(), MBAP_SIZE, responseTimeout, frameTimeout);

        if (rc.Count < MBAP_SIZE) {
            throw Modbus::TMalformedResponseError("Can't read full MBAP");
        }

        auto len = GetLengthFromMBAP(response);
        // MBAP length should be at least 1 byte for unit identifier
        if (len == 0) {
            throw Modbus::TMalformedResponseError("Wrong MBAP length value: 0");
        }
        --len; // length includes one byte of unit identifier which is already in buffer

        if (len + MBAP_SIZE > response.size()) {
            response.resize(len + MBAP_SIZE);
        }

        rc = port.ReadFrame(response.data() + MBAP_SIZE, len, frameTimeout, frameTimeout);
        if (rc.Count != len) {
            throw Modbus::TMalformedResponseError("Wrong PDU size: " + to_string(rc.Count) + ", expected " +
                                                  to_string(len));
        }
        rc.Count += MBAP_SIZE;

        // check transaction id
        if (((transactionId >> 8) & 0xFF) == response[0] && (transactionId & 0xFF) == response[1]) {
            // check unit identifier
            if (matchSlaveId && slaveId != response[6]) {
                throw Modbus::TUnexpectedResponseError("request and response unit identifier mismatch");
            }
            return rc;
        }
    }
    throw TResponseTimeoutException();
}

Modbus::TReadResult Modbus::TModbusTCPTraits::Transaction(TPort& port,
                                                          uint8_t slaveId,
                                                          const std::vector<uint8_t>& requestPdu,
                                                          size_t expectedResponsePduSize,
                                                          const std::chrono::milliseconds& responseTimeout,
                                                          const std::chrono::milliseconds& frameTimeout,
                                                          bool matchSlaveId)
{
    auto transactionId = GetTransactionId(port);
    std::vector<uint8_t> request(GetPacketSize(requestPdu.size()));
    std::copy(requestPdu.begin(), requestPdu.end(), request.begin() + MBAP_SIZE);
    FinalizeRequest(request, slaveId, transactionId);

    port.WriteBytes(request.data(), request.size());

    std::vector<uint8_t> response(GetPacketSize(expectedResponsePduSize));

    auto readRes = ReadFrame(port, slaveId, transactionId, responseTimeout, frameTimeout, response, matchSlaveId);

    TReadResult res;
    res.ResponseTime = readRes.ResponseTime;
    res.SlaveId = response[6];
    res.Pdu.assign(response.begin() + MBAP_SIZE, response.begin() + readRes.Count);
    return res;
}

std::unique_ptr<Modbus::IModbusTraits> Modbus::TModbusRTUTraitsFactory::GetModbusTraits(bool forceFrameTimeout)
{
    return std::make_unique<Modbus::TModbusRTUTraits>(forceFrameTimeout);
}

std::unique_ptr<Modbus::IModbusTraits> Modbus::TModbusTCPTraitsFactory::GetModbusTraits(bool forceFrameTimeout)
{
    return std::make_unique<Modbus::TModbusTCPTraits>();
}

bool Modbus::IsException(uint8_t functionCode) noexcept
{
    return functionCode & EXCEPTION_BIT;
}

Modbus::TErrorBase::TErrorBase(const std::string& what): std::runtime_error(what)
{}

Modbus::TMalformedResponseError::TMalformedResponseError(const std::string& what)
    : Modbus::TErrorBase("malformed response: " + what)
{}

Modbus::TMalformedRequestError::TMalformedRequestError(const std::string& what)
    : Modbus::TErrorBase("malformed request: " + what)
{}

Modbus::TUnexpectedResponseError::TUnexpectedResponseError(const std::string& what): Modbus::TErrorBase(what)
{}

size_t Modbus::CalcResponsePDUSize(Modbus::EFunction function, size_t registerCount)
{
    if (IsWriteFunction(function)) {
        return WRITE_RESPONSE_PDU_SIZE;
    }
    if (IsSingleBitFunction(function)) {
        return 2 + GetCoilsByteSize(registerCount);
    }
    return 2 + registerCount * 2;
}

std::vector<uint8_t> Modbus::ExtractResponseData(EFunction requestFunction, const std::vector<uint8_t>& pdu)
{
    if (pdu.size() < 2) {
        throw Modbus::TMalformedResponseError("PDU is too small");
    }

    auto functionCode = GetFunctionCode(pdu[0]);
    if (requestFunction != functionCode) {
        throw Modbus::TUnexpectedResponseError("request and response function code mismatch");
    }

    if (IsException(pdu[0])) {
        ThrowIfModbusException(pdu[1]);
    }

    auto function = GetFunction(functionCode);
    if (IsReadFunction(function) || IsReadWriteFunction(function)) {
        size_t byteCount = pdu[1];
        if (pdu.size() != byteCount + 2) {
            throw Modbus::TMalformedResponseError("invalid read response byte count: " + std::to_string(byteCount) +
                                                  ", got " + std::to_string(pdu.size() - 2));
        }
        return std::vector<uint8_t>(pdu.begin() + 2, pdu.end());
    }

    if (IsWriteFunction(function)) {
        if (WRITE_RESPONSE_PDU_SIZE != pdu.size()) {
            throw Modbus::TMalformedResponseError("invalid write response PDU size: " + std::to_string(pdu.size()) +
                                                  ", expected " + std::to_string(WRITE_RESPONSE_PDU_SIZE));
        }
    }

    return std::vector<uint8_t>();
}

bool Modbus::IsSupportedFunction(uint8_t functionCode) noexcept
{
    auto function = static_cast<Modbus::EFunction>(functionCode);
    return IsReadFunction(function) || IsWriteFunction(function) || IsReadWriteFunction(function);
}

std::vector<uint8_t> Modbus::MakePDU(Modbus::EFunction function,
                                     uint16_t address,
                                     uint16_t count,
                                     const std::vector<uint8_t>& data)
{
    if (IsReadFunction(function)) {
        return MakeReadRequestPDU(function, address, count);
    }

    if (function == Modbus::EFunction::FN_WRITE_SINGLE_COIL || function == Modbus::EFunction::FN_WRITE_SINGLE_REGISTER)
    {
        return MakeWriteSingleRequestPDU(function, address, data);
    }

    if (function == Modbus::EFunction::FN_WRITE_MULTIPLE_REGISTERS ||
        function == Modbus::EFunction::FN_WRITE_MULTIPLE_COILS)
    {
        return MakeWriteMultipleRequestPDU(function, address, count, data);
    }

    return std::vector<uint8_t>();
}

std::vector<uint8_t> Modbus::MakePDU(Modbus::EFunction function,
                                     uint16_t address,
                                     uint16_t count,
                                     uint16_t writeAddress,
                                     uint16_t writeCount,
                                     const std::vector<uint8_t>& data)
{
    if (IsReadWriteFunction(function)) {
        if (data.size() != writeCount * 2) {
            throw Modbus::TMalformedRequestError("data size " + std::to_string(data.size()) +
                                                 " doesn't match function code 23");
        }
        std::vector<uint8_t> res(10 + data.size());
        res[0] = Modbus::EFunction::FN_READ_WRITE_MULTIPLE_REGISTERS;
        WriteAs2Bytes(res.data() + 1, address);
        WriteAs2Bytes(res.data() + 3, count);
        WriteAs2Bytes(res.data() + 5, writeAddress);
        WriteAs2Bytes(res.data() + 7, writeCount);
        res[9] = data.size();
        std::copy(data.begin(), data.end(), res.begin() + 10);
        return res;
    }

    return MakePDU(function, address, count, data);
}

Modbus::TModbusExceptionError::TModbusExceptionError(uint8_t exceptionCode)
    : Modbus::TErrorBase(GetModbusExceptionMessage(exceptionCode)),
      ExceptionCode(exceptionCode)
{}

uint8_t Modbus::TModbusExceptionError::GetExceptionCode() const
{
    return ExceptionCode;
}

1	#include "modbus_base.h"
2
3	#include "bin_utils.h"
4	#include "crc16.h"
5	#include "serial_exc.h"
6
7	using namespace std;
8	using namespace BinUtils;
9
10	namespace
11	{
12	const size_t CRC_SIZE = 2;
13
14	std::string GetModbusExceptionMessage(uint8_t code)	33✔
15	{
16	if (code == 0) {	33✔
17	return std::string();	×
18	}
19	// clang-format off
20	const char* errs[] =	33✔
21	{ "illegal function", // 0x01
22	"illegal data address", // 0x02
23	"illegal data value", // 0x03
24	"server device failure", // 0x04
25	"long operation (acknowledge)", // 0x05
26	"server device is busy", // 0x06
27	"", // 0x07
28	"memory parity error", // 0x08
29	"", // 0x09
30	"gateway path is unavailable", // 0x0A
31	"gateway target device failed to respond" // 0x0B
32	};
33	// clang-format on
34	--code;	33✔
35	if (code < sizeof(errs) / sizeof(char*)) {	33✔
36	return errs[code];	32✔
37	}
38	return "invalid modbus error code (" + std::to_string(code + 1) + ")";	2✔
39	}
40
41	// throws C++ exception on modbus error code
42	void ThrowIfModbusException(uint8_t code)	32✔
43	{
44	if (code == 0) {	32✔
45	return;	×
46	}
47	throw Modbus::TModbusExceptionError(code);	32✔
48	}
49
50	bool IsWriteFunction(Modbus::EFunction function)	744✔
51	{
52	return function == Modbus::EFunction::FN_WRITE_MULTIPLE_COILS \|\|	744✔
53	function == Modbus::EFunction::FN_WRITE_MULTIPLE_REGISTERS \|\|	675✔
54	function == Modbus::EFunction::FN_WRITE_SINGLE_COIL \|\|	1,488✔
55	function == Modbus::EFunction::FN_WRITE_SINGLE_REGISTER;	744✔
56	}
57
58	bool IsReadFunction(Modbus::EFunction function)	1,632✔
59	{
60	return function == Modbus::EFunction::FN_READ_COILS \|\| function == Modbus::EFunction::FN_READ_DISCRETE \|\|	1,478✔
61	function == Modbus::EFunction::FN_READ_HOLDING \|\| function == Modbus::EFunction::FN_READ_INPUT;	3,110✔
62	}
63
64	bool IsReadWriteFunction(Modbus::EFunction function)	79✔
65	{
66	return function == Modbus::EFunction::FN_READ_WRITE_MULTIPLE_REGISTERS;	79✔
67	}
68
69	bool IsSingleBitFunction(Modbus::EFunction function)	498✔
70	{
71	return function == Modbus::EFunction::FN_READ_COILS \|\| function == Modbus::EFunction::FN_READ_DISCRETE \|\|	442✔
72	function == Modbus::EFunction::FN_WRITE_SINGLE_COIL \|\|	940✔
73	function == Modbus::EFunction::FN_WRITE_MULTIPLE_COILS;	498✔
74	}
75
76	uint16_t GetCoilsByteSize(uint16_t count)	69✔
77	{
78	return count / 8 + (count % 8 != 0 ? 1 : 0);	69✔
79	}
80
81	std::vector<uint8_t> MakeReadRequestPDU(Modbus::EFunction function, uint16_t address, uint16_t count)	498✔
82	{
83	std::vector<uint8_t> res(5);	498✔
84	res[0] = function;	498✔
85	WriteAs2Bytes(res.data() + 1, address);	498✔
86	WriteAs2Bytes(res.data() + 3, count);	498✔
87	return res;	498✔
88	}
89
90	std::vector<uint8_t> MakeWriteSingleRequestPDU(Modbus::EFunction function,	71✔
91	uint16_t address,
92	const std::vector<uint8_t>& data)
93	{
94	if (data.size() != 2) {	71✔
95	throw Modbus::TMalformedRequestError("data size " + std::to_string(data.size()) +	×
96	" doesn't match function code " + std::to_string(function));	×
97	}
98	std::vector<uint8_t> res(5);	71✔
99	res[0] = function;	71✔
100	WriteAs2Bytes(res.data() + 1, address);	71✔
101	res[3] = data[0];	71✔
102	res[4] = data[1];	71✔
103	return res;	71✔
104	}
105
106	std::vector<uint8_t> MakeWriteMultipleRequestPDU(Modbus::EFunction function,	23✔
107	uint16_t address,
108	uint16_t count,
109	const std::vector<uint8_t>& data)
110	{
111	if ((function == Modbus::EFunction::FN_WRITE_MULTIPLE_COILS && data.size() != GetCoilsByteSize(count)) \|\|	46✔
112	(function == Modbus::EFunction::FN_WRITE_MULTIPLE_REGISTERS && data.size() != count * 2))	23✔
113	{
114	throw Modbus::TMalformedRequestError("data size " + std::to_string(data.size()) +	×
115	" doesn't match function code " + std::to_string(function));	×
116	}
117	std::vector<uint8_t> res(6 + data.size());	23✔
118	res[0] = function;	23✔
119	WriteAs2Bytes(res.data() + 1, address);	23✔
120	WriteAs2Bytes(res.data() + 3, count);	23✔
121	res[5] = data.size();	23✔
122	std::copy(data.begin(), data.end(), res.begin() + 6);	23✔
123	return res;	23✔
124	}
125
126	// get actual function code even if exception
127	uint8_t GetFunctionCode(uint8_t functionCodeByte)	556✔
128	{
129	return functionCodeByte & 127;	556✔
130	}
131
132	Modbus::EFunction GetFunction(uint8_t functionCode)	520✔
133	{
134	if (Modbus::IsSupportedFunction(functionCode)) {	520✔
135	return static_cast<Modbus::EFunction>(functionCode);	520✔
136	}
137	throw Modbus::TUnexpectedResponseError("unknown modbus function code: " + to_string(functionCode));	×
138	}
139	}
140
141	Modbus::IModbusTraits::IModbusTraits(bool forceFrameTimeout): ForceFrameTimeout(forceFrameTimeout)	290✔
142	{}	290✔
143
144	bool Modbus::IModbusTraits::GetForceFrameTimeout()	1,001✔
145	{
146	return ForceFrameTimeout;	1,001✔
147	}
148
149	// TModbusRTUTraits
150
151	Modbus::TModbusRTUTraits::TModbusRTUTraits(bool forceFrameTimeout): IModbusTraits(forceFrameTimeout)	188✔
152	{}	188✔
153
154	TPort::TFrameCompletePred Modbus::TModbusRTUTraits::ExpectNBytes(size_t n) const	611✔
155	{
156	return [=](uint8_t* buf, size_t size) {	4,676✔
157	if (size < 2)	4,676✔
158	return false;	573✔
159	if (IsException(buf[1])) // GetPDU	4,103✔
160	return size >= EXCEPTION_RESPONSE_PDU_SIZE + DATA_SIZE;	136✔
161	return size >= n;	3,967✔
162	};	611✔
163	}
164
165	size_t Modbus::TModbusRTUTraits::GetPacketSize(size_t pduSize) const	1,228✔
166	{
167	return DATA_SIZE + pduSize;	1,228✔
168	}
169
170	void Modbus::TModbusRTUTraits::FinalizeRequest(std::vector<uint8_t>& request, uint8_t slaveId)	617✔
171	{
172	request[0] = slaveId;	617✔
173	WriteAs2Bytes(&request[request.size() - 2], CRC16::CalculateCRC16(request.data(), request.size() - 2));	617✔
174	}	617✔
175
176	TReadFrameResult Modbus::TModbusRTUTraits::ReadFrame(TPort& port,	611✔
177	uint8_t slaveId,
178	const std::chrono::milliseconds& responseTimeout,
179	const std::chrono::milliseconds& frameTimeout,
180	std::vector<uint8_t>& response,
181	bool matchSlaveId) const
182	{
183	auto rc =
184	port.ReadFrame(response.data(), response.size(), responseTimeout, frameTimeout, ExpectNBytes(response.size()));	638✔
185	// RTU response should be at least 3 bytes: 1 byte slave_id, 2 bytes CRC
186	if (rc.Count < DATA_SIZE) {	584✔
187	throw Modbus::TMalformedResponseError("invalid data size");	×
188	}
189
190	uint16_t crc = (response[rc.Count - 2] << 8) + response[rc.Count - 1];	584✔
191	if (crc != CRC16::CalculateCRC16(response.data(), rc.Count - 2)) {	584✔
192	throw Modbus::TMalformedResponseError("invalid crc");	3✔
193	}
194
195	if (ForceFrameTimeout) {	581✔
196	std::array<uint8_t, 256> buf;
197	try {
198	port.ReadFrame(buf.data(), buf.size(), frameTimeout, frameTimeout);	3✔
199	} catch (const TResponseTimeoutException& e) {	1✔
200	// No extra data
201	}
202	}
203
204	if (matchSlaveId && slaveId != response[0]) {	581✔
205	throw Modbus::TUnexpectedResponseError("request and response slave id mismatch");	3✔
206	}
207	return rc;	578✔
208	}
209
210	Modbus::TReadResult Modbus::TModbusRTUTraits::Transaction(TPort& port,	617✔
211	uint8_t slaveId,
212	const std::vector<uint8_t>& requestPdu,
213	size_t expectedResponsePduSize,
214	const std::chrono::milliseconds& responseTimeout,
215	const std::chrono::milliseconds& frameTimeout,
216	bool matchSlaveId)
217	{
218	std::vector<uint8_t> request(GetPacketSize(requestPdu.size()));	1,234✔
219	std::copy(requestPdu.begin(), requestPdu.end(), request.begin() + 1);	617✔
220	FinalizeRequest(request, slaveId);	617✔
221
222	port.WriteBytes(request.data(), request.size());	617✔
223
224	std::vector<uint8_t> response(GetPacketSize(expectedResponsePduSize));	1,222✔
225
226	auto readRes = ReadFrame(port, slaveId, responseTimeout, frameTimeout, response, matchSlaveId);	611✔
227
228	TReadResult res;	578✔
229	res.ResponseTime = readRes.ResponseTime;	578✔
230	res.SlaveId = slaveId;	578✔
231	res.Pdu.assign(response.begin() + 1, response.begin() + (readRes.Count - CRC_SIZE));	578✔
232	return res;	1,156✔
233	}
234
235	// TModbusTCPTraits
236
237	std::mutex Modbus::TModbusTCPTraits::TransactionIdMutex;
238	std::unordered_map<std::string, uint16_t> Modbus::TModbusTCPTraits::TransactionIds;
239
240	uint16_t Modbus::TModbusTCPTraits::GetTransactionId(TPort& port)	9✔
241	{
242	auto portDescription = port.GetDescription(false);	18✔
243	std::unique_lock lk(TransactionIdMutex);	18✔
244	try {
245	return ++TransactionIds.at(portDescription);	9✔
246	} catch (const std::out_of_range&) {	8✔
247	TransactionIds.emplace(portDescription, 1);	8✔
248	return 1;	8✔
249	}
250	}
251
252	void Modbus::TModbusTCPTraits::ResetTransactionId(TPort& port)	8✔
253	{
254	auto portDescription = port.GetDescription(false);	16✔
255	std::unique_lock lk(TransactionIdMutex);	16✔
256	TransactionIds.erase(portDescription);	8✔
257	}	8✔
258
259	Modbus::TModbusTCPTraits::TModbusTCPTraits()	9✔
260	{}	9✔
261
262	void Modbus::TModbusTCPTraits::SetMBAP(std::vector<uint8_t>& req,	9✔
263	uint16_t transactionId,
264	size_t pduSize,
265	uint8_t slaveId) const
266	{
267	req[0] = ((transactionId >> 8) & 0xFF);	9✔
268	req[1] = (transactionId & 0xFF);	9✔
269	req[2] = 0; // MODBUS	9✔
270	req[3] = 0;	9✔
271	++pduSize; // length includes additional byte of unit identifier	9✔
272	req[4] = ((pduSize >> 8) & 0xFF);	9✔
273	req[5] = (pduSize & 0xFF);	9✔
274	req[6] = slaveId;	9✔
275	}	9✔
276
277	uint16_t Modbus::TModbusTCPTraits::GetLengthFromMBAP(const std::vector<uint8_t>& buf) const	11✔
278	{
279	uint16_t l = buf[4];	11✔
280	l <<= 8;	11✔
281	l += buf[5];	11✔
282	return l;	11✔
283	}
284
285	size_t Modbus::TModbusTCPTraits::GetPacketSize(size_t pduSize) const	18✔
286	{
287	return MBAP_SIZE + pduSize;	18✔
288	}
289
290	void Modbus::TModbusTCPTraits::FinalizeRequest(std::vector<uint8_t>& request, uint8_t slaveId, uint16_t transactionId)	9✔
291	{
292	SetMBAP(request, transactionId, request.size() - MBAP_SIZE, slaveId);	9✔
293	}	9✔
294
295	TReadFrameResult Modbus::TModbusTCPTraits::ReadFrame(TPort& port,	9✔
296	uint8_t slaveId,
297	uint16_t transactionId,
298	const std::chrono::milliseconds& responseTimeout,
299	const std::chrono::milliseconds& frameTimeout,
300	std::vector<uint8_t>& response,
301	bool matchSlaveId) const
302	{
303	auto startTime = chrono::steady_clock::now();	9✔
304	// Timeout for reading packet with expected transaction ID
305	auto packetTimeout = port.CalcResponseTimeout(responseTimeout + frameTimeout);	9✔
306	while (chrono::duration_cast<chrono::microseconds>(chrono::steady_clock::now() - startTime) < packetTimeout) {	13✔
307	if (response.size() < MBAP_SIZE) {	12✔
308	response.resize(MBAP_SIZE);	×
309	}
310	auto rc = port.ReadFrame(response.data(), MBAP_SIZE, responseTimeout, frameTimeout);	12✔
311
312	if (rc.Count < MBAP_SIZE) {	12✔
313	throw Modbus::TMalformedResponseError("Can't read full MBAP");	1✔
314	}
315
316	auto len = GetLengthFromMBAP(response);	11✔
317	// MBAP length should be at least 1 byte for unit identifier
318	if (len == 0) {	11✔
319	throw Modbus::TMalformedResponseError("Wrong MBAP length value: 0");	1✔
320	}
321	--len; // length includes one byte of unit identifier which is already in buffer	10✔
322
323	if (len + MBAP_SIZE > response.size()) {	10✔
324	response.resize(len + MBAP_SIZE);	4✔
325	}
326
327	rc = port.ReadFrame(response.data() + MBAP_SIZE, len, frameTimeout, frameTimeout);	10✔
328	if (rc.Count != len) {	10✔
329	throw Modbus::TMalformedResponseError("Wrong PDU size: " + to_string(rc.Count) + ", expected " +	3✔
330	to_string(len));	4✔
331	}
332	rc.Count += MBAP_SIZE;	9✔
333
334	// check transaction id
335	if (((transactionId >> 8) & 0xFF) == response[0] && (transactionId & 0xFF) == response[1]) {	9✔
336	// check unit identifier
337	if (matchSlaveId && slaveId != response[6]) {	5✔
338	throw Modbus::TUnexpectedResponseError("request and response unit identifier mismatch");	1✔
339	}
340	return rc;	4✔
341	}
342	}
343	throw TResponseTimeoutException();	1✔
344	}
345
346	Modbus::TReadResult Modbus::TModbusTCPTraits::Transaction(TPort& port,	9✔
347	uint8_t slaveId,
348	const std::vector<uint8_t>& requestPdu,
349	size_t expectedResponsePduSize,
350	const std::chrono::milliseconds& responseTimeout,
351	const std::chrono::milliseconds& frameTimeout,
352	bool matchSlaveId)
353	{
354	auto transactionId = GetTransactionId(port);	9✔
355	std::vector<uint8_t> request(GetPacketSize(requestPdu.size()));	18✔
356	std::copy(requestPdu.begin(), requestPdu.end(), request.begin() + MBAP_SIZE);	9✔
357	FinalizeRequest(request, slaveId, transactionId);	9✔
358
359	port.WriteBytes(request.data(), request.size());	9✔
360
361	std::vector<uint8_t> response(GetPacketSize(expectedResponsePduSize));	18✔
362
363	auto readRes = ReadFrame(port, slaveId, transactionId, responseTimeout, frameTimeout, response, matchSlaveId);	9✔
364
365	TReadResult res;	4✔
366	res.ResponseTime = readRes.ResponseTime;	4✔
367	res.SlaveId = response[6];	4✔
368	res.Pdu.assign(response.begin() + MBAP_SIZE, response.begin() + readRes.Count);	4✔
369	return res;	8✔
370	}
371
372	std::unique_ptr<Modbus::IModbusTraits> Modbus::TModbusRTUTraitsFactory::GetModbusTraits(bool forceFrameTimeout)	65✔
373	{
374	return std::make_unique<Modbus::TModbusRTUTraits>(forceFrameTimeout);	65✔
375	}
376
377	std::unique_ptr<Modbus::IModbusTraits> Modbus::TModbusTCPTraitsFactory::GetModbusTraits(bool forceFrameTimeout)	×
378	{
379	return std::make_unique<Modbus::TModbusTCPTraits>();	×
380	}
381
382	bool Modbus::IsException(uint8_t functionCode) noexcept	4,656✔
383	{
384	return functionCode & EXCEPTION_BIT;	4,656✔
385	}
386
387	Modbus::TErrorBase::TErrorBase(const std::string& what): std::runtime_error(what)	66✔
388	{}	66✔
389
390	Modbus::TMalformedResponseError::TMalformedResponseError(const std::string& what)	22✔
391	: Modbus::TErrorBase("malformed response: " + what)	22✔
392	{}	22✔
393
394	Modbus::TMalformedRequestError::TMalformedRequestError(const std::string& what)	×
395	: Modbus::TErrorBase("malformed request: " + what)	×
396	{}
397
398	Modbus::TUnexpectedResponseError::TUnexpectedResponseError(const std::string& what): Modbus::TErrorBase(what)	11✔
399	{}	11✔
400
401	size_t Modbus::CalcResponsePDUSize(Modbus::EFunction function, size_t registerCount)	586✔
402	{
403	if (IsWriteFunction(function)) {	586✔
404	return WRITE_RESPONSE_PDU_SIZE;	88✔
405	}
406	if (IsSingleBitFunction(function)) {	498✔
407	return 2 + GetCoilsByteSize(registerCount);	69✔
408	}
409	return 2 + registerCount * 2;	429✔
410	}
411
412	std::vector<uint8_t> Modbus::ExtractResponseData(EFunction requestFunction, const std::vector<uint8_t>& pdu)	556✔
413	{
414	if (pdu.size() < 2) {	556✔
415	throw Modbus::TMalformedResponseError("PDU is too small");	×
416	}
417
418	auto functionCode = GetFunctionCode(pdu[0]);	556✔
419	if (requestFunction != functionCode) {	556✔
420	throw Modbus::TUnexpectedResponseError("request and response function code mismatch");	4✔
421	}
422
423	if (IsException(pdu[0])) {	552✔
424	ThrowIfModbusException(pdu[1]);	32✔
425	}
426
427	auto function = GetFunction(functionCode);	520✔
428	if (IsReadFunction(function) \|\| IsReadWriteFunction(function)) {	520✔
429	size_t byteCount = pdu[1];	441✔
430	if (pdu.size() != byteCount + 2) {	441✔
431	throw Modbus::TMalformedResponseError("invalid read response byte count: " + std::to_string(byteCount) +	3✔
432	", got " + std::to_string(pdu.size() - 2));	4✔
433	}
434	return std::vector<uint8_t>(pdu.begin() + 2, pdu.end());	880✔
435	}
436
437	if (IsWriteFunction(function)) {	79✔
438	if (WRITE_RESPONSE_PDU_SIZE != pdu.size()) {	79✔
439	throw Modbus::TMalformedResponseError("invalid write response PDU size: " + std::to_string(pdu.size()) +	×
440	", expected " + std::to_string(WRITE_RESPONSE_PDU_SIZE));	×
441	}
442	}
443
444	return std::vector<uint8_t>();	79✔
445	}
446
447	bool Modbus::IsSupportedFunction(uint8_t functionCode) noexcept	520✔
448	{
449	auto function = static_cast<Modbus::EFunction>(functionCode);	520✔
450	return IsReadFunction(function) \|\| IsWriteFunction(function) \|\| IsReadWriteFunction(function);	520✔
451	}
452
453	std::vector<uint8_t> Modbus::MakePDU(Modbus::EFunction function,	592✔
454	uint16_t address,
455	uint16_t count,
456	const std::vector<uint8_t>& data)
457	{
458	if (IsReadFunction(function)) {	592✔
459	return MakeReadRequestPDU(function, address, count);	498✔
460	}
461
462	if (function == Modbus::EFunction::FN_WRITE_SINGLE_COIL \|\| function == Modbus::EFunction::FN_WRITE_SINGLE_REGISTER)	94✔
463	{
464	return MakeWriteSingleRequestPDU(function, address, data);	71✔
465	}
466
467	if (function == Modbus::EFunction::FN_WRITE_MULTIPLE_REGISTERS \|\|	23✔
468	function == Modbus::EFunction::FN_WRITE_MULTIPLE_COILS)
469	{
470	return MakeWriteMultipleRequestPDU(function, address, count, data);	23✔
471	}
472
473	return std::vector<uint8_t>();	×
474	}
475
476	std::vector<uint8_t> Modbus::MakePDU(Modbus::EFunction function,	×
477	uint16_t address,
478	uint16_t count,
479	uint16_t writeAddress,
480	uint16_t writeCount,
481	const std::vector<uint8_t>& data)
482	{
483	if (IsReadWriteFunction(function)) {	×
484	if (data.size() != writeCount * 2) {	×
485	throw Modbus::TMalformedRequestError("data size " + std::to_string(data.size()) +	×
486	" doesn't match function code 23");	×
487	}
488	std::vector<uint8_t> res(10 + data.size());	×
489	res[0] = Modbus::EFunction::FN_READ_WRITE_MULTIPLE_REGISTERS;	×
490	WriteAs2Bytes(res.data() + 1, address);	×
491	WriteAs2Bytes(res.data() + 3, count);	×
492	WriteAs2Bytes(res.data() + 5, writeAddress);	×
493	WriteAs2Bytes(res.data() + 7, writeCount);	×
494	res[9] = data.size();	×
495	std::copy(data.begin(), data.end(), res.begin() + 10);	×
496	return res;	×
497	}
498
499	return MakePDU(function, address, count, data);	×
500	}
501
502	Modbus::TModbusExceptionError::TModbusExceptionError(uint8_t exceptionCode)	33✔
503	: Modbus::TErrorBase(GetModbusExceptionMessage(exceptionCode)),	33✔
504	ExceptionCode(exceptionCode)	33✔
505	{}	33✔
506
507	uint8_t Modbus::TModbusExceptionError::GetExceptionCode() const	85✔
508	{
509	return ExceptionCode;	85✔
510	}

wirenboard / wb-mqtt-serial / 6

Source File Press 'n' to go to next uncovered line, 'b' for previous

Source File
Press 'n' to go to next uncovered line, 'b' for previous