wirenboard / wb-mqtt-serial / 2

Committed 29 Dec 2025 12:28PM UTC coverage: 76.817% (+4.0%) from 72.836%

Build # 2

Build Type

Pull #1045

github

Specific Base 54aa0c

Committed by

pgasheev

Commit Message

up changelog

Pull Request Pull Request #1045: Fix firmware version in WB-M1W2 template

Run Details

6873 of 9161 branches covered (75.02%)

12966 of 16879 relevant lines covered (76.82%)

1651.61 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)	66✔
15	{
16	if (code == 0) {	66✔
17	return std::string();	×
18	}
19	// clang-format off
20	const char* errs[] =	66✔
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;	66✔
35	if (code < sizeof(errs) / sizeof(char*)) {	66✔
36	return errs[code];	64✔
37	}
38	return "invalid modbus error code (" + std::to_string(code + 1) + ")";	4✔
39	}
40
41	// throws C++ exception on modbus error code
42	void ThrowIfModbusException(uint8_t code)	64✔
43	{
44	if (code == 0) {	64✔
45	return;	×
46	}
47	throw Modbus::TModbusExceptionError(code);	64✔
48	}
49
50	bool IsWriteFunction(Modbus::EFunction function)	1,488✔
51	{
52	return function == Modbus::EFunction::FN_WRITE_MULTIPLE_COILS \|\|	1,488✔
53	function == Modbus::EFunction::FN_WRITE_MULTIPLE_REGISTERS \|\|	1,350✔
54	function == Modbus::EFunction::FN_WRITE_SINGLE_COIL \|\|	2,976✔
55	function == Modbus::EFunction::FN_WRITE_SINGLE_REGISTER;	1,488✔
56	}
57
58	bool IsReadFunction(Modbus::EFunction function)	3,264✔
59	{
60	return function == Modbus::EFunction::FN_READ_COILS \|\| function == Modbus::EFunction::FN_READ_DISCRETE \|\|	2,956✔
61	function == Modbus::EFunction::FN_READ_HOLDING \|\| function == Modbus::EFunction::FN_READ_INPUT;	6,220✔
62	}
63
64	bool IsReadWriteFunction(Modbus::EFunction function)	158✔
65	{
66	return function == Modbus::EFunction::FN_READ_WRITE_MULTIPLE_REGISTERS;	158✔
67	}
68
69	bool IsSingleBitFunction(Modbus::EFunction function)	996✔
70	{
71	return function == Modbus::EFunction::FN_READ_COILS \|\| function == Modbus::EFunction::FN_READ_DISCRETE \|\|	884✔
72	function == Modbus::EFunction::FN_WRITE_SINGLE_COIL \|\|	1,880✔
73	function == Modbus::EFunction::FN_WRITE_MULTIPLE_COILS;	996✔
74	}
75
76	uint16_t GetCoilsByteSize(uint16_t count)	138✔
77	{
78	return count / 8 + (count % 8 != 0 ? 1 : 0);	138✔
79	}
80
81	std::vector<uint8_t> MakeReadRequestPDU(Modbus::EFunction function, uint16_t address, uint16_t count)	996✔
82	{
83	std::vector<uint8_t> res(5);	996✔
84	res[0] = function;	996✔
85	WriteAs2Bytes(res.data() + 1, address);	996✔
86	WriteAs2Bytes(res.data() + 3, count);	996✔
87	return res;	996✔
88	}
89
90	std::vector<uint8_t> MakeWriteSingleRequestPDU(Modbus::EFunction function,	142✔
91	uint16_t address,
92	const std::vector<uint8_t>& data)
93	{
94	if (data.size() != 2) {	142✔
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);	142✔
99	res[0] = function;	142✔
100	WriteAs2Bytes(res.data() + 1, address);	142✔
101	res[3] = data[0];	142✔
102	res[4] = data[1];	142✔
103	return res;	142✔
104	}
105
106	std::vector<uint8_t> MakeWriteMultipleRequestPDU(Modbus::EFunction function,	46✔
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)) \|\|	92✔
112	(function == Modbus::EFunction::FN_WRITE_MULTIPLE_REGISTERS && data.size() != count * 2))	46✔
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());	46✔
118	res[0] = function;	46✔
119	WriteAs2Bytes(res.data() + 1, address);	46✔
120	WriteAs2Bytes(res.data() + 3, count);	46✔
121	res[5] = data.size();	46✔
122	std::copy(data.begin(), data.end(), res.begin() + 6);	46✔
123	return res;	46✔
124	}
125
126	// get actual function code even if exception
127	uint8_t GetFunctionCode(uint8_t functionCodeByte)	1,112✔
128	{
129	return functionCodeByte & 127;	1,112✔
130	}
131
132	Modbus::EFunction GetFunction(uint8_t functionCode)	1,040✔
133	{
134	if (Modbus::IsSupportedFunction(functionCode)) {	1,040✔
135	return static_cast<Modbus::EFunction>(functionCode);	1,040✔
136	}
137	throw Modbus::TUnexpectedResponseError("unknown modbus function code: " + to_string(functionCode));	×
138	}
139	}
140
141	Modbus::IModbusTraits::IModbusTraits(bool forceFrameTimeout): ForceFrameTimeout(forceFrameTimeout)	580✔
142	{}	580✔
143
144	bool Modbus::IModbusTraits::GetForceFrameTimeout()	2,002✔
145	{
146	return ForceFrameTimeout;	2,002✔
147	}
148
149	// TModbusRTUTraits
150
151	Modbus::TModbusRTUTraits::TModbusRTUTraits(bool forceFrameTimeout): IModbusTraits(forceFrameTimeout)	376✔
152	{}	376✔
153
154	TPort::TFrameCompletePred Modbus::TModbusRTUTraits::ExpectNBytes(size_t n) const	1,222✔
155	{
156	return [=](uint8_t* buf, size_t size) {	9,352✔
157	if (size < 2)	9,352✔
158	return false;	1,146✔
159	if (IsException(buf[1])) // GetPDU	8,206✔
160	return size >= EXCEPTION_RESPONSE_PDU_SIZE + DATA_SIZE;	272✔
161	return size >= n;	7,934✔
162	};	1,222✔
163	}
164
165	size_t Modbus::TModbusRTUTraits::GetPacketSize(size_t pduSize) const	2,456✔
166	{
167	return DATA_SIZE + pduSize;	2,456✔
168	}
169
170	void Modbus::TModbusRTUTraits::FinalizeRequest(std::vector<uint8_t>& request, uint8_t slaveId)	1,234✔
171	{
172	request[0] = slaveId;	1,234✔
173	WriteAs2Bytes(&request[request.size() - 2], CRC16::CalculateCRC16(request.data(), request.size() - 2));	1,234✔
174	}	1,234✔
175
176	TReadFrameResult Modbus::TModbusRTUTraits::ReadFrame(TPort& port,	1,222✔
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()));	1,276✔
185	// RTU response should be at least 3 bytes: 1 byte slave_id, 2 bytes CRC
186	if (rc.Count < DATA_SIZE) {	1,168✔
187	throw Modbus::TMalformedResponseError("invalid data size");	×
188	}
189
190	uint16_t crc = (response[rc.Count - 2] << 8) + response[rc.Count - 1];	1,168✔
191	if (crc != CRC16::CalculateCRC16(response.data(), rc.Count - 2)) {	1,168✔
192	throw Modbus::TMalformedResponseError("invalid crc");	6✔
193	}
194
195	if (ForceFrameTimeout) {	1,162✔
196	std::array<uint8_t, 256> buf;
197	try {
198	port.ReadFrame(buf.data(), buf.size(), frameTimeout, frameTimeout);	6✔
199	} catch (const TResponseTimeoutException& e) {	2✔
200	// No extra data
201	}
202	}
203
204	if (matchSlaveId && slaveId != response[0]) {	1,162✔
205	throw Modbus::TUnexpectedResponseError("request and response slave id mismatch");	6✔
206	}
207	return rc;	1,156✔
208	}
209
210	Modbus::TReadResult Modbus::TModbusRTUTraits::Transaction(TPort& port,	1,234✔
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()));	2,468✔
219	std::copy(requestPdu.begin(), requestPdu.end(), request.begin() + 1);	1,234✔
220	FinalizeRequest(request, slaveId);	1,234✔
221
222	port.WriteBytes(request.data(), request.size());	1,234✔
223
224	std::vector<uint8_t> response(GetPacketSize(expectedResponsePduSize));	2,444✔
225
226	auto readRes = ReadFrame(port, slaveId, responseTimeout, frameTimeout, response, matchSlaveId);	1,222✔
227
228	TReadResult res;	1,156✔
229	res.ResponseTime = readRes.ResponseTime;	1,156✔
230	res.SlaveId = slaveId;	1,156✔
231	res.Pdu.assign(response.begin() + 1, response.begin() + (readRes.Count - CRC_SIZE));	1,156✔
232	return res;	2,312✔
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)	18✔
241	{
242	auto portDescription = port.GetDescription(false);	36✔
243	std::unique_lock lk(TransactionIdMutex);	36✔
244	try {
245	return ++TransactionIds.at(portDescription);	18✔
246	} catch (const std::out_of_range&) {	16✔
247	TransactionIds.emplace(portDescription, 1);	16✔
248	return 1;	16✔
249	}
250	}
251
252	void Modbus::TModbusTCPTraits::ResetTransactionId(TPort& port)	16✔
253	{
254	auto portDescription = port.GetDescription(false);	32✔
255	std::unique_lock lk(TransactionIdMutex);	32✔
256	TransactionIds.erase(portDescription);	16✔
257	}	16✔
258
259	Modbus::TModbusTCPTraits::TModbusTCPTraits()	18✔
260	{}	18✔
261
262	void Modbus::TModbusTCPTraits::SetMBAP(std::vector<uint8_t>& req,	18✔
263	uint16_t transactionId,
264	size_t pduSize,
265	uint8_t slaveId) const
266	{
267	req[0] = ((transactionId >> 8) & 0xFF);	18✔
268	req[1] = (transactionId & 0xFF);	18✔
269	req[2] = 0; // MODBUS	18✔
270	req[3] = 0;	18✔
271	++pduSize; // length includes additional byte of unit identifier	18✔
272	req[4] = ((pduSize >> 8) & 0xFF);	18✔
273	req[5] = (pduSize & 0xFF);	18✔
274	req[6] = slaveId;	18✔
275	}	18✔
276
277	uint16_t Modbus::TModbusTCPTraits::GetLengthFromMBAP(const std::vector<uint8_t>& buf) const	22✔
278	{
279	uint16_t l = buf[4];	22✔
280	l <<= 8;	22✔
281	l += buf[5];	22✔
282	return l;	22✔
283	}
284
285	size_t Modbus::TModbusTCPTraits::GetPacketSize(size_t pduSize) const	36✔
286	{
287	return MBAP_SIZE + pduSize;	36✔
288	}
289
290	void Modbus::TModbusTCPTraits::FinalizeRequest(std::vector<uint8_t>& request, uint8_t slaveId, uint16_t transactionId)	18✔
291	{
292	SetMBAP(request, transactionId, request.size() - MBAP_SIZE, slaveId);	18✔
293	}	18✔
294
295	TReadFrameResult Modbus::TModbusTCPTraits::ReadFrame(TPort& port,	18✔
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();	18✔
304	// Timeout for reading packet with expected transaction ID
305	auto packetTimeout = port.CalcResponseTimeout(responseTimeout + frameTimeout);	18✔
306	while (chrono::duration_cast<chrono::microseconds>(chrono::steady_clock::now() - startTime) < packetTimeout) {	26✔
307	if (response.size() < MBAP_SIZE) {	24✔
308	response.resize(MBAP_SIZE);	×
309	}
310	auto rc = port.ReadFrame(response.data(), MBAP_SIZE, responseTimeout, frameTimeout);	24✔
311
312	if (rc.Count < MBAP_SIZE) {	24✔
313	throw Modbus::TMalformedResponseError("Can't read full MBAP");	2✔
314	}
315
316	auto len = GetLengthFromMBAP(response);	22✔
317	// MBAP length should be at least 1 byte for unit identifier
318	if (len == 0) {	22✔
319	throw Modbus::TMalformedResponseError("Wrong MBAP length value: 0");	2✔
320	}
321	--len; // length includes one byte of unit identifier which is already in buffer	20✔
322
323	if (len + MBAP_SIZE > response.size()) {	20✔
324	response.resize(len + MBAP_SIZE);	8✔
325	}
326
327	rc = port.ReadFrame(response.data() + MBAP_SIZE, len, frameTimeout, frameTimeout);	20✔
328	if (rc.Count != len) {	20✔
329	throw Modbus::TMalformedResponseError("Wrong PDU size: " + to_string(rc.Count) + ", expected " +	6✔
330	to_string(len));	8✔
331	}
332	rc.Count += MBAP_SIZE;	18✔
333
334	// check transaction id
335	if (((transactionId >> 8) & 0xFF) == response[0] && (transactionId & 0xFF) == response[1]) {	18✔
336	// check unit identifier
337	if (matchSlaveId && slaveId != response[6]) {	10✔
338	throw Modbus::TUnexpectedResponseError("request and response unit identifier mismatch");	2✔
339	}
340	return rc;	8✔
341	}
342	}
343	throw TResponseTimeoutException();	2✔
344	}
345
346	Modbus::TReadResult Modbus::TModbusTCPTraits::Transaction(TPort& port,	18✔
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);	18✔
355	std::vector<uint8_t> request(GetPacketSize(requestPdu.size()));	36✔
356	std::copy(requestPdu.begin(), requestPdu.end(), request.begin() + MBAP_SIZE);	18✔
357	FinalizeRequest(request, slaveId, transactionId);	18✔
358
359	port.WriteBytes(request.data(), request.size());	18✔
360
361	std::vector<uint8_t> response(GetPacketSize(expectedResponsePduSize));	36✔
362
363	auto readRes = ReadFrame(port, slaveId, transactionId, responseTimeout, frameTimeout, response, matchSlaveId);	18✔
364
365	TReadResult res;	8✔
366	res.ResponseTime = readRes.ResponseTime;	8✔
367	res.SlaveId = response[6];	8✔
368	res.Pdu.assign(response.begin() + MBAP_SIZE, response.begin() + readRes.Count);	8✔
369	return res;	16✔
370	}
371
372	std::unique_ptr<Modbus::IModbusTraits> Modbus::TModbusRTUTraitsFactory::GetModbusTraits(bool forceFrameTimeout)	130✔
373	{
374	return std::make_unique<Modbus::TModbusRTUTraits>(forceFrameTimeout);	130✔
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	9,312✔
383	{
384	return functionCode & EXCEPTION_BIT;	9,312✔
385	}
386
387	Modbus::TErrorBase::TErrorBase(const std::string& what): std::runtime_error(what)	132✔
388	{}	132✔
389
390	Modbus::TMalformedResponseError::TMalformedResponseError(const std::string& what)	44✔
391	: Modbus::TErrorBase("malformed response: " + what)	44✔
392	{}	44✔
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)	22✔
399	{}	22✔
400
401	size_t Modbus::CalcResponsePDUSize(Modbus::EFunction function, size_t registerCount)	1,172✔
402	{
403	if (IsWriteFunction(function)) {	1,172✔
404	return WRITE_RESPONSE_PDU_SIZE;	176✔
405	}
406	if (IsSingleBitFunction(function)) {	996✔
407	return 2 + GetCoilsByteSize(registerCount);	138✔
408	}
409	return 2 + registerCount * 2;	858✔
410	}
411
412	std::vector<uint8_t> Modbus::ExtractResponseData(EFunction requestFunction, const std::vector<uint8_t>& pdu)	1,112✔
413	{
414	if (pdu.size() < 2) {	1,112✔
415	throw Modbus::TMalformedResponseError("PDU is too small");	×
416	}
417
418	auto functionCode = GetFunctionCode(pdu[0]);	1,112✔
419	if (requestFunction != functionCode) {	1,112✔
420	throw Modbus::TUnexpectedResponseError("request and response function code mismatch");	8✔
421	}
422
423	if (IsException(pdu[0])) {	1,104✔
424	ThrowIfModbusException(pdu[1]);	64✔
425	}
426
427	auto function = GetFunction(functionCode);	1,040✔
428	if (IsReadFunction(function) \|\| IsReadWriteFunction(function)) {	1,040✔
429	size_t byteCount = pdu[1];	882✔
430	if (pdu.size() != byteCount + 2) {	882✔
431	throw Modbus::TMalformedResponseError("invalid read response byte count: " + std::to_string(byteCount) +	6✔
432	", got " + std::to_string(pdu.size() - 2));	8✔
433	}
434	return std::vector<uint8_t>(pdu.begin() + 2, pdu.end());	1,760✔
435	}
436
437	if (IsWriteFunction(function)) {	158✔
438	if (WRITE_RESPONSE_PDU_SIZE != pdu.size()) {	158✔
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>();	158✔
445	}
446
447	bool Modbus::IsSupportedFunction(uint8_t functionCode) noexcept	1,040✔
448	{
449	auto function = static_cast<Modbus::EFunction>(functionCode);	1,040✔
450	return IsReadFunction(function) \|\| IsWriteFunction(function) \|\| IsReadWriteFunction(function);	1,040✔
451	}
452
453	std::vector<uint8_t> Modbus::MakePDU(Modbus::EFunction function,	1,184✔
454	uint16_t address,
455	uint16_t count,
456	const std::vector<uint8_t>& data)
457	{
458	if (IsReadFunction(function)) {	1,184✔
459	return MakeReadRequestPDU(function, address, count);	996✔
460	}
461
462	if (function == Modbus::EFunction::FN_WRITE_SINGLE_COIL \|\| function == Modbus::EFunction::FN_WRITE_SINGLE_REGISTER)	188✔
463	{
464	return MakeWriteSingleRequestPDU(function, address, data);	142✔
465	}
466
467	if (function == Modbus::EFunction::FN_WRITE_MULTIPLE_REGISTERS \|\|	46✔
468	function == Modbus::EFunction::FN_WRITE_MULTIPLE_COILS)
469	{
470	return MakeWriteMultipleRequestPDU(function, address, count, data);	46✔
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)	66✔
503	: Modbus::TErrorBase(GetModbusExceptionMessage(exceptionCode)),	66✔
504	ExceptionCode(exceptionCode)	66✔
505	{}	66✔
506
507	uint8_t Modbus::TModbusExceptionError::GetExceptionCode() const	170✔
508	{
509	return ExceptionCode;	170✔
510	}

wirenboard / wb-mqtt-serial / 2

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