flowzl/softbus_daemon
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

modbus rtu 1.1 plugins

Browse Source
This commit is contained in:
flower_linux
2026-04-10 10:52:10 +08:00
parent 51175ffd18
commit b44f5e275e
66 changed files with 1088 additions and 1124 deletions
Download Patch File Download Diff File Expand all files Collapse all files

428
plugins/protocols/modbus_rtu/codec/ModbusRtuCodec.cpp Normal file
View File

@@ -0,0 +1,428 @@
#include "plugins/protocols/modbus_rtu/codec/ModbusRtuCodec.h"
#include <variant>
#include "message_bus/pipeline/PipelineContext.h"
#include "plugins/protocols/modbus_rtu/framer/ModbusRtuFraming.h"
namespace softbus::message_bus::pipeline::protocol
{
namespace
{
using softbus::message_bus::pipeline::BusDirection;
using softbus::message_bus::pipeline::protocol::modbusrtu::crc16;
using softbus::message_bus::pipeline::protocol::modbusrtu::crcOk;
void appendCrcLe(QByteArray& adu)
{
const auto crc = crc16(reinterpret_cast<const std::uint8_t*>(adu.constData()),
static_cast<std::size_t>(adu.size()));
adu.append(static_cast<char>(crc & 0xFF));
adu.append(static_cast<char>((crc >> 8) & 0xFF));
}
} // namespace
// 功能 ：解码Modbus RTU帧 如果解码失败 则返回false 如果解码成功 则返回true
bool decodeModbusRtu(BusDirection dir,
const std::uint8_t* p,
std::size_t n,
ModbusRtuPdu& out,
QString* errMsg)
{
auto fail = [&](const char* msg) {
if (errMsg) {
*errMsg = QString::fromUtf8(msg);
}
return false;
};
if (!p || n < 4) {
return fail("adu_too_short");
}
if (p[1] & 0x80) {
if (n < 5) {
return fail("exception_incomplete");
}
if (!crcOk(p, 5)) {
return fail("exception_crc");
}
out = ModbusExceptionPdu{p[0], p[1], p[2]};
return true;
}
const std::uint8_t unit = p[0];
const std::uint8_t fc = p[1];
auto parseReadReq = [&]() -> bool {
if (n != 8 || !crcOk(p, 8)) {
return false;
}
const std::uint16_t addr = static_cast<std::uint16_t>((p[2] << 8) | p[3]);
const std::uint16_t qty = static_cast<std::uint16_t>((p[4] << 8) | p[5]);
switch (fc) {
case 0x01:
out = ReadCoilsRequest{unit, addr, qty};
return true;
case 0x02:
out = ReadDiscreteInputsRequest{unit, addr, qty};
return true;
case 0x03:
out = ReadHoldingRegistersRequest{unit, addr, qty};
return true;
case 0x04:
out = ReadInputRegistersRequest{unit, addr, qty};
return true;
default:
return false;
}
};
auto parseWriteSingleReqOrResp = [&]() -> bool {
if (n != 8 || !crcOk(p, 8)) {
return false;
}
const std::uint16_t addr = static_cast<std::uint16_t>((p[2] << 8) | p[3]);
const std::uint16_t val16 = static_cast<std::uint16_t>((p[4] << 8) | p[5]);
if (fc == 0x05) {
const std::uint8_t coil = (val16 == 0xFF00) ? 1 : 0;
if (dir == BusDirection::Upstream) {
out = WriteSingleCoilResponse{unit, addr, coil};
} else {
out = WriteSingleCoilRequest{unit, addr, coil};
}
return true;
}
if (fc == 0x06) {
if (dir == BusDirection::Upstream) {
out = WriteSingleRegisterResponse{unit, addr, val16};
} else {
out = WriteSingleRegisterRequest{unit, addr, val16};
}
return true;
}
return false;
};
auto parseWriteMultiReq = [&]() -> bool {
if (n < 9) {
return false;
}
const std::uint16_t start = static_cast<std::uint16_t>((p[2] << 8) | p[3]);
const std::uint16_t qty = static_cast<std::uint16_t>((p[4] << 8) | p[5]);
const std::uint8_t byteCount = p[6];
if (n != static_cast<std::size_t>(9) + byteCount) {
return false;
}
if (!crcOk(p, n)) {
return false;
}
if (fc == 0x0F) {
QVector<std::uint8_t> vals;
vals.reserve(byteCount);
for (std::uint8_t i = 0; i < byteCount; ++i) {
vals.append(p[7 + i]);
}
out = WriteMultipleCoilsRequest{unit, start, qty, std::move(vals)};
return true;
}
if (fc == 0x10) {
QVector<std::uint16_t> vals;
vals.reserve(byteCount / 2);
for (std::uint8_t i = 0; i + 1 < byteCount; i += 2) {
vals.append(static_cast<std::uint16_t>((p[7 + i] << 8) | p[7 + i + 1]));
}
out = WriteMultipleRegistersRequest{unit, start, qty, std::move(vals)};
return true;
}
return false;
};
auto parseWriteMultiResp = [&]() -> bool {
if (n != 8 || !crcOk(p, 8)) {
return false;
}
const std::uint16_t start = static_cast<std::uint16_t>((p[2] << 8) | p[3]);
const std::uint16_t qty = static_cast<std::uint16_t>((p[4] << 8) | p[5]);
if (fc == 0x0F) {
out = WriteMultipleCoilsResponse{unit, start, qty, {}};
return true;
}
if (fc == 0x10) {
out = WriteMultipleRegistersResponse{unit, start, qty, {}};
return true;
}
return false;
};
// 功能 ：解析Modbus的读取响应帧
// function code range 01 02 03 04
auto parseReadResp = [&]() -> bool {
if (n < 5) {
return false;
}
const std::uint8_t bc = p[2];
const std::size_t total = static_cast<std::size_t>(5) + bc;
if (n < total || !crcOk(p, total)) {
return false;
}
if (fc == 0x01) {
QVector<std::uint8_t> coils;
coils.reserve(bc);
for (std::uint8_t i = 0; i < bc; ++i) {
coils.append(p[3 + i]);
}
out = ReadCoilsResponse{unit, std::move(coils)};
return true;
}
if (fc == 0x02) {
QVector<std::uint8_t> inputs;
inputs.reserve(bc);
for (std::uint8_t i = 0; i < bc; ++i) {
inputs.append(p[3 + i]);
}
out = ReadDiscreteInputsResponse{unit, std::move(inputs)};
return true;
}
if (fc == 0x03 || fc == 0x04) {
QVector<std::uint16_t> regs;
regs.reserve(bc / 2);
for (std::uint8_t i = 0; i + 1 < bc; i += 2) {
regs.append(static_cast<std::uint16_t>((p[3 + i] << 8) | p[3 + i + 1]));
}
if (fc == 0x03) {
out = ReadHoldingRegistersResponse{unit, std::move(regs)};
} else {
out = ReadInputRegistersResponse{unit, std::move(regs)};
}
return true;
}
return false;
};
// if (dir == BusDirection::Downstream) {
// // 功能 ：解析Modbus的下行请求帧
// // function code range 01 02 03 04 05 06 0F 10
// if (parseReadReq() || parseWriteSingleReqOrResp() || parseWriteMultiReq()) {
// return true;
// }
// out = ModbusRtuUnsupportedPdu{unit, fc};
// return true;
// }
// if (dir == BusDirection::Upstream) {
// // 功能 ：解析Modbus的上行响应帧
// // function code range 01 02 03 04 05 06 0F 10
// if (parseReadResp() || parseWriteSingleReqOrResp() || parseWriteMultiResp()) {
// return true;
// }
// // 功能 ：解析Modbus的下行请求帧
// if (parseReadReq() || parseWriteMultiReq()) {
// return true;
// }
// out = ModbusRtuUnsupportedPdu{unit, fc};
// return true;
// }
if (parseReadReq() || parseWriteSingleReqOrResp() || parseWriteMultiReq() || parseReadResp()
|| parseWriteMultiResp()) {
return true;
}
out = ModbusRtuUnsupportedPdu{unit, fc};
return true;
}
bool encodeModbusRtu(const ModbusRtuPdu& pdu, QByteArray& out, QString* errMsg)
{
auto fail = [&](const char* msg) {
if (errMsg) {
*errMsg = QString::fromUtf8(msg);
}
return false;
};
out.clear();
if (const auto* ex = std::get_if<ModbusExceptionPdu>(&pdu)) {
out.append(static_cast<char>(ex->unitId));
out.append(static_cast<char>(ex->function));
out.append(static_cast<char>(ex->exceptionCode));
appendCrcLe(out);
return true;
}
auto encodeReadReq = [&](std::uint8_t unit, std::uint8_t fc, std::uint16_t start, std::uint16_t qty) {
out.append(static_cast<char>(unit));
out.append(static_cast<char>(fc));
out.append(static_cast<char>((start >> 8) & 0xFF));
out.append(static_cast<char>(start & 0xFF));
out.append(static_cast<char>((qty >> 8) & 0xFF));
out.append(static_cast<char>(qty & 0xFF));
appendCrcLe(out);
return true;
};
auto encodeReadRegResp = [&](std::uint8_t unit, std::uint8_t fc, const QVector<std::uint16_t>& regs) {
if (regs.size() > 127) {
return false;
}
out.append(static_cast<char>(unit));
out.append(static_cast<char>(fc));
out.append(static_cast<char>(regs.size() * 2));
for (std::uint16_t v : regs) {
out.append(static_cast<char>((v >> 8) & 0xFF));
out.append(static_cast<char>(v & 0xFF));
}
appendCrcLe(out);
return true;
};
auto encodeReadBitsResp = [&](std::uint8_t unit, std::uint8_t fc, const QVector<std::uint8_t>& bytes) {
if (bytes.size() > 255) {
return false;
}
out.append(static_cast<char>(unit));
out.append(static_cast<char>(fc));
out.append(static_cast<char>(bytes.size()));
for (std::uint8_t b : bytes) {
out.append(static_cast<char>(b));
}
appendCrcLe(out);
return true;
};
if (const auto* req = std::get_if<ReadCoilsRequest>(&pdu)) {
return encodeReadReq(req->unitId, 0x01, req->startAddress, req->quantity);
}
if (const auto* req = std::get_if<ReadDiscreteInputsRequest>(&pdu)) {
return encodeReadReq(req->unitId, 0x02, req->startAddress, req->quantity);
}
if (const auto* req = std::get_if<ReadHoldingRegistersRequest>(&pdu)) {
return encodeReadReq(req->unitId, 0x03, req->startAddress, req->quantity);
}
if (const auto* req = std::get_if<ReadInputRegistersRequest>(&pdu)) {
return encodeReadReq(req->unitId, 0x04, req->startAddress, req->quantity);
}
if (const auto* resp = std::get_if<ReadHoldingRegistersResponse>(&pdu)) {
if (!encodeReadRegResp(resp->unitId, 0x03, resp->registers)) {
return fail("too_many_registers");
}
return true;
}
if (const auto* resp = std::get_if<ReadInputRegistersResponse>(&pdu)) {
if (!encodeReadRegResp(resp->unitId, 0x04, resp->inputRegisters)) {
return fail("too_many_registers");
}
return true;
}
if (const auto* resp = std::get_if<ReadCoilsResponse>(&pdu)) {
if (!encodeReadBitsResp(resp->unitId, 0x01, resp->coils)) {
return fail("too_many_bytes");
}
return true;
}
if (const auto* resp = std::get_if<ReadDiscreteInputsResponse>(&pdu)) {
if (!encodeReadBitsResp(resp->unitId, 0x02, resp->discreteInputs)) {
return fail("too_many_bytes");
}
return true;
}
if (const auto* req = std::get_if<WriteSingleCoilRequest>(&pdu)) {
out.append(static_cast<char>(req->unitId));
out.append(static_cast<char>(0x05));
out.append(static_cast<char>((req->address >> 8) & 0xFF));
out.append(static_cast<char>(req->address & 0xFF));
const std::uint16_t coilWord = req->value ? 0xFF00 : 0x0000;
out.append(static_cast<char>((coilWord >> 8) & 0xFF));
out.append(static_cast<char>(coilWord & 0xFF));
appendCrcLe(out);
return true;
}
if (const auto* resp = std::get_if<WriteSingleCoilResponse>(&pdu)) {
out.append(static_cast<char>(resp->unitId));
out.append(static_cast<char>(0x05));
out.append(static_cast<char>((resp->address >> 8) & 0xFF));
out.append(static_cast<char>(resp->address & 0xFF));
const std::uint16_t coilWord = resp->value ? 0xFF00 : 0x0000;
out.append(static_cast<char>((coilWord >> 8) & 0xFF));
out.append(static_cast<char>(coilWord & 0xFF));
appendCrcLe(out);
return true;
}
if (const auto* req = std::get_if<WriteSingleRegisterRequest>(&pdu)) {
out.append(static_cast<char>(req->unitId));
out.append(static_cast<char>(0x06));
out.append(static_cast<char>((req->address >> 8) & 0xFF));
out.append(static_cast<char>(req->address & 0xFF));
out.append(static_cast<char>((req->value >> 8) & 0xFF));
out.append(static_cast<char>(req->value & 0xFF));
appendCrcLe(out);
return true;
}
if (const auto* resp = std::get_if<WriteSingleRegisterResponse>(&pdu)) {
out.append(static_cast<char>(resp->unitId));
out.append(static_cast<char>(0x06));
out.append(static_cast<char>((resp->address >> 8) & 0xFF));
out.append(static_cast<char>(resp->address & 0xFF));
out.append(static_cast<char>((resp->value >> 8) & 0xFF));
out.append(static_cast<char>(resp->value & 0xFF));
appendCrcLe(out);
return true;
}
if (const auto* req = std::get_if<WriteMultipleCoilsRequest>(&pdu)) {
if (req->values.size() > 255) {
return fail("too_many_bytes");
}
out.append(static_cast<char>(req->unitId));
out.append(static_cast<char>(0x0F));
out.append(static_cast<char>((req->startAddress >> 8) & 0xFF));
out.append(static_cast<char>(req->startAddress & 0xFF));
out.append(static_cast<char>((req->quantity >> 8) & 0xFF));
out.append(static_cast<char>(req->quantity & 0xFF));
out.append(static_cast<char>(req->values.size()));
for (std::uint8_t b : req->values) {
out.append(static_cast<char>(b));
}
appendCrcLe(out);
return true;
}
if (const auto* resp = std::get_if<WriteMultipleCoilsResponse>(&pdu)) {
out.append(static_cast<char>(resp->unitId));
out.append(static_cast<char>(0x0F));
out.append(static_cast<char>((resp->startAddress >> 8) & 0xFF));
out.append(static_cast<char>(resp->startAddress & 0xFF));
out.append(static_cast<char>((resp->quantity >> 8) & 0xFF));
out.append(static_cast<char>(resp->quantity & 0xFF));
appendCrcLe(out);
return true;
}
if (const auto* req = std::get_if<WriteMultipleRegistersRequest>(&pdu)) {
if (req->values.size() > 127) {
return fail("too_many_registers");
}
out.append(static_cast<char>(req->unitId));
out.append(static_cast<char>(0x10));
out.append(static_cast<char>((req->startAddress >> 8) & 0xFF));
out.append(static_cast<char>(req->startAddress & 0xFF));
out.append(static_cast<char>((req->quantity >> 8) & 0xFF));
out.append(static_cast<char>(req->quantity & 0xFF));
out.append(static_cast<char>(req->values.size() * 2));
for (std::uint16_t v : req->values) {
out.append(static_cast<char>((v >> 8) & 0xFF));
out.append(static_cast<char>(v & 0xFF));
}
appendCrcLe(out);
return true;
}
if (const auto* resp = std::get_if<WriteMultipleRegistersResponse>(&pdu)) {
out.append(static_cast<char>(resp->unitId));
out.append(static_cast<char>(0x10));
out.append(static_cast<char>((resp->startAddress >> 8) & 0xFF));
out.append(static_cast<char>(resp->startAddress & 0xFF));
out.append(static_cast<char>((resp->quantity >> 8) & 0xFF));
out.append(static_cast<char>(resp->quantity & 0xFF));
appendCrcLe(out);
return true;
}
return fail("unsupported_encode");
}
} // namespace softbus::message_bus::pipeline::protocol

27
plugins/protocols/modbus_rtu/codec/ModbusRtuCodec.h Normal file
View File

@@ -0,0 +1,27 @@
#pragma once
#include <cstddef>
#include <cstdint>
#include <QByteArray>
#include <QString>
#include "plugins/protocols/modbus_rtu/types/ModbusRtuPdu.h"
namespace softbus::message_bus::pipeline
{
enum class BusDirection;
}
namespace softbus::message_bus::pipeline::protocol
{
bool decodeModbusRtu(softbus::message_bus::pipeline::BusDirection dir,
const std::uint8_t* p,
std::size_t n,
ModbusRtuPdu& out,
QString* errMsg = nullptr);
bool encodeModbusRtu(const ModbusRtuPdu& pdu, QByteArray& out, QString* errMsg = nullptr);
} // namespace softbus::message_bus::pipeline::protocol