89 lines
2.7 KiB
C++
89 lines
2.7 KiB
C++
#include "plugins/protocols/modbusrtu/framer/ModbusRtuFraming.h"
|
|
|
|
namespace softbus::message_bus::pipeline::protocol::modbusrtu
|
|
{
|
|
|
|
std::uint16_t crc16(const std::uint8_t* data, std::size_t len)
|
|
{
|
|
std::uint16_t crc = 0xFFFF;
|
|
for (std::size_t i = 0; i < len; ++i) {
|
|
crc ^= data[i];
|
|
for (int k = 0; k < 8; ++k) {
|
|
crc = (crc & 1) ? static_cast<std::uint16_t>((crc >> 1) ^ 0xA001)
|
|
: static_cast<std::uint16_t>(crc >> 1);
|
|
}
|
|
}
|
|
return crc;
|
|
}
|
|
|
|
bool crcOk(const std::uint8_t* adu, std::size_t aduLen)
|
|
{
|
|
if (!adu || aduLen < 4) {
|
|
return false;
|
|
}
|
|
const std::size_t body = aduLen - 2;
|
|
const std::uint16_t got =
|
|
static_cast<std::uint16_t>(adu[body] | (static_cast<std::uint16_t>(adu[body + 1]) << 8));
|
|
return got == crc16(adu, body);
|
|
}
|
|
|
|
static bool isReadFc(std::uint8_t fc)
|
|
{
|
|
return fc == 0x01 || fc == 0x02 || fc == 0x03 || fc == 0x04;
|
|
}
|
|
|
|
std::optional<std::size_t> predictAduLength(const std::uint8_t* buf, std::size_t len, std::size_t maxAdu)
|
|
{
|
|
if (!buf || len < 2) {
|
|
return std::nullopt;
|
|
}
|
|
const std::uint8_t fc = buf[1];
|
|
if (fc & 0x80) {
|
|
return len >= 5 ? std::optional<std::size_t>(5) : std::nullopt;
|
|
}
|
|
|
|
if (isReadFc(fc)) {
|
|
if (len >= 8 && crcOk(buf, 8)) {
|
|
return std::optional<std::size_t>(8); // read request
|
|
}
|
|
if (len < 3) {
|
|
return std::nullopt;
|
|
}
|
|
const std::size_t total = static_cast<std::size_t>(5) + buf[2];
|
|
if (total > maxAdu || len < total) {
|
|
return std::nullopt; // wait more bytes
|
|
}
|
|
return crcOk(buf, total) ? std::optional<std::size_t>(total) : std::nullopt;
|
|
}
|
|
|
|
if (fc == 0x05 || fc == 0x06) {
|
|
return len >= 8 ? std::optional<std::size_t>(8) : std::nullopt;
|
|
}
|
|
if (fc == 0x16) {
|
|
return len >= 10 ? std::optional<std::size_t>(10) : std::nullopt;
|
|
}
|
|
if (fc == 0x0F || fc == 0x10) {
|
|
if (len >= 8 && crcOk(buf, 8)) {
|
|
return std::optional<std::size_t>(8); // response
|
|
}
|
|
if (len < 7) {
|
|
return std::nullopt;
|
|
}
|
|
const std::size_t req = static_cast<std::size_t>(9) + buf[6];
|
|
if (req > maxAdu || len < req) {
|
|
return std::nullopt;
|
|
}
|
|
return crcOk(buf, req) ? std::optional<std::size_t>(req) : std::nullopt;
|
|
}
|
|
return len >= 8 ? std::optional<std::size_t>(8) : std::nullopt;
|
|
}
|
|
|
|
bool isModbusRtuHint(const QString& hint)
|
|
{
|
|
const QString h = hint.trimmed().toLower();
|
|
return h == QStringLiteral("modbus_rtu") || h == QStringLiteral("modbus-rtu")
|
|
|| h == QStringLiteral("modbusrtu");
|
|
}
|
|
|
|
} // namespace softbus::message_bus::pipeline::protocol::modbusrtu
|