wirenboard / wb-mqtt-serial / 674

Committed 25 Jul 2025 11:09AM UTC coverage: 72.932% (-0.1%) from 73.028%

Build # 674

Build Type

push

github

Committed by

web-flow

Commit Message

Аdd modbus function 0x17 (23) support

Run Details

6463 of 9226 branches covered (70.05%)

4 of 29 new or added lines in 2 files covered. (13.79%)

1 existing line in 1 file now uncovered.

12370 of 16961 relevant lines covered (72.93%)

373.1 hits per line

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

87.5

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

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

wirenboard / wb-mqtt-serial / 674

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