513 lines
16 KiB
C++
513 lines
16 KiB
C++
#include "ModbusTCPServer.h"
|
||
#include <iostream>
|
||
#include <cstring>
|
||
#include <stdexcept>
|
||
|
||
#include "VrError.h"
|
||
#include "VrLog.h"
|
||
|
||
#ifdef _WIN32
|
||
#include <winsock2.h>
|
||
#include <ws2tcpip.h>
|
||
#pragma comment(lib, "ws2_32.lib")
|
||
#else
|
||
#include <unistd.h>
|
||
#include <sys/socket.h>
|
||
#include <netinet/in.h>
|
||
#include <arpa/inet.h>
|
||
#include <sys/select.h>
|
||
#include <errno.h>
|
||
#endif
|
||
|
||
ModbusTCPServer::ModbusTCPServer()
|
||
: m_modbusCtx(nullptr)
|
||
, m_mapping(nullptr)
|
||
, m_isRunning(false)
|
||
, m_shouldStop(false)
|
||
, m_serverSocket(-1)
|
||
, m_port(502)
|
||
, m_maxConnections(10)
|
||
{
|
||
}
|
||
|
||
ModbusTCPServer::~ModbusTCPServer()
|
||
{
|
||
stop();
|
||
}
|
||
|
||
int ModbusTCPServer::start(int port, int maxConnections)
|
||
{
|
||
if (m_isRunning.load()) {
|
||
setLastError("服务器已在运行");
|
||
return SUCCESS;
|
||
}
|
||
|
||
m_port = port;
|
||
m_maxConnections = maxConnections;
|
||
|
||
// 创建modbus TCP context
|
||
m_modbusCtx = modbus_new_tcp("0.0.0.0", port);
|
||
if (m_modbusCtx == nullptr) {
|
||
setLastError("创建Modbus TCP上下文失败");
|
||
return ERR_CODE(NET_ERR_CREAT_INIT);
|
||
}
|
||
|
||
// 创建数据映射 - 分配足够的空间
|
||
// 线圈: 0-9999, 离散输入: 0-9999, 保持寄存器: 0-9999, 输入寄存器: 0-9999
|
||
m_mapping = modbus_mapping_new(10000, 10000, 10000, 10000);
|
||
if (m_mapping == nullptr) {
|
||
setLastError("创建数据映射失败");
|
||
modbus_free(m_modbusCtx);
|
||
m_modbusCtx = nullptr;
|
||
return ERR_CODE(NET_ERR_CONFIG);
|
||
}
|
||
|
||
// 监听连接
|
||
m_serverSocket = modbus_tcp_listen(m_modbusCtx, maxConnections);
|
||
if (m_serverSocket == -1) {
|
||
setLastError(std::string("监听失败: ") + modbus_strerror(errno));
|
||
modbus_mapping_free(m_mapping);
|
||
m_mapping = nullptr;
|
||
modbus_free(m_modbusCtx);
|
||
m_modbusCtx = nullptr;
|
||
return ERR_CODE(NET_ERR_CREAT_LISTEN);
|
||
}
|
||
|
||
m_isRunning.store(true);
|
||
m_shouldStop.store(false);
|
||
|
||
// 启动服务器线程
|
||
m_serverThread = std::make_unique<std::thread>(&ModbusTCPServer::serverLoop, this);
|
||
|
||
// 初始状态：服务器启动但没有客户端连接
|
||
if (m_connectionStatusCallback) {
|
||
m_connectionStatusCallback(false);
|
||
}
|
||
|
||
return SUCCESS;
|
||
}
|
||
|
||
void ModbusTCPServer::stop()
|
||
{
|
||
if (!m_isRunning.load()) {
|
||
return;
|
||
}
|
||
|
||
m_shouldStop.store(true);
|
||
|
||
// 关闭服务器socket以中断accept调用
|
||
if (m_serverSocket != -1) {
|
||
#ifdef _WIN32
|
||
closesocket(m_serverSocket);
|
||
#else
|
||
close(m_serverSocket);
|
||
#endif
|
||
m_serverSocket = -1;
|
||
}
|
||
|
||
// 等待服务器线程退出
|
||
if (m_serverThread && m_serverThread->joinable()) {
|
||
m_serverThread->join();
|
||
}
|
||
|
||
// 清理所有客户端连接
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_clientsMutex);
|
||
m_clients.clear();
|
||
}
|
||
|
||
// 清理资源
|
||
if (m_mapping) {
|
||
modbus_mapping_free(m_mapping);
|
||
m_mapping = nullptr;
|
||
}
|
||
|
||
if (m_modbusCtx) {
|
||
modbus_free(m_modbusCtx);
|
||
m_modbusCtx = nullptr;
|
||
}
|
||
|
||
m_isRunning.store(false);
|
||
}
|
||
|
||
void ModbusTCPServer::serverLoop()
|
||
{
|
||
fd_set readfds;
|
||
int maxfd = m_serverSocket;
|
||
|
||
while (!m_shouldStop.load()) {
|
||
FD_ZERO(&readfds);
|
||
FD_SET(m_serverSocket, &readfds);
|
||
|
||
maxfd = m_serverSocket;
|
||
|
||
// 添加所有客户端socket到监听集合
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_clientsMutex);
|
||
for (const auto& pair : m_clients) {
|
||
int clientSocket = pair.first;
|
||
FD_SET(clientSocket, &readfds);
|
||
if (clientSocket > maxfd) {
|
||
maxfd = clientSocket;
|
||
}
|
||
}
|
||
}
|
||
|
||
// 设置超时时间（1秒），避免阻塞太久
|
||
struct timeval timeout;
|
||
timeout.tv_sec = 1;
|
||
timeout.tv_usec = 0;
|
||
|
||
int selectResult = select(maxfd + 1, &readfds, nullptr, nullptr, &timeout);
|
||
|
||
if (selectResult == -1) {
|
||
if (!m_shouldStop.load()) {
|
||
LOG_WARNING("select error: %s\n", strerror(errno));
|
||
}
|
||
break;
|
||
}
|
||
|
||
if (selectResult == 0) {
|
||
// 超时，继续循环
|
||
continue;
|
||
}
|
||
|
||
// 检查服务器socket是否有新连接
|
||
if (FD_ISSET(m_serverSocket, &readfds)) {
|
||
handleNewConnection();
|
||
}
|
||
|
||
// 检查客户端socket是否有数据
|
||
std::vector<int> socketsToRemove;
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_clientsMutex);
|
||
for (const auto& pair : m_clients) {
|
||
int clientSocket = pair.first;
|
||
if (FD_ISSET(clientSocket, &readfds)) {
|
||
try {
|
||
handleClientData(pair.second);
|
||
} catch (...) {
|
||
// 客户端处理出错，标记删除
|
||
socketsToRemove.push_back(clientSocket);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
// 删除有问题的客户端连接
|
||
for (int socket : socketsToRemove) {
|
||
removeClient(socket);
|
||
}
|
||
}
|
||
}
|
||
|
||
void ModbusTCPServer::handleNewConnection()
|
||
{
|
||
int clientSocket = modbus_tcp_accept(m_modbusCtx, &m_serverSocket);
|
||
if (clientSocket == -1) {
|
||
if (!m_shouldStop.load()) {
|
||
LOG_ERRO("Accept connection failed: %s\n", modbus_strerror(errno));
|
||
}
|
||
return;
|
||
}
|
||
|
||
// 创建新的modbus context用于此客户端
|
||
modbus_t* clientCtx = modbus_new_tcp("0.0.0.0", m_port);
|
||
if (clientCtx == nullptr) {
|
||
LOG_ERRO("Create Modbus context for client failed\n");
|
||
#ifdef _WIN32
|
||
closesocket(clientSocket);
|
||
#else
|
||
close(clientSocket);
|
||
#endif
|
||
return;
|
||
}
|
||
|
||
// 设置socket为非阻塞模式
|
||
modbus_set_socket(clientCtx, clientSocket);
|
||
|
||
// 创建客户端连接对象
|
||
auto client = std::make_shared<ClientConnection>(clientSocket, clientCtx);
|
||
|
||
// 添加到客户端列表
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_clientsMutex);
|
||
m_clients[clientSocket] = client;
|
||
}
|
||
|
||
LOG_INFO("New client connected: socket=%d\n", clientSocket);
|
||
|
||
// 触发连接状态回调
|
||
if (m_connectionStatusCallback) {
|
||
m_connectionStatusCallback(true);
|
||
}
|
||
}
|
||
|
||
void ModbusTCPServer::handleClientData(std::shared_ptr<ClientConnection> client)
|
||
{
|
||
uint8_t buffer[MODBUS_TCP_MAX_ADU_LENGTH];
|
||
|
||
// 接收数据
|
||
int bytesReceived = recv(client->socket, reinterpret_cast<char*>(buffer), sizeof(buffer), 0);
|
||
|
||
if (bytesReceived <= 0) {
|
||
// 连接断开或错误
|
||
if (bytesReceived == 0) {
|
||
LOG_VERBOSE("Client disconnected: socket=%d\n", client->socket);
|
||
} else {
|
||
LOG_ERRO("Receive data error: %s\n", strerror(errno));
|
||
}
|
||
throw std::runtime_error("客户端连接错误");
|
||
}
|
||
|
||
// 处理Modbus请求
|
||
processModbusRequest(client, buffer, bytesReceived);
|
||
}
|
||
|
||
void ModbusTCPServer::removeClient(int socket)
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_clientsMutex);
|
||
auto it = m_clients.find(socket);
|
||
if (it != m_clients.end()) {
|
||
LOG_INFO("Remove client connection: socket=%d\n", socket);
|
||
m_clients.erase(it);
|
||
|
||
// 如果没有客户端连接了，触发断开回调
|
||
if (m_clients.empty() && m_connectionStatusCallback) {
|
||
m_connectionStatusCallback(false);
|
||
}
|
||
}
|
||
}
|
||
|
||
void ModbusTCPServer::processModbusRequest(std::shared_ptr<ClientConnection> client, const uint8_t* query, int queryLength)
|
||
{
|
||
// 处理请求并发送响应
|
||
std::lock_guard<std::mutex> lock(m_dataMutex);
|
||
int responseRc = modbus_reply(client->modbusCtx, query, queryLength, m_mapping);
|
||
|
||
if (responseRc == -1) {
|
||
LOG_WARNING("Send response failed: %s\n", modbus_strerror(errno));
|
||
throw std::runtime_error("发送响应失败");
|
||
}
|
||
|
||
// 解析ModbusTCP ADU - 最小长度检查
|
||
if (queryLength < 8) {
|
||
LOG_DEBUG("Query too short: %d bytes\n", queryLength);
|
||
return;
|
||
}
|
||
|
||
// ModbusTCP ADU格式: [Transaction ID(2)] [Protocol ID(2)] [Length(2)] [Unit ID(1)] [Function Code(1)] [Data...]
|
||
uint16_t transactionId = (query[0] << 8) | query[1]; // Transaction ID
|
||
uint16_t protocolId = (query[2] << 8) | query[3]; // Protocol ID (should be 0 for ModbusTCP)
|
||
uint16_t length = (query[4] << 8) | query[5]; // Length field
|
||
uint8_t unitId = query[6]; // Unit ID
|
||
uint8_t function = query[7]; // Function Code
|
||
|
||
// 验证协议ID
|
||
if (protocolId != 0) {
|
||
LOG_DEBUG("Invalid protocol ID: %d\n", protocolId);
|
||
return;
|
||
}
|
||
|
||
// 验证长度字段
|
||
if (length != (queryLength - 6)) {
|
||
LOG_DEBUG("Length mismatch: expected %d, got %d\n", length, queryLength - 6);
|
||
return;
|
||
}
|
||
|
||
LOG_DEBUG("ModbusTCP request - TransID:%d, UnitID:%d, Function:0x%02X\n",
|
||
transactionId, unitId, function);
|
||
|
||
// 根据功能码解析数据
|
||
switch (function) {
|
||
case MODBUS_FC_WRITE_SINGLE_COIL:
|
||
{
|
||
if (queryLength >= 12) {
|
||
uint16_t address = (query[8] << 8) | query[9];
|
||
uint16_t value = (query[10] << 8) | query[11];
|
||
|
||
LOG_DEBUG("Write single coil - Address:%d, Value:0x%x\n", address, value);
|
||
|
||
if (m_writeCoilsCallback) {
|
||
uint8_t coilValue = (value == 0xFF00) ? 1 : 0;
|
||
ErrorCode result = m_writeCoilsCallback(unitId, address, 1, &coilValue);
|
||
if (result != ErrorCode::SUCCESS) {
|
||
LOG_DEBUG("Write coil callback returned error: %d\n", (int)result);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
break;
|
||
|
||
case MODBUS_FC_WRITE_MULTIPLE_COILS:
|
||
{
|
||
if (queryLength >= 13) {
|
||
uint16_t address = (query[8] << 8) | query[9];
|
||
uint16_t quantity = (query[10] << 8) | query[11];
|
||
uint8_t byteCount = query[12];
|
||
|
||
if (queryLength >= (13 + byteCount)) {
|
||
LOG_DEBUG("Write multiple coils - Address:%d, Quantity:%d, Bytes:%d\n",
|
||
address, quantity, byteCount);
|
||
|
||
if (m_writeCoilsCallback) {
|
||
const uint8_t* values = &query[13];
|
||
ErrorCode result = m_writeCoilsCallback(unitId, address, quantity, values);
|
||
if (result != ErrorCode::SUCCESS) {
|
||
LOG_DEBUG("Write coils callback returned error: %d\n", (int)result);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
break;
|
||
|
||
case MODBUS_FC_WRITE_SINGLE_REGISTER:
|
||
{
|
||
if (queryLength >= 12) {
|
||
uint16_t address = (query[8] << 8) | query[9];
|
||
uint16_t value = (query[10] << 8) | query[11];
|
||
|
||
LOG_DEBUG("Write single register - Address:%d, Value:%d\n", address, value);
|
||
|
||
if (m_writeRegistersCallback) {
|
||
ErrorCode result = m_writeRegistersCallback(unitId, address, 1, &value);
|
||
if (result != ErrorCode::SUCCESS) {
|
||
LOG_DEBUG("Write register callback returned error: %d\n", (int)result);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
break;
|
||
|
||
case MODBUS_FC_WRITE_MULTIPLE_REGISTERS:
|
||
{
|
||
if (queryLength >= 13) {
|
||
uint16_t address = (query[8] << 8) | query[9];
|
||
uint16_t quantity = (query[10] << 8) | query[11];
|
||
uint8_t byteCount = query[12];
|
||
|
||
if (queryLength >= (13 + byteCount) && byteCount == (quantity * 2)) {
|
||
LOG_DEBUG("Write multiple registers - Address:%d, Quantity:%d, Bytes:%d\n",
|
||
address, quantity, byteCount);
|
||
|
||
if (m_writeRegistersCallback) {
|
||
// 解析寄存器值（注意字节序）
|
||
std::vector<uint16_t> values;
|
||
values.reserve(quantity);
|
||
|
||
for (int i = 0; i < quantity; i++) {
|
||
uint16_t value = (query[13 + i*2] << 8) | query[13 + i*2 + 1];
|
||
values.push_back(value);
|
||
}
|
||
|
||
ErrorCode result = m_writeRegistersCallback(unitId, address, quantity, values.data());
|
||
if (result != ErrorCode::SUCCESS) {
|
||
LOG_DEBUG("Write registers callback returned error: %d\n", (int)result);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
break;
|
||
|
||
default:
|
||
LOG_DEBUG("Unhandled function code: 0x%02X\n", function);
|
||
break;
|
||
}
|
||
}
|
||
|
||
void ModbusTCPServer::updateCoil(uint16_t address, bool value)
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_dataMutex);
|
||
if (m_mapping && address < m_mapping->nb_bits) {
|
||
m_mapping->tab_bits[address] = value ? 1 : 0;
|
||
}
|
||
}
|
||
|
||
void ModbusTCPServer::updateCoils(uint16_t startAddress, const std::vector<bool>& values)
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_dataMutex);
|
||
if (!m_mapping) return;
|
||
|
||
for (size_t i = 0; i < values.size() && (startAddress + i) < m_mapping->nb_bits; ++i) {
|
||
m_mapping->tab_bits[startAddress + i] = values[i] ? 1 : 0;
|
||
}
|
||
}
|
||
|
||
void ModbusTCPServer::updateDiscreteInput(uint16_t address, bool value)
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_dataMutex);
|
||
if (m_mapping && address < m_mapping->nb_input_bits) {
|
||
m_mapping->tab_input_bits[address] = value ? 1 : 0;
|
||
}
|
||
}
|
||
|
||
void ModbusTCPServer::updateDiscreteInputs(uint16_t startAddress, const std::vector<bool>& values)
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_dataMutex);
|
||
if (!m_mapping) return;
|
||
|
||
for (size_t i = 0; i < values.size() && (startAddress + i) < m_mapping->nb_input_bits; ++i) {
|
||
m_mapping->tab_input_bits[startAddress + i] = values[i] ? 1 : 0;
|
||
}
|
||
}
|
||
|
||
void ModbusTCPServer::updateHoldingRegister(uint16_t address, uint16_t value)
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_dataMutex);
|
||
if (m_mapping && address < m_mapping->nb_registers) {
|
||
m_mapping->tab_registers[address] = value;
|
||
}
|
||
}
|
||
|
||
void ModbusTCPServer::updateHoldingRegisters(uint16_t startAddress, const std::vector<uint16_t>& values)
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_dataMutex);
|
||
if (!m_mapping) return;
|
||
|
||
for (size_t i = 0; i < values.size() && (startAddress + i) < m_mapping->nb_registers; ++i) {
|
||
m_mapping->tab_registers[startAddress + i] = values[i];
|
||
}
|
||
}
|
||
|
||
void ModbusTCPServer::updateInputRegister(uint16_t address, uint16_t value)
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_dataMutex);
|
||
if (m_mapping && address < m_mapping->nb_input_registers) {
|
||
m_mapping->tab_input_registers[address] = value;
|
||
}
|
||
}
|
||
|
||
void ModbusTCPServer::updateInputRegisters(uint16_t startAddress, const std::vector<uint16_t>& values)
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_dataMutex);
|
||
if (!m_mapping) return;
|
||
|
||
for (size_t i = 0; i < values.size() && (startAddress + i) < m_mapping->nb_input_registers; ++i) {
|
||
m_mapping->tab_input_registers[startAddress + i] = values[i];
|
||
}
|
||
}
|
||
|
||
std::string ModbusTCPServer::getLastError() const
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_errorMutex);
|
||
return m_lastError;
|
||
}
|
||
|
||
void ModbusTCPServer::setLastError(const std::string& error)
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_errorMutex);
|
||
m_lastError = error;
|
||
}
|
||
|
||
bool IYModbusTCPServer::CreateInstance(IYModbusTCPServer** ppModbusTCPServer)
|
||
{
|
||
if (ppModbusTCPServer == nullptr) {
|
||
return false;
|
||
}
|
||
|
||
*ppModbusTCPServer = new ModbusTCPServer();
|
||
return true;
|
||
}
|