Revision control
++ // Copyright (C) 2021 The Qt Company Ltd.
++ // SPDX-License-Identifier: LicenseRef-Qt-Commercial OR BSD-3-Clause
++
++ #include "modbusserver.h"
++ #include "modbusclient.h"
++
++ // The request must contain the start address (quint16) and the number of
++ // registers to read (quint16).
++ static constexpr int ReadRequestSize {4};
++
++ // The request must contain the start address (quint16), the number of
++ // registers to write (quint16), the byte count (quint8) and at least one
++ // register value (quint16).
++ static constexpr int WriteHeaderSize {5};
++ static constexpr int MinimumWriteRequestSize {WriteHeaderSize + 2}; // 2 payload bytes
++
++ ModbusServer::ModbusServer(QObject *parent)
++ : QModbusTcpServer(parent)
++ {
++ setMap({{QModbusDataUnit::HoldingRegisters, {QModbusDataUnit::HoldingRegisters, 0, 10}}});
++
++ QModbusRequest::registerDataSizeCalculator(ModbusClient::CustomRead, [](const QModbusRequest &request) {
++ if (!request.isValid())
++ return -1;
++ return (request.dataSize() != ReadRequestSize ? -1 : ReadRequestSize);
++ });
++
++ QModbusRequest::registerDataSizeCalculator(ModbusClient::CustomWrite, [](const QModbusRequest &request) {
++ if (!request.isValid())
++ return -1;
++
++ if (request.dataSize() < MinimumWriteRequestSize)
++ return -1;
++ return WriteHeaderSize + quint8(request.data().at(WriteHeaderSize - 1));
++ });
++ }
++
++ QModbusResponse ModbusServer::processPrivateRequest(const QModbusPdu &request)
++ {
++ if (!request.isValid())
++ return QModbusServer::processPrivateRequest(request);
++
++ //! [handle_custom_read]
++ if (ModbusClient::CustomRead == request.functionCode()) {
++ quint16 startAddress, count;
++ request.decodeData(&startAddress, &count);
++
++ QModbusDataUnit unit(QModbusDataUnit::HoldingRegisters, startAddress, count);
++ if (!data(&unit)) {
++ return QModbusExceptionResponse(request.functionCode(),
++ QModbusExceptionResponse::IllegalDataAddress);
++ }
++ return QModbusResponse(request.functionCode(), startAddress, quint8(count * 2), unit.values());
++ }
++ //! [handle_custom_read]
++
++ //! [handle_custom_write]
++ if (ModbusClient::CustomWrite == request.functionCode()) {
++ quint8 byteCount;
++ quint16 startAddress, numberOfRegisters;
++ request.decodeData(&startAddress, &numberOfRegisters, &byteCount);
++
++ if (byteCount % 2 != 0) {
++ return QModbusExceptionResponse(request.functionCode(),
++ QModbusExceptionResponse::IllegalDataValue);
++ }
++
++ const QByteArray pduData = request.data().remove(0, WriteHeaderSize);
++ QDataStream stream(pduData);
++
++ QList<quint16> values;
++ for (int i = 0; i < numberOfRegisters; i++) {
++ quint16 tmp;
++ stream >> tmp;
++ values.append(tmp);
++ }
++
++ if (!writeData({QModbusDataUnit::HoldingRegisters, startAddress, values})) {
++ return QModbusExceptionResponse(request.functionCode(),
++ QModbusExceptionResponse::ServerDeviceFailure);
++ }
++
++ return QModbusResponse(request.functionCode(), startAddress, numberOfRegisters);
++ }
++ //! [handle_custom_write]
++
++ return QModbusServer::processPrivateRequest(request);
++ }