1 /*
2  * SPDX-FileCopyrightText: 2006 Christian Walter
3  *
4  * SPDX-License-Identifier: BSD-3-Clause
5  *
6  * SPDX-FileContributor: 2016-2021 Espressif Systems (Shanghai) CO LTD
7  */
8 /*
9  * FreeModbus Libary: ESP32 TCP Port
10  * Copyright (C) 2006 Christian Walter <wolti@sil.at>
11  * Parts of crt0.S Copyright (c) 1995, 1996, 1998 Cygnus Support
12  *
13  * Redistribution and use in source and binary forms, with or without
14  * modification, are permitted provided that the following conditions
15  * are met:
16  * 1. Redistributions of source code must retain the above copyright
17  *   notice, this list of conditions and the following disclaimer.
18  * 2. Redistributions in binary form must reproduce the above copyright
19  *   notice, this list of conditions and the following disclaimer in the
20  *   documentation and/or other materials provided with the distribution.
21  * 3. The name of the author may not be used to endorse or promote products
22  *   derived from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
25  * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
26  * IF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
27  * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
29  * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
30  * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
31  * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
32  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
33  * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
34  *
35  * File: $Id: port.c,v 1.2 2006/09/04 14:39:20 wolti Exp $
36  */
37 
38 /* ----------------------- System includes ----------------------------------*/
39 #include <stdio.h>
40 #include <string.h>
41 #include "esp_err.h"
42 
43 /* ----------------------- lwIP includes ------------------------------------*/
44 #include "lwip/err.h"
45 #include "lwip/sockets.h"
46 #include "lwip/netdb.h"
47 #include "esp_netif.h"
48 
49 /* ----------------------- Modbus includes ----------------------------------*/
50 #include "mb_m.h"
51 #include "port.h"
52 #include "mbport.h"
53 #include "mbframe.h"
54 #include "port_tcp_master.h"
55 
56 #if MB_MASTER_TCP_ENABLED
57 
58 /* ----------------------- Defines  -----------------------------------------*/
59 #define MB_TCP_CONNECTION_TIMEOUT_MS    ( 20 )      // Connection timeout in mS
60 #define MB_TCP_RECONNECT_TIMEOUT        ( 5000000 ) // Connection timeout in uS
61 
62 #define MB_EVENT_REQ_DONE_MASK          (   EV_MASTER_PROCESS_SUCCESS | \
63                                             EV_MASTER_ERROR_RESPOND_TIMEOUT | \
64                                             EV_MASTER_ERROR_RECEIVE_DATA | \
65                                             EV_MASTER_ERROR_EXECUTE_FUNCTION )
66 
67 #define MB_EVENT_REQ_ERR_MASK           ( EV_MASTER_PROCESS_SUCCESS )
68 
69 #define MB_EVENT_WAIT_TOUT_MS           ( 3000 )
70 
71 #define MB_TCP_READ_TICK_MS             ( 1 )
72 #define MB_TCP_READ_BUF_RETRY_CNT       ( 4 )
73 #define MB_SLAVE_FMT(fmt)               "Slave #%d, Socket(#%d)(%s)"fmt
74 
75 /* ----------------------- Types & Prototypes --------------------------------*/
76 void vMBPortEventClose( void );
77 
78 /* ----------------------- Static variables ---------------------------------*/
79 static const char *TAG = "MB_TCP_MASTER_PORT";
80 static MbPortConfig_t xMbPortConfig;
81 static EventGroupHandle_t xMasterEventHandle = NULL;
82 static SemaphoreHandle_t xShutdownSemaphore = NULL;
83 static EventBits_t xMasterEvent = 0;
84 
85 /* ----------------------- Static functions ---------------------------------*/
86 static void vMBTCPPortMasterTask(void *pvParameters);
87 
88 /* ----------------------- Begin implementation -----------------------------*/
89 
90 // Waits for stack start event to start Modbus event processing
xMBTCPPortMasterWaitEvent(EventGroupHandle_t xEventHandle,EventBits_t xEvent,USHORT usTimeout)91 BOOL xMBTCPPortMasterWaitEvent(EventGroupHandle_t xEventHandle, EventBits_t xEvent, USHORT usTimeout)
92 {
93     xMasterEventHandle = xEventHandle;
94     xMasterEvent = xEvent;
95     BaseType_t status = xEventGroupWaitBits(xMasterEventHandle,
96                                                (BaseType_t)(xEvent),
97                                                pdFALSE, // do not clear start bit
98                                                pdFALSE,
99                                                usTimeout);
100     return (BOOL)(status & xEvent);
101 }
102 
103 BOOL
xMBMasterTCPPortInit(USHORT usTCPPort)104 xMBMasterTCPPortInit( USHORT usTCPPort )
105 {
106     BOOL bOkay = FALSE;
107 
108     xMbPortConfig.pxMbSlaveInfo = calloc(MB_TCP_PORT_MAX_CONN, sizeof(MbSlaveInfo_t*));
109     if (!xMbPortConfig.pxMbSlaveInfo) {
110         ESP_LOGE(TAG, "TCP slave info alloc failure.");
111         return FALSE;
112     }
113     for(int idx = 0; idx < MB_TCP_PORT_MAX_CONN; xMbPortConfig.pxMbSlaveInfo[idx] = NULL, idx++);
114 
115     xMbPortConfig.xConnectQueue = NULL;
116     xMbPortConfig.usPort = usTCPPort;
117     xMbPortConfig.usMbSlaveInfoCount = 0;
118     xMbPortConfig.ucCurSlaveIndex = 1;
119     xMbPortConfig.pxMbSlaveCurrInfo = NULL;
120 
121     xMbPortConfig.xConnectQueue = xQueueCreate(2, sizeof(MbSlaveAddrInfo_t));
122     if (xMbPortConfig.xConnectQueue == 0)
123     {
124         // Queue was not created and must not be used.
125         ESP_LOGE(TAG, "TCP master queue creation failure.");
126         return FALSE;
127     }
128 
129     // Create task for packet processing
130     BaseType_t xErr = xTaskCreatePinnedToCore(vMBTCPPortMasterTask,
131                                     "tcp_master_task",
132                                     MB_TCP_STACK_SIZE,
133                                     NULL,
134                                     MB_TCP_TASK_PRIO,
135                                     &xMbPortConfig.xMbTcpTaskHandle,
136                                     MB_PORT_TASK_AFFINITY);
137     if (xErr != pdTRUE)
138     {
139         ESP_LOGE(TAG, "TCP master task creation failure.");
140         (void)vTaskDelete(xMbPortConfig.xMbTcpTaskHandle);
141     } else {
142         ESP_LOGI(TAG, "TCP master stack initialized.");
143         bOkay = TRUE;
144     }
145 
146     vTaskSuspend(xMbPortConfig.xMbTcpTaskHandle);
147     return bOkay;
148 }
149 
vMBTCPPortMasterFindSlaveInfo(UCHAR ucSlaveAddr)150 static MbSlaveInfo_t* vMBTCPPortMasterFindSlaveInfo(UCHAR ucSlaveAddr)
151 {
152     int xIndex;
153     BOOL xFound = false;
154     for (xIndex = 0; xIndex < xMbPortConfig.usMbSlaveInfoCount; xIndex++) {
155         if (xMbPortConfig.pxMbSlaveInfo[xIndex]->ucSlaveAddr == ucSlaveAddr) {
156             xMbPortConfig.pxMbSlaveCurrInfo = xMbPortConfig.pxMbSlaveInfo[xIndex];
157             xFound = TRUE;
158             xMbPortConfig.ucCurSlaveIndex = xIndex;
159         }
160     }
161     if (!xFound) {
162         xMbPortConfig.pxMbSlaveCurrInfo = NULL;
163         ESP_LOGE(TAG, "Slave info for short address %d not found.", ucSlaveAddr);
164     }
165     return xMbPortConfig.pxMbSlaveCurrInfo;
166 }
167 
vMBTCPPortMasterGetCurrInfo(void)168 static MbSlaveInfo_t* vMBTCPPortMasterGetCurrInfo(void)
169 {
170     if (!xMbPortConfig.pxMbSlaveCurrInfo) {
171         ESP_LOGE(TAG, "Incorrect current slave info.");
172     }
173     return xMbPortConfig.pxMbSlaveCurrInfo;
174 }
175 
176 // Start Modbus event state machine
vMBTCPPortMasterStartPoll(void)177 static void vMBTCPPortMasterStartPoll(void)
178 {
179     if (xMasterEventHandle) {
180         // Set the mbcontroller start flag
181         EventBits_t xFlags = xEventGroupSetBits(xMasterEventHandle,
182                                                 (EventBits_t)xMasterEvent);
183         if (!(xFlags & xMasterEvent)) {
184             ESP_LOGE(TAG, "Fail to start TCP stack.");
185         }
186     } else {
187         ESP_LOGE(TAG, "Fail to start polling. Incorrect event handle...");
188     }
189 }
190 
191 // Stop Modbus event state machine
vMBTCPPortMasterStopPoll(void)192 static void vMBTCPPortMasterStopPoll(void)
193 {
194     if (xMasterEventHandle) {
195         // Set the mbcontroller start flag
196         EventBits_t xFlags = xEventGroupClearBits(xMasterEventHandle,
197                                                 (EventBits_t)xMasterEvent);
198         if (!(xFlags & xMasterEvent)) {
199             ESP_LOGE(TAG, "Fail to stop polling.");
200         }
201     } else {
202         ESP_LOGE(TAG, "Fail to stop polling. Incorrect event handle...");
203     }
204 }
205 
206 // The helper function to get time stamp in microseconds
xMBTCPGetTimeStamp(void)207 static int64_t xMBTCPGetTimeStamp(void)
208 {
209     int64_t xTimeStamp = esp_timer_get_time();
210     return xTimeStamp;
211 }
212 
vMBTCPPortMasterMStoTimeVal(USHORT usTimeoutMs,struct timeval * tv)213 static void vMBTCPPortMasterMStoTimeVal(USHORT usTimeoutMs, struct timeval *tv)
214 {
215     tv->tv_sec = usTimeoutMs / 1000;
216     tv->tv_usec = (usTimeoutMs - (tv->tv_sec * 1000)) * 1000;
217 }
218 
xMBTCPPortMasterCheckShutdown(void)219 static void xMBTCPPortMasterCheckShutdown(void) {
220     // First check if the task is not flagged for shutdown
221     if (xShutdownSemaphore) {
222         xSemaphoreGive(xShutdownSemaphore);
223         vTaskDelete(NULL);
224     }
225 }
226 
xMBTCPPortMasterCloseConnection(MbSlaveInfo_t * pxInfo)227 static BOOL xMBTCPPortMasterCloseConnection(MbSlaveInfo_t* pxInfo)
228 {
229     if (!pxInfo) {
230         return FALSE;
231     }
232     if (pxInfo->xSockId == -1) {
233         ESP_LOGE(TAG, "Wrong socket info or disconnected socket: %d, skip.", pxInfo->xSockId);
234         return FALSE;
235     }
236     if (shutdown(pxInfo->xSockId, SHUT_RDWR) == -1) {
237         ESP_LOGV(TAG, "Shutdown failed sock %d, errno=%d", pxInfo->xSockId, errno);
238     }
239     close(pxInfo->xSockId);
240     pxInfo->xSockId = -1;
241     return TRUE;
242 }
243 
vMBTCPPortMasterSetNetOpt(void * pvNetIf,eMBPortIpVer xIpVersion,eMBPortProto xProto)244 void vMBTCPPortMasterSetNetOpt(void* pvNetIf, eMBPortIpVer xIpVersion, eMBPortProto xProto)
245 {
246     xMbPortConfig.pvNetIface = pvNetIf;
247     xMbPortConfig.eMbProto = xProto;
248     xMbPortConfig.eMbIpVer = xIpVersion;
249 }
250 
251 // Function returns time left for response processing according to response timeout
xMBTCPPortMasterGetRespTimeLeft(MbSlaveInfo_t * pxInfo)252 static int64_t xMBTCPPortMasterGetRespTimeLeft(MbSlaveInfo_t* pxInfo)
253 {
254     if (!pxInfo) {
255         return 0;
256     }
257     int64_t xTimeStamp = xMBTCPGetTimeStamp() - pxInfo->xSendTimeStamp;
258     return (xTimeStamp > (1000 * MB_MASTER_TIMEOUT_MS_RESPOND)) ? 0 :
259                     (MB_MASTER_TIMEOUT_MS_RESPOND - (xTimeStamp / 1000) - 1);
260 }
261 
262 // Wait socket ready to read state
vMBTCPPortMasterRxCheck(int xSd,fd_set * pxFdSet,int xTimeMs)263 static int vMBTCPPortMasterRxCheck(int xSd, fd_set* pxFdSet, int xTimeMs)
264 {
265     fd_set xReadSet = *pxFdSet;
266     fd_set xErrorSet = *pxFdSet;
267     int xRes = 0;
268     struct timeval xTimeout;
269 
270     vMBTCPPortMasterMStoTimeVal(xTimeMs, &xTimeout);
271     xRes = select(xSd + 1, &xReadSet, NULL, &xErrorSet, &xTimeout);
272     if (xRes == 0) {
273         // No respond from slave during timeout
274         xRes = ERR_TIMEOUT;
275     } else if ((xRes < 0) || FD_ISSET(xSd, &xErrorSet)) {
276         xRes = -1;
277     }
278 
279     *pxFdSet = xReadSet;
280     return xRes;
281 }
282 
xMBTCPPortMasterGetBuf(MbSlaveInfo_t * pxInfo,UCHAR * pucDstBuf,USHORT usLength)283 static int xMBTCPPortMasterGetBuf(MbSlaveInfo_t* pxInfo, UCHAR* pucDstBuf, USHORT usLength)
284 {
285     int xLength = 0;
286     UCHAR* pucBuf = pucDstBuf;
287     USHORT usBytesLeft = usLength;
288 
289     MB_PORT_CHECK((pxInfo && pxInfo->xSockId > -1), -1, "Try to read incorrect socket = #%d.", pxInfo->xSockId);
290 
291     // Receive data from connected client
292     while (usBytesLeft > 0) {
293         xMBTCPPortMasterCheckShutdown();
294         // none blocking read from socket with timeout
295         xLength = recv(pxInfo->xSockId, pucBuf, usBytesLeft, MSG_DONTWAIT);
296         if (xLength < 0) {
297             if (errno == EAGAIN) {
298                 // Read timeout occurred, continue reading
299                 continue;
300             } else if (errno == ENOTCONN) {
301                 // Socket connection closed
302                 ESP_LOGE(TAG, "Socket(#%d)(%s) connection closed.",
303                                             pxInfo->xSockId, pxInfo->pcIpAddr);
304                 return ERR_CONN;
305             } else {
306                 // Other error occurred during receiving
307                 ESP_LOGE(TAG, "Socket(#%d)(%s) receive error, length=%d, errno=%d",
308                                             pxInfo->xSockId, pxInfo->pcIpAddr, xLength, errno);
309                 return -1;
310             }
311         } else if (xLength) {
312             pucBuf += xLength;
313             usBytesLeft -= xLength;
314         }
315         if (xMBTCPPortMasterGetRespTimeLeft(pxInfo) == 0) {
316             return ERR_TIMEOUT;
317         }
318         vTaskDelay(1);
319     }
320     return usLength;
321 }
322 
vMBTCPPortMasterReadPacket(MbSlaveInfo_t * pxInfo)323 static int vMBTCPPortMasterReadPacket(MbSlaveInfo_t* pxInfo)
324 {
325     int xLength = 0;
326     int xRet = 0;
327     USHORT usTidRcv = 0;
328 
329     // Receive data from connected client
330     if (pxInfo) {
331         MB_PORT_CHECK((pxInfo->xSockId > 0), -1, "Try to read incorrect socket = #%d.", pxInfo->xSockId);
332         // Read packet header
333         xRet = xMBTCPPortMasterGetBuf(pxInfo, &pxInfo->pucRcvBuf[0], MB_TCP_UID);
334         if (xRet < 0) {
335             pxInfo->xRcvErr = xRet;
336             return xRet;
337         } else if (xRet != MB_TCP_UID) {
338             ESP_LOGD(TAG, "Socket (#%d)(%s), Fail to read modbus header. ret=%d",
339                                                                 pxInfo->xSockId, pxInfo->pcIpAddr, xRet);
340             pxInfo->xRcvErr = ERR_VAL;
341             return ERR_VAL;
342         }
343         // If we have received the MBAP header we can analyze it and calculate
344         // the number of bytes left to complete the current request.
345         xLength = (int)MB_TCP_GET_FIELD(pxInfo->pucRcvBuf, MB_TCP_LEN);
346         xRet = xMBTCPPortMasterGetBuf(pxInfo, &pxInfo->pucRcvBuf[MB_TCP_UID], xLength);
347         if (xRet < 0) {
348             pxInfo->xRcvErr = xRet;
349             return xRet;
350         } else if (xRet != xLength) {
351             // Received incorrect or fragmented packet.
352             ESP_LOGD(TAG, "Socket(#%d)(%s) incorrect packet, length=%d, TID=0x%02x, errno=%d(%s)",
353                                                pxInfo->xSockId, pxInfo->pcIpAddr, pxInfo->usRcvPos,
354                                                usTidRcv, errno, strerror(errno));
355             pxInfo->xRcvErr = ERR_VAL;
356             return ERR_VAL;
357         }
358         usTidRcv = MB_TCP_GET_FIELD(pxInfo->pucRcvBuf, MB_TCP_TID);
359 
360         // Check transaction identifier field in the incoming packet.
361         if ((pxInfo->usTidCnt - 1) != usTidRcv) {
362             ESP_LOGD(TAG, "Socket (#%d)(%s), incorrect TID(0x%02x)!=(0x%02x) received, discard data.",
363                                                 pxInfo->xSockId, pxInfo->pcIpAddr, usTidRcv, (pxInfo->usTidCnt - 1));
364             pxInfo->xRcvErr = ERR_BUF;
365             return ERR_BUF;
366         }
367         pxInfo->usRcvPos += xRet + MB_TCP_UID;
368         ESP_LOGD(TAG, "Socket(#%d)(%s) get data, length=%d, TID=0x%02x, errno=%d(%s)",
369                                            pxInfo->xSockId, pxInfo->pcIpAddr, pxInfo->usRcvPos,
370                                            usTidRcv, errno, strerror(errno));
371         pxInfo->xRcvErr = ERR_OK;
372         return pxInfo->usRcvPos;
373     }
374     return -1;
375 }
376 
xMBTCPPortMasterSetNonBlocking(MbSlaveInfo_t * pxInfo)377 static err_t xMBTCPPortMasterSetNonBlocking(MbSlaveInfo_t* pxInfo)
378 {
379     if (!pxInfo) {
380         return ERR_CONN;
381     }
382     // Set non blocking attribute for socket
383     ULONG ulFlags = fcntl(pxInfo->xSockId, F_GETFL);
384     if (fcntl(pxInfo->xSockId, F_SETFL, ulFlags | O_NONBLOCK) == -1) {
385         ESP_LOGE(TAG, "Socket(#%d)(%s), fcntl() call error=%d",
386                                               pxInfo->xSockId, pxInfo->pcIpAddr, errno);
387         return ERR_WOULDBLOCK;
388     }
389     return ERR_OK;
390 }
391 
vMBTCPPortSetKeepAlive(MbSlaveInfo_t * pxInfo)392 static void vMBTCPPortSetKeepAlive(MbSlaveInfo_t* pxInfo)
393 {
394     int optval = 1;
395     setsockopt(pxInfo->xSockId, SOL_SOCKET, SO_KEEPALIVE, &optval, sizeof(optval));
396 }
397 
398 // Check connection for timeout helper
xMBTCPPortMasterCheckAlive(MbSlaveInfo_t * pxInfo,ULONG xTimeoutMs)399 static err_t xMBTCPPortMasterCheckAlive(MbSlaveInfo_t* pxInfo, ULONG xTimeoutMs)
400 {
401     fd_set xWriteSet;
402     fd_set xErrorSet;
403     err_t xErr = -1;
404     struct timeval xTimeVal;
405 
406     if (pxInfo && pxInfo->xSockId != -1) {
407         FD_ZERO(&xWriteSet);
408         FD_ZERO(&xErrorSet);
409         FD_SET(pxInfo->xSockId, &xWriteSet);
410         FD_SET(pxInfo->xSockId, &xErrorSet);
411         vMBTCPPortMasterMStoTimeVal(xTimeoutMs, &xTimeVal);
412         // Check if the socket is writable
413         xErr = select(pxInfo->xSockId + 1, NULL, &xWriteSet, &xErrorSet, &xTimeVal);
414         if ((xErr < 0) || FD_ISSET(pxInfo->xSockId, &xErrorSet)) {
415             if (errno == EINPROGRESS) {
416                 xErr = ERR_INPROGRESS;
417             } else {
418                 ESP_LOGV(TAG, MB_SLAVE_FMT(" connection, select write err(errno) = %d(%d)."),
419                                                     pxInfo->xIndex, pxInfo->xSockId, pxInfo->pcIpAddr, xErr, errno);
420                 xErr = ERR_CONN;
421             }
422         } else if (xErr == 0) {
423             ESP_LOGV(TAG, "Socket(#%d)(%s), connection timeout occurred, err(errno) = %d(%d).",
424                                         pxInfo->xSockId, pxInfo->pcIpAddr, xErr, errno);
425             return ERR_INPROGRESS;
426         } else {
427             int xOptErr = 0;
428             ULONG ulOptLen = sizeof(xOptErr);
429             // Check socket error
430             xErr = getsockopt(pxInfo->xSockId, SOL_SOCKET, SO_ERROR, (void*)&xOptErr, (socklen_t*)&ulOptLen);
431             if (xOptErr != 0) {
432                 ESP_LOGD(TAG, "Socket(#%d)(%s), sock error occurred (%d).",
433                                             pxInfo->xSockId, pxInfo->pcIpAddr, xOptErr);
434                 return ERR_CONN;
435             }
436             ESP_LOGV(TAG, "Socket(#%d)(%s), is alive.",
437                                         pxInfo->xSockId, pxInfo->pcIpAddr);
438             return ERR_OK;
439         }
440     } else {
441         xErr = ERR_CONN;
442     }
443     return xErr;
444 }
445 
446 // Resolve host name and/or fill the IP address structure
xMBTCPPortMasterCheckHost(const CHAR * pcHostStr,ip_addr_t * pxHostAddr)447 static BOOL xMBTCPPortMasterCheckHost(const CHAR* pcHostStr, ip_addr_t* pxHostAddr)
448 {
449     MB_PORT_CHECK((pcHostStr), FALSE, "Wrong host name or IP.");
450     CHAR cStr[45];
451     CHAR* pcStr = &cStr[0];
452     ip_addr_t xTargetAddr;
453     struct addrinfo xHint;
454     struct addrinfo* pxAddrList;
455     memset(&xHint, 0, sizeof(xHint));
456     // Do name resolution for both protocols
457     xHint.ai_family = AF_UNSPEC;
458     xHint.ai_flags = AI_ADDRCONFIG; // get IPV6 address if supported, otherwise IPV4
459     memset(&xTargetAddr, 0, sizeof(xTargetAddr));
460 
461     // convert domain name to IP address
462     // Todo: check EAI_FAIL error when resolve host name
463     int xRet = getaddrinfo(pcHostStr, NULL, &xHint, &pxAddrList);
464 
465     if (xRet != 0) {
466         ESP_LOGE(TAG, "Incorrect host name or IP: %s", pcHostStr);
467         return FALSE;
468     }
469     if (pxAddrList->ai_family == AF_INET) {
470         struct in_addr addr4 = ((struct sockaddr_in *) (pxAddrList->ai_addr))->sin_addr;
471         inet_addr_to_ip4addr(ip_2_ip4(&xTargetAddr), &addr4);
472         pcStr = ip4addr_ntoa_r(ip_2_ip4(&xTargetAddr), cStr, sizeof(cStr));
473     }
474 #if CONFIG_LWIP_IPV6
475     else {
476         struct in6_addr addr6 = ((struct sockaddr_in6 *) (pxAddrList->ai_addr))->sin6_addr;
477         inet6_addr_to_ip6addr(ip_2_ip6(&xTargetAddr), &addr6);
478         pcStr = ip6addr_ntoa_r(ip_2_ip6(&xTargetAddr), cStr, sizeof(cStr));
479     }
480 #endif
481     if (pxHostAddr) {
482         *pxHostAddr = xTargetAddr;
483     }
484     ESP_LOGI(TAG, "Host[IP]: \"%s\"[%s]", pxAddrList->ai_canonname, pcStr);
485     freeaddrinfo(pxAddrList);
486     return TRUE;
487 }
488 
xMBTCPPortMasterAddSlaveIp(const USHORT usIndex,const CHAR * pcIpStr,UCHAR ucSlaveAddress)489 BOOL xMBTCPPortMasterAddSlaveIp(const USHORT usIndex, const CHAR* pcIpStr, UCHAR ucSlaveAddress)
490 {
491     BOOL xRes = FALSE;
492     MbSlaveAddrInfo_t xSlaveAddrInfo = { 0 };
493     MB_PORT_CHECK(xMbPortConfig.xConnectQueue != NULL, FALSE, "Wrong slave IP address to add.");
494     if (pcIpStr && (usIndex != 0xFF)) {
495         xRes = xMBTCPPortMasterCheckHost(pcIpStr, NULL);
496     }
497     if (xRes || !pcIpStr) {
498         xSlaveAddrInfo.pcIPAddr = pcIpStr;
499         xSlaveAddrInfo.usIndex = usIndex;
500         xSlaveAddrInfo.ucSlaveAddr = ucSlaveAddress;
501         BaseType_t xStatus = xQueueSend(xMbPortConfig.xConnectQueue, (void*)&xSlaveAddrInfo, 100);
502         MB_PORT_CHECK((xStatus == pdTRUE), FALSE, "FAIL to add slave IP address: [%s].", pcIpStr);
503     }
504     return xRes;
505 }
506 
507 // Unblocking connect function
xMBTCPPortMasterConnect(MbSlaveInfo_t * pxInfo)508 static err_t xMBTCPPortMasterConnect(MbSlaveInfo_t* pxInfo)
509 {
510     if (!pxInfo) {
511         return ERR_CONN;
512     }
513 
514     err_t xErr = ERR_OK;
515     CHAR cStr[128];
516     CHAR* pcStr = NULL;
517     ip_addr_t xTargetAddr;
518     struct addrinfo xHint;
519     struct addrinfo* pxAddrList;
520     struct addrinfo* pxCurAddr;
521 
522     memset(&xHint, 0, sizeof(xHint));
523     // Do name resolution for both protocols
524     //xHint.ai_family = AF_UNSPEC; Todo: Find a reason why AF_UNSPEC does not work
525     xHint.ai_flags = AI_ADDRCONFIG; // get IPV6 address if supported, otherwise IPV4
526     xHint.ai_family = (xMbPortConfig.eMbIpVer == MB_PORT_IPV4) ? AF_INET : AF_INET6;
527     xHint.ai_socktype = (pxInfo->xMbProto == MB_PROTO_UDP) ? SOCK_DGRAM : SOCK_STREAM;
528     xHint.ai_protocol = (pxInfo->xMbProto == MB_PROTO_UDP) ? IPPROTO_UDP : IPPROTO_TCP;
529     memset(&xTargetAddr, 0, sizeof(xTargetAddr));
530 
531     if (asprintf(&pcStr, "%u", xMbPortConfig.usPort) == -1) {
532         abort();
533     }
534 
535     // convert domain name to IP address
536     int xRet = getaddrinfo(pxInfo->pcIpAddr, pcStr, &xHint, &pxAddrList);
537     free(pcStr);
538     if (xRet != 0) {
539         ESP_LOGE(TAG, "Cannot resolve host: %s", pxInfo->pcIpAddr);
540         return ERR_CONN;
541     }
542 
543     for (pxCurAddr = pxAddrList; pxCurAddr != NULL; pxCurAddr = pxCurAddr->ai_next) {
544         if (pxCurAddr->ai_family == AF_INET) {
545             struct in_addr addr4 = ((struct sockaddr_in *) (pxCurAddr->ai_addr))->sin_addr;
546             inet_addr_to_ip4addr(ip_2_ip4(&xTargetAddr), &addr4);
547             pcStr = ip4addr_ntoa_r(ip_2_ip4(&xTargetAddr), cStr, sizeof(cStr));
548         }
549 #if CONFIG_LWIP_IPV6
550         else if (pxCurAddr->ai_family == AF_INET6) {
551             struct in6_addr addr6 = ((struct sockaddr_in6 *) (pxCurAddr->ai_addr))->sin6_addr;
552             inet6_addr_to_ip6addr(ip_2_ip6(&xTargetAddr), &addr6);
553             pcStr = ip6addr_ntoa_r(ip_2_ip6(&xTargetAddr), cStr, sizeof(cStr));
554             // Set scope id to fix routing issues with local address
555             ((struct sockaddr_in6 *) (pxCurAddr->ai_addr))->sin6_scope_id =
556                                 esp_netif_get_netif_impl_index(xMbPortConfig.pvNetIface);
557         }
558 #endif
559         if (pxInfo->xSockId <= 0) {
560             pxInfo->xSockId = socket(pxCurAddr->ai_family, pxCurAddr->ai_socktype, pxCurAddr->ai_protocol);
561             if (pxInfo->xSockId < 0) {
562                 ESP_LOGE(TAG, "Unable to create socket: #%d, errno %d", pxInfo->xSockId, errno);
563                 xErr = ERR_IF;
564                 continue;
565             }
566         } else {
567             ESP_LOGV(TAG, "Socket (#%d)(%s) created.", pxInfo->xSockId, cStr);
568         }
569 
570         // Set non blocking attribute for socket
571         xMBTCPPortMasterSetNonBlocking(pxInfo);
572 
573         // Can return EINPROGRESS as an error which means
574         // that connection is in progress and should be checked later
575         xErr = connect(pxInfo->xSockId, (struct sockaddr*)pxCurAddr->ai_addr, pxCurAddr->ai_addrlen);
576         if ((xErr < 0) && (errno == EINPROGRESS || errno == EALREADY)) {
577             // The unblocking connect is pending (check status later) or already connected
578             ESP_LOGV(TAG, "Socket(#%d)(%s) connection is pending, errno %d (%s).",
579                                         pxInfo->xSockId, cStr, errno, strerror(errno));
580 
581             // Set keep alive flag in socket options
582             vMBTCPPortSetKeepAlive(pxInfo);
583             xErr = xMBTCPPortMasterCheckAlive(pxInfo, MB_TCP_CONNECTION_TIMEOUT_MS);
584             continue;
585         } else if ((xErr < 0) && (errno == EISCONN)) {
586             // Socket already connected
587             xErr = ERR_OK;
588             continue;
589         } else if (xErr != ERR_OK) {
590             // Other error occurred during connection
591             ESP_LOGV(TAG, MB_SLAVE_FMT(" unable to connect, error=%d, errno %d (%s)"),
592                                                 pxInfo->xIndex, pxInfo->xSockId, cStr, xErr, errno, strerror(errno));
593             xMBTCPPortMasterCloseConnection(pxInfo);
594             xErr = ERR_CONN;
595         } else {
596             ESP_LOGI(TAG, MB_SLAVE_FMT(", successfully connected."),
597                                                   pxInfo->xIndex, pxInfo->xSockId, cStr);
598             continue;
599         }
600     }
601     freeaddrinfo(pxAddrList);
602     return xErr;
603 }
604 
605 // Find the first slave info whose descriptor is set in xFdSet
xMBTCPPortMasterGetSlaveReady(fd_set * pxFdSet)606 static MbSlaveInfo_t* xMBTCPPortMasterGetSlaveReady(fd_set* pxFdSet)
607 {
608     MbSlaveInfo_t* pxInfo = NULL;
609 
610     // Slave connection loop
611     for (int xIndex = 0; (xIndex < MB_TCP_PORT_MAX_CONN); xIndex++) {
612         pxInfo = xMbPortConfig.pxMbSlaveInfo[xIndex];
613         if (pxInfo) {
614             // Is this response for current processing slave
615             if (FD_ISSET(pxInfo->xSockId, pxFdSet)) {
616                 FD_CLR(pxInfo->xSockId, pxFdSet);
617                 return pxInfo;
618             }
619         }
620     }
621     return (MbSlaveInfo_t*)NULL;
622 }
623 
xMBTCPPortMasterCheckConnState(fd_set * pxFdSet)624 static int xMBTCPPortMasterCheckConnState(fd_set* pxFdSet)
625 {
626     fd_set xConnSetCheck = *pxFdSet;
627     MbSlaveInfo_t* pxInfo = NULL;
628     int64_t xTime = 0;
629     int xErr = 0;
630     int xCount = 0;
631     do {
632         xTime = xMBTCPGetTimeStamp();
633         pxInfo = xMBTCPPortMasterGetSlaveReady(&xConnSetCheck);
634         if (pxInfo) {
635             xErr = xMBTCPPortMasterCheckAlive(pxInfo, 0);
636             if ((xErr < 0) && (((xTime - pxInfo->xRecvTimeStamp) > MB_TCP_RECONNECT_TIMEOUT) ||
637                                 ((xTime - pxInfo->xSendTimeStamp) > MB_TCP_RECONNECT_TIMEOUT))) {
638                 ESP_LOGI(TAG, MB_SLAVE_FMT(", slave is down, off_time[r][w](us) = [%ju][%ju]."),
639                                                             pxInfo->xIndex,
640                                                             pxInfo->xSockId,
641                                                             pxInfo->pcIpAddr,
642                                                             (int64_t)(xTime - pxInfo->xRecvTimeStamp),
643                                                             (int64_t)(xTime - pxInfo->xSendTimeStamp));
644                 xCount++;
645             }
646         }
647     } while (pxInfo && (xCount < MB_TCP_PORT_MAX_CONN));
648     return xCount;
649 }
650 
xMBTCPPortMasterFsmSetError(eMBMasterErrorEventType xErrType,eMBMasterEventType xPostEvent)651 static void xMBTCPPortMasterFsmSetError(eMBMasterErrorEventType xErrType, eMBMasterEventType xPostEvent)
652 {
653     vMBMasterPortTimersDisable();
654     vMBMasterSetErrorType(xErrType);
655     xMBMasterPortEventPost(xPostEvent);
656 }
657 
vMBTCPPortMasterTask(void * pvParameters)658 static void vMBTCPPortMasterTask(void *pvParameters)
659 {
660     MbSlaveInfo_t* pxInfo;
661     MbSlaveInfo_t* pxCurrInfo;
662 
663     fd_set xConnSet;
664     fd_set xReadSet;
665     int xMaxSd = 0;
666     err_t xErr = ERR_ABRT;
667     USHORT usSlaveConnCnt = 0;
668     int64_t xTime = 0;
669 
670     // Register each slave in the connection info structure
671     while (1) {
672         MbSlaveAddrInfo_t xSlaveAddrInfo = { 0 };
673         BaseType_t xStatus = xQueueReceive(xMbPortConfig.xConnectQueue, (void*)&xSlaveAddrInfo, pdMS_TO_TICKS(MB_EVENT_WAIT_TOUT_MS));
674 	    xMBTCPPortMasterCheckShutdown();
675         if (xStatus != pdTRUE) {
676             ESP_LOGE(TAG, "Fail to register slave IP.");
677         } else {
678             if (xSlaveAddrInfo.pcIPAddr == NULL && xMbPortConfig.usMbSlaveInfoCount && xSlaveAddrInfo.usIndex == 0xFF) {
679                 break;
680             }
681             if (xMbPortConfig.usMbSlaveInfoCount > MB_TCP_PORT_MAX_CONN) {
682                 ESP_LOGE(TAG, "Exceeds maximum connections limit=%d.", MB_TCP_PORT_MAX_CONN);
683                 break;
684             }
685             pxInfo = calloc(1, sizeof(MbSlaveInfo_t));
686             if (!pxInfo) {
687                 ESP_LOGE(TAG, "Slave(#%d), info structure allocation fail.",
688                                                     xMbPortConfig.usMbSlaveInfoCount);
689                 free(pxInfo);
690                 break;
691             }
692             pxInfo->pucRcvBuf = calloc(MB_TCP_BUF_SIZE, sizeof(UCHAR));
693             if (!pxInfo->pucRcvBuf) {
694                 ESP_LOGE(TAG, "Slave(#%d), receive buffer allocation fail.",
695                                                     xMbPortConfig.usMbSlaveInfoCount);
696                 free(pxInfo->pucRcvBuf);
697                 break;
698             }
699             pxInfo->usRcvPos = 0;
700             pxInfo->pcIpAddr = xSlaveAddrInfo.pcIPAddr;
701             pxInfo->xSockId = -1;
702             pxInfo->xError = -1;
703             pxInfo->xRecvTimeStamp = xMBTCPGetTimeStamp();
704             pxInfo->xSendTimeStamp = xMBTCPGetTimeStamp();
705             pxInfo->xMbProto = MB_PROTO_TCP;
706             pxInfo->ucSlaveAddr = xSlaveAddrInfo.ucSlaveAddr;
707             pxInfo->xIndex = xSlaveAddrInfo.usIndex;
708             pxInfo->usTidCnt = (USHORT)(xMbPortConfig.usMbSlaveInfoCount << 8U);
709             // Register slave
710             xMbPortConfig.pxMbSlaveInfo[xMbPortConfig.usMbSlaveInfoCount++] = pxInfo;
711             ESP_LOGI(TAG, "Add slave IP: %s", xSlaveAddrInfo.pcIPAddr);
712         }
713     }
714 
715     // Main connection cycle
716     while (1)
717     {
718         ESP_LOGI(TAG, "Connecting to slaves...");
719         xTime = xMBTCPGetTimeStamp();
720         usSlaveConnCnt = 0;
721         CHAR ucDot = '.';
722         while(usSlaveConnCnt < xMbPortConfig.usMbSlaveInfoCount) {
723             usSlaveConnCnt = 0;
724             FD_ZERO(&xConnSet);
725             ucDot ^= 0x03;
726             // Slave connection loop
727             for (UCHAR ucCnt = 0; (ucCnt < MB_TCP_PORT_MAX_CONN); ucCnt++) {
728                 pxInfo = xMbPortConfig.pxMbSlaveInfo[ucCnt];
729                 // if slave descriptor is NULL then it is end of list or connection closed.
730                 if (!pxInfo) {
731                     ESP_LOGV(TAG, "Index: %d is not initialized, skip.", ucCnt);
732                     if (xMbPortConfig.usMbSlaveInfoCount) {
733                         continue;
734                     }
735                     break;
736                 }
737                 putchar(ucDot);
738                 xErr = xMBTCPPortMasterConnect(pxInfo);
739                 switch(xErr)
740                 {
741                     case ERR_CONN:
742                     case ERR_INPROGRESS:
743                         // In case of connection errors remove the socket from set
744                         if (FD_ISSET(pxInfo->xSockId, &xConnSet)) {
745                             FD_CLR(pxInfo->xSockId, &xConnSet);
746                             ESP_LOGE(TAG, MB_SLAVE_FMT(" connect failed, error = %d."),
747                                                                             pxInfo->xIndex, pxInfo->xSockId,
748                                                                             (char*)pxInfo->pcIpAddr, xErr);
749                             if (usSlaveConnCnt) {
750                                 usSlaveConnCnt--;
751                             }
752                         }
753                         break;
754                     case ERR_OK:
755                         // if connection is successful, add the descriptor into set
756                         if (!FD_ISSET(pxInfo->xSockId, &xConnSet)) {
757                             FD_SET(pxInfo->xSockId, &xConnSet);
758                             usSlaveConnCnt++;
759                             xMaxSd = (pxInfo->xSockId > xMaxSd) ? pxInfo->xSockId : xMaxSd;
760                             ESP_LOGD(TAG, MB_SLAVE_FMT(", connected %d slave(s), error = %d."),
761                                                                 pxInfo->xIndex, pxInfo->xSockId,
762                                                                 pxInfo->pcIpAddr,
763                                                                 usSlaveConnCnt, xErr);
764                             // Update time stamp for connected slaves
765                             pxInfo->xRecvTimeStamp = xMBTCPGetTimeStamp();
766                             pxInfo->xSendTimeStamp = xMBTCPGetTimeStamp();
767                         }
768                         break;
769                     default:
770                         ESP_LOGE(TAG, MB_SLAVE_FMT(", unexpected error = %d."),
771                                                             pxInfo->xIndex,
772                                                             pxInfo->xSockId,
773                                                             pxInfo->pcIpAddr, xErr);
774                         break;
775                 }
776                 if (pxInfo) {
777                     pxInfo->xError = xErr;
778                 }
779                 xMBTCPPortMasterCheckShutdown();
780             }
781         }
782         ESP_LOGI(TAG, "Connected %d slaves, start polling...", usSlaveConnCnt);
783 
784         vMBTCPPortMasterStartPoll(); // Send event to start stack
785 
786         // Slave receive data loop
787         while(usSlaveConnCnt) {
788             xReadSet = xConnSet;
789             // Check transmission event to clear appropriate bit.
790             xMBMasterPortFsmWaitConfirmation(EV_MASTER_FRAME_TRANSMIT, pdMS_TO_TICKS(MB_EVENT_WAIT_TOUT_MS));
791             // Synchronize state machine with send packet event
792             if (xMBMasterPortFsmWaitConfirmation(EV_MASTER_FRAME_SENT, pdMS_TO_TICKS(MB_EVENT_WAIT_TOUT_MS))) {
793                 ESP_LOGD(TAG, "FSM Synchronized with sent event.");
794             }
795             // Get slave info for the current slave.
796             pxCurrInfo = vMBTCPPortMasterGetCurrInfo();
797             if (!pxCurrInfo) {
798                 ESP_LOGE(TAG, "Incorrect connection options for slave index: %d.",
799                                             xMbPortConfig.ucCurSlaveIndex);
800                 vMBTCPPortMasterStopPoll();
801                 xMBTCPPortMasterCheckShutdown();
802                 break; // incorrect slave descriptor, reconnect.
803             }
804             xTime = xMBTCPPortMasterGetRespTimeLeft(pxCurrInfo);
805             ESP_LOGD(TAG, "Set select timeout, left time: %ju ms.",
806                                         xMBTCPPortMasterGetRespTimeLeft(pxCurrInfo));
807             // Wait respond from current slave during respond timeout
808             int xRes = vMBTCPPortMasterRxCheck(pxCurrInfo->xSockId, &xReadSet, xTime);
809             if (xRes == ERR_TIMEOUT) {
810                 // No respond from current slave, process timeout.
811                 // Need to drop response later if it is received after timeout.
812                 ESP_LOGD(TAG, "Select timeout, left time: %ju ms.",
813                                                     xMBTCPPortMasterGetRespTimeLeft(pxCurrInfo));
814                 xTime = xMBTCPPortMasterGetRespTimeLeft(pxCurrInfo);
815                 // Wait completion of last transaction
816                 xMBMasterPortFsmWaitConfirmation(MB_EVENT_REQ_DONE_MASK, pdMS_TO_TICKS(xTime + 1));
817                 xMBTCPPortMasterCheckShutdown();
818                 continue;
819             } else if (xRes < 0) {
820                 // Select error (slave connection or r/w failure).
821                 ESP_LOGD(TAG, MB_SLAVE_FMT(", socket select error. Slave disconnected?"),
822                             pxCurrInfo->xIndex, pxCurrInfo->xSockId, pxCurrInfo->pcIpAddr);
823                 xTime = xMBTCPPortMasterGetRespTimeLeft(pxCurrInfo);
824                 // Wait completion of last transaction
825                 xMBMasterPortFsmWaitConfirmation(MB_EVENT_REQ_DONE_MASK, pdMS_TO_TICKS(xTime));
826                 // Stop polling process
827                 vMBTCPPortMasterStopPoll();
828                 xMBTCPPortMasterCheckShutdown();
829                 // Check disconnected slaves, do not need a result just to print information.
830                 xMBTCPPortMasterCheckConnState(&xConnSet);
831                 break;
832             } else {
833                 // Check to make sure that active slave data is ready
834                 if (FD_ISSET(pxCurrInfo->xSockId, &xReadSet)) {
835                     int xRet = ERR_BUF;
836                     for (int retry = 0; (xRet == ERR_BUF) && (retry < MB_TCP_READ_BUF_RETRY_CNT); retry++) {
837                         xRet = vMBTCPPortMasterReadPacket(pxCurrInfo);
838                         // The error ERR_BUF means received response to previous request
839                         // (due to timeout) with the same socket ID and incorrect TID,
840                         // then ignore it and try to get next response buffer.
841                     }
842                     if (xRet > 0) {
843                         // Response received correctly, send an event to stack
844                         xMBTCPPortMasterFsmSetError(EV_ERROR_INIT, EV_MASTER_FRAME_RECEIVED);
845                         ESP_LOGD(TAG, MB_SLAVE_FMT(", frame received."),
846                                     pxCurrInfo->xIndex, pxCurrInfo->xSockId, pxCurrInfo->pcIpAddr);
847                     } else if ((xRet == ERR_TIMEOUT) || (xMBTCPPortMasterGetRespTimeLeft(pxCurrInfo) == 0)) {
848                         // Timeout occurred when receiving frame, process respond timeout
849                         ESP_LOGD(TAG, MB_SLAVE_FMT(", frame read timeout."),
850                                     pxCurrInfo->xIndex, pxCurrInfo->xSockId, pxCurrInfo->pcIpAddr);
851                     } else if (xRet == ERR_BUF) {
852                         // After retries a response with incorrect TID received, process failure.
853                         xMBTCPPortMasterFsmSetError(EV_ERROR_RECEIVE_DATA, EV_MASTER_ERROR_PROCESS);
854                         ESP_LOGD(TAG, MB_SLAVE_FMT(", frame error."),
855                                     pxCurrInfo->xIndex, pxCurrInfo->xSockId, pxCurrInfo->pcIpAddr);
856                     } else {
857                         ESP_LOGE(TAG, MB_SLAVE_FMT(", critical error=%d."),
858                                     pxCurrInfo->xIndex, pxCurrInfo->xSockId, pxCurrInfo->pcIpAddr, xRet);
859                         // Stop polling process
860                         vMBTCPPortMasterStopPoll();
861                         xMBTCPPortMasterCheckShutdown();
862                         // Check disconnected slaves, do not need a result just to print information.
863                         xMBTCPPortMasterCheckConnState(&xConnSet);
864                         break;
865                     }
866                     xTime = xMBTCPPortMasterGetRespTimeLeft(pxCurrInfo);
867                     ESP_LOGD(TAG, "Slave #%d, data processing left time %ju [ms].", pxCurrInfo->xIndex, xTime);
868                     // Wait completion of Modbus frame processing before start of new transaction.
869                     if (xMBMasterPortFsmWaitConfirmation(MB_EVENT_REQ_DONE_MASK, pdMS_TO_TICKS(xTime))) {
870                         ESP_LOGD(TAG, MB_SLAVE_FMT(", data processing completed."),
871                                 pxCurrInfo->xIndex, pxCurrInfo->xSockId, pxCurrInfo->pcIpAddr);
872                     }
873                     xTime = xMBTCPGetTimeStamp() - pxCurrInfo->xSendTimeStamp;
874                     ESP_LOGD(TAG, MB_SLAVE_FMT(", processing time[us] = %ju."),
875                                                     pxCurrInfo->xIndex, pxCurrInfo->xSockId, pxCurrInfo->pcIpAddr, xTime);
876                 }
877             }
878             xMBTCPPortMasterCheckShutdown();
879         } // while(usMbSlaveInfoCount)
880     } // while (1)
881     vTaskDelete(NULL);
882 }
883 
884 extern void vMBMasterPortEventClose(void);
885 extern void vMBMasterPortTimerClose(void);
886 
887 void
vMBMasterTCPPortEnable(void)888 vMBMasterTCPPortEnable(void)
889 {
890     vTaskResume(xMbPortConfig.xMbTcpTaskHandle);
891 }
892 
893 void
vMBMasterTCPPortDisable(void)894 vMBMasterTCPPortDisable(void)
895 {
896     // Try to exit the task gracefully, so select could release its internal callbacks
897     // that were allocated on the stack of the task we're going to delete
898     xShutdownSemaphore = xSemaphoreCreateBinary();
899     // if no semaphore (alloc issues) or couldn't acquire it, just delete the task
900     if (xShutdownSemaphore == NULL || xSemaphoreTake(xShutdownSemaphore, pdMS_TO_TICKS(MB_EVENT_WAIT_TOUT_MS)) != pdTRUE) {
901         ESP_LOGW(TAG, "Modbus port task couldn't exit gracefully within timeout -> abruptly deleting the task.");
902         vTaskDelete(xMbPortConfig.xMbTcpTaskHandle);
903     }
904     if (xShutdownSemaphore) {
905         vSemaphoreDelete(xShutdownSemaphore);
906         xShutdownSemaphore = NULL;
907     }
908     for (USHORT ucCnt = 0; ucCnt < MB_TCP_PORT_MAX_CONN; ucCnt++) {
909         MbSlaveInfo_t* pxInfo = xMbPortConfig.pxMbSlaveInfo[ucCnt];
910         if (pxInfo) {
911             xMBTCPPortMasterCloseConnection(pxInfo);
912             if (pxInfo->pucRcvBuf) {
913                 free(pxInfo->pucRcvBuf);
914             }
915             free(pxInfo);
916             xMbPortConfig.pxMbSlaveInfo[ucCnt] = NULL;
917         }
918     }
919     free(xMbPortConfig.pxMbSlaveInfo);
920 }
921 
922 void
vMBMasterTCPPortClose(void)923 vMBMasterTCPPortClose(void)
924 {
925     vQueueDelete(xMbPortConfig.xConnectQueue);
926     vMBMasterPortTimerClose();
927     // Release resources for the event queue.
928     vMBMasterPortEventClose();
929 }
930 
931 BOOL
xMBMasterTCPPortGetRequest(UCHAR ** ppucMBTCPFrame,USHORT * usTCPLength)932 xMBMasterTCPPortGetRequest( UCHAR ** ppucMBTCPFrame, USHORT * usTCPLength )
933 {
934     MbSlaveInfo_t* pxInfo = vMBTCPPortMasterGetCurrInfo();
935     *ppucMBTCPFrame = pxInfo->pucRcvBuf;
936     *usTCPLength = pxInfo->usRcvPos;
937 
938     // Reset the buffer.
939     pxInfo->usRcvPos = 0;
940     // Save slave receive timestamp
941     if (pxInfo->xRcvErr == ERR_OK && *usTCPLength > 0) {
942         pxInfo->xRecvTimeStamp = xMBTCPGetTimeStamp();
943         return TRUE;
944     }
945     return FALSE;
946 }
947 
xMBMasterTCPPortWritePoll(MbSlaveInfo_t * pxInfo,const UCHAR * pucMBTCPFrame,USHORT usTCPLength,ULONG xTimeout)948 int xMBMasterTCPPortWritePoll(MbSlaveInfo_t* pxInfo, const UCHAR * pucMBTCPFrame, USHORT usTCPLength, ULONG xTimeout)
949 {
950     // Check if the socket is alive (writable and SO_ERROR == 0)
951     int xRes = (int)xMBTCPPortMasterCheckAlive(pxInfo, xTimeout);
952     if ((xRes < 0) && (xRes != ERR_INPROGRESS))
953     {
954         ESP_LOGE(TAG, MB_SLAVE_FMT(", is not writable, error: %d, errno %d"),
955                                     pxInfo->xIndex, pxInfo->xSockId, pxInfo->pcIpAddr, xRes, errno);
956         return xRes;
957     }
958     xRes = send(pxInfo->xSockId, pucMBTCPFrame, usTCPLength, TCP_NODELAY);
959     if (xRes < 0) {
960         ESP_LOGE(TAG, MB_SLAVE_FMT(", send data error: %d, errno %d"),
961                                         pxInfo->xIndex, pxInfo->xSockId, pxInfo->pcIpAddr, xRes, errno);
962     }
963     return xRes;
964 }
965 
966 BOOL
xMBMasterTCPPortSendResponse(UCHAR * pucMBTCPFrame,USHORT usTCPLength)967 xMBMasterTCPPortSendResponse( UCHAR * pucMBTCPFrame, USHORT usTCPLength )
968 {
969     BOOL bFrameSent = FALSE;
970     USHORT ucCurSlaveIndex = ucMBMasterGetDestAddress();
971     MbSlaveInfo_t* pxInfo = vMBTCPPortMasterFindSlaveInfo(ucCurSlaveIndex);
972 
973     // If the slave is correct and active then send data
974     // otherwise treat slave as died and skip
975     if (pxInfo != NULL) {
976         if (pxInfo->xSockId < 0) {
977             ESP_LOGD(TAG, MB_SLAVE_FMT(", send to died slave, error = %d"),
978                                                   pxInfo->xIndex, pxInfo->xSockId, pxInfo->pcIpAddr, pxInfo->xError);
979         } else {
980             // Apply TID field to the frame before send
981             pucMBTCPFrame[MB_TCP_TID] = (UCHAR)(pxInfo->usTidCnt >> 8U);
982             pucMBTCPFrame[MB_TCP_TID + 1] = (UCHAR)(pxInfo->usTidCnt & 0xFF);
983             int xRes = xMBMasterTCPPortWritePoll(pxInfo, pucMBTCPFrame, usTCPLength, MB_TCP_SEND_TIMEOUT_MS);
984             if (xRes < 0) {
985                 ESP_LOGE(TAG, MB_SLAVE_FMT(", send data failure, err(errno) = %d(%d)."),
986                                             pxInfo->xIndex, pxInfo->xSockId, pxInfo->pcIpAddr, xRes, errno);
987                 bFrameSent = FALSE;
988                 pxInfo->xError = xRes;
989             } else {
990                 bFrameSent = TRUE;
991                 ESP_LOGD(TAG, MB_SLAVE_FMT(", send data successful: TID=0x%02x, %d (bytes), errno %d"),
992                                                     pxInfo->xIndex, pxInfo->xSockId, pxInfo->pcIpAddr, pxInfo->usTidCnt, xRes, errno);
993                 pxInfo->xError = 0;
994                 pxInfo->usRcvPos = 0;
995                 if (pxInfo->usTidCnt < (USHRT_MAX - 1)) {
996                     pxInfo->usTidCnt++;
997                 } else {
998                     pxInfo->usTidCnt = (USHORT)(pxInfo->xIndex << 8U);
999                 }
1000             }
1001             pxInfo->xSendTimeStamp = xMBTCPGetTimeStamp();
1002         }
1003     } else {
1004         ESP_LOGD(TAG, "Send data to died slave, address = %d", ucCurSlaveIndex);
1005     }
1006     vMBMasterPortTimersRespondTimeoutEnable();
1007     xMBMasterPortEventPost(EV_MASTER_FRAME_SENT);
1008     return bFrameSent;
1009 }
1010 
1011 // Timer handler to check timeout of socket response
1012 BOOL MB_PORT_ISR_ATTR
xMBMasterTCPTimerExpired(void)1013 xMBMasterTCPTimerExpired(void)
1014 {
1015     BOOL xNeedPoll = FALSE;
1016 
1017     vMBMasterPortTimersDisable();
1018     // If timer mode is respond timeout, the master event then turns EV_MASTER_EXECUTE status.
1019     if (xMBMasterGetCurTimerMode() == MB_TMODE_RESPOND_TIMEOUT) {
1020         vMBMasterSetErrorType(EV_ERROR_RESPOND_TIMEOUT);
1021         xNeedPoll = xMBMasterPortEventPost(EV_MASTER_ERROR_PROCESS);
1022     }
1023 
1024     return xNeedPoll;
1025 }
1026 
1027 #endif //#if MB_MASTER_TCP_ENABLED
1028