/*
 * SPDX-FileCopyrightText: 2006 Christian Walter
 *
 * SPDX-License-Identifier: BSD-3-Clause
 *
 * SPDX-FileContributor: 2016-2022 Espressif Systems (Shanghai) CO LTD
 */
/*
 * FreeModbus Libary: ESP32 TCP Port
 * Copyright (C) 2006 Christian Walter <wolti@sil.at>
 * Parts of crt0.S Copyright (c) 1995, 1996, 1998 Cygnus Support
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *   notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *   notice, this list of conditions and the following disclaimer in the
 *   documentation and/or other materials provided with the distribution.
 * 3. The name of the author may not be used to endorse or promote products
 *   derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
 * IF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
 * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
 * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
 * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * File: $Id: port.c,v 1.2 2006/09/04 14:39:20 wolti Exp $
 */

/* ----------------------- System includes ----------------------------------*/
#include <stdio.h>
#include <string.h>
#include "esp_err.h"
#include "esp_timer.h"

/* ----------------------- lwIP includes ------------------------------------*/
#include "lwip/err.h"
#include "lwip/sockets.h"
#include "lwip/netdb.h"
#include "esp_netif.h"

/* ----------------------- Modbus includes ----------------------------------*/
#include "mb_m.h"
#include "port.h"
#include "mbport.h"
#include "mbframe.h"
#include "port_tcp_master.h"

#if MB_MASTER_TCP_ENABLED

/* ----------------------- Defines  -----------------------------------------*/
#define MB_TCP_CONNECTION_TIMEOUT_MS    ( 20 )      // Connection timeout in mS
#define MB_TCP_RECONNECT_TIMEOUT        ( 5000000 ) // Connection timeout in uS

#define MB_EVENT_REQ_DONE_MASK          (   EV_MASTER_PROCESS_SUCCESS | \
                                            EV_MASTER_ERROR_RESPOND_TIMEOUT | \
                                            EV_MASTER_ERROR_RECEIVE_DATA | \
                                            EV_MASTER_ERROR_EXECUTE_FUNCTION )

#define MB_EVENT_REQ_ERR_MASK           ( EV_MASTER_PROCESS_SUCCESS )

#define MB_EVENT_WAIT_TOUT_MS           ( 3000 )
#define MB_SHDN_WAIT_TOUT_MS            ( 5000 )

#define MB_TCP_READ_TICK_MS             ( 1 )
#define MB_TCP_READ_BUF_RETRY_CNT       ( 4 )
#define MB_SLAVE_FMT(fmt)               "Slave #%d, Socket(#%d)(%s)"fmt

/* ----------------------- Types & Prototypes --------------------------------*/
void vMBPortEventClose(void);

/* ----------------------- Static variables ---------------------------------*/
static const char *TAG = "MB_TCP_MASTER_PORT";
static MbPortConfig_t xMbPortConfig;
static EventGroupHandle_t xMasterEventHandle = NULL;
static SemaphoreHandle_t xShutdownSema = NULL;
static EventBits_t xMasterEvent = 0;

/* ----------------------- Static functions ---------------------------------*/
static void vMBTCPPortMasterTask(void *pvParameters);

/* ----------------------- Begin implementation -----------------------------*/

// Waits for stack start event to start Modbus event processing
BOOL xMBTCPPortMasterWaitEvent(EventGroupHandle_t xEventHandle, EventBits_t xEvent, USHORT usTimeout)
{
    xMasterEventHandle = xEventHandle;
    xMasterEvent = xEvent;
    BaseType_t status = xEventGroupWaitBits(xMasterEventHandle,
                                            (BaseType_t)(xEvent),
                                            pdFALSE, // do not clear start bit
                                            pdFALSE,
                                            usTimeout);
    return (BOOL)(status & xEvent);
}

BOOL
xMBMasterTCPPortInit( USHORT usTCPPort )
{
    BOOL bOkay = FALSE;

    xMbPortConfig.pxMbSlaveInfo = calloc(MB_TCP_PORT_MAX_CONN, sizeof(MbSlaveInfo_t*));
    if (!xMbPortConfig.pxMbSlaveInfo) {
        ESP_LOGE(TAG, "TCP slave info alloc failure.");
        return FALSE;
    }
    for(int idx = 0; idx < MB_TCP_PORT_MAX_CONN; xMbPortConfig.pxMbSlaveInfo[idx] = NULL, idx++);

    xMbPortConfig.xConnectQueue = NULL;
    xMbPortConfig.usPort = usTCPPort;
    xMbPortConfig.usMbSlaveInfoCount = 0;
    xMbPortConfig.ucCurSlaveIndex = 1;
    xMbPortConfig.pxMbSlaveCurrInfo = NULL;

    xMbPortConfig.xConnectQueue = xQueueCreate(2, sizeof(MbSlaveAddrInfo_t));
    if (xMbPortConfig.xConnectQueue == 0)
    {
        // Queue was not created and must not be used.
        ESP_LOGE(TAG, "TCP master queue creation failure.");
        return FALSE;
    }

    // Create task for packet processing
    BaseType_t xErr = xTaskCreatePinnedToCore(vMBTCPPortMasterTask,
                                              "mbm_port_tcp_task",
                                              MB_TCP_STACK_SIZE,
                                              NULL,
                                              MB_TCP_TASK_PRIO,
                                              &xMbPortConfig.xMbTcpTaskHandle,
                                              MB_PORT_TASK_AFFINITY);
    if (xErr != pdTRUE)
    {
        ESP_LOGE(TAG, "TCP master task creation failure.");
        (void)vTaskDelete(xMbPortConfig.xMbTcpTaskHandle);
    } else {
        ESP_LOGI(TAG, "TCP master stack initialized.");
        bOkay = TRUE;
    }
    return bOkay;
}

static MbSlaveInfo_t *vMBTCPPortMasterFindSlaveInfo(UCHAR ucSlaveAddr)
{
    int xIndex;
    BOOL xFound = false;
    for (xIndex = 0; xIndex < xMbPortConfig.usMbSlaveInfoCount; xIndex++) {
        if (xMbPortConfig.pxMbSlaveInfo[xIndex]->ucSlaveAddr == ucSlaveAddr) {
            xMbPortConfig.pxMbSlaveCurrInfo = xMbPortConfig.pxMbSlaveInfo[xIndex];
            xFound = TRUE;
            xMbPortConfig.ucCurSlaveIndex = xIndex;
        }
    }
    if (!xFound) {
        xMbPortConfig.pxMbSlaveCurrInfo = NULL;
        ESP_LOGE(TAG, "Slave info for short address %u not found.", ucSlaveAddr);
    }
    return xMbPortConfig.pxMbSlaveCurrInfo;
}

static MbSlaveInfo_t *vMBTCPPortMasterGetCurrInfo(void)
{
    if (!xMbPortConfig.pxMbSlaveCurrInfo) {
        ESP_LOGE(TAG, "Incorrect current slave info.");
    }
    return xMbPortConfig.pxMbSlaveCurrInfo;
}

// Start Modbus event state machine
static void vMBTCPPortMasterStartPoll(void)
{
    if (xMasterEventHandle) {
        // Set the mbcontroller start flag
        EventBits_t xFlags = xEventGroupSetBits(xMasterEventHandle,
                                                (EventBits_t)xMasterEvent);
        if (!(xFlags & xMasterEvent)) {
            ESP_LOGE(TAG, "Fail to start TCP stack.");
        }
    } else {
        ESP_LOGE(TAG, "Fail to start polling. Incorrect event handle...");
    }
}

// Stop Modbus event state machine
static void vMBTCPPortMasterStopPoll(void)
{
    if (xMasterEventHandle) {
        // Set the mbcontroller start flag
        EventBits_t xFlags = xEventGroupClearBits(xMasterEventHandle,
                                                (EventBits_t)xMasterEvent);
        if (!(xFlags & xMasterEvent)) {
            ESP_LOGE(TAG, "Fail to stop polling.");
        }
    } else {
        ESP_LOGE(TAG, "Fail to stop polling. Incorrect event handle...");
    }
}

// The helper function to get time stamp in microseconds
static int64_t xMBTCPGetTimeStamp(void)
{
    int64_t xTimeStamp = esp_timer_get_time();
    return xTimeStamp;
}

static void vMBTCPPortMasterMStoTimeVal(USHORT usTimeoutMs, struct timeval *tv)
{
    tv->tv_sec = usTimeoutMs / 1000;
    tv->tv_usec = (usTimeoutMs - (tv->tv_sec * 1000)) * 1000;
}

static BOOL xMBTCPPortMasterCloseConnection(MbSlaveInfo_t *pxInfo)
{
    if (!pxInfo) {
        return FALSE;
    }
    if (pxInfo->xSockId == -1) {
        ESP_LOGE(TAG, "Wrong socket info or disconnected socket: %d, skip.", (int)pxInfo->xSockId);
        return FALSE;
    }
    if (shutdown(pxInfo->xSockId, SHUT_RDWR) == -1) {
        ESP_LOGV(TAG, "Shutdown failed sock %d, errno=%u", (int)pxInfo->xSockId, (unsigned)errno);
    }
    close(pxInfo->xSockId);
    pxInfo->xSockId = -1;
    return TRUE;
}

static void xMBTCPPortMasterShutdown(void)
{
    xSemaphoreGive(xShutdownSema);

    for (USHORT ucCnt = 0; ucCnt < MB_TCP_PORT_MAX_CONN; ucCnt++) {
        MbSlaveInfo_t* pxInfo = xMbPortConfig.pxMbSlaveInfo[ucCnt];
        if (pxInfo) {
            xMBTCPPortMasterCloseConnection(pxInfo);
            if (pxInfo->pucRcvBuf) {
                free(pxInfo->pucRcvBuf);
            }
            free(pxInfo);
            xMbPortConfig.pxMbSlaveInfo[ucCnt] = NULL;
        }
    }
    free(xMbPortConfig.pxMbSlaveInfo);
    vTaskDelete(NULL);
    xMbPortConfig.xMbTcpTaskHandle = NULL;
}

void vMBTCPPortMasterSetNetOpt(void *pvNetIf, eMBPortIpVer xIpVersion, eMBPortProto xProto)
{
    xMbPortConfig.pvNetIface = pvNetIf;
    xMbPortConfig.eMbProto = xProto;
    xMbPortConfig.eMbIpVer = xIpVersion;
}

// Function returns time left for response processing according to response timeout
static int64_t xMBTCPPortMasterGetRespTimeLeft(MbSlaveInfo_t *pxInfo)
{
    if (!pxInfo) {
        return 0;
    }
    int64_t xTimeStamp = xMBTCPGetTimeStamp() - pxInfo->xSendTimeStamp;
    return (xTimeStamp > (1000 * MB_MASTER_TIMEOUT_MS_RESPOND)) ? 0 :
                    (MB_MASTER_TIMEOUT_MS_RESPOND - (xTimeStamp / 1000) - 1);
}

// Wait socket ready to read state
static int vMBTCPPortMasterRxCheck(int xSd, fd_set *pxFdSet, int xTimeMs)
{
    fd_set xReadSet = *pxFdSet;
    fd_set xErrorSet = *pxFdSet;
    int xRes = 0;
    struct timeval xTimeout;

    vMBTCPPortMasterMStoTimeVal(xTimeMs, &xTimeout);
    xRes = select(xSd + 1, &xReadSet, NULL, &xErrorSet, &xTimeout);
    if (xRes == 0) {
        // No respond from slave during timeout
        xRes = ERR_TIMEOUT;
    } else if ((xRes < 0) || FD_ISSET(xSd, &xErrorSet)) {
        xRes = -1;
    }

    *pxFdSet = xReadSet;
    return xRes;
}

static int xMBTCPPortMasterGetBuf(MbSlaveInfo_t *pxInfo, UCHAR *pucDstBuf, USHORT usLength, uint16_t xTimeMs)
{
    int xLength = 0;
    UCHAR *pucBuf = pucDstBuf;
    USHORT usBytesLeft = usLength;
    struct timeval xTime;

    MB_PORT_CHECK((pxInfo && pxInfo->xSockId > -1), -1, "Try to read incorrect socket = #%d.", (int)pxInfo->xSockId);

    // Set receive timeout for socket <= slave respond time
    xTime.tv_sec = xTimeMs / 1000;
    xTime.tv_usec = (xTimeMs % 1000) * 1000;
    setsockopt(pxInfo->xSockId, SOL_SOCKET, SO_RCVTIMEO, &xTime, sizeof(xTime));

    // Receive data from connected client
    while (usBytesLeft > 0) {
        TCP_PORT_CHECK_SHDN(xShutdownSema, xMBTCPPortMasterShutdown);
        xLength = recv(pxInfo->xSockId, pucBuf, usBytesLeft, 0);
        if (xLength < 0) {
            if (errno == EAGAIN) {
                // Read timeout occurred, check the timeout and return
            } else if (errno == ENOTCONN) {
                // Socket connection closed
                ESP_LOGE(TAG, "Socket(#%d)(%s) connection closed.",
                         (int)pxInfo->xSockId, pxInfo->pcIpAddr);
                return ERR_CONN;
            } else {
                // Other error occurred during receiving
                ESP_LOGE(TAG, "Socket(#%d)(%s) receive error, length=%u, errno=%u",
                         (int)pxInfo->xSockId, pxInfo->pcIpAddr, (unsigned)xLength, (unsigned)errno);
                return -1;
            }
        } else if (xLength) {
            pucBuf += xLength;
            usBytesLeft -= xLength;
        }
        if (xMBTCPPortMasterGetRespTimeLeft(pxInfo) == 0) {
            return ERR_TIMEOUT;
        }
    }
    return usLength;
}

static int vMBTCPPortMasterReadPacket(MbSlaveInfo_t *pxInfo)
{
    int xLength = 0;
    int xRet = 0;
    USHORT usTidRcv = 0;

    // Receive data from connected client
    if (pxInfo) {
        MB_PORT_CHECK((pxInfo->xSockId > 0), -1, "Try to read incorrect socket = #%d.", pxInfo->xSockId);
        // Read packet header
        xRet = xMBTCPPortMasterGetBuf(pxInfo, &pxInfo->pucRcvBuf[0],
                                        MB_TCP_UID, xMBTCPPortMasterGetRespTimeLeft(pxInfo));
        if (xRet < 0) {
            pxInfo->xRcvErr = xRet;
            return xRet;
        } else if (xRet != MB_TCP_UID) {
            ESP_LOGD(TAG, "Socket (#%d)(%s), Fail to read modbus header. ret=%d",
                     (int)pxInfo->xSockId, pxInfo->pcIpAddr, (int)xRet);
            pxInfo->xRcvErr = ERR_VAL;
            return ERR_VAL;
        }
        // If we have received the MBAP header we can analyze it and calculate
        // the number of bytes left to complete the current request.
        xLength = (int)MB_TCP_GET_FIELD(pxInfo->pucRcvBuf, MB_TCP_LEN);
        xRet = xMBTCPPortMasterGetBuf(pxInfo, &pxInfo->pucRcvBuf[MB_TCP_UID],
                                        xLength, xMBTCPPortMasterGetRespTimeLeft(pxInfo));
        if (xRet < 0) {
            pxInfo->xRcvErr = xRet;
            return xRet;
        } else if (xRet != xLength) {
            // Received incorrect or fragmented packet.
            ESP_LOGD(TAG, "Socket(#%d)(%s) incorrect packet, length=%u, TID=0x%02x, errno=%u(%s)",
                     (int)pxInfo->xSockId, pxInfo->pcIpAddr, (int)pxInfo->usRcvPos,
                     (int)usTidRcv, (unsigned)errno, strerror(errno));
            pxInfo->xRcvErr = ERR_VAL;
            return ERR_VAL;
        }
        usTidRcv = MB_TCP_GET_FIELD(pxInfo->pucRcvBuf, MB_TCP_TID);

        // Check transaction identifier field in the incoming packet.
        if ((pxInfo->usTidCnt - 1) != usTidRcv) {
            ESP_LOGD(TAG, "Socket (#%d)(%s), incorrect TID(0x%02x)!=(0x%02x) received, discard data.",
                     (int)pxInfo->xSockId, pxInfo->pcIpAddr, (int)usTidRcv, (int)(pxInfo->usTidCnt - 1));
            pxInfo->xRcvErr = ERR_BUF;
            return ERR_BUF;
        }
        pxInfo->usRcvPos += xRet + MB_TCP_UID;
        ESP_LOGD(TAG, "Socket(#%d)(%s) get data, length=%u, TID=0x%02x, errno=%u(%s)",
                 (int)pxInfo->xSockId, pxInfo->pcIpAddr, (unsigned)pxInfo->usRcvPos,
                 (unsigned)usTidRcv, (unsigned)errno, strerror(errno));
        pxInfo->xRcvErr = ERR_OK;
        return pxInfo->usRcvPos;
    }
    return -1;
}

static err_t xMBTCPPortMasterSetNonBlocking(MbSlaveInfo_t *pxInfo)
{
    if (!pxInfo) {
        return ERR_CONN;
    }
    // Set non blocking attribute for socket
    ULONG ulFlags = fcntl(pxInfo->xSockId, F_GETFL);
    if (fcntl(pxInfo->xSockId, F_SETFL, ulFlags | O_NONBLOCK) == -1) {
        ESP_LOGE(TAG, "Socket(#%d)(%s), fcntl() call error=%u",
                 (int)pxInfo->xSockId, pxInfo->pcIpAddr, (unsigned)errno);
        return ERR_WOULDBLOCK;
    }
    return ERR_OK;
}

static void vMBTCPPortSetKeepAlive(MbSlaveInfo_t *pxInfo)
{
    int optval = 1;
    setsockopt(pxInfo->xSockId, SOL_SOCKET, SO_KEEPALIVE, &optval, sizeof(optval));
}

// Check connection for timeout helper
static err_t xMBTCPPortMasterCheckAlive(MbSlaveInfo_t *pxInfo, ULONG xTimeoutMs)
{
    fd_set xWriteSet;
    fd_set xErrorSet;
    err_t xErr = -1;
    struct timeval xTimeVal;

    if (pxInfo && pxInfo->xSockId != -1) {
        FD_ZERO(&xWriteSet);
        FD_ZERO(&xErrorSet);
        FD_SET(pxInfo->xSockId, &xWriteSet);
        FD_SET(pxInfo->xSockId, &xErrorSet);
        vMBTCPPortMasterMStoTimeVal(xTimeoutMs, &xTimeVal);
        // Check if the socket is writable
        xErr = select(pxInfo->xSockId + 1, NULL, &xWriteSet, &xErrorSet, &xTimeVal);
        if ((xErr < 0) || FD_ISSET(pxInfo->xSockId, &xErrorSet)) {
            if (errno == EINPROGRESS) {
                xErr = ERR_INPROGRESS;
            } else {
                ESP_LOGV(TAG, MB_SLAVE_FMT(" connection, select write err(errno) = 0x%x(%u)."),
                         (int)pxInfo->xIndex, (int)pxInfo->xSockId, pxInfo->pcIpAddr, (int)xErr, (unsigned)errno);
                xErr = ERR_CONN;
            }
        } else if (xErr == 0) {
            ESP_LOGV(TAG, "Socket(#%d)(%s), connection timeout occurred, err(errno) = 0x%x(%u).",
                     (int)pxInfo->xSockId, pxInfo->pcIpAddr, (int)xErr, (unsigned)errno);
            return ERR_INPROGRESS;
        } else {
            int xOptErr = 0;
            ULONG ulOptLen = sizeof(xOptErr);
            // Check socket error
            xErr = getsockopt(pxInfo->xSockId, SOL_SOCKET, SO_ERROR, (void*)&xOptErr, (socklen_t*)&ulOptLen);
            if (xOptErr != 0) {
                ESP_LOGD(TAG, "Socket(#%d)(%s), sock error occurred (%u).",
                         (int)pxInfo->xSockId, pxInfo->pcIpAddr, (unsigned)xOptErr);
                return ERR_CONN;
            }
            ESP_LOGV(TAG, "Socket(#%d)(%s), is alive.",
                     (int)pxInfo->xSockId, pxInfo->pcIpAddr);
            return ERR_OK;
        }
    } else {
        xErr = ERR_CONN;
    }
    return xErr;
}

// Resolve host name and/or fill the IP address structure
static BOOL xMBTCPPortMasterCheckHost(const CHAR *pcHostStr, ip_addr_t *pxHostAddr)
{
    MB_PORT_CHECK((pcHostStr), FALSE, "Wrong host name or IP.");
    CHAR cStr[45];
    CHAR *pcStr = &cStr[0];
    ip_addr_t xTargetAddr;
    struct addrinfo xHint;
    struct addrinfo *pxAddrList;
    memset(&xHint, 0, sizeof(xHint));
    // Do name resolution for both protocols
    xHint.ai_family = AF_UNSPEC;
    xHint.ai_flags = AI_ADDRCONFIG; // get IPV6 address if supported, otherwise IPV4
    memset(&xTargetAddr, 0, sizeof(xTargetAddr));

    // convert domain name to IP address
    // Todo: check EAI_FAIL error when resolve host name
    int xRet = getaddrinfo(pcHostStr, NULL, &xHint, &pxAddrList);

    if (xRet != 0) {
        ESP_LOGE(TAG, "Incorrect host name or IP: %s", pcHostStr);
        return FALSE;
    }
    if (pxAddrList->ai_family == AF_INET) {
        struct in_addr addr4 = ((struct sockaddr_in *) (pxAddrList->ai_addr))->sin_addr;
        inet_addr_to_ip4addr(ip_2_ip4(&xTargetAddr), &addr4);
        pcStr = ip4addr_ntoa_r(ip_2_ip4(&xTargetAddr), cStr, sizeof(cStr));
    }
#if CONFIG_LWIP_IPV6
    else {
        struct in6_addr addr6 = ((struct sockaddr_in6 *) (pxAddrList->ai_addr))->sin6_addr;
        inet6_addr_to_ip6addr(ip_2_ip6(&xTargetAddr), &addr6);
        pcStr = ip6addr_ntoa_r(ip_2_ip6(&xTargetAddr), cStr, sizeof(cStr));
    }
#endif
    if (pxHostAddr) {
        *pxHostAddr = xTargetAddr;
    }
    ESP_LOGI(TAG, "Host IP: [%s]", pcStr);
    freeaddrinfo(pxAddrList);
    return TRUE;
}

BOOL xMBTCPPortMasterAddSlaveIp(const USHORT usIndex, const CHAR *pcIpStr, UCHAR ucSlaveAddress)
{
    BOOL xRes = FALSE;
    MbSlaveAddrInfo_t xSlaveAddrInfo = {0};
    MB_PORT_CHECK(xMbPortConfig.xConnectQueue != NULL, FALSE, "Wrong slave IP address to add.");
    if (pcIpStr && (usIndex != 0xFF)) {
        xRes = xMBTCPPortMasterCheckHost(pcIpStr, NULL);
    }
    if (xRes || !pcIpStr) {
        xSlaveAddrInfo.pcIPAddr = pcIpStr;
        xSlaveAddrInfo.usIndex = usIndex;
        xSlaveAddrInfo.ucSlaveAddr = ucSlaveAddress;
        BaseType_t xStatus = xQueueSend(xMbPortConfig.xConnectQueue, (void *)&xSlaveAddrInfo, 100);
        MB_PORT_CHECK((xStatus == pdTRUE), FALSE, "FAIL to add slave IP address: [%s].", pcIpStr);
    }
    return xRes;
}

// Unblocking connect function
static err_t xMBTCPPortMasterConnect(MbSlaveInfo_t *pxInfo)
{
    if (!pxInfo) {
        return ERR_CONN;
    }

    err_t xErr = ERR_OK;
    CHAR cStr[128];
    CHAR *pcStr = NULL;
    ip_addr_t xTargetAddr;
    struct addrinfo xHint;
    struct addrinfo *pxAddrList;
    struct addrinfo *pxCurAddr;

    memset(&xHint, 0, sizeof(xHint));
    // Do name resolution for both protocols
    // xHint.ai_family = AF_UNSPEC; Todo: Find a reason why AF_UNSPEC does not work
    xHint.ai_flags = AI_ADDRCONFIG; // get IPV6 address if supported, otherwise IPV4
    xHint.ai_family = (xMbPortConfig.eMbIpVer == MB_PORT_IPV4) ? AF_INET : AF_INET6;
    xHint.ai_socktype = (pxInfo->xMbProto == MB_PROTO_UDP) ? SOCK_DGRAM : SOCK_STREAM;
    xHint.ai_protocol = (pxInfo->xMbProto == MB_PROTO_UDP) ? IPPROTO_UDP : IPPROTO_TCP;
    memset(&xTargetAddr, 0, sizeof(xTargetAddr));

    if (asprintf(&pcStr, "%u", xMbPortConfig.usPort) == -1) {
        abort();
    }

    // convert domain name to IP address
    int xRet = getaddrinfo(pxInfo->pcIpAddr, pcStr, &xHint, &pxAddrList);
    free(pcStr);
    if (xRet != 0) {
        ESP_LOGE(TAG, "Cannot resolve host: %s", pxInfo->pcIpAddr);
        return ERR_CONN;
    }

    for (pxCurAddr = pxAddrList; pxCurAddr != NULL; pxCurAddr = pxCurAddr->ai_next) {
        if (pxCurAddr->ai_family == AF_INET) {
            struct in_addr addr4 = ((struct sockaddr_in *) (pxCurAddr->ai_addr))->sin_addr;
            inet_addr_to_ip4addr(ip_2_ip4(&xTargetAddr), &addr4);
            pcStr = ip4addr_ntoa_r(ip_2_ip4(&xTargetAddr), cStr, sizeof(cStr));
        }
#if CONFIG_LWIP_IPV6
        else if (pxCurAddr->ai_family == AF_INET6) {
            struct in6_addr addr6 = ((struct sockaddr_in6 *) (pxCurAddr->ai_addr))->sin6_addr;
            inet6_addr_to_ip6addr(ip_2_ip6(&xTargetAddr), &addr6);
            pcStr = ip6addr_ntoa_r(ip_2_ip6(&xTargetAddr), cStr, sizeof(cStr));
            // Set scope id to fix routing issues with local address
            ((struct sockaddr_in6 *)(pxCurAddr->ai_addr))->sin6_scope_id =
                esp_netif_get_netif_impl_index(xMbPortConfig.pvNetIface);
        }
#endif
        if (pxInfo->xSockId <= 0) {
            pxInfo->xSockId = socket(pxCurAddr->ai_family, pxCurAddr->ai_socktype, pxCurAddr->ai_protocol);
            if (pxInfo->xSockId < 0) {
                ESP_LOGE(TAG, "Unable to create socket: #%d, errno %u", (int)pxInfo->xSockId, (unsigned)errno);
                xErr = ERR_IF;
                continue;
            }
        } else {
            ESP_LOGV(TAG, "Socket (#%d)(%s) created.", (int)pxInfo->xSockId, cStr);
        }

        // Set non blocking attribute for socket
        xMBTCPPortMasterSetNonBlocking(pxInfo);

        // Can return EINPROGRESS as an error which means
        // that connection is in progress and should be checked later
        xErr = connect(pxInfo->xSockId, (struct sockaddr*)pxCurAddr->ai_addr, pxCurAddr->ai_addrlen);
        if ((xErr < 0) && (errno == EINPROGRESS || errno == EALREADY)) {
            // The unblocking connect is pending (check status later) or already connected
            ESP_LOGV(TAG, "Socket(#%d)(%s) connection is pending, errno %u (%s).",
                     (int)pxInfo->xSockId, cStr, (unsigned)errno, strerror(errno));

            // Set keep alive flag in socket options
            vMBTCPPortSetKeepAlive(pxInfo);
            xErr = xMBTCPPortMasterCheckAlive(pxInfo, MB_TCP_CONNECTION_TIMEOUT_MS);
            continue;
        } else if ((xErr < 0) && (errno == EISCONN)) {
            // Socket already connected
            xErr = ERR_OK;
            continue;
        } else if (xErr != ERR_OK) {
            // Other error occurred during connection
            ESP_LOGV(TAG, MB_SLAVE_FMT(" unable to connect, error=0x%x, errno %u (%s)"),
                     (int)pxInfo->xIndex, (int)pxInfo->xSockId, cStr, (int)xErr, (unsigned)errno, strerror(errno));
            xMBTCPPortMasterCloseConnection(pxInfo);
            xErr = ERR_CONN;
        } else {
            ESP_LOGI(TAG, MB_SLAVE_FMT(", successfully connected."),
                     (int)pxInfo->xIndex, (int)pxInfo->xSockId, cStr);
            continue;
        }
    }
    freeaddrinfo(pxAddrList);
    return xErr;
}

// Find the first slave info whose descriptor is set in xFdSet
static MbSlaveInfo_t *xMBTCPPortMasterGetSlaveReady(fd_set *pxFdSet)
{
    MbSlaveInfo_t *pxInfo = NULL;

    // Slave connection loop
    for (int xIndex = 0; (xIndex < MB_TCP_PORT_MAX_CONN); xIndex++) {
        pxInfo = xMbPortConfig.pxMbSlaveInfo[xIndex];
        if (pxInfo) {
            // Is this response for current processing slave
            if (FD_ISSET(pxInfo->xSockId, pxFdSet)) {
                FD_CLR(pxInfo->xSockId, pxFdSet);
                return pxInfo;
            }
        }
    }
    return (MbSlaveInfo_t *)NULL;
}

static int xMBTCPPortMasterCheckConnState(fd_set *pxFdSet)
{
    fd_set xConnSetCheck = *pxFdSet;
    MbSlaveInfo_t *pxInfo = NULL;
    int64_t xTime = 0;
    int xErr = 0;
    int xCount = 0;
    do {
        xTime = xMBTCPGetTimeStamp();
        pxInfo = xMBTCPPortMasterGetSlaveReady(&xConnSetCheck);
        if (pxInfo) {
            xErr = xMBTCPPortMasterCheckAlive(pxInfo, 0);
            if ((xErr < 0) && (((xTime - pxInfo->xRecvTimeStamp) > MB_TCP_RECONNECT_TIMEOUT) ||
                                ((xTime - pxInfo->xSendTimeStamp) > MB_TCP_RECONNECT_TIMEOUT))) {
                ESP_LOGI(TAG, MB_SLAVE_FMT(", slave is down, off_time[r][w](us) = [%" PRIu64 "][%" PRIu64 "]."),
                         (int)pxInfo->xIndex,
                         (int)pxInfo->xSockId,
                         pxInfo->pcIpAddr,
                         (int64_t)(xTime - pxInfo->xRecvTimeStamp),
                         (int64_t)(xTime - pxInfo->xSendTimeStamp));
                xCount++;
            }
        }
    } while (pxInfo && (xCount < MB_TCP_PORT_MAX_CONN));
    return xCount;
}

static void xMBTCPPortMasterFsmSetError(eMBMasterErrorEventType xErrType, eMBMasterEventEnum xPostEvent)
{
    vMBMasterPortTimersDisable();
    vMBMasterSetErrorType(xErrType);
    xMBMasterPortEventPost(xPostEvent);
}

static void vMBTCPPortMasterTask(void *pvParameters)
{
    MbSlaveInfo_t *pxInfo;
    MbSlaveInfo_t *pxCurrInfo;

    fd_set xConnSet;
    fd_set xReadSet;
    int xMaxSd = 0;
    err_t xErr = ERR_ABRT;
    USHORT usSlaveConnCnt = 0;
    int64_t xTime = 0;

    // Register each slave in the connection info structure
    while (1) {
        MbSlaveAddrInfo_t xSlaveAddrInfo = { 0 };
        BaseType_t xStatus = xQueueReceive(xMbPortConfig.xConnectQueue, (void*)&xSlaveAddrInfo, pdMS_TO_TICKS(MB_EVENT_WAIT_TOUT_MS));
        TCP_PORT_CHECK_SHDN(xShutdownSema, xMBTCPPortMasterShutdown);
        if (xStatus != pdTRUE) {
            ESP_LOGE(TAG, "Fail to register slave IP.");
        } else {
            if (xSlaveAddrInfo.pcIPAddr == NULL && xMbPortConfig.usMbSlaveInfoCount && xSlaveAddrInfo.usIndex == 0xFF) {
                // Init start timeout that allows to initialize the main FSM
                xMBMasterPortEventPost(EV_MASTER_READY);
                break;
            }
            if (xMbPortConfig.usMbSlaveInfoCount > MB_TCP_PORT_MAX_CONN) {
                ESP_LOGE(TAG, "Exceeds maximum connections limit=%u.", (unsigned)MB_TCP_PORT_MAX_CONN);
                break;
            }
            pxInfo = calloc(1, sizeof(MbSlaveInfo_t));
            if (!pxInfo) {
                ESP_LOGE(TAG, "Slave(#%u), info structure allocation fail.",
                         (unsigned)xMbPortConfig.usMbSlaveInfoCount);
                free(pxInfo);
                break;
            }
            pxInfo->pucRcvBuf = calloc(MB_TCP_BUF_SIZE, sizeof(UCHAR));
            if (!pxInfo->pucRcvBuf) {
                ESP_LOGE(TAG, "Slave(#%u), receive buffer allocation fail.",
                         (unsigned)xMbPortConfig.usMbSlaveInfoCount);
                free(pxInfo->pucRcvBuf);
                break;
            }
            pxInfo->usRcvPos = 0;
            pxInfo->pcIpAddr = xSlaveAddrInfo.pcIPAddr;
            pxInfo->xSockId = -1;
            pxInfo->xError = -1;
            pxInfo->xRecvTimeStamp = xMBTCPGetTimeStamp();
            pxInfo->xSendTimeStamp = xMBTCPGetTimeStamp();
            pxInfo->xMbProto = MB_PROTO_TCP;
            pxInfo->ucSlaveAddr = xSlaveAddrInfo.ucSlaveAddr;
            pxInfo->xIndex = xSlaveAddrInfo.usIndex;
            pxInfo->usTidCnt = (USHORT)(xMbPortConfig.usMbSlaveInfoCount << 8U);
            // Register slave
            xMbPortConfig.pxMbSlaveInfo[xMbPortConfig.usMbSlaveInfoCount++] = pxInfo;
            ESP_LOGI(TAG, "Add slave IP: %s", xSlaveAddrInfo.pcIPAddr);
        }
    }

    // Main connection cycle
    while (1)
    {
        ESP_LOGI(TAG, "Connecting to slaves...");
        xTime = xMBTCPGetTimeStamp();
        usSlaveConnCnt = 0;
        CHAR ucDot = '.';
        while(usSlaveConnCnt < xMbPortConfig.usMbSlaveInfoCount) {
            usSlaveConnCnt = 0;
            FD_ZERO(&xConnSet);
            ucDot ^= 0x03;
            // Slave connection loop
            for (UCHAR ucCnt = 0; (ucCnt < MB_TCP_PORT_MAX_CONN); ucCnt++) {
                pxInfo = xMbPortConfig.pxMbSlaveInfo[ucCnt];
                // if slave descriptor is NULL then it is end of list or connection closed.
                if (!pxInfo) {
                    ESP_LOGV(TAG, "Index: % is not initialized, skip.", ucCnt);
                    if (xMbPortConfig.usMbSlaveInfoCount) {
                        continue;
                    }
                    break;
                }
                putchar(ucDot);
                // if we don't yield we run the risk of hogging CPU
                vTaskDelay(pdMS_TO_TICKS(MB_TCP_CONNECTION_TIMEOUT_MS));
                xErr = xMBTCPPortMasterConnect(pxInfo);
                switch(xErr)
                {
                    case ERR_CONN:
                    case ERR_INPROGRESS:
                        // In case of connection errors remove the socket from set
                        if (FD_ISSET(pxInfo->xSockId, &xConnSet)) {
                            FD_CLR(pxInfo->xSockId, &xConnSet);
                            ESP_LOGE(TAG, MB_SLAVE_FMT(" connect failed, error = 0x%x."),
                                                                            (int)pxInfo->xIndex, (int)pxInfo->xSockId,
                                                                            (char*)pxInfo->pcIpAddr, (int)xErr);
                            if (usSlaveConnCnt) {
                                usSlaveConnCnt--;
                            }
                        }
                        break;
                    case ERR_OK:
                        // if connection is successful, add the descriptor into set
                        if (!FD_ISSET(pxInfo->xSockId, &xConnSet)) {
                            FD_SET(pxInfo->xSockId, &xConnSet);
                            usSlaveConnCnt++;
                            xMaxSd = (pxInfo->xSockId > xMaxSd) ? pxInfo->xSockId : xMaxSd;
                            ESP_LOGD(TAG, MB_SLAVE_FMT(", connected %u slave(s), error = 0x%x."),
                                                                (int)pxInfo->xIndex, (int)pxInfo->xSockId,
                                                                pxInfo->pcIpAddr,
                                                                (unsigned)usSlaveConnCnt, (int)xErr);
                            // Update time stamp for connected slaves
                            pxInfo->xRecvTimeStamp = xMBTCPGetTimeStamp();
                            pxInfo->xSendTimeStamp = xMBTCPGetTimeStamp();
                        }
                        break;
                    default:
                        ESP_LOGE(TAG, MB_SLAVE_FMT(", unexpected error = 0x%x."),
                                                            (int)pxInfo->xIndex,
                                                            (int)pxInfo->xSockId,
                                                            pxInfo->pcIpAddr, (int)xErr);
                        break;
                }
                if (pxInfo) {
                    pxInfo->xError = xErr;
                }
                TCP_PORT_CHECK_SHDN(xShutdownSema, xMBTCPPortMasterShutdown);
            }
        }
        ESP_LOGI(TAG, "Connected %u slaves, start polling...", (unsigned)usSlaveConnCnt);

        vMBTCPPortMasterStartPoll(); // Send event to start stack

        // Slave receive data loop
        while(usSlaveConnCnt) {
            xReadSet = xConnSet;
            // Check transmission event to clear appropriate bit.
            xMBMasterPortFsmWaitConfirmation(EV_MASTER_FRAME_TRANSMIT, pdMS_TO_TICKS(MB_EVENT_WAIT_TOUT_MS));
            TCP_PORT_CHECK_SHDN(xShutdownSema, xMBTCPPortMasterShutdown);
            // Synchronize state machine with send packet event
            if (xMBMasterPortFsmWaitConfirmation(EV_MASTER_FRAME_SENT, pdMS_TO_TICKS(MB_EVENT_WAIT_TOUT_MS))) {
                ESP_LOGD(TAG, "FSM Synchronized with sent event.");
            }
            // Get slave info for the current slave.
            pxCurrInfo = vMBTCPPortMasterGetCurrInfo();
            if (!pxCurrInfo) {
                ESP_LOGE(TAG, "Incorrect connection options for slave index: %d.",
                         (int)xMbPortConfig.ucCurSlaveIndex);
                vMBTCPPortMasterStopPoll();
                TCP_PORT_CHECK_SHDN(xShutdownSema, xMBTCPPortMasterShutdown);
                break; // incorrect slave descriptor, reconnect.
            }
            xTime = xMBTCPPortMasterGetRespTimeLeft(pxCurrInfo);
            ESP_LOGD(TAG, "Set select timeout, left time: %" PRIu64 " ms.",
                     xMBTCPPortMasterGetRespTimeLeft(pxCurrInfo));
            // Wait respond from current slave during respond timeout
            int xRes = vMBTCPPortMasterRxCheck(pxCurrInfo->xSockId, &xReadSet, xTime);
            if (xRes == ERR_TIMEOUT) {
                // No respond from current slave, process timeout.
                // Need to drop response later if it is received after timeout.
                ESP_LOGD(TAG, "Select timeout, left time: %" PRIu64 " ms.",
                         xMBTCPPortMasterGetRespTimeLeft(pxCurrInfo));
                xTime = xMBTCPPortMasterGetRespTimeLeft(pxCurrInfo);
                // Wait completion of last transaction
                xMBMasterPortFsmWaitConfirmation(MB_EVENT_REQ_DONE_MASK, pdMS_TO_TICKS(xTime + 1));
                TCP_PORT_CHECK_SHDN(xShutdownSema, xMBTCPPortMasterShutdown);
                continue;
            } else if (xRes < 0) {
                // Select error (slave connection or r/w failure).
                ESP_LOGD(TAG, MB_SLAVE_FMT(", socket select error. Slave disconnected?"),
                         (int)pxCurrInfo->xIndex, (int)pxCurrInfo->xSockId, pxCurrInfo->pcIpAddr);
                xTime = xMBTCPPortMasterGetRespTimeLeft(pxCurrInfo);
                // Wait completion of last transaction
                xMBMasterPortFsmWaitConfirmation(MB_EVENT_REQ_DONE_MASK, pdMS_TO_TICKS(xTime));
                // Stop polling process
                vMBTCPPortMasterStopPoll();
                TCP_PORT_CHECK_SHDN(xShutdownSema, xMBTCPPortMasterShutdown);
                // Check disconnected slaves, do not need a result just to print information.
                xMBTCPPortMasterCheckConnState(&xConnSet);
                break;
            } else {
                // Check to make sure that active slave data is ready
                if (FD_ISSET(pxCurrInfo->xSockId, &xReadSet)) {
                    int xRet = ERR_BUF;
                    for (int retry = 0; (xRet == ERR_BUF) && (retry < MB_TCP_READ_BUF_RETRY_CNT); retry++) {
                        xRet = vMBTCPPortMasterReadPacket(pxCurrInfo);
                        // The error ERR_BUF means received response to previous request
                        // (due to timeout) with the same socket ID and incorrect TID,
                        // then ignore it and try to get next response buffer.
                    }
                    if (xRet > 0) {
                        // Response received correctly, send an event to stack
                        xMBTCPPortMasterFsmSetError(EV_ERROR_INIT, EV_MASTER_FRAME_RECEIVED);
                        ESP_LOGD(TAG, MB_SLAVE_FMT(", frame received."),
                                    (int)pxCurrInfo->xIndex, (int)pxCurrInfo->xSockId, pxCurrInfo->pcIpAddr);
                    } else if ((xRet == ERR_TIMEOUT) || (xMBTCPPortMasterGetRespTimeLeft(pxCurrInfo) == 0)) {
                        // Timeout occurred when receiving frame, process respond timeout
                        xMBTCPPortMasterFsmSetError(EV_ERROR_RESPOND_TIMEOUT, EV_MASTER_ERROR_PROCESS);
                        ESP_LOGD(TAG, MB_SLAVE_FMT(", frame read timeout."),
                                    (int)pxCurrInfo->xIndex, (int)pxCurrInfo->xSockId, pxCurrInfo->pcIpAddr);
                    } else if (xRet == ERR_BUF) {
                        // After retries a response with incorrect TID received, process failure.
                        xMBTCPPortMasterFsmSetError(EV_ERROR_RECEIVE_DATA, EV_MASTER_ERROR_PROCESS);
                        ESP_LOGD(TAG, MB_SLAVE_FMT(", frame error."),
                                    (int)pxCurrInfo->xIndex, (int)pxCurrInfo->xSockId, pxCurrInfo->pcIpAddr);
                    } else {
                        ESP_LOGE(TAG, MB_SLAVE_FMT(", critical error=%d."),
                                 (int)pxCurrInfo->xIndex, (int)pxCurrInfo->xSockId, pxCurrInfo->pcIpAddr, (int)xRet);
                        // Stop polling process
                        vMBTCPPortMasterStopPoll();
                        TCP_PORT_CHECK_SHDN(xShutdownSema, xMBTCPPortMasterShutdown);
                        // Check disconnected slaves, do not need a result just to print information.
                        xMBTCPPortMasterCheckConnState(&xConnSet);
                        break;
                    }
                    xTime = xMBTCPPortMasterGetRespTimeLeft(pxCurrInfo);
                    ESP_LOGD(TAG, "Slave #%d, data processing left time %" PRIu64 " [ms].", (int)pxCurrInfo->xIndex, xTime);
                    // Wait completion of Modbus frame processing before start of new transaction.
                    if (xMBMasterPortFsmWaitConfirmation(MB_EVENT_REQ_DONE_MASK, pdMS_TO_TICKS(xTime))) {
                        ESP_LOGD(TAG, MB_SLAVE_FMT(", data processing completed."),
                                 (int)pxCurrInfo->xIndex, (int)pxCurrInfo->xSockId, pxCurrInfo->pcIpAddr);
                    }
                    xTime = xMBTCPGetTimeStamp() - pxCurrInfo->xSendTimeStamp;
                    ESP_LOGD(TAG, MB_SLAVE_FMT(", processing time[us] = %" PRIu64 "."),
                             (int)pxCurrInfo->xIndex, (int)pxCurrInfo->xSockId, pxCurrInfo->pcIpAddr, xTime);
                }
            }
            TCP_PORT_CHECK_SHDN(xShutdownSema, xMBTCPPortMasterShutdown);
        } // while(usMbSlaveInfoCount)
    } // while (1)
    vTaskDelete(NULL);
}

extern void vMBMasterPortEventClose(void);
extern void vMBMasterPortTimerClose(void);

void vMBMasterTCPPortEnable(void)
{

}

void vMBMasterTCPPortDisable(void)
{
    // Try to exit the task gracefully, so select could release its internal callbacks
    // that were allocated on the stack of the task we're going to delete
    xShutdownSema = xSemaphoreCreateBinary();
    // if no semaphore (alloc issues) or couldn't acquire it, just delete the task
    if (xShutdownSema == NULL || xSemaphoreTake(xShutdownSema, pdMS_TO_TICKS(MB_SHDN_WAIT_TOUT_MS)) != pdTRUE) {
        ESP_LOGW(TAG, "Modbus port task couldn't exit gracefully within timeout -> abruptly deleting the task.");
        vTaskDelete(xMbPortConfig.xMbTcpTaskHandle);
    }
    if (xShutdownSema) {
        vSemaphoreDelete(xShutdownSema);
        xShutdownSema = NULL;
    }
}

void vMBMasterTCPPortClose(void)
{
    vQueueDelete(xMbPortConfig.xConnectQueue);
    vMBMasterPortTimerClose();
    // Release resources for the event queue.
    vMBMasterPortEventClose();
}

BOOL xMBMasterTCPPortGetRequest(UCHAR **ppucMBTCPFrame, USHORT *usTCPLength)
{
    MbSlaveInfo_t *pxInfo = vMBTCPPortMasterGetCurrInfo();
    *ppucMBTCPFrame = pxInfo->pucRcvBuf;
    *usTCPLength = pxInfo->usRcvPos;

    // Reset the buffer.
    pxInfo->usRcvPos = 0;
    // Save slave receive timestamp
    if (pxInfo->xRcvErr == ERR_OK && *usTCPLength > 0) {
        pxInfo->xRecvTimeStamp = xMBTCPGetTimeStamp();
        return TRUE;
    }
    return FALSE;
}

int xMBMasterTCPPortWritePoll(MbSlaveInfo_t *pxInfo, const UCHAR *pucMBTCPFrame, USHORT usTCPLength, ULONG xTimeout)
{
    // Check if the socket is alive (writable and SO_ERROR == 0)
    int xRes = (int)xMBTCPPortMasterCheckAlive(pxInfo, xTimeout);
    if ((xRes < 0) && (xRes != ERR_INPROGRESS)) {
        ESP_LOGE(TAG, MB_SLAVE_FMT(", is not writable, error: %d, errno %u"),
                 (int)pxInfo->xIndex, (int)pxInfo->xSockId, pxInfo->pcIpAddr, (int)xRes, (unsigned)errno);
        return xRes;
    }
    xRes = send(pxInfo->xSockId, pucMBTCPFrame, usTCPLength, TCP_NODELAY);
    if (xRes < 0) {
        ESP_LOGE(TAG, MB_SLAVE_FMT(", send data error: %d, errno %u"),
                 (int)pxInfo->xIndex, (int)pxInfo->xSockId, pxInfo->pcIpAddr, (int)xRes, (unsigned)errno);
    }
    return xRes;
}

BOOL xMBMasterTCPPortSendResponse(UCHAR *pucMBTCPFrame, USHORT usTCPLength)
{
    BOOL bFrameSent = FALSE;
    USHORT ucCurSlaveIndex = ucMBMasterGetDestAddress();
    MbSlaveInfo_t *pxInfo = vMBTCPPortMasterFindSlaveInfo(ucCurSlaveIndex);

    // If the slave is correct and active then send data
    // otherwise treat slave as died and skip
    if (pxInfo != NULL) {
        if (pxInfo->xSockId < 0) {
            ESP_LOGD(TAG, MB_SLAVE_FMT(", send to died slave, error = %u"),
                                                (int)pxInfo->xIndex, (int)pxInfo->xSockId, pxInfo->pcIpAddr, (unsigned)pxInfo->xError);
        } else {
            // Apply TID field to the frame before send
            pucMBTCPFrame[MB_TCP_TID] = (UCHAR)(pxInfo->usTidCnt >> 8U);
            pucMBTCPFrame[MB_TCP_TID + 1] = (UCHAR)(pxInfo->usTidCnt & 0xFF);

            int xRes = xMBMasterTCPPortWritePoll(pxInfo, pucMBTCPFrame, usTCPLength, MB_TCP_SEND_TIMEOUT_MS);
            if (xRes < 0) {
                ESP_LOGE(TAG, MB_SLAVE_FMT(", send data failure, err(errno) = %d(%u)."),
                         (int)pxInfo->xIndex, (int)pxInfo->xSockId, pxInfo->pcIpAddr, (int)xRes, (unsigned)errno);
                bFrameSent = FALSE;
                pxInfo->xError = xRes;
            } else {
                bFrameSent = TRUE;
                ESP_LOGD(TAG, MB_SLAVE_FMT(", send data successful: TID=0x%02x, %d (bytes), errno %u"),
                         (int)pxInfo->xIndex, (int)pxInfo->xSockId, pxInfo->pcIpAddr, pxInfo->usTidCnt, (int)xRes, (unsigned)errno);
                pxInfo->xError = 0;
                pxInfo->usRcvPos = 0;
                if (pxInfo->usTidCnt < (USHRT_MAX - 1)) {
                    pxInfo->usTidCnt++;
                } else {
                    pxInfo->usTidCnt = (USHORT)(pxInfo->xIndex << 8U);
                }
            }
            pxInfo->xSendTimeStamp = xMBTCPGetTimeStamp();
        }
    } else {
        ESP_LOGD(TAG, "Send data to died slave, address = %u", (unsigned)ucCurSlaveIndex);
    }
    vMBMasterPortTimersRespondTimeoutEnable();
    xMBMasterPortEventPost(EV_MASTER_FRAME_SENT);
    return bFrameSent;
}

// Timer handler to check timeout of socket response
BOOL MB_PORT_ISR_ATTR
xMBMasterTCPTimerExpired(void)
{
    BOOL xNeedPoll = FALSE;
    eMBMasterTimerMode eTimerMode = xMBMasterGetCurTimerMode();

    vMBMasterPortTimersDisable();

    // If timer mode is respond timeout, the master event then turns EV_MASTER_EXECUTE status.
    if (eTimerMode == MB_TMODE_RESPOND_TIMEOUT) {
        vMBMasterSetErrorType(EV_ERROR_RESPOND_TIMEOUT);
        xNeedPoll = xMBMasterPortEventPost(EV_MASTER_ERROR_PROCESS);
    }

    return xNeedPoll;
}

#endif // #if MB_MASTER_TCP_ENABLED
