wirenboard / wb-mqtt-serial / 666

Committed 08 Jul 2025 11:10AM UTC coverage: 73.854% (+0.1%) from 73.706%

Build # 666

Build Type

push

github

Committed by

web-flow

Commit Message

Port handling refactoring (#960)

Pass port by reference when needed instead of storing it in every device

Run Details

6444 of 9057 branches covered (71.15%)

526 of 700 new or added lines in 56 files covered. (75.14%)

6 existing lines in 5 files now uncovered.

12341 of 16710 relevant lines covered (73.85%)

305.53 hits per line

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

92.89

/src/modbus_base.cpp

#include "modbus_base.h"

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

using namespace std;

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 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;
    }

    void WriteAs2Bytes(uint8_t* dst, uint16_t val)
    {
        dst[0] = static_cast<uint8_t>(val >> 8);
        dst[1] = static_cast<uint8_t>(val);
    }

    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) const
{
    auto rc = port.ReadFrame(response.data(),
                             response.size(),
                             responseTimeout + frameTimeout,
                             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
        }
    }

    auto responseSlaveId = response[0];
    if (slaveId != responseSlaveId) {
        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)
{
    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);

    TReadResult res;
    res.ResponseTime = readRes.ResponseTime;
    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) const
{
    auto startTime = chrono::steady_clock::now();
    while (chrono::duration_cast<chrono::milliseconds>(chrono::steady_clock::now() - startTime) <
           responseTimeout + frameTimeout)
    {
        if (response.size() < MBAP_SIZE) {
            response.resize(MBAP_SIZE);
        }
        auto rc = port.ReadFrame(response.data(), MBAP_SIZE, responseTimeout + frameTimeout, 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 (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)
{
    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);

    TReadResult res;
    res.ResponseTime = readRes.ResponseTime;
    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)) {
        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);
}

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>();
}

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 "crc16.h"
4	#include "serial_exc.h"
5
6	using namespace std;
7
8	namespace
9	{
10	const size_t CRC_SIZE = 2;
11
12	std::string GetModbusExceptionMessage(uint8_t code)	33✔
13	{
14	if (code == 0) {	33✔
15	return std::string();	×
16	}
17	// clang-format off
18	const char* errs[] =	33✔
19	{ "illegal function", // 0x01
20	"illegal data address", // 0x02
21	"illegal data value", // 0x03
22	"server device failure", // 0x04
23	"long operation (acknowledge)", // 0x05
24	"server device is busy", // 0x06
25	"", // 0x07
26	"memory parity error", // 0x08
27	"", // 0x09
28	"gateway path is unavailable", // 0x0A
29	"gateway target device failed to respond" // 0x0B
30	};
31	// clang-format on
32	--code;	33✔
33	if (code < sizeof(errs) / sizeof(char*)) {	33✔
34	return errs[code];	32✔
35	}
36	return "invalid modbus error code (" + std::to_string(code + 1) + ")";	2✔
37	}
38
39	// throws C++ exception on modbus error code
40	void ThrowIfModbusException(uint8_t code)	32✔
41	{
42	if (code == 0) {	32✔
43	return;	×
44	}
45	throw Modbus::TModbusExceptionError(code);	32✔
46	}
47
48	bool IsWriteFunction(Modbus::EFunction function)	687✔
49	{
50	return function == Modbus::EFunction::FN_WRITE_MULTIPLE_COILS \|\|	687✔
51	function == Modbus::EFunction::FN_WRITE_MULTIPLE_REGISTERS \|\|	618✔
52	function == Modbus::EFunction::FN_WRITE_SINGLE_COIL \|\|	1,374✔
53	function == Modbus::EFunction::FN_WRITE_SINGLE_REGISTER;	687✔
54	}
55
56	bool IsReadFunction(Modbus::EFunction function)	1,529✔
57	{
58	return function == Modbus::EFunction::FN_READ_COILS \|\| function == Modbus::EFunction::FN_READ_DISCRETE \|\|	1,375✔
59	function == Modbus::EFunction::FN_READ_HOLDING \|\| function == Modbus::EFunction::FN_READ_INPUT;	2,904✔
60	}
61
62	bool IsSingleBitFunction(Modbus::EFunction function)	450✔
63	{
64	return function == Modbus::EFunction::FN_READ_COILS \|\| function == Modbus::EFunction::FN_READ_DISCRETE \|\|	394✔
65	function == Modbus::EFunction::FN_WRITE_SINGLE_COIL \|\|	844✔
66	function == Modbus::EFunction::FN_WRITE_MULTIPLE_COILS;	450✔
67	}
68
69	void WriteAs2Bytes(uint8_t* dst, uint16_t val)	1,559✔
70	{
71	dst[0] = static_cast<uint8_t>(val >> 8);	1,559✔
72	dst[1] = static_cast<uint8_t>(val);	1,559✔
73	}	1,559✔
74
75	uint16_t GetCoilsByteSize(uint16_t count)	69✔
76	{
77	return count / 8 + (count % 8 != 0 ? 1 : 0);	69✔
78	}
79
80	std::vector<uint8_t> MakeReadRequestPDU(Modbus::EFunction function, uint16_t address, uint16_t count)	450✔
81	{
82	std::vector<uint8_t> res(5);	450✔
83	res[0] = function;	450✔
84	WriteAs2Bytes(res.data() + 1, address);	450✔
85	WriteAs2Bytes(res.data() + 3, count);	450✔
86	return res;	450✔
87	}
88
89	std::vector<uint8_t> MakeWriteSingleRequestPDU(Modbus::EFunction function,	70✔
90	uint16_t address,
91	const std::vector<uint8_t>& data)
92	{
93	if (data.size() != 2) {	70✔
94	throw Modbus::TMalformedRequestError("data size " + std::to_string(data.size()) +	×
95	" doesn't match function code " + std::to_string(function));	×
96	}
97	std::vector<uint8_t> res(5);	70✔
98	res[0] = function;	70✔
99	WriteAs2Bytes(res.data() + 1, address);	70✔
100	res[3] = data[0];	70✔
101	res[4] = data[1];	70✔
102	return res;	70✔
103	}
104
105	std::vector<uint8_t> MakeWriteMultipleRequestPDU(Modbus::EFunction function,	23✔
106	uint16_t address,
107	uint16_t count,
108	const std::vector<uint8_t>& data)
109	{
110	if ((function == Modbus::EFunction::FN_WRITE_MULTIPLE_COILS && data.size() != GetCoilsByteSize(count)) \|\|	46✔
111	(function == Modbus::EFunction::FN_WRITE_MULTIPLE_REGISTERS && data.size() != count * 2))	23✔
112	{
113	throw Modbus::TMalformedRequestError("data size " + std::to_string(data.size()) +	×
114	" doesn't match function code " + std::to_string(function));	×
115	}
116	std::vector<uint8_t> res(6 + data.size());	23✔
117	res[0] = function;	23✔
118	WriteAs2Bytes(res.data() + 1, address);	23✔
119	WriteAs2Bytes(res.data() + 3, count);	23✔
120	res[5] = data.size();	23✔
121	std::copy(data.begin(), data.end(), res.begin() + 6);	23✔
122	return res;	23✔
123	}
124
125	// get actual function code even if exception
126	uint8_t GetFunctionCode(uint8_t functionCodeByte)	529✔
127	{
128	return functionCodeByte & 127;	529✔
129	}
130
131	Modbus::EFunction GetFunction(uint8_t functionCode)	493✔
132	{
133	if (Modbus::IsSupportedFunction(functionCode)) {	493✔
134	return static_cast<Modbus::EFunction>(functionCode);	493✔
135	}
136	throw Modbus::TUnexpectedResponseError("unknown modbus function code: " + to_string(functionCode));	×
137	}
138	}
139
140	Modbus::IModbusTraits::IModbusTraits(bool forceFrameTimeout): ForceFrameTimeout(forceFrameTimeout)	116✔
141	{}	116✔
142
143	bool Modbus::IModbusTraits::GetForceFrameTimeout()	956✔
144	{
145	return ForceFrameTimeout;	956✔
146	}
147
148	// TModbusRTUTraits
149
150	Modbus::TModbusRTUTraits::TModbusRTUTraits(bool forceFrameTimeout): IModbusTraits(forceFrameTimeout)	103✔
151	{}	103✔
152
153	TPort::TFrameCompletePred Modbus::TModbusRTUTraits::ExpectNBytes(size_t n) const	534✔
154	{
155	return [=](uint8_t* buf, size_t size) {	4,371✔
156	if (size < 2)	4,371✔
157	return false;	532✔
158	if (IsException(buf[1])) // GetPDU	3,839✔
159	return size >= EXCEPTION_RESPONSE_PDU_SIZE + DATA_SIZE;	136✔
160	return size >= n;	3,703✔
161	};	534✔
162	}
163
164	size_t Modbus::TModbusRTUTraits::GetPacketSize(size_t pduSize) const	1,077✔
165	{
166	return DATA_SIZE + pduSize;	1,077✔
167	}
168
169	void Modbus::TModbusRTUTraits::FinalizeRequest(std::vector<uint8_t>& request, uint8_t slaveId)	543✔
170	{
171	request[0] = slaveId;	543✔
172	WriteAs2Bytes(&request[request.size() - 2], CRC16::CalculateCRC16(request.data(), request.size() - 2));	543✔
173	}	543✔
174
175	TReadFrameResult Modbus::TModbusRTUTraits::ReadFrame(TPort& port,	534✔
176	uint8_t slaveId,
177	const std::chrono::milliseconds& responseTimeout,
178	const std::chrono::milliseconds& frameTimeout,
179	std::vector<uint8_t>& response) const
180	{
181	auto rc = port.ReadFrame(response.data(),
182	response.size(),
183	responseTimeout + frameTimeout,	536✔
184	frameTimeout,
185	ExpectNBytes(response.size()));	1,070✔
186	// RTU response should be at least 3 bytes: 1 byte slave_id, 2 bytes CRC
187	if (rc.Count < DATA_SIZE) {	532✔
188	throw Modbus::TMalformedResponseError("invalid data size");	×
189	}
190
191	uint16_t crc = (response[rc.Count - 2] << 8) + response[rc.Count - 1];	532✔
192	if (crc != CRC16::CalculateCRC16(response.data(), rc.Count - 2)) {	532✔
193	throw Modbus::TMalformedResponseError("invalid crc");	1✔
194	}
195
196	if (ForceFrameTimeout) {	531✔
197	std::array<uint8_t, 256> buf;
198	try {
199	port.ReadFrame(buf.data(), buf.size(), frameTimeout, frameTimeout);	3✔
200	} catch (const TResponseTimeoutException& e) {	1✔
201	// No extra data
202	}
203	}
204
205	auto responseSlaveId = response[0];	531✔
206	if (slaveId != responseSlaveId) {	531✔
207	throw Modbus::TUnexpectedResponseError("request and response slave id mismatch");	2✔
208	}
209	return rc;	529✔
210	}
211
212	Modbus::TReadResult Modbus::TModbusRTUTraits::Transaction(TPort& port,	543✔
213	uint8_t slaveId,
214	const std::vector<uint8_t>& requestPdu,
215	size_t expectedResponsePduSize,
216	const std::chrono::milliseconds& responseTimeout,
217	const std::chrono::milliseconds& frameTimeout)
218	{
219	std::vector<uint8_t> request(GetPacketSize(requestPdu.size()));	1,086✔
220	std::copy(requestPdu.begin(), requestPdu.end(), request.begin() + 1);	543✔
221	FinalizeRequest(request, slaveId);	543✔
222
223	port.WriteBytes(request.data(), request.size());	543✔
224
225	std::vector<uint8_t> response(GetPacketSize(expectedResponsePduSize));	1,068✔
226
227	auto readRes = ReadFrame(port, slaveId, responseTimeout, frameTimeout, response);	534✔
228
229	TReadResult res;	529✔
230	res.ResponseTime = readRes.ResponseTime;	529✔
231	res.Pdu.assign(response.begin() + 1, response.begin() + (readRes.Count - CRC_SIZE));	529✔
232	return res;	1,058✔
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) const
301	{
302	auto startTime = chrono::steady_clock::now();	9✔
303	while (chrono::duration_cast<chrono::milliseconds>(chrono::steady_clock::now() - startTime) <	26✔
304	responseTimeout + frameTimeout)	26✔
305	{
306	if (response.size() < MBAP_SIZE) {	12✔
307	response.resize(MBAP_SIZE);	×
308	}
309	auto rc = port.ReadFrame(response.data(), MBAP_SIZE, responseTimeout + frameTimeout, frameTimeout);	12✔
310
311	if (rc.Count < MBAP_SIZE) {	12✔
312	throw Modbus::TMalformedResponseError("Can't read full MBAP");	1✔
313	}
314
315	auto len = GetLengthFromMBAP(response);	11✔
316	// MBAP length should be at least 1 byte for unit identifier
317	if (len == 0) {	11✔
318	throw Modbus::TMalformedResponseError("Wrong MBAP length value: 0");	1✔
319	}
320	--len; // length includes one byte of unit identifier which is already in buffer	10✔
321
322	if (len + MBAP_SIZE > response.size()) {	10✔
323	response.resize(len + MBAP_SIZE);	4✔
324	}
325
326	rc = port.ReadFrame(response.data() + MBAP_SIZE, len, frameTimeout, frameTimeout);	10✔
327	if (rc.Count != len) {	10✔
328	throw Modbus::TMalformedResponseError("Wrong PDU size: " + to_string(rc.Count) + ", expected " +	3✔
329	to_string(len));	4✔
330	}
331	rc.Count += MBAP_SIZE;	9✔
332
333	// check transaction id
334	if (((transactionId >> 8) & 0xFF) == response[0] && (transactionId & 0xFF) == response[1]) {	9✔
335	// check unit identifier
336	if (slaveId != response[6]) {	5✔
337	throw Modbus::TUnexpectedResponseError("request and response unit identifier mismatch");	1✔
338	}
339	return rc;	4✔
340	}
341	}
342	throw TResponseTimeoutException();	1✔
343	}
344
345	Modbus::TReadResult Modbus::TModbusTCPTraits::Transaction(TPort& port,	9✔
346	uint8_t slaveId,
347	const std::vector<uint8_t>& requestPdu,
348	size_t expectedResponsePduSize,
349	const std::chrono::milliseconds& responseTimeout,
350	const std::chrono::milliseconds& frameTimeout)
351	{
352	auto transactionId = GetTransactionId(port);	9✔
353	std::vector<uint8_t> request(GetPacketSize(requestPdu.size()));	18✔
354	std::copy(requestPdu.begin(), requestPdu.end(), request.begin() + MBAP_SIZE);	9✔
355	FinalizeRequest(request, slaveId, transactionId);	9✔
356
357	port.WriteBytes(request.data(), request.size());	9✔
358
359	std::vector<uint8_t> response(GetPacketSize(expectedResponsePduSize));	18✔
360
361	auto readRes = ReadFrame(port, slaveId, transactionId, responseTimeout, frameTimeout, response);	9✔
362
363	TReadResult res;	4✔
364	res.ResponseTime = readRes.ResponseTime;	4✔
365	res.Pdu.assign(response.begin() + MBAP_SIZE, response.begin() + readRes.Count);	4✔
366	return res;	8✔
367	}
368
369	std::unique_ptr<Modbus::IModbusTraits> Modbus::TModbusRTUTraitsFactory::GetModbusTraits(bool forceFrameTimeout)	64✔
370	{
371	return std::make_unique<Modbus::TModbusRTUTraits>(forceFrameTimeout);	64✔
372	}
373
NEW 374	std::unique_ptr<Modbus::IModbusTraits> Modbus::TModbusTCPTraitsFactory::GetModbusTraits(bool forceFrameTimeout)	×
375	{
NEW 376	return std::make_unique<Modbus::TModbusTCPTraits>();	×
377	}
378
379	bool Modbus::IsException(uint8_t functionCode) noexcept	4,380✔
380	{
381	return functionCode & EXCEPTION_BIT;	4,380✔
382	}
383
384	Modbus::TErrorBase::TErrorBase(const std::string& what): std::runtime_error(what)	65✔
385	{}	65✔
386
387	Modbus::TMalformedResponseError::TMalformedResponseError(const std::string& what)	21✔
388	: Modbus::TErrorBase("malformed response: " + what)	21✔
389	{}	21✔
390
391	Modbus::TMalformedRequestError::TMalformedRequestError(const std::string& what)	×
392	: Modbus::TErrorBase("malformed request: " + what)	×
393	{}
394
395	Modbus::TUnexpectedResponseError::TUnexpectedResponseError(const std::string& what): Modbus::TErrorBase(what)	11✔
396	{}	11✔
397
398	size_t Modbus::CalcResponsePDUSize(Modbus::EFunction function, size_t registerCount)	537✔
399	{
400	if (IsWriteFunction(function)) {	537✔
401	return WRITE_RESPONSE_PDU_SIZE;	87✔
402	}
403
404	if (IsSingleBitFunction(function)) {	450✔
405	return 2 + GetCoilsByteSize(registerCount);	69✔
406	}
407	return 2 + registerCount * 2;	381✔
408	}
409
410	std::vector<uint8_t> Modbus::ExtractResponseData(EFunction requestFunction, const std::vector<uint8_t>& pdu)	529✔
411	{
412	if (pdu.size() < 2) {	529✔
413	throw Modbus::TMalformedResponseError("PDU is too small");	×
414	}
415
416	auto functionCode = GetFunctionCode(pdu[0]);	529✔
417	if (requestFunction != functionCode) {	529✔
418	throw Modbus::TUnexpectedResponseError("request and response function code mismatch");	4✔
419	}
420
421	if (IsException(pdu[0])) {	525✔
422	ThrowIfModbusException(pdu[1]);	32✔
423	}
424
425	auto function = GetFunction(functionCode);	493✔
426	if (IsReadFunction(function)) {	493✔
427	size_t byteCount = pdu[1];	418✔
428	if (pdu.size() != byteCount + 2) {	418✔
429	throw Modbus::TMalformedResponseError("invalid read response byte count: " + std::to_string(byteCount) +	3✔
430	", got " + std::to_string(pdu.size() - 2));	4✔
431	}
432	return std::vector<uint8_t>(pdu.begin() + 2, pdu.end());	834✔
433	}
434
435	if (IsWriteFunction(function)) {	75✔
436	if (WRITE_RESPONSE_PDU_SIZE != pdu.size()) {	75✔
437	throw Modbus::TMalformedResponseError("invalid write response PDU size: " + std::to_string(pdu.size()) +	×
438	", expected " + std::to_string(WRITE_RESPONSE_PDU_SIZE));	×
439	}
440	}
441	return std::vector<uint8_t>();	75✔
442	}
443
444	bool Modbus::IsSupportedFunction(uint8_t functionCode) noexcept	493✔
445	{
446	auto function = static_cast<Modbus::EFunction>(functionCode);	493✔
447	return IsReadFunction(function) \|\| IsWriteFunction(function);	493✔
448	}
449
450	std::vector<uint8_t> Modbus::MakePDU(Modbus::EFunction function,	543✔
451	uint16_t address,
452	uint16_t count,
453	const std::vector<uint8_t>& data)
454	{
455	if (IsReadFunction(function)) {	543✔
456	return MakeReadRequestPDU(function, address, count);	450✔
457	}
458
459	if (function == Modbus::EFunction::FN_WRITE_SINGLE_COIL \|\| function == Modbus::EFunction::FN_WRITE_SINGLE_REGISTER)	93✔
460	{
461	return MakeWriteSingleRequestPDU(function, address, data);	70✔
462	}
463
464	if (function == Modbus::EFunction::FN_WRITE_MULTIPLE_REGISTERS \|\|	23✔
465	function == Modbus::EFunction::FN_WRITE_MULTIPLE_COILS)
466	{
467	return MakeWriteMultipleRequestPDU(function, address, count, data);	23✔
468	}
469
470	return std::vector<uint8_t>();	×
471	}
472
473	Modbus::TModbusExceptionError::TModbusExceptionError(uint8_t exceptionCode)	33✔
474	: Modbus::TErrorBase(GetModbusExceptionMessage(exceptionCode)),	33✔
475	ExceptionCode(exceptionCode)	33✔
476	{}	33✔
477
478	uint8_t Modbus::TModbusExceptionError::GetExceptionCode() const	85✔
479	{
480	return ExceptionCode;	85✔
481	}

wirenboard / wb-mqtt-serial / 666

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