/* Copyright 2018 Espressif Systems (Shanghai) PTE LTD
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
*/
/*
  * FreeModbus Libary: ESP32 TCP Port
  * Copyright (C) 2006 Christian Walter <wolti@sil.at>
  * Parts of crt0.S Copyright (c) 1995, 1996, 1998 Cygnus Support
  *
  * This library is free software; you can redistribute it and/or
  * modify it under the terms of the GNU Lesser General Public
  * License as published by the Free Software Foundation; either
  * version 2.1 of the License, or (at your option) any later version.
  *
  * This library is distributed in the hope that it will be useful,
  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  * Lesser General Public License for more details.
  *
  * You should have received a copy of the GNU Lesser General Public
  * License along with this library; if not, write to the Free Software
  * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  *
  * File: $Id: port.h,v 1.2 2006/09/04 14:39:20 wolti Exp $
  */

/* ----------------------- System includes ----------------------------------*/
#include <stdio.h>
#include <string.h>
#include "esp_err.h"
#include "sys/time.h"
#include "esp_netif.h"

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

/* ----------------------- Modbus includes ----------------------------------*/
#include "mb.h"
#include "mbport.h"
#include "port.h"
#include "mbframe.h"
#include "port_tcp_slave.h"
#include "esp_modbus_common.h"      // for common types for network options

#if MB_TCP_ENABLED

/* ----------------------- Defines  -----------------------------------------*/
#define MB_TCP_DISCONNECT_TIMEOUT       ( CONFIG_FMB_TCP_CONNECTION_TOUT_SEC * 1000000 ) // disconnect timeout in uS
#define MB_TCP_RESP_TIMEOUT_MS          ( MB_MASTER_TIMEOUT_MS_RESPOND - 2 ) // slave response time limit
#define MB_TCP_SLAVE_PORT_TAG           "MB_TCP_SLAVE_PORT"
#define MB_TCP_NET_LISTEN_BACKLOG       ( SOMAXCONN )

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

/* ----------------------- Static variables ---------------------------------*/
static int xListenSock = -1;
static SemaphoreHandle_t xShutdownSemaphore = NULL;
static MbSlavePortConfig_t xConfig = { 0 };

/* ----------------------- Static functions ---------------------------------*/
// 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 vxMBTCPPortMStoTimeVal(USHORT usTimeoutMs, struct timeval *pxTimeout)
{
    pxTimeout->tv_sec = usTimeoutMs / 1000;
    pxTimeout->tv_usec = (usTimeoutMs - (pxTimeout->tv_sec * 1000)) * 1000;
}

static xQueueHandle xMBTCPPortRespQueueCreate(void)
{
    xQueueHandle xRespQueueHandle = xQueueCreate(2, sizeof(void*));
    MB_PORT_CHECK((xRespQueueHandle != NULL), NULL, "TCP respond queue creation failure.");
    return xRespQueueHandle;
}

static void vMBTCPPortRespQueueDelete(xQueueHandle xRespQueueHandle)
{
    vQueueDelete(xRespQueueHandle);
}

static void* vxMBTCPPortRespQueueRecv(xQueueHandle xRespQueueHandle)
{
    void* pvResp = NULL;
    MB_PORT_CHECK(xRespQueueHandle != NULL, NULL, "Response queue is not initialized.");
    BaseType_t xStatus = xQueueReceive(xRespQueueHandle,
                                        (void*)&pvResp,
                                        pdMS_TO_TICKS(MB_TCP_RESP_TIMEOUT_MS));
    MB_PORT_CHECK((xStatus == pdTRUE), NULL, "Could not get respond confirmation.");
    MB_PORT_CHECK((pvResp), NULL, "Incorrect response processing.");
    return pvResp;
}

static BOOL vxMBTCPPortRespQueueSend(xQueueHandle xRespQueueHandle, void* pvResp)
{
    MB_PORT_CHECK(xRespQueueHandle != NULL, FALSE, "Response queue is not initialized.");
    BaseType_t xStatus = xQueueSend(xConfig.xRespQueueHandle,
                                    (const void*)&pvResp,
                                    pdMS_TO_TICKS(MB_TCP_RESP_TIMEOUT_MS));
    MB_PORT_CHECK((xStatus == pdTRUE), FALSE, "FAIL to send to response queue.");
    return TRUE;
}

static void vMBTCPPortServerTask(void *pvParameters);

/* ----------------------- Begin implementation -----------------------------*/
BOOL
xMBTCPPortInit( USHORT usTCPPort )
{
    BOOL bOkay = FALSE;

    xConfig.pxMbClientInfo = calloc(MB_TCP_PORT_MAX_CONN + 1, sizeof(MbClientInfo_t*));
    if (!xConfig.pxMbClientInfo) {
        ESP_LOGE(MB_TCP_SLAVE_PORT_TAG, "TCP client info allocation failure.");
        return FALSE;
    }
    for(int idx = 0; idx < MB_TCP_PORT_MAX_CONN; xConfig.pxMbClientInfo[idx] = NULL, idx++);

    xConfig.xRespQueueHandle = xMBTCPPortRespQueueCreate();
    if (!xConfig.xRespQueueHandle) {
        ESP_LOGE(MB_TCP_SLAVE_PORT_TAG, "Response queue allocation failure.");
        return FALSE;
    }

    xConfig.usPort = usTCPPort;
    xConfig.eMbProto = MB_PROTO_TCP;
    xConfig.usClientCount = 0;
    xConfig.pvNetIface = NULL;
    xConfig.xIpVer = MB_PORT_IPV4;
    xConfig.pcBindAddr = NULL;

    // Create task for packet processing
    BaseType_t xErr = xTaskCreate(vMBTCPPortServerTask,
                                    "tcp_server_task",
                                    MB_TCP_STACK_SIZE,
                                    NULL,
                                    MB_TCP_TASK_PRIO,
                                    &xConfig.xMbTcpTaskHandle);
    vTaskSuspend(xConfig.xMbTcpTaskHandle);
    if (xErr != pdTRUE)
    {
        ESP_LOGE(MB_TCP_SLAVE_PORT_TAG, "Server task creation failure.");
        vTaskDelete(xConfig.xMbTcpTaskHandle);
    } else {
        ESP_LOGI(MB_TCP_SLAVE_PORT_TAG, "Protocol stack initialized.");
        bOkay = TRUE;
    }
    return bOkay;
}

void vMBTCPPortSlaveSetNetOpt(void* pvNetIf, eMBPortIpVer xIpVersion, eMBPortProto xProto, CHAR* pcBindAddrStr)
{
    // Set network options
    xConfig.pvNetIface = pvNetIf;
    xConfig.eMbProto = xProto;
    xConfig.xIpVer = xIpVersion;
    xConfig.pcBindAddr = pcBindAddrStr;
}

void vMBTCPPortSlaveStartServerTask(void)
{
    vTaskResume(xConfig.xMbTcpTaskHandle);
}

static int xMBTCPPortAcceptConnection(int xListenSockId, char** pcIPAddr)
{
    MB_PORT_CHECK(pcIPAddr, -1, "Wrong IP address pointer.");
    MB_PORT_CHECK((xListenSockId > 0), -1, "Incorrect listen socket ID.");

    // Address structure large enough for both IPv4 or IPv6 address
    struct sockaddr_storage xSrcAddr;
    CHAR cAddrStr[128];
    int xSockId = -1;
    CHAR* pcStr = NULL;
    socklen_t xSize = sizeof(struct sockaddr_storage);

    // Accept new socket connection if not active
    xSockId = accept(xListenSockId, (struct sockaddr *)&xSrcAddr, &xSize);
    if (xSockId < 0) {
        ESP_LOGE(MB_TCP_SLAVE_PORT_TAG, "Unable to accept connection: errno=%d", errno);
        close(xSockId);
    } else {
        // Get the sender's ip address as string
        if (xSrcAddr.ss_family == PF_INET) {
            inet_ntoa_r(((struct sockaddr_in *)&xSrcAddr)->sin_addr.s_addr, cAddrStr, sizeof(cAddrStr) - 1);
        }
#if CONFIG_LWIP_IPV6
        else if (xSrcAddr.ss_family == PF_INET6) {
            inet6_ntoa_r(((struct sockaddr_in6 *)&xSrcAddr)->sin6_addr, cAddrStr, sizeof(cAddrStr) - 1);
        }
#endif
        ESP_LOGI(MB_TCP_SLAVE_PORT_TAG, "Socket (#%d), accept client connection from address: %s", xSockId, cAddrStr);
        pcStr = calloc(1, strlen(cAddrStr) + 1);
        if (pcStr && pcIPAddr) {
            memcpy(pcStr, cAddrStr, strlen(cAddrStr));
            pcStr[strlen(cAddrStr)] = '\0';
            *pcIPAddr = pcStr; // Set IP address of connected client
        }
    }
    return xSockId;
}

static BOOL xMBTCPPortCloseConnection(MbClientInfo_t* pxInfo)
{
    MB_PORT_CHECK(pxInfo, FALSE, "Client info is NULL.");

    if (pxInfo->xSockId == -1) {
        ESP_LOGE(MB_TCP_SLAVE_PORT_TAG, "Wrong socket info or disconnected socket: %d.", pxInfo->xSockId);
        return FALSE;
    }
    if (shutdown(pxInfo->xSockId, SHUT_RDWR) == -1) {
        ESP_LOGE(MB_TCP_SLAVE_PORT_TAG, "Socket (#%d), shutdown failed: errno %d", pxInfo->xSockId, errno);
    }
    close(pxInfo->xSockId);
    pxInfo->xSockId = -1;
    if (xConfig.usClientCount) {
        xConfig.usClientCount--; // decrement counter of client connections
    } else {
        xConfig.pxCurClientInfo = NULL;
    }
    return TRUE;
}

static int xMBTCPPortRxPoll(MbClientInfo_t* pxClientInfo, ULONG xTimeoutMs)
{
    int xRet = ERR_CLSD;
    struct timeval xTimeVal;
    fd_set xReadSet;
    int64_t xStartTimeStamp = 0;

    // Receive data from connected client
    if (pxClientInfo && pxClientInfo->xSockId > -1) {
        // Set receive timeout
        vxMBTCPPortMStoTimeVal(xTimeoutMs, &xTimeVal);
        xStartTimeStamp = xMBTCPGetTimeStamp();
        while (1)
        {
            FD_ZERO(&xReadSet);
            FD_SET(pxClientInfo->xSockId, &xReadSet);
            xRet = select(pxClientInfo->xSockId + 1, &xReadSet, NULL, NULL, &xTimeVal);
            if (xRet == -1)
            {
                // If select an error occurred
                xRet = ERR_CLSD;
                break;
            } else if (xRet == 0) {
                // timeout occurred
                if ((xStartTimeStamp + xTimeoutMs * 1000) > xMBTCPGetTimeStamp()) {
                    ESP_LOGD(MB_TCP_SLAVE_PORT_TAG, "Socket (#%d) Read timeout.", pxClientInfo->xSockId);
                    xRet = ERR_TIMEOUT;
                    break;
                }
            }
            if (FD_ISSET(pxClientInfo->xSockId, &xReadSet)) {
                // If new buffer received then read Modbus packet into buffer
                MB_PORT_CHECK((pxClientInfo->usTCPBufPos + pxClientInfo->usTCPFrameBytesLeft < MB_TCP_BUF_SIZE),
                                    ERR_BUF, "Socket (#%d), incorrect request buffer size = %d, ignore.",
                                    pxClientInfo->xSockId,
                                    (pxClientInfo->usTCPBufPos + pxClientInfo->usTCPFrameBytesLeft));
                int xLength = recv(pxClientInfo->xSockId, &pxClientInfo->pucTCPBuf[pxClientInfo->usTCPBufPos],
                                      pxClientInfo->usTCPFrameBytesLeft, MSG_DONTWAIT);
                if (xLength < 0) {
                    // If an error occurred during receiving
                    ESP_LOGE(MB_TCP_SLAVE_PORT_TAG, "Receive failed: length=%d, errno=%d", xLength, errno);
                    xRet = (err_t)xLength;
                    break;
                } else if (xLength == 0) {
                    // Socket connection closed
                    ESP_LOGD(MB_TCP_SLAVE_PORT_TAG, "Socket (#%d)(%s), connection closed.",
                                                        pxClientInfo->xSockId, pxClientInfo->pcIpAddr);
                    xRet = ERR_CLSD;
                    break;
                } else {
                    // New data received
                    pxClientInfo->usTCPBufPos += xLength;
                    pxClientInfo->usTCPFrameBytesLeft -= xLength;
                    if (pxClientInfo->usTCPBufPos >= MB_TCP_FUNC) {
                        // Length is a byte count of Modbus PDU (function code + data) and the
                        // unit identifier.
                        xLength = (int)MB_TCP_GET_FIELD(pxClientInfo->pucTCPBuf, MB_TCP_LEN);
                        // Is the frame already complete.
                        if (pxClientInfo->usTCPBufPos < (MB_TCP_UID + xLength)) {
                            // The incomplete frame is received
                            pxClientInfo->usTCPFrameBytesLeft = xLength + MB_TCP_UID - pxClientInfo->usTCPBufPos;
                        } else if (pxClientInfo->usTCPBufPos == (MB_TCP_UID + xLength)) {
#if MB_TCP_DEBUG
                            prvvMBTCPLogFrame(MB_TCP_SLAVE_PORT_TAG, (UCHAR*)&pxClientInfo->pucTCPBuf[0], pxClientInfo->usTCPBufPos);
#endif
                            // Copy TID field from incoming packet
                            pxClientInfo->usTidCnt = MB_TCP_GET_FIELD(pxClientInfo->pucTCPBuf, MB_TCP_TID);
                            xRet = pxClientInfo->usTCPBufPos;
                            break;
                        } else if ((pxClientInfo->usTCPBufPos + xLength) >= MB_TCP_BUF_SIZE) {
                            ESP_LOGE(MB_TCP_SLAVE_PORT_TAG, "Incorrect buffer received (%u) bytes.", xLength);
                            // This should not happen. We can't deal with such a client and
                            // drop the connection for security reasons.
                            xRet = ERR_BUF;
                            break;
                        }
                    } // if ( pxClientInfo->usTCPBufPos >= MB_TCP_FUNC )
                } // if data received
            } // if (FD_ISSET(pxClientInfo->xSockId, &xReadSet))
        } // while (1)
    }
    return (xRet);
}

// Create a listening socket on pcBindIp: Port
static int
vMBTCPPortBindAddr(const CHAR* pcBindIp)
{
    int xPar, xRet;
    int xListenSockFd = -1;
    struct addrinfo xHint;
    struct addrinfo* pxAddrList;
    struct addrinfo* pxCurAddr;
    CHAR* pcStr = NULL;

    memset( &xHint, 0, sizeof( xHint ) );

    // Bind to IPv6 and/or IPv4, but only in the desired protocol
    // Todo: Find a reason why AF_UNSPEC does not work for IPv6
    xHint.ai_family = (xConfig.xIpVer == MB_PORT_IPV4) ? AF_INET : AF_INET6;
    xHint.ai_socktype = (xConfig.eMbProto == MB_PROTO_UDP) ? SOCK_DGRAM : SOCK_STREAM;
    // The LWIP has an issue when connection to IPv6 socket
    xHint.ai_protocol = (xConfig.eMbProto == MB_PROTO_UDP) ? IPPROTO_UDP : IPPROTO_TCP;
    xHint.ai_flags = AI_NUMERICSERV;

    if (pcBindIp == NULL) {
        xHint.ai_flags |= AI_PASSIVE;
    } else {
        xHint.ai_flags |= AI_CANONNAME;
    }

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

    xRet = getaddrinfo(pcBindIp, pcStr, &xHint, &pxAddrList);
    free(pcStr);

    if (xRet != 0) {
        return -1;
    }

    // Try the sockaddr until a binding succeeds
    for (pxCurAddr = pxAddrList; pxCurAddr != NULL; pxCurAddr = pxCurAddr->ai_next)
    {
        xListenSockFd = (int)socket(pxCurAddr->ai_family, pxCurAddr->ai_socktype,
                                        pxCurAddr->ai_protocol);
        if (xListenSockFd < 0)
        {
            continue;
        }

        xPar = 1;
        // Allow multi client connections
        if (setsockopt(xListenSockFd, SOL_SOCKET, SO_REUSEADDR,
                        (const char*)&xPar, sizeof(xPar)) != 0)
        {
            close(xListenSockFd);
            xListenSockFd = -1;
            continue;
        }

        if (bind(xListenSockFd, (struct sockaddr *)pxCurAddr->ai_addr,
                                        (socklen_t)pxCurAddr->ai_addrlen) != 0 )
        {
            close(xListenSockFd);
            xListenSockFd = -1;
            continue;
        }

        // Listen only makes sense for TCP
        if (xConfig.eMbProto == MB_PROTO_TCP)
        {
            if (listen(xListenSockFd, MB_TCP_NET_LISTEN_BACKLOG) != 0)
            {
                ESP_LOGE(MB_TCP_SLAVE_PORT_TAG, "Error occurred during listen: errno=%d", errno);
                close(xListenSockFd);
                xListenSockFd = -1;
                continue;
            }
        }
        // Bind was successful
        pcStr = (pxCurAddr->ai_canonname == NULL) ? (CHAR*)"\0" : pxCurAddr->ai_canonname;
        ESP_LOGI(MB_TCP_SLAVE_PORT_TAG, "Socket (#%d), listener %s on port: %d, errno=%d",
                                            xListenSockFd, pcStr, xConfig.usPort, errno);
        break;
    }

    freeaddrinfo(pxAddrList);
    return(xListenSockFd);
}

static void
vMBTCPPortFreeClientInfo(MbClientInfo_t* pxClientInfo)
{
    if (pxClientInfo) {
        if (pxClientInfo->pucTCPBuf) {
            free((void*)pxClientInfo->pucTCPBuf);
        }
        if (pxClientInfo->pcIpAddr) {
            free((void*)pxClientInfo->pcIpAddr);
        }
        free((void*)pxClientInfo);
    }
}


static void vMBTCPPortServerTask(void *pvParameters)
{
    int xErr = 0;
    fd_set xReadSet;
    int i;
    CHAR* pcClientIp = NULL;
    struct timeval xTimeVal;

    // Main connection cycle
    while (1) {
        // Create listen socket
        xListenSock = vMBTCPPortBindAddr(xConfig.pcBindAddr);
        if (xListenSock < 0) {
            continue;
        }

        // Connections handling cycle
        while (1) {
            //clear the socket set
            FD_ZERO(&xReadSet);
            //add master socket to set
            FD_SET(xListenSock, &xReadSet);
            int xMaxSd = xListenSock;
            xConfig.usClientCount = 0;

            vxMBTCPPortMStoTimeVal(1, &xTimeVal);
            // Initialize read set and file descriptor according to
            // all registered connected clients
            for (i = 0; i < MB_TCP_PORT_MAX_CONN; i++) {
                if ((xConfig.pxMbClientInfo[i] != NULL) && (xConfig.pxMbClientInfo[i]->xSockId > 0)) {
                    // calculate max file descriptor for select
                    xMaxSd = (xConfig.pxMbClientInfo[i]->xSockId > xMaxSd) ?
                            xConfig.pxMbClientInfo[i]->xSockId : xMaxSd;
                    FD_SET(xConfig.pxMbClientInfo[i]->xSockId, &xReadSet);
                    xConfig.usClientCount++;
                }
            }

            // Wait for an activity on one of the sockets, timeout is NULL, so wait indefinitely
            xErr = select(xMaxSd + 1 , &xReadSet , NULL , NULL , NULL);
            if ((xErr < 0) && (errno != EINTR)) {
                // First check if the task is not flagged for shutdown
                if (xListenSock == -1 && xShutdownSemaphore) {
                    xSemaphoreGive(xShutdownSemaphore);
                    vTaskDelete(NULL);
                }
                // error occurred during wait for read
                ESP_LOGE(MB_TCP_SLAVE_PORT_TAG, "select() errno = %d.", errno);
                continue;
            } else if (xErr == 0) {
                // If timeout happened, something is wrong
                ESP_LOGE(MB_TCP_SLAVE_PORT_TAG, "select() timeout, errno = %d.", errno);
            }

            // If something happened on the master socket, then its an incoming connection.
            if (FD_ISSET(xListenSock, &xReadSet) && xConfig.usClientCount < MB_TCP_PORT_MAX_CONN) {
                MbClientInfo_t* pxClientInfo = NULL;
                // find first empty place to insert connection info
                for (i = 0; i < MB_TCP_PORT_MAX_CONN; i++) {
                    pxClientInfo = xConfig.pxMbClientInfo[i];
                    if (pxClientInfo == NULL) {
                        break;
                    }
                }
                // if request for new connection but no space left
                if (pxClientInfo != NULL) {
                    if (xConfig.pxMbClientInfo[MB_TCP_PORT_MAX_CONN] == NULL) {
                        ESP_LOGE(MB_TCP_SLAVE_PORT_TAG, "Fail to accept connection %d, only %d connections supported.", i + 1, MB_TCP_PORT_MAX_CONN);
                    }
                    xConfig.pxMbClientInfo[MB_TCP_PORT_MAX_CONN] = pxClientInfo; // set last connection info
                } else {
                    // allocate memory for new client info
                    pxClientInfo = calloc(1, sizeof(MbClientInfo_t));
                    if (!pxClientInfo) {
                        ESP_LOGE(MB_TCP_SLAVE_PORT_TAG, "Client info allocation fail.");
                        vMBTCPPortFreeClientInfo(pxClientInfo);
                        pxClientInfo = NULL;
                    } else {
                        // Accept new client connection
                        pxClientInfo->xSockId = xMBTCPPortAcceptConnection(xListenSock, &pcClientIp);
                        if (pxClientInfo->xSockId < 0) {
                            ESP_LOGE(MB_TCP_SLAVE_PORT_TAG, "Fail to accept connection for client %d.", (xConfig.usClientCount - 1));
                            // Accept connection fail, then free client info and continue polling.
                            vMBTCPPortFreeClientInfo(pxClientInfo);
                            pxClientInfo = NULL;
                            continue;
                        }
                        pxClientInfo->pucTCPBuf = calloc(MB_TCP_BUF_SIZE, sizeof(UCHAR));
                        if (!pxClientInfo->pucTCPBuf) {
                            ESP_LOGE(MB_TCP_SLAVE_PORT_TAG, "Fail to allocate buffer for client %d.", (xConfig.usClientCount - 1));
                            vMBTCPPortFreeClientInfo(pxClientInfo);
                            pxClientInfo = NULL;
                            continue;
                        }
                        // Fill the connection info structure
                        xConfig.pxMbClientInfo[i] = pxClientInfo;
                        pxClientInfo->xIndex = i;
                        xConfig.usClientCount++;
                        pxClientInfo->pcIpAddr = pcClientIp;
                        pxClientInfo->xRecvTimeStamp = xMBTCPGetTimeStamp();
                        xConfig.pxMbClientInfo[MB_TCP_PORT_MAX_CONN] = NULL;
                        pxClientInfo->usTCPFrameBytesLeft = MB_TCP_FUNC;
                        pxClientInfo->usTCPBufPos = 0;
                    }
                }
            }
            // Handle data request from client
            if (xErr > 0) {
                // Handling client connection requests
                for (i = 0; i < MB_TCP_PORT_MAX_CONN; i++) {
                    MbClientInfo_t* pxClientInfo = xConfig.pxMbClientInfo[i];
                    if ((pxClientInfo != NULL) && (pxClientInfo->xSockId > 0)) {
                        if (FD_ISSET(pxClientInfo->xSockId, &xReadSet)) {
                            // Other sockets are ready to be read
                            xErr = xMBTCPPortRxPoll(pxClientInfo, MB_TCP_READ_TIMEOUT_MS);
                            // If an invalid data received from socket or connection fail
                            // or if timeout then drop connection and restart
                            if (xErr < 0) {
                                uint64_t xTimeStamp = xMBTCPGetTimeStamp();
                                // If data update is timed out
                                switch(xErr)
                                {
                                    case ERR_TIMEOUT:
                                        ESP_LOGE(MB_TCP_SLAVE_PORT_TAG, "Socket (#%d)(%s), data receive timeout, time[us]: %d, close active connection.",
                                                                            pxClientInfo->xSockId, pxClientInfo->pcIpAddr,
                                                                            (int)(xTimeStamp - pxClientInfo->xRecvTimeStamp));
                                        break;
                                    case ERR_CLSD:
                                        ESP_LOGE(MB_TCP_SLAVE_PORT_TAG, "Socket (#%d)(%s), connection closed by peer.",
                                                                            pxClientInfo->xSockId, pxClientInfo->pcIpAddr);
                                        break;
                                    case ERR_BUF:
                                    default:
                                        ESP_LOGE(MB_TCP_SLAVE_PORT_TAG, "Socket (#%d)(%s), read data error: %d",
                                                                            pxClientInfo->xSockId, pxClientInfo->pcIpAddr, xErr);
                                        break;
                                }
                                // Close client connection
                                xMBTCPPortCloseConnection(pxClientInfo);

                                // This client does not respond, then unregister it
                                vMBTCPPortFreeClientInfo(pxClientInfo);
                                xConfig.pxMbClientInfo[i] = NULL;
                                xConfig.pxMbClientInfo[MB_TCP_PORT_MAX_CONN] = NULL;
                                // If no any active connections, break
                                if (!xConfig.usClientCount) {
                                    xConfig.pxCurClientInfo = NULL;
                                    break;
                                }
                            } else {
                                pxClientInfo->xRecvTimeStamp = xMBTCPGetTimeStamp();

                                // set current client info to active client from which we received request
                                xConfig.pxCurClientInfo = pxClientInfo;

                                // Complete frame received, inform state machine to process frame
                                xMBPortEventPost(EV_FRAME_RECEIVED);

                                ESP_LOGD(MB_TCP_SLAVE_PORT_TAG, "Socket (#%d)(%s), get packet TID=0x%X, %d bytes.",
                                                                    pxClientInfo->xSockId, pxClientInfo->pcIpAddr,
                                                                    pxClientInfo->usTidCnt, xErr);

                                // Wait while response is not processed by stack by timeout
                                UCHAR* pucSentBuffer = vxMBTCPPortRespQueueRecv(xConfig.xRespQueueHandle);
                                if (pucSentBuffer == NULL) {
                                    ESP_LOGE(MB_TCP_SLAVE_PORT_TAG, "Response time exceeds configured %d [ms], ignore packet.",
                                                                        MB_TCP_RESP_TIMEOUT_MS);
                                } else  {
                                    USHORT usSentTid = MB_TCP_GET_FIELD(pucSentBuffer, MB_TCP_TID);
                                    if (usSentTid != pxClientInfo->usTidCnt) {
                                        ESP_LOGE(MB_TCP_SLAVE_PORT_TAG, "Sent TID(%x) != Recv TID(%x), ignore packet.",
                                                                            usSentTid, pxClientInfo->usTidCnt);
                                    }
                                }

                                // Get time stamp of last data update
                                pxClientInfo->xSendTimeStamp = xMBTCPGetTimeStamp();
                                ESP_LOGD(MB_TCP_SLAVE_PORT_TAG, "Client %d, Socket(#%d), processing time = %d (us).",
                                                            pxClientInfo->xIndex, pxClientInfo->xSockId,
                                                            (int)(pxClientInfo->xSendTimeStamp - pxClientInfo->xRecvTimeStamp));
                            }
                        } else {
                            if (pxClientInfo) {
                                // client is not ready to be read
                                int64_t xTime = xMBTCPGetTimeStamp() - pxClientInfo->xRecvTimeStamp;
                                if (xTime > MB_TCP_DISCONNECT_TIMEOUT) {
                                    ESP_LOGE(MB_TCP_SLAVE_PORT_TAG, "Client %d, Socket(#%d) do not answer for %d (us). Drop connection...",
                                                                    pxClientInfo->xIndex, pxClientInfo->xSockId, (int)(xTime));
                                    xMBTCPPortCloseConnection(pxClientInfo);

                                    // This client does not respond, then delete registered data
                                    vMBTCPPortFreeClientInfo(pxClientInfo);
                                    xConfig.pxMbClientInfo[i] = NULL;
                                }
                            } else {
                                ESP_LOGE(MB_TCP_SLAVE_PORT_TAG, "Client %d is disconnected.", i);
                            }
                        }
                    } // if ((pxClientInfo != NULL)
                } // Handling client connection requests
            }
        } // while(1) // Handle connection cycle
    } // Main connection cycle
    vTaskDelete(NULL);
}

void
vMBTCPPortClose( )
{
    // Release resources for the event queue.

    // 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
    xShutdownSemaphore = xSemaphoreCreateBinary();
    vTaskResume(xConfig.xMbTcpTaskHandle);
    if (xShutdownSemaphore == NULL || // if no semaphore (alloc issues) or couldn't acquire it, just delete the task
        xSemaphoreTake(xShutdownSemaphore, 2*pdMS_TO_TICKS(CONFIG_FMB_MASTER_TIMEOUT_MS_RESPOND)) != pdTRUE) {
        ESP_LOGE(MB_TCP_SLAVE_PORT_TAG, "Task couldn't exit gracefully within timeout -> abruptly deleting the task");
        vTaskDelete(xConfig.xMbTcpTaskHandle);
    }
    if (xShutdownSemaphore) {
        vSemaphoreDelete(xShutdownSemaphore);
        xShutdownSemaphore = NULL;
    }

    vMBPortEventClose( );
}

void
vMBTCPPortDisable( void )
{
    vTaskSuspend(xConfig.xMbTcpTaskHandle);
    for (int i = 0; i < MB_TCP_PORT_MAX_CONN; i++) {
        MbClientInfo_t* pxClientInfo = xConfig.pxMbClientInfo[i];
        if ((pxClientInfo != NULL) && (pxClientInfo->xSockId > 0)) {
            xMBTCPPortCloseConnection(pxClientInfo);
            vMBTCPPortFreeClientInfo(pxClientInfo);
            xConfig.pxMbClientInfo[i] = NULL;
        }
    }
    close(xListenSock);
    xListenSock = -1;
    vMBTCPPortRespQueueDelete(xConfig.xRespQueueHandle);
}

BOOL
xMBTCPPortGetRequest( UCHAR ** ppucMBTCPFrame, USHORT * usTCPLength )
{
    BOOL xRet = FALSE;
    if (xConfig.pxCurClientInfo) {
        *ppucMBTCPFrame = &xConfig.pxCurClientInfo->pucTCPBuf[0];
        *usTCPLength = xConfig.pxCurClientInfo->usTCPBufPos;

        // Reset the buffer.
        xConfig.pxCurClientInfo->usTCPBufPos = 0;
        xConfig.pxCurClientInfo->usTCPFrameBytesLeft = MB_TCP_FUNC;
        xRet = TRUE;
    }
    return xRet;
}

BOOL
xMBTCPPortSendResponse( UCHAR * pucMBTCPFrame, USHORT usTCPLength )
{
    BOOL bFrameSent = FALSE;
    fd_set xWriteSet;
    fd_set xErrorSet;
    int xErr = -1;
    struct timeval xTimeVal;

    if (xConfig.pxCurClientInfo) {
        FD_ZERO(&xWriteSet);
        FD_ZERO(&xErrorSet);
        FD_SET(xConfig.pxCurClientInfo->xSockId, &xWriteSet);
        FD_SET(xConfig.pxCurClientInfo->xSockId, &xErrorSet);
        vxMBTCPPortMStoTimeVal(MB_TCP_SEND_TIMEOUT_MS, &xTimeVal);
        // Check if socket writable
        xErr = select(xConfig.pxCurClientInfo->xSockId + 1, NULL, &xWriteSet, &xErrorSet, &xTimeVal);
        if ((xErr == -1) || FD_ISSET(xConfig.pxCurClientInfo->xSockId, &xErrorSet)) {
            ESP_LOGE(MB_TCP_SLAVE_PORT_TAG, "Socket(#%d) , send select() error = %d.",
                    xConfig.pxCurClientInfo->xSockId, errno);
            return FALSE;
        }

        // Apply TID field from request to the frame before send response
        pucMBTCPFrame[MB_TCP_TID] = (UCHAR)(xConfig.pxCurClientInfo->usTidCnt >> 8U);
        pucMBTCPFrame[MB_TCP_TID + 1] = (UCHAR)(xConfig.pxCurClientInfo->usTidCnt & 0xFF);

        // Write message into socket and disable Nagle's algorithm
        xErr = send(xConfig.pxCurClientInfo->xSockId, pucMBTCPFrame, usTCPLength, TCP_NODELAY);
        if (xErr < 0) {
            ESP_LOGE(MB_TCP_SLAVE_PORT_TAG, "Socket(#%d), fail to send data, errno = %d",
                    xConfig.pxCurClientInfo->xSockId, errno);
            xConfig.pxCurClientInfo->xError = xErr;
        } else {
            bFrameSent = TRUE;
            vxMBTCPPortRespQueueSend(xConfig.xRespQueueHandle, (void*)pucMBTCPFrame);
        }
    } else {
        ESP_LOGD(MB_TCP_SLAVE_PORT_TAG, "Port is not active. Release lock.");
        vxMBTCPPortRespQueueSend(xConfig.xRespQueueHandle, (void*)pucMBTCPFrame);
    }
    return bFrameSent;
}

#endif //#if MB_TCP_ENABLED
