332 lines
11 KiB
C++
332 lines
11 KiB
C++
#include "plugins/protocols/modbus_rtu/codec/ModbusRtuCodec.h"
|
||
|
||
#include <variant>
|
||
|
||
#include "plugins/protocols/modbus_rtu/framer/ModbusRtuFraming.h"
|
||
|
||
namespace softbus::message_bus::pipeline::protocol
|
||
{
|
||
|
||
namespace
|
||
{
|
||
|
||
using softbus::core::models::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;
|
||
}
|
||
// TODO: re-enable FC05/06/0F/10 encode branches after type-name collision investigation.
|
||
return fail("unsupported_encode");
|
||
}
|
||
|
||
} // namespace softbus::message_bus::pipeline::protocol
|