GrabBag/Module/ModbusTCPServer/Src/ModbusTCPServer.cpp

#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;
}