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 #include "sys/time.h"
43 #include "esp_netif.h"
44
45 /* ----------------------- lwIP includes ------------------------------------*/
46 #include "lwip/err.h"
47 #include "lwip/sockets.h"
48 #include "lwip/netdb.h"
49 #include "net/if.h"
50
51 /* ----------------------- Modbus includes ----------------------------------*/
52 #include "mb.h"
53 #include "mbport.h"
54 #include "port.h"
55 #include "mbframe.h"
56 #include "port_tcp_slave.h"
57 #include "esp_modbus_common.h" // for common types for network options
58
59 #if MB_TCP_ENABLED
60
61 /* ----------------------- Defines -----------------------------------------*/
62 #define MB_TCP_DISCONNECT_TIMEOUT ( CONFIG_FMB_TCP_CONNECTION_TOUT_SEC * 1000000 ) // disconnect timeout in uS
63 #define MB_TCP_RESP_TIMEOUT_MS ( MB_MASTER_TIMEOUT_MS_RESPOND - 2 ) // slave response time limit
64 #define MB_TCP_NET_LISTEN_BACKLOG ( SOMAXCONN )
65
66 /* ----------------------- Prototypes ---------------------------------------*/
67 void vMBPortEventClose( void );
68
69 /* ----------------------- Static variables ---------------------------------*/
70 static const char *TAG = "MB_TCP_SLAVE_PORT";
71 static int xListenSock = -1;
72 static SemaphoreHandle_t xShutdownSemaphore = NULL;
73 static MbSlavePortConfig_t xConfig = { 0 };
74
75 /* ----------------------- Static functions ---------------------------------*/
76 // The helper function to get time stamp in microseconds
xMBTCPGetTimeStamp(void)77 static int64_t xMBTCPGetTimeStamp(void)
78 {
79 int64_t xTimeStamp = esp_timer_get_time();
80 return xTimeStamp;
81 }
82
vxMBTCPPortMStoTimeVal(USHORT usTimeoutMs,struct timeval * pxTimeout)83 static void vxMBTCPPortMStoTimeVal(USHORT usTimeoutMs, struct timeval *pxTimeout)
84 {
85 pxTimeout->tv_sec = usTimeoutMs / 1000;
86 pxTimeout->tv_usec = (usTimeoutMs - (pxTimeout->tv_sec * 1000)) * 1000;
87 }
88
xMBTCPPortRespQueueCreate(void)89 static xQueueHandle xMBTCPPortRespQueueCreate(void)
90 {
91 xQueueHandle xRespQueueHandle = xQueueCreate(2, sizeof(void*));
92 MB_PORT_CHECK((xRespQueueHandle != NULL), NULL, "TCP respond queue creation failure.");
93 return xRespQueueHandle;
94 }
95
vMBTCPPortRespQueueDelete(xQueueHandle xRespQueueHandle)96 static void vMBTCPPortRespQueueDelete(xQueueHandle xRespQueueHandle)
97 {
98 vQueueDelete(xRespQueueHandle);
99 }
100
vxMBTCPPortRespQueueRecv(xQueueHandle xRespQueueHandle)101 static void* vxMBTCPPortRespQueueRecv(xQueueHandle xRespQueueHandle)
102 {
103 void* pvResp = NULL;
104 MB_PORT_CHECK(xRespQueueHandle != NULL, NULL, "Response queue is not initialized.");
105 BaseType_t xStatus = xQueueReceive(xRespQueueHandle,
106 (void*)&pvResp,
107 pdMS_TO_TICKS(MB_TCP_RESP_TIMEOUT_MS));
108 MB_PORT_CHECK((xStatus == pdTRUE), NULL, "Could not get respond confirmation.");
109 MB_PORT_CHECK((pvResp), NULL, "Incorrect response processing.");
110 return pvResp;
111 }
112
vxMBTCPPortRespQueueSend(xQueueHandle xRespQueueHandle,void * pvResp)113 static BOOL vxMBTCPPortRespQueueSend(xQueueHandle xRespQueueHandle, void* pvResp)
114 {
115 MB_PORT_CHECK(xRespQueueHandle != NULL, FALSE, "Response queue is not initialized.");
116 BaseType_t xStatus = xQueueSend(xConfig.xRespQueueHandle,
117 (const void*)&pvResp,
118 pdMS_TO_TICKS(MB_TCP_RESP_TIMEOUT_MS));
119 MB_PORT_CHECK((xStatus == pdTRUE), FALSE, "FAIL to send to response queue.");
120 return TRUE;
121 }
122
123 static void vMBTCPPortServerTask(void *pvParameters);
124
125 /* ----------------------- Begin implementation -----------------------------*/
126 BOOL
xMBTCPPortInit(USHORT usTCPPort)127 xMBTCPPortInit( USHORT usTCPPort )
128 {
129 BOOL bOkay = FALSE;
130
131 xConfig.pxMbClientInfo = calloc(MB_TCP_PORT_MAX_CONN + 1, sizeof(MbClientInfo_t*));
132 if (!xConfig.pxMbClientInfo) {
133 ESP_LOGE(TAG, "TCP client info allocation failure.");
134 return FALSE;
135 }
136 for(int idx = 0; idx < MB_TCP_PORT_MAX_CONN; xConfig.pxMbClientInfo[idx] = NULL, idx++);
137
138 xConfig.xRespQueueHandle = xMBTCPPortRespQueueCreate();
139 if (!xConfig.xRespQueueHandle) {
140 ESP_LOGE(TAG, "Response queue allocation failure.");
141 return FALSE;
142 }
143
144 xConfig.usPort = usTCPPort;
145 xConfig.eMbProto = MB_PROTO_TCP;
146 xConfig.usClientCount = 0;
147 xConfig.pvNetIface = NULL;
148 xConfig.xIpVer = MB_PORT_IPV4;
149 xConfig.pcBindAddr = NULL;
150
151 // Create task for packet processing
152 BaseType_t xErr = xTaskCreate(vMBTCPPortServerTask,
153 "tcp_server_task",
154 MB_TCP_STACK_SIZE,
155 NULL,
156 MB_TCP_TASK_PRIO,
157 &xConfig.xMbTcpTaskHandle);
158 vTaskSuspend(xConfig.xMbTcpTaskHandle);
159 if (xErr != pdTRUE)
160 {
161 ESP_LOGE(TAG, "Server task creation failure.");
162 vTaskDelete(xConfig.xMbTcpTaskHandle);
163 } else {
164 ESP_LOGI(TAG, "Protocol stack initialized.");
165 bOkay = TRUE;
166 }
167 return bOkay;
168 }
169
vMBTCPPortSlaveSetNetOpt(void * pvNetIf,eMBPortIpVer xIpVersion,eMBPortProto xProto,CHAR * pcBindAddrStr)170 void vMBTCPPortSlaveSetNetOpt(void* pvNetIf, eMBPortIpVer xIpVersion, eMBPortProto xProto, CHAR* pcBindAddrStr)
171 {
172 // Set network options
173 xConfig.pvNetIface = pvNetIf;
174 xConfig.eMbProto = xProto;
175 xConfig.xIpVer = xIpVersion;
176 xConfig.pcBindAddr = pcBindAddrStr;
177 }
178
xMBTCPPortAcceptConnection(int xListenSockId,char ** pcIPAddr)179 static int xMBTCPPortAcceptConnection(int xListenSockId, char** pcIPAddr)
180 {
181 MB_PORT_CHECK(pcIPAddr, -1, "Wrong IP address pointer.");
182 MB_PORT_CHECK((xListenSockId > 0), -1, "Incorrect listen socket ID.");
183
184 // Address structure large enough for both IPv4 or IPv6 address
185 struct sockaddr_storage xSrcAddr;
186 CHAR cAddrStr[128];
187 int xSockId = -1;
188 CHAR* pcStr = NULL;
189 socklen_t xSize = sizeof(struct sockaddr_storage);
190
191 // Accept new socket connection if not active
192 xSockId = accept(xListenSockId, (struct sockaddr *)&xSrcAddr, &xSize);
193 if (xSockId < 0) {
194 ESP_LOGE(TAG, "Unable to accept connection: errno=%d", errno);
195 close(xSockId);
196 } else {
197 // Get the sender's ip address as string
198 if (xSrcAddr.ss_family == PF_INET) {
199 inet_ntoa_r(((struct sockaddr_in *)&xSrcAddr)->sin_addr.s_addr, cAddrStr, sizeof(cAddrStr) - 1);
200 }
201 #if CONFIG_LWIP_IPV6
202 else if (xSrcAddr.ss_family == PF_INET6) {
203 inet6_ntoa_r(((struct sockaddr_in6 *)&xSrcAddr)->sin6_addr, cAddrStr, sizeof(cAddrStr) - 1);
204 }
205 #endif
206 ESP_LOGI(TAG, "Socket (#%d), accept client connection from address: %s", xSockId, cAddrStr);
207 pcStr = calloc(1, strlen(cAddrStr) + 1);
208 if (pcStr && pcIPAddr) {
209 memcpy(pcStr, cAddrStr, strlen(cAddrStr));
210 pcStr[strlen(cAddrStr)] = '\0';
211 *pcIPAddr = pcStr; // Set IP address of connected client
212 }
213 }
214 return xSockId;
215 }
216
xMBTCPPortCloseConnection(MbClientInfo_t * pxInfo)217 static BOOL xMBTCPPortCloseConnection(MbClientInfo_t* pxInfo)
218 {
219 MB_PORT_CHECK(pxInfo, FALSE, "Client info is NULL.");
220
221 if (pxInfo->xSockId == -1) {
222 ESP_LOGE(TAG, "Wrong socket info or disconnected socket: %d.", pxInfo->xSockId);
223 return FALSE;
224 }
225 if (shutdown(pxInfo->xSockId, SHUT_RDWR) == -1) {
226 ESP_LOGE(TAG, "Socket (#%d), shutdown failed: errno %d", pxInfo->xSockId, errno);
227 }
228 close(pxInfo->xSockId);
229 pxInfo->xSockId = -1;
230 if (xConfig.usClientCount) {
231 xConfig.usClientCount--; // decrement counter of client connections
232 } else {
233 xConfig.pxCurClientInfo = NULL;
234 }
235 return TRUE;
236 }
237
xMBTCPPortRxPoll(MbClientInfo_t * pxClientInfo,ULONG xTimeoutMs)238 static int xMBTCPPortRxPoll(MbClientInfo_t* pxClientInfo, ULONG xTimeoutMs)
239 {
240 int xRet = ERR_CLSD;
241 struct timeval xTimeVal;
242 fd_set xReadSet;
243 int64_t xStartTimeStamp = 0;
244
245 // Receive data from connected client
246 if (pxClientInfo && pxClientInfo->xSockId > -1) {
247 // Set receive timeout
248 vxMBTCPPortMStoTimeVal(xTimeoutMs, &xTimeVal);
249 xStartTimeStamp = xMBTCPGetTimeStamp();
250 while (1)
251 {
252 FD_ZERO(&xReadSet);
253 FD_SET(pxClientInfo->xSockId, &xReadSet);
254 xRet = select(pxClientInfo->xSockId + 1, &xReadSet, NULL, NULL, &xTimeVal);
255 if (xRet == -1)
256 {
257 // If select an error occurred
258 xRet = ERR_CLSD;
259 break;
260 } else if (xRet == 0) {
261 // timeout occurred
262 if ((xStartTimeStamp + xTimeoutMs * 1000) > xMBTCPGetTimeStamp()) {
263 ESP_LOGD(TAG, "Socket (#%d) Read timeout.", pxClientInfo->xSockId);
264 xRet = ERR_TIMEOUT;
265 break;
266 }
267 }
268 if (FD_ISSET(pxClientInfo->xSockId, &xReadSet)) {
269 // If new buffer received then read Modbus packet into buffer
270 MB_PORT_CHECK((pxClientInfo->usTCPBufPos + pxClientInfo->usTCPFrameBytesLeft < MB_TCP_BUF_SIZE),
271 ERR_BUF, "Socket (#%d), incorrect request buffer size = %d, ignore.",
272 pxClientInfo->xSockId,
273 (pxClientInfo->usTCPBufPos + pxClientInfo->usTCPFrameBytesLeft));
274 int xLength = recv(pxClientInfo->xSockId, &pxClientInfo->pucTCPBuf[pxClientInfo->usTCPBufPos],
275 pxClientInfo->usTCPFrameBytesLeft, MSG_DONTWAIT);
276 if (xLength < 0) {
277 // If an error occurred during receiving
278 ESP_LOGE(TAG, "Receive failed: length=%d, errno=%d", xLength, errno);
279 xRet = (err_t)xLength;
280 break;
281 } else if (xLength == 0) {
282 // Socket connection closed
283 ESP_LOGD(TAG, "Socket (#%d)(%s), connection closed.",
284 pxClientInfo->xSockId, pxClientInfo->pcIpAddr);
285 xRet = ERR_CLSD;
286 break;
287 } else {
288 // New data received
289 pxClientInfo->usTCPBufPos += xLength;
290 pxClientInfo->usTCPFrameBytesLeft -= xLength;
291 if (pxClientInfo->usTCPBufPos >= MB_TCP_FUNC) {
292 // Length is a byte count of Modbus PDU (function code + data) and the
293 // unit identifier.
294 xLength = (int)MB_TCP_GET_FIELD(pxClientInfo->pucTCPBuf, MB_TCP_LEN);
295 // Is the frame already complete.
296 if (pxClientInfo->usTCPBufPos < (MB_TCP_UID + xLength)) {
297 // The incomplete frame is received
298 pxClientInfo->usTCPFrameBytesLeft = xLength + MB_TCP_UID - pxClientInfo->usTCPBufPos;
299 } else if (pxClientInfo->usTCPBufPos == (MB_TCP_UID + xLength)) {
300 #if MB_TCP_DEBUG
301 prvvMBTCPLogFrame(TAG, (UCHAR*)&pxClientInfo->pucTCPBuf[0], pxClientInfo->usTCPBufPos);
302 #endif
303 // Copy TID field from incoming packet
304 pxClientInfo->usTidCnt = MB_TCP_GET_FIELD(pxClientInfo->pucTCPBuf, MB_TCP_TID);
305 xRet = pxClientInfo->usTCPBufPos;
306 break;
307 } else if ((pxClientInfo->usTCPBufPos + xLength) >= MB_TCP_BUF_SIZE) {
308 ESP_LOGE(TAG, "Incorrect buffer received (%u) bytes.", xLength);
309 // This should not happen. We can't deal with such a client and
310 // drop the connection for security reasons.
311 xRet = ERR_BUF;
312 break;
313 }
314 } // if ( pxClientInfo->usTCPBufPos >= MB_TCP_FUNC )
315 } // if data received
316 } // if (FD_ISSET(pxClientInfo->xSockId, &xReadSet))
317 } // while (1)
318 }
319 return (xRet);
320 }
321
322 // Create a listening socket on pcBindIp: Port
323 static int
vMBTCPPortBindAddr(const CHAR * pcBindIp)324 vMBTCPPortBindAddr(const CHAR* pcBindIp)
325 {
326 int xPar, xRet;
327 int xListenSockFd = -1;
328 struct addrinfo xHint;
329 struct addrinfo* pxAddrList;
330 struct addrinfo* pxCurAddr;
331 CHAR* pcStr = NULL;
332
333 memset( &xHint, 0, sizeof( xHint ) );
334
335 // Bind to IPv6 and/or IPv4, but only in the desired protocol
336 // Todo: Find a reason why AF_UNSPEC does not work for IPv6
337 xHint.ai_family = (xConfig.xIpVer == MB_PORT_IPV4) ? AF_INET : AF_INET6;
338 xHint.ai_socktype = (xConfig.eMbProto == MB_PROTO_UDP) ? SOCK_DGRAM : SOCK_STREAM;
339 // The LWIP has an issue when connection to IPv6 socket
340 xHint.ai_protocol = (xConfig.eMbProto == MB_PROTO_UDP) ? IPPROTO_UDP : IPPROTO_TCP;
341 xHint.ai_flags = AI_NUMERICSERV;
342
343 if (pcBindIp == NULL) {
344 xHint.ai_flags |= AI_PASSIVE;
345 } else {
346 xHint.ai_flags |= AI_CANONNAME;
347 }
348
349 if (asprintf(&pcStr, "%u", xConfig.usPort) == -1) {
350 abort();
351 }
352
353 xRet = getaddrinfo(pcBindIp, pcStr, &xHint, &pxAddrList);
354 free(pcStr);
355
356 if (xRet != 0) {
357 return -1;
358 }
359
360 // Try the sockaddr until a binding succeeds
361 for (pxCurAddr = pxAddrList; pxCurAddr != NULL; pxCurAddr = pxCurAddr->ai_next)
362 {
363 xListenSockFd = (int)socket(pxCurAddr->ai_family, pxCurAddr->ai_socktype,
364 pxCurAddr->ai_protocol);
365 if (xListenSockFd < 0)
366 {
367 continue;
368 }
369
370 xPar = 1;
371 // Allow multi client connections
372 if (setsockopt(xListenSockFd, SOL_SOCKET, SO_REUSEADDR,
373 (const char*)&xPar, sizeof(xPar)) != 0)
374 {
375 close(xListenSockFd);
376 xListenSockFd = -1;
377 continue;
378 }
379
380 if (bind(xListenSockFd, (struct sockaddr *)pxCurAddr->ai_addr,
381 (socklen_t)pxCurAddr->ai_addrlen) != 0 )
382 {
383 close(xListenSockFd);
384 xListenSockFd = -1;
385 continue;
386 }
387
388 // Listen only makes sense for TCP
389 if (xConfig.eMbProto == MB_PROTO_TCP)
390 {
391 if (listen(xListenSockFd, MB_TCP_NET_LISTEN_BACKLOG) != 0)
392 {
393 ESP_LOGE(TAG, "Error occurred during listen: errno=%d", errno);
394 close(xListenSockFd);
395 xListenSockFd = -1;
396 continue;
397 }
398 }
399 // Bind was successful
400 pcStr = (pxCurAddr->ai_canonname == NULL) ? (CHAR*)"\0" : pxCurAddr->ai_canonname;
401 ESP_LOGI(TAG, "Socket (#%d), listener %s on port: %d, errno=%d",
402 xListenSockFd, pcStr, xConfig.usPort, errno);
403 break;
404 }
405
406 freeaddrinfo(pxAddrList);
407 return(xListenSockFd);
408 }
409
410 static void
vMBTCPPortFreeClientInfo(MbClientInfo_t * pxClientInfo)411 vMBTCPPortFreeClientInfo(MbClientInfo_t* pxClientInfo)
412 {
413 if (pxClientInfo) {
414 if (pxClientInfo->pucTCPBuf) {
415 free((void*)pxClientInfo->pucTCPBuf);
416 }
417 if (pxClientInfo->pcIpAddr) {
418 free((void*)pxClientInfo->pcIpAddr);
419 }
420 free((void*)pxClientInfo);
421 }
422 }
423
424
vMBTCPPortServerTask(void * pvParameters)425 static void vMBTCPPortServerTask(void *pvParameters)
426 {
427 int xErr = 0;
428 fd_set xReadSet;
429 int i;
430 CHAR* pcClientIp = NULL;
431 struct timeval xTimeVal;
432
433 // Main connection cycle
434 while (1) {
435 // Create listen socket
436 xListenSock = vMBTCPPortBindAddr(xConfig.pcBindAddr);
437 if (xListenSock < 0) {
438 continue;
439 }
440
441 // Connections handling cycle
442 while (1) {
443 //clear the socket set
444 FD_ZERO(&xReadSet);
445 //add master socket to set
446 FD_SET(xListenSock, &xReadSet);
447 int xMaxSd = xListenSock;
448 xConfig.usClientCount = 0;
449
450 vxMBTCPPortMStoTimeVal(1, &xTimeVal);
451 // Initialize read set and file descriptor according to
452 // all registered connected clients
453 for (i = 0; i < MB_TCP_PORT_MAX_CONN; i++) {
454 if ((xConfig.pxMbClientInfo[i] != NULL) && (xConfig.pxMbClientInfo[i]->xSockId > 0)) {
455 // calculate max file descriptor for select
456 xMaxSd = (xConfig.pxMbClientInfo[i]->xSockId > xMaxSd) ?
457 xConfig.pxMbClientInfo[i]->xSockId : xMaxSd;
458 FD_SET(xConfig.pxMbClientInfo[i]->xSockId, &xReadSet);
459 xConfig.usClientCount++;
460 }
461 }
462
463 // Wait for an activity on one of the sockets, timeout is NULL, so wait indefinitely
464 xErr = select(xMaxSd + 1 , &xReadSet , NULL , NULL , NULL);
465 if ((xErr < 0) && (errno != EINTR)) {
466 // First check if the task is not flagged for shutdown
467 if (xListenSock == -1 && xShutdownSemaphore) {
468 xSemaphoreGive(xShutdownSemaphore);
469 vTaskDelete(NULL);
470 }
471 // error occurred during wait for read
472 ESP_LOGE(TAG, "select() errno = %d.", errno);
473 continue;
474 } else if (xErr == 0) {
475 // If timeout happened, something is wrong
476 ESP_LOGE(TAG, "select() timeout, errno = %d.", errno);
477 }
478
479 // If something happened on the master socket, then its an incoming connection.
480 if (FD_ISSET(xListenSock, &xReadSet) && xConfig.usClientCount < MB_TCP_PORT_MAX_CONN) {
481 MbClientInfo_t* pxClientInfo = NULL;
482 // find first empty place to insert connection info
483 for (i = 0; i < MB_TCP_PORT_MAX_CONN; i++) {
484 pxClientInfo = xConfig.pxMbClientInfo[i];
485 if (pxClientInfo == NULL) {
486 break;
487 }
488 }
489 // if request for new connection but no space left
490 if (pxClientInfo != NULL) {
491 if (xConfig.pxMbClientInfo[MB_TCP_PORT_MAX_CONN] == NULL) {
492 ESP_LOGE(TAG, "Fail to accept connection %d, only %d connections supported.", i + 1, MB_TCP_PORT_MAX_CONN);
493 }
494 xConfig.pxMbClientInfo[MB_TCP_PORT_MAX_CONN] = pxClientInfo; // set last connection info
495 } else {
496 // allocate memory for new client info
497 pxClientInfo = calloc(1, sizeof(MbClientInfo_t));
498 if (!pxClientInfo) {
499 ESP_LOGE(TAG, "Client info allocation fail.");
500 vMBTCPPortFreeClientInfo(pxClientInfo);
501 pxClientInfo = NULL;
502 } else {
503 // Accept new client connection
504 pxClientInfo->xSockId = xMBTCPPortAcceptConnection(xListenSock, &pcClientIp);
505 if (pxClientInfo->xSockId < 0) {
506 ESP_LOGE(TAG, "Fail to accept connection for client %d.", (xConfig.usClientCount - 1));
507 // Accept connection fail, then free client info and continue polling.
508 vMBTCPPortFreeClientInfo(pxClientInfo);
509 pxClientInfo = NULL;
510 continue;
511 }
512 pxClientInfo->pucTCPBuf = calloc(MB_TCP_BUF_SIZE, sizeof(UCHAR));
513 if (!pxClientInfo->pucTCPBuf) {
514 ESP_LOGE(TAG, "Fail to allocate buffer for client %d.", (xConfig.usClientCount - 1));
515 vMBTCPPortFreeClientInfo(pxClientInfo);
516 pxClientInfo = NULL;
517 continue;
518 }
519 // Fill the connection info structure
520 xConfig.pxMbClientInfo[i] = pxClientInfo;
521 pxClientInfo->xIndex = i;
522 xConfig.usClientCount++;
523 pxClientInfo->pcIpAddr = pcClientIp;
524 pxClientInfo->xRecvTimeStamp = xMBTCPGetTimeStamp();
525 xConfig.pxMbClientInfo[MB_TCP_PORT_MAX_CONN] = NULL;
526 pxClientInfo->usTCPFrameBytesLeft = MB_TCP_FUNC;
527 pxClientInfo->usTCPBufPos = 0;
528 }
529 }
530 }
531 // Handle data request from client
532 if (xErr > 0) {
533 // Handling client connection requests
534 for (i = 0; i < MB_TCP_PORT_MAX_CONN; i++) {
535 MbClientInfo_t* pxClientInfo = xConfig.pxMbClientInfo[i];
536 if ((pxClientInfo != NULL) && (pxClientInfo->xSockId > 0)) {
537 if (FD_ISSET(pxClientInfo->xSockId, &xReadSet)) {
538 // Other sockets are ready to be read
539 xErr = xMBTCPPortRxPoll(pxClientInfo, MB_TCP_READ_TIMEOUT_MS);
540 // If an invalid data received from socket or connection fail
541 // or if timeout then drop connection and restart
542 if (xErr < 0) {
543 uint64_t xTimeStamp = xMBTCPGetTimeStamp();
544 // If data update is timed out
545 switch(xErr)
546 {
547 case ERR_TIMEOUT:
548 ESP_LOGE(TAG, "Socket (#%d)(%s), data receive timeout, time[us]: %d, close active connection.",
549 pxClientInfo->xSockId, pxClientInfo->pcIpAddr,
550 (int)(xTimeStamp - pxClientInfo->xRecvTimeStamp));
551 break;
552 case ERR_CLSD:
553 ESP_LOGE(TAG, "Socket (#%d)(%s), connection closed by peer.",
554 pxClientInfo->xSockId, pxClientInfo->pcIpAddr);
555 break;
556 case ERR_BUF:
557 default:
558 ESP_LOGE(TAG, "Socket (#%d)(%s), read data error: %d",
559 pxClientInfo->xSockId, pxClientInfo->pcIpAddr, xErr);
560 break;
561 }
562 // Close client connection
563 xMBTCPPortCloseConnection(pxClientInfo);
564
565 // This client does not respond, then unregister it
566 vMBTCPPortFreeClientInfo(pxClientInfo);
567 xConfig.pxMbClientInfo[i] = NULL;
568 xConfig.pxMbClientInfo[MB_TCP_PORT_MAX_CONN] = NULL;
569 // If no any active connections, break
570 if (!xConfig.usClientCount) {
571 xConfig.pxCurClientInfo = NULL;
572 break;
573 }
574 } else {
575 pxClientInfo->xRecvTimeStamp = xMBTCPGetTimeStamp();
576
577 // set current client info to active client from which we received request
578 xConfig.pxCurClientInfo = pxClientInfo;
579
580 // Complete frame received, inform state machine to process frame
581 xMBPortEventPost(EV_FRAME_RECEIVED);
582
583 ESP_LOGD(TAG, "Socket (#%d)(%s), get packet TID=0x%X, %d bytes.",
584 pxClientInfo->xSockId, pxClientInfo->pcIpAddr,
585 pxClientInfo->usTidCnt, xErr);
586
587 // Wait while response is not processed by stack by timeout
588 UCHAR* pucSentBuffer = vxMBTCPPortRespQueueRecv(xConfig.xRespQueueHandle);
589 if (pucSentBuffer == NULL) {
590 ESP_LOGE(TAG, "Response time exceeds configured %d [ms], ignore packet.",
591 MB_TCP_RESP_TIMEOUT_MS);
592 } else {
593 USHORT usSentTid = MB_TCP_GET_FIELD(pucSentBuffer, MB_TCP_TID);
594 if (usSentTid != pxClientInfo->usTidCnt) {
595 ESP_LOGE(TAG, "Sent TID(%x) != Recv TID(%x), ignore packet.",
596 usSentTid, pxClientInfo->usTidCnt);
597 }
598 }
599
600 // Get time stamp of last data update
601 pxClientInfo->xSendTimeStamp = xMBTCPGetTimeStamp();
602 ESP_LOGD(TAG, "Client %d, Socket(#%d), processing time = %d (us).",
603 pxClientInfo->xIndex, pxClientInfo->xSockId,
604 (int)(pxClientInfo->xSendTimeStamp - pxClientInfo->xRecvTimeStamp));
605 }
606 } else {
607 if (pxClientInfo) {
608 // client is not ready to be read
609 int64_t xTime = xMBTCPGetTimeStamp() - pxClientInfo->xRecvTimeStamp;
610 if (xTime > MB_TCP_DISCONNECT_TIMEOUT) {
611 ESP_LOGE(TAG, "Client %d, Socket(#%d) do not answer for %d (us). Drop connection...",
612 pxClientInfo->xIndex, pxClientInfo->xSockId, (int)(xTime));
613 xMBTCPPortCloseConnection(pxClientInfo);
614
615 // This client does not respond, then delete registered data
616 vMBTCPPortFreeClientInfo(pxClientInfo);
617 xConfig.pxMbClientInfo[i] = NULL;
618 }
619 } else {
620 ESP_LOGE(TAG, "Client %d is disconnected.", i);
621 }
622 }
623 } // if ((pxClientInfo != NULL)
624 } // Handling client connection requests
625 }
626 } // while(1) // Handle connection cycle
627 } // Main connection cycle
628 vTaskDelete(NULL);
629 }
630
631 void
vMBTCPPortClose()632 vMBTCPPortClose( )
633 {
634 // Release resources for the event queue.
635
636 // Try to exit the task gracefully, so select could release its internal callbacks
637 // that were allocated on the stack of the task we're going to delete
638 xShutdownSemaphore = xSemaphoreCreateBinary();
639 vTaskResume(xConfig.xMbTcpTaskHandle);
640 if (xShutdownSemaphore == NULL || // if no semaphore (alloc issues) or couldn't acquire it, just delete the task
641 xSemaphoreTake(xShutdownSemaphore, 2*pdMS_TO_TICKS(CONFIG_FMB_MASTER_TIMEOUT_MS_RESPOND)) != pdTRUE) {
642 ESP_LOGE(TAG, "Task couldn't exit gracefully within timeout -> abruptly deleting the task");
643 vTaskDelete(xConfig.xMbTcpTaskHandle);
644 }
645 if (xShutdownSemaphore) {
646 vSemaphoreDelete(xShutdownSemaphore);
647 xShutdownSemaphore = NULL;
648 }
649
650 vMBPortEventClose( );
651 }
652
vMBTCPPortEnable(void)653 void vMBTCPPortEnable( void )
654 {
655 vTaskResume(xConfig.xMbTcpTaskHandle);
656 }
657
658 void
vMBTCPPortDisable(void)659 vMBTCPPortDisable( void )
660 {
661 vTaskSuspend(xConfig.xMbTcpTaskHandle);
662 for (int i = 0; i < MB_TCP_PORT_MAX_CONN; i++) {
663 MbClientInfo_t* pxClientInfo = xConfig.pxMbClientInfo[i];
664 if ((pxClientInfo != NULL) && (pxClientInfo->xSockId > 0)) {
665 xMBTCPPortCloseConnection(pxClientInfo);
666 vMBTCPPortFreeClientInfo(pxClientInfo);
667 xConfig.pxMbClientInfo[i] = NULL;
668 }
669 }
670 free(xConfig.pxMbClientInfo);
671 close(xListenSock);
672 xListenSock = -1;
673 vMBTCPPortRespQueueDelete(xConfig.xRespQueueHandle);
674 }
675
676 BOOL
xMBTCPPortGetRequest(UCHAR ** ppucMBTCPFrame,USHORT * usTCPLength)677 xMBTCPPortGetRequest( UCHAR ** ppucMBTCPFrame, USHORT * usTCPLength )
678 {
679 BOOL xRet = FALSE;
680 if (xConfig.pxCurClientInfo) {
681 *ppucMBTCPFrame = &xConfig.pxCurClientInfo->pucTCPBuf[0];
682 *usTCPLength = xConfig.pxCurClientInfo->usTCPBufPos;
683
684 // Reset the buffer.
685 xConfig.pxCurClientInfo->usTCPBufPos = 0;
686 xConfig.pxCurClientInfo->usTCPFrameBytesLeft = MB_TCP_FUNC;
687 xRet = TRUE;
688 }
689 return xRet;
690 }
691
692 BOOL
xMBTCPPortSendResponse(UCHAR * pucMBTCPFrame,USHORT usTCPLength)693 xMBTCPPortSendResponse( UCHAR * pucMBTCPFrame, USHORT usTCPLength )
694 {
695 BOOL bFrameSent = FALSE;
696 fd_set xWriteSet;
697 fd_set xErrorSet;
698 int xErr = -1;
699 struct timeval xTimeVal;
700
701 if (xConfig.pxCurClientInfo) {
702 FD_ZERO(&xWriteSet);
703 FD_ZERO(&xErrorSet);
704 FD_SET(xConfig.pxCurClientInfo->xSockId, &xWriteSet);
705 FD_SET(xConfig.pxCurClientInfo->xSockId, &xErrorSet);
706 vxMBTCPPortMStoTimeVal(MB_TCP_SEND_TIMEOUT_MS, &xTimeVal);
707 // Check if socket writable
708 xErr = select(xConfig.pxCurClientInfo->xSockId + 1, NULL, &xWriteSet, &xErrorSet, &xTimeVal);
709 if ((xErr == -1) || FD_ISSET(xConfig.pxCurClientInfo->xSockId, &xErrorSet)) {
710 ESP_LOGE(TAG, "Socket(#%d) , send select() error = %d.",
711 xConfig.pxCurClientInfo->xSockId, errno);
712 return FALSE;
713 }
714
715 // Apply TID field from request to the frame before send response
716 pucMBTCPFrame[MB_TCP_TID] = (UCHAR)(xConfig.pxCurClientInfo->usTidCnt >> 8U);
717 pucMBTCPFrame[MB_TCP_TID + 1] = (UCHAR)(xConfig.pxCurClientInfo->usTidCnt & 0xFF);
718
719 // Write message into socket and disable Nagle's algorithm
720 xErr = send(xConfig.pxCurClientInfo->xSockId, pucMBTCPFrame, usTCPLength, TCP_NODELAY);
721 if (xErr < 0) {
722 ESP_LOGE(TAG, "Socket(#%d), fail to send data, errno = %d",
723 xConfig.pxCurClientInfo->xSockId, errno);
724 xConfig.pxCurClientInfo->xError = xErr;
725 } else {
726 bFrameSent = TRUE;
727 vxMBTCPPortRespQueueSend(xConfig.xRespQueueHandle, (void*)pucMBTCPFrame);
728 }
729 } else {
730 ESP_LOGD(TAG, "Port is not active. Release lock.");
731 vxMBTCPPortRespQueueSend(xConfig.xRespQueueHandle, (void*)pucMBTCPFrame);
732 }
733 return bFrameSent;
734 }
735
736 #endif //#if MB_TCP_ENABLED
737
