src/modbus-tcp.c

   1 /*
   2  * Copyright © Stéphane Raimbault <stephane.raimbault@gmail.com>
   3  *
   4  * SPDX-License-Identifier: LGPL-2.1-or-later
   5  */
   6
   7 // clang-format off
   8 #if defined(_WIN32)
   9 # define OS_WIN32
  10 /* ws2_32.dll has getaddrinfo and freeaddrinfo on Windows XP and later.
  11  * minwg32 headers check WINVER before allowing the use of these */
  12 # ifndef WINVER
  13 #   define WINVER 0x0501
  14 # endif
  15 #endif
  16
  17 #include <stdio.h>
  18 #include <stdlib.h>
  19 #include <stdint.h>
  20 #include <limits.h>
  21 #include <string.h>
  22 #include <errno.h>
  23 #ifndef _MSC_VER
  24 #include <unistd.h>
  25 #endif
  26 #include <signal.h>
  27 #include <sys/types.h>
  28
  29 #if defined(_WIN32)
  30 /* Already set in modbus-tcp.h but it seems order matters in VS2005 */
  31 # include <winsock2.h>
  32 # include <ws2tcpip.h>
  33 # define SHUT_RDWR 2
  34 # define close closesocket
  35 # define strdup _strdup
  36 #else
  37 # include <sys/socket.h>
  38 # include <sys/ioctl.h>
  39
  40 #if defined(__OpenBSD__) || (defined(__FreeBSD__) && __FreeBSD__ < 5)
  41 # define OS_BSD
  42 # include <netinet/in_systm.h>
  43 #endif
  44
  45 # include <netinet/in.h>
  46 # include <netinet/ip.h>
  47 # include <netinet/tcp.h>
  48 # include <arpa/inet.h>
  49 # include <netdb.h>
  50 #endif
  51
  52 #if !defined(MSG_NOSIGNAL)
  53 #define MSG_NOSIGNAL 0
  54 #endif
  55
  56 #if defined(_AIX) && !defined(MSG_DONTWAIT)
  57 #define MSG_DONTWAIT MSG_NONBLOCK
  58 #endif
  59 // clang-format on
  60
  61 #include "modbus-private.h"
  62
  63 #include "modbus-tcp-private.h"
  64 #include "modbus-tcp.h"
  65
  66 #ifdef OS_WIN32
  67 static int _modbus_tcp_init_win32(void)
  68 {
  69     /* Initialise Windows Socket API */
  70     WSADATA wsaData;
  71
  72     if (WSAStartup(MAKEWORD(2, 2), &wsaData) != 0) {
  73         fprintf(stderr,
  74                 "WSAStartup() returned error code %d\n",
  75                 (unsigned int) GetLastError());
  76         errno = EIO;
  77         return -1;
  78     }
  79     return 0;
  80 }
  81 #endif
  82
  83 static int _modbus_set_slave(modbus_t *ctx, int slave)
  84 {
  85     int max_slave = (ctx->quirks & MODBUS_QUIRK_MAX_SLAVE) ? 255 : 247;
  86
  87     /* Broadcast address is 0 (MODBUS_BROADCAST_ADDRESS) */
  88     if (slave >= 0 && slave <= max_slave) {
  89         ctx->slave = slave;
  90     } else if (slave == MODBUS_TCP_SLAVE) {
  91         /* The special value MODBUS_TCP_SLAVE (0xFF) can be used in TCP mode to
  92          * restore the default value. */
  93         ctx->slave = slave;
  94     } else {
  95         errno = EINVAL;
  96         return -1;
  97     }
  98
  99     return 0;
 100 }
 101
 102 /* Builds a TCP request header */
 103 static int _modbus_tcp_build_request_basis(
 104     modbus_t *ctx, int function, int addr, int nb, uint8_t *req)
 105 {
 106     modbus_tcp_t *ctx_tcp = ctx->backend_data;
 107
 108     /* Increase transaction ID */
 109     if (ctx_tcp->t_id < UINT16_MAX)
 110         ctx_tcp->t_id++;
 111     else
 112         ctx_tcp->t_id = 0;
 113     req[0] = ctx_tcp->t_id >> 8;
 114     req[1] = ctx_tcp->t_id & 0x00ff;
 115
 116     /* Protocol Modbus */
 117     req[2] = 0;
 118     req[3] = 0;
 119
 120     /* Length will be defined later by set_req_length_tcp at offsets 4
 121        and 5 */
 122
 123     req[6] = ctx->slave;
 124     req[7] = function;
 125     req[8] = addr >> 8;
 126     req[9] = addr & 0x00ff;
 127     req[10] = nb >> 8;
 128     req[11] = nb & 0x00ff;
 129
 130     return _MODBUS_TCP_PRESET_REQ_LENGTH;
 131 }
 132
 133 /* Builds a TCP response header */
 134 static int _modbus_tcp_build_response_basis(sft_t *sft, uint8_t *rsp)
 135 {
 136     /* Extract from MODBUS Messaging on TCP/IP Implementation
 137        Guide V1.0b (page 23/46):
 138        The transaction identifier is used to associate the future
 139        response with the request. */
 140     rsp[0] = sft->t_id >> 8;
 141     rsp[1] = sft->t_id & 0x00ff;
 142
 143     /* Protocol Modbus */
 144     rsp[2] = 0;
 145     rsp[3] = 0;
 146
 147     /* Length will be set later by send_msg (4 and 5) */
 148
 149     /* The slave ID is copied from the indication */
 150     rsp[6] = sft->slave;
 151     rsp[7] = sft->function;
 152
 153     return _MODBUS_TCP_PRESET_RSP_LENGTH;
 154 }
 155
 156 static int _modbus_tcp_get_response_tid(const uint8_t *req)
 157 {
 158     return (req[0] << 8) + req[1];
 159 }
 160
 161 static int _modbus_tcp_send_msg_pre(uint8_t *req, int req_length)
 162 {
 163     /* Subtract the header length to the message length */
 164     int mbap_length = req_length - 6;
 165
 166     req[4] = mbap_length >> 8;
 167     req[5] = mbap_length & 0x00FF;
 168
 169     return req_length;
 170 }
 171
 172 static ssize_t _modbus_tcp_send(modbus_t *ctx, const uint8_t *req, int req_length)
 173 {
 174     /* MSG_NOSIGNAL
 175        Requests not to send SIGPIPE on errors on stream oriented
 176        sockets when the other end breaks the connection.  The EPIPE
 177        error is still returned. */
 178     return send(ctx->s, (const char *) req, req_length, MSG_NOSIGNAL);
 179 }
 180
 181 static int _modbus_tcp_receive(modbus_t *ctx, uint8_t *req)
 182 {
 183     return _modbus_receive_msg(ctx, req, MSG_INDICATION);
 184 }
 185
 186 static ssize_t _modbus_tcp_recv(modbus_t *ctx, uint8_t *rsp, int rsp_length)
 187 {
 188     return recv(ctx->s, (char *) rsp, rsp_length, 0);
 189 }
 190
 191 static int _modbus_tcp_check_integrity(modbus_t *ctx, uint8_t *msg, const int msg_length)
 192 {
 193     return msg_length;
 194 }
 195
 196 static int _modbus_tcp_pre_check_confirmation(modbus_t *ctx,
 197                                               const uint8_t *req,
 198                                               const uint8_t *rsp,
 199                                               int rsp_length)
 200 {
 201     unsigned int protocol_id;
 202     /* Check transaction ID */
 203     if (req[0] != rsp[0] || req[1] != rsp[1]) {
 204         if (ctx->debug) {
 205             fprintf(stderr,
 206                     "Invalid transaction ID received 0x%X (not 0x%X)\n",
 207                     (rsp[0] << 8) + rsp[1],
 208                     (req[0] << 8) + req[1]);
 209         }
 210         errno = EMBBADDATA;
 211         return -1;
 212     }
 213
 214     /* Check protocol ID */
 215     protocol_id = (rsp[2] << 8) + rsp[3];
 216     if (protocol_id != 0x0) {
 217         if (ctx->debug) {
 218             fprintf(stderr, "Invalid protocol ID received 0x%X (not 0x0)\n", protocol_id);
 219         }
 220         errno = EMBBADDATA;
 221         return -1;
 222     }
 223
 224     return 0;
 225 }
 226
 227 static int _modbus_tcp_set_ipv4_options(int s)
 228 {
 229     int rc;
 230     int option;
 231
 232     /* Set the TCP no delay flag */
 233     /* SOL_TCP = IPPROTO_TCP */
 234     option = 1;
 235     rc = setsockopt(s, IPPROTO_TCP, TCP_NODELAY, &option, sizeof(int));
 236     if (rc == -1) {
 237         return -1;
 238     }
 239
 240     /* If the OS does not offer SOCK_NONBLOCK, fall back to setting FIONBIO to
 241      * make sockets non-blocking */
 242     /* Do not care about the return value, this is optional */
 243 #if !defined(SOCK_NONBLOCK) && defined(FIONBIO)
 244 #ifdef OS_WIN32
 245     {
 246         /* Setting FIONBIO expects an unsigned long according to MSDN */
 247         u_long loption = 1;
 248         ioctlsocket(s, FIONBIO, &loption);
 249     }
 250 #else
 251     option = 1;
 252     ioctl(s, FIONBIO, &option);
 253 #endif
 254 #endif
 255
 256 #ifndef OS_WIN32
 257     /**
 258      * Cygwin defines IPTOS_LOWDELAY but can't handle that flag so it's
 259      * necessary to workaround that problem.
 260      **/
 261     /* Set the IP low delay option */
 262     option = IPTOS_LOWDELAY;
 263     rc = setsockopt(s, IPPROTO_IP, IP_TOS, &option, sizeof(int));
 264     if (rc == -1) {
 265         return -1;
 266     }
 267 #endif
 268
 269     return 0;
 270 }
 271
 272 static int _connect(int sockfd,
 273                     const struct sockaddr *addr,
 274                     socklen_t addrlen,
 275                     const struct timeval *ro_tv)
 276 {
 277     int rc = connect(sockfd, addr, addrlen);
 278
 279 #ifdef OS_WIN32
 280     int wsaError = 0;
 281     if (rc == -1) {
 282         wsaError = WSAGetLastError();
 283     }
 284
 285     if (wsaError == WSAEWOULDBLOCK || wsaError == WSAEINPROGRESS) {
 286 #else
 287     if (rc == -1 && errno == EINPROGRESS) {
 288 #endif
 289         fd_set wset;
 290         int optval;
 291         socklen_t optlen = sizeof(optval);
 292         struct timeval tv = *ro_tv;
 293
 294         /* Wait to be available in writing */
 295         FD_ZERO(&wset);
 296         FD_SET(sockfd, &wset);
 297         rc = select(sockfd + 1, NULL, &wset, NULL, &tv);
 298         if (rc < 0) {
 299             /* Fail */
 300             return -1;
 301         }
 302
 303         if (rc == 0) {
 304             /* Timeout */
 305             errno = ETIMEDOUT;
 306             return -1;
 307         }
 308
 309         /* The connection is established if SO_ERROR and optval are set to 0 */
 310         rc = getsockopt(sockfd, SOL_SOCKET, SO_ERROR, (void *) &optval, &optlen);
 311         if (rc == 0 && optval == 0) {
 312             return 0;
 313         } else {
 314             errno = ECONNREFUSED;
 315             return -1;
 316         }
 317     }
 318     return rc;
 319 }
 320
 321 /* Establishes a modbus TCP connection with a Modbus server. */
 322 static int _modbus_tcp_connect(modbus_t *ctx)
 323 {
 324     int rc;
 325     /* Specialized version of sockaddr for Internet socket address (same size) */
 326     struct sockaddr_in addr;
 327     modbus_tcp_t *ctx_tcp = ctx->backend_data;
 328     int flags = SOCK_STREAM;
 329
 330 #ifdef OS_WIN32
 331     if (_modbus_tcp_init_win32() == -1) {
 332         return -1;
 333     }
 334 #endif
 335
 336 #ifdef SOCK_CLOEXEC
 337     flags |= SOCK_CLOEXEC;
 338 #endif
 339
 340 #ifdef SOCK_NONBLOCK
 341     flags |= SOCK_NONBLOCK;
 342 #endif
 343
 344     ctx->s = socket(PF_INET, flags, 0);
 345     if (ctx->s < 0) {
 346         return -1;
 347     }
 348
 349     rc = _modbus_tcp_set_ipv4_options(ctx->s);
 350     if (rc == -1) {
 351         close(ctx->s);
 352         ctx->s = -1;
 353         return -1;
 354     }
 355
 356     if (ctx->debug) {
 357         printf("Connecting to %s:%d\n", ctx_tcp->ip, ctx_tcp->port);
 358     }
 359
 360     addr.sin_family = AF_INET;
 361     addr.sin_port = htons(ctx_tcp->port);
 362     rc = inet_pton(addr.sin_family, ctx_tcp->ip, &(addr.sin_addr));
 363     if (rc <= 0) {
 364         if (ctx->debug) {
 365             fprintf(stderr, "Invalid IP address: %s\n", ctx_tcp->ip);
 366         }
 367         close(ctx->s);
 368         ctx->s = -1;
 369         return -1;
 370     }
 371
 372     rc =
 373         _connect(ctx->s, (struct sockaddr *) &addr, sizeof(addr), &ctx->response_timeout);
 374     if (rc == -1) {
 375         close(ctx->s);
 376         ctx->s = -1;
 377         return -1;
 378     }
 379
 380     return 0;
 381 }
 382
 383 /* Establishes a modbus TCP PI connection with a Modbus server. */
 384 static int _modbus_tcp_pi_connect(modbus_t *ctx)
 385 {
 386     int rc;
 387     struct addrinfo *ai_list;
 388     struct addrinfo *ai_ptr;
 389     struct addrinfo ai_hints;
 390     modbus_tcp_pi_t *ctx_tcp_pi = ctx->backend_data;
 391
 392 #ifdef OS_WIN32
 393     if (_modbus_tcp_init_win32() == -1) {
 394         return -1;
 395     }
 396 #endif
 397
 398     memset(&ai_hints, 0, sizeof(ai_hints));
 399 #ifdef AI_ADDRCONFIG
 400     ai_hints.ai_flags |= AI_ADDRCONFIG;
 401 #endif
 402     ai_hints.ai_family = AF_UNSPEC;
 403     ai_hints.ai_socktype = SOCK_STREAM;
 404     ai_hints.ai_addr = NULL;
 405     ai_hints.ai_canonname = NULL;
 406     ai_hints.ai_next = NULL;
 407
 408     ai_list = NULL;
 409     rc = getaddrinfo(ctx_tcp_pi->node, ctx_tcp_pi->service, &ai_hints, &ai_list);
 410     if (rc != 0) {
 411         if (ctx->debug) {
 412 #ifdef HAVE_GAI_STRERROR
 413             fprintf(stderr, "Error returned by getaddrinfo: %s\n", gai_strerror(rc));
 414 #else
 415             fprintf(stderr, "Error returned by getaddrinfo: %d\n", rc);
 416 #endif
 417         }
 418         freeaddrinfo(ai_list);
 419         errno = ECONNREFUSED;
 420         return -1;
 421     }
 422
 423     for (ai_ptr = ai_list; ai_ptr != NULL; ai_ptr = ai_ptr->ai_next) {
 424         int flags = ai_ptr->ai_socktype;
 425         int s;
 426
 427 #ifdef SOCK_CLOEXEC
 428         flags |= SOCK_CLOEXEC;
 429 #endif
 430
 431 #ifdef SOCK_NONBLOCK
 432         flags |= SOCK_NONBLOCK;
 433 #endif
 434
 435         s = socket(ai_ptr->ai_family, flags, ai_ptr->ai_protocol);
 436         if (s < 0)
 437             continue;
 438
 439         if (ai_ptr->ai_family == AF_INET)
 440             _modbus_tcp_set_ipv4_options(s);
 441
 442         if (ctx->debug) {
 443             printf("Connecting to [%s]:%s\n", ctx_tcp_pi->node, ctx_tcp_pi->service);
 444         }
 445
 446         rc = _connect(s, ai_ptr->ai_addr, ai_ptr->ai_addrlen, &ctx->response_timeout);
 447         if (rc == -1) {
 448             close(s);
 449             continue;
 450         }
 451
 452         ctx->s = s;
 453         break;
 454     }
 455
 456     freeaddrinfo(ai_list);
 457
 458     if (ctx->s < 0) {
 459         return -1;
 460     }
 461
 462     return 0;
 463 }
 464
 465 static unsigned int _modbus_tcp_is_connected(modbus_t *ctx)
 466 {
 467     return ctx->s >= 0;
 468 }
 469
 470 /* Closes the network connection and socket in TCP mode */
 471 static void _modbus_tcp_close(modbus_t *ctx)
 472 {
 473     if (ctx->s >= 0) {
 474         shutdown(ctx->s, SHUT_RDWR);
 475         close(ctx->s);
 476         ctx->s = -1;
 477     }
 478 }
 479
 480 static int _modbus_tcp_flush(modbus_t *ctx)
 481 {
 482     int rc;
 483     // Use an unsigned 16-bit integer to reduce overflow risk. The flush function
 484     // is not expected to handle huge amounts of data (> 2GB).
 485     uint16_t rc_sum = 0;
 486
 487     do {
 488         /* Extract the garbage from the socket */
 489         char devnull[MODBUS_TCP_MAX_ADU_LENGTH];
 490 #ifndef OS_WIN32
 491         rc = recv(ctx->s, devnull, MODBUS_TCP_MAX_ADU_LENGTH, MSG_DONTWAIT);
 492 #else
 493         /* On Win32, it's a bit more complicated to not wait */
 494         fd_set rset;
 495         struct timeval tv;
 496
 497         tv.tv_sec = 0;
 498         tv.tv_usec = 0;
 499         FD_ZERO(&rset);
 500         FD_SET(ctx->s, &rset);
 501         rc = select(ctx->s + 1, &rset, NULL, NULL, &tv);
 502         if (rc == -1) {
 503             return -1;
 504         }
 505
 506         if (rc == 1) {
 507             /* There is data to flush */
 508             rc = recv(ctx->s, devnull, MODBUS_TCP_MAX_ADU_LENGTH, 0);
 509         }
 510 #endif
 511         if (rc > 0) {
 512             // Check for overflow before adding
 513             if (rc_sum <= UINT16_MAX - rc) {
 514                 rc_sum += rc;
 515             } else {
 516                 // Handle overflow
 517                 ctx->error_recovery = MODBUS_ERROR_RECOVERY_PROTOCOL;
 518                 errno = EOVERFLOW;
 519                 return -1;
 520             }
 521         }
 522     } while (rc == MODBUS_TCP_MAX_ADU_LENGTH);
 523
 524     return rc_sum;
 525 }
 526
 527 /* Listens for any request from one or many modbus masters in TCP */
 528 int modbus_tcp_listen(modbus_t *ctx, int nb_connection)
 529 {
 530     int new_s;
 531     int enable;
 532     int flags;
 533     struct sockaddr_in addr;
 534     modbus_tcp_t *ctx_tcp;
 535     int rc;
 536
 537     if (ctx == NULL) {
 538         errno = EINVAL;
 539         return -1;
 540     }
 541
 542     ctx_tcp = ctx->backend_data;
 543
 544 #ifdef OS_WIN32
 545     if (_modbus_tcp_init_win32() == -1) {
 546         return -1;
 547     }
 548 #endif
 549
 550     flags = SOCK_STREAM;
 551
 552 #ifdef SOCK_CLOEXEC
 553     flags |= SOCK_CLOEXEC;
 554 #endif
 555
 556     new_s = socket(PF_INET, flags, IPPROTO_TCP);
 557     if (new_s == -1) {
 558         return -1;
 559     }
 560
 561     enable = 1;
 562     if (setsockopt(new_s, SOL_SOCKET, SO_REUSEADDR, &enable, sizeof(enable)) == -1) {
 563         close(new_s);
 564         return -1;
 565     }
 566
 567     memset(&addr, 0, sizeof(addr));
 568     addr.sin_family = AF_INET;
 569     /* If the modbus port is < to 1024, we need the setuid root. */
 570     addr.sin_port = htons(ctx_tcp->port);
 571     if (ctx_tcp->ip[0] == '0') {
 572         /* Listen any addresses */
 573         addr.sin_addr.s_addr = htonl(INADDR_ANY);
 574     } else {
 575         /* Listen only specified IP address */
 576         rc = inet_pton(addr.sin_family, ctx_tcp->ip, &(addr.sin_addr));
 577         if (rc <= 0) {
 578             if (ctx->debug) {
 579                 fprintf(stderr, "Invalid IP address: %s\n", ctx_tcp->ip);
 580             }
 581             close(new_s);
 582             return -1;
 583         }
 584     }
 585
 586     if (bind(new_s, (struct sockaddr *) &addr, sizeof(addr)) == -1) {
 587         close(new_s);
 588         return -1;
 589     }
 590
 591     if (listen(new_s, nb_connection) == -1) {
 592         close(new_s);
 593         return -1;
 594     }
 595
 596     return new_s;
 597 }
 598
 599 int modbus_tcp_pi_listen(modbus_t *ctx, int nb_connection)
 600 {
 601     int rc;
 602     struct addrinfo *ai_list;
 603     struct addrinfo *ai_ptr;
 604     struct addrinfo ai_hints;
 605     const char *node;
 606     const char *service;
 607     int new_s;
 608     modbus_tcp_pi_t *ctx_tcp_pi;
 609
 610     if (ctx == NULL) {
 611         errno = EINVAL;
 612         return -1;
 613     }
 614
 615     ctx_tcp_pi = ctx->backend_data;
 616
 617 #ifdef OS_WIN32
 618     if (_modbus_tcp_init_win32() == -1) {
 619         return -1;
 620     }
 621 #endif
 622
 623     if (ctx_tcp_pi->node[0] == 0) {
 624         node = NULL; /* == any */
 625     } else {
 626         node = ctx_tcp_pi->node;
 627     }
 628
 629     if (ctx_tcp_pi->service[0] == 0) {
 630         service = "502";
 631     } else {
 632         service = ctx_tcp_pi->service;
 633     }
 634
 635     memset(&ai_hints, 0, sizeof(ai_hints));
 636     /* If node is not NULL, than the AI_PASSIVE flag is ignored. */
 637     ai_hints.ai_flags |= AI_PASSIVE;
 638 #ifdef AI_ADDRCONFIG
 639     ai_hints.ai_flags |= AI_ADDRCONFIG;
 640 #endif
 641     ai_hints.ai_family = AF_UNSPEC;
 642     ai_hints.ai_socktype = SOCK_STREAM;
 643     ai_hints.ai_addr = NULL;
 644     ai_hints.ai_canonname = NULL;
 645     ai_hints.ai_next = NULL;
 646
 647     ai_list = NULL;
 648     rc = getaddrinfo(node, service, &ai_hints, &ai_list);
 649     if (rc != 0) {
 650         if (ctx->debug) {
 651 #ifdef HAVE_GAI_STRERROR
 652             fprintf(stderr, "Error returned by getaddrinfo: %s\n", gai_strerror(rc));
 653 #else
 654             fprintf(stderr, "Error returned by getaddrinfo: %d\n", rc);
 655 #endif
 656         }
 657         freeaddrinfo(ai_list);
 658         errno = ECONNREFUSED;
 659         return -1;
 660     }
 661
 662     new_s = -1;
 663     for (ai_ptr = ai_list; ai_ptr != NULL; ai_ptr = ai_ptr->ai_next) {
 664         int flags = ai_ptr->ai_socktype;
 665         int s;
 666
 667 #ifdef SOCK_CLOEXEC
 668         flags |= SOCK_CLOEXEC;
 669 #endif
 670
 671         s = socket(ai_ptr->ai_family, flags, ai_ptr->ai_protocol);
 672         if (s < 0) {
 673             if (ctx->debug) {
 674                 perror("socket");
 675             }
 676             continue;
 677         } else {
 678             int enable = 1;
 679             rc = setsockopt(s, SOL_SOCKET, SO_REUSEADDR, &enable, sizeof(enable));
 680             if (rc != 0) {
 681                 close(s);
 682                 if (ctx->debug) {
 683                     perror("setsockopt");
 684                 }
 685                 continue;
 686             }
 687         }
 688
 689         rc = bind(s, ai_ptr->ai_addr, ai_ptr->ai_addrlen);
 690         if (rc != 0) {
 691             close(s);
 692             if (ctx->debug) {
 693                 perror("bind");
 694             }
 695             continue;
 696         }
 697
 698         rc = listen(s, nb_connection);
 699         if (rc != 0) {
 700             close(s);
 701             if (ctx->debug) {
 702                 perror("listen");
 703             }
 704             continue;
 705         }
 706
 707         new_s = s;
 708         break;
 709     }
 710     freeaddrinfo(ai_list);
 711
 712     if (new_s < 0) {
 713         return -1;
 714     }
 715
 716     return new_s;
 717 }
 718
 719 int modbus_tcp_accept(modbus_t *ctx, int *s)
 720 {
 721     struct sockaddr_in addr;
 722     socklen_t addrlen;
 723
 724     if (ctx == NULL) {
 725         errno = EINVAL;
 726         return -1;
 727     }
 728
 729     addrlen = sizeof(addr);
 730 #ifdef HAVE_ACCEPT4
 731     /* Inherit socket flags and use accept4 call */
 732     ctx->s = accept4(*s, (struct sockaddr *) &addr, &addrlen, SOCK_CLOEXEC);
 733 #else
 734     ctx->s = accept(*s, (struct sockaddr *) &addr, &addrlen);
 735 #endif
 736
 737     if (ctx->s < 0) {
 738         return -1;
 739     }
 740
 741     if (ctx->debug) {
 742         char buf[INET_ADDRSTRLEN];
 743         if (inet_ntop(AF_INET, &(addr.sin_addr), buf, INET_ADDRSTRLEN) == NULL) {
 744             fprintf(stderr, "Client connection accepted from unparsable IP.\n");
 745         } else {
 746             printf("Client connection accepted from %s.\n", buf);
 747         }
 748     }
 749
 750     return ctx->s;
 751 }
 752
 753 int modbus_tcp_pi_accept(modbus_t *ctx, int *s)
 754 {
 755     struct sockaddr_in6 addr;
 756     socklen_t addrlen;
 757
 758     if (ctx == NULL) {
 759         errno = EINVAL;
 760         return -1;
 761     }
 762
 763     addrlen = sizeof(addr);
 764 #ifdef HAVE_ACCEPT4
 765     /* Inherit socket flags and use accept4 call */
 766     ctx->s = accept4(*s, (struct sockaddr *) &addr, &addrlen, SOCK_CLOEXEC);
 767 #else
 768     ctx->s = accept(*s, (struct sockaddr *) &addr, &addrlen);
 769 #endif
 770
 771     if (ctx->s < 0) {
 772         return -1;
 773     }
 774
 775     if (ctx->debug) {
 776         char buf[INET6_ADDRSTRLEN];
 777         if (inet_ntop(AF_INET6, &(addr.sin6_addr), buf, INET6_ADDRSTRLEN) == NULL) {
 778             fprintf(stderr, "Client connection accepted from unparsable IP.\n");
 779         } else {
 780             printf("Client connection accepted from %s.\n", buf);
 781         }
 782     }
 783
 784     return ctx->s;
 785 }
 786
 787 static int
 788 _modbus_tcp_select(modbus_t *ctx, fd_set *rset, struct timeval *tv, int length_to_read)
 789 {
 790     int s_rc;
 791     while ((s_rc = select(ctx->s + 1, rset, NULL, NULL, tv)) == -1) {
 792         if (errno == EINTR) {
 793             if (ctx->debug) {
 794                 fprintf(stderr, "A non blocked signal was caught\n");
 795             }
 796             /* Necessary after an error */
 797             FD_ZERO(rset);
 798             FD_SET(ctx->s, rset);
 799         } else {
 800             return -1;
 801         }
 802     }
 803
 804     if (s_rc == 0) {
 805         errno = ETIMEDOUT;
 806         return -1;
 807     }
 808
 809     return s_rc;
 810 }
 811
 812 static void _modbus_tcp_free(modbus_t *ctx)
 813 {
 814     if (ctx->backend_data) {
 815         free(ctx->backend_data);
 816     }
 817     free(ctx);
 818 }
 819
 820 static void _modbus_tcp_pi_free(modbus_t *ctx)
 821 {
 822     if (ctx->backend_data) {
 823         modbus_tcp_pi_t *ctx_tcp_pi = ctx->backend_data;
 824         free(ctx_tcp_pi->node);
 825         free(ctx_tcp_pi->service);
 826         free(ctx->backend_data);
 827     }
 828
 829     free(ctx);
 830 }
 831
 832 // clang-format off
 833 const modbus_backend_t _modbus_tcp_backend = {
 834     _MODBUS_BACKEND_TYPE_TCP,
 835     _MODBUS_TCP_HEADER_LENGTH,
 836     _MODBUS_TCP_CHECKSUM_LENGTH,
 837     MODBUS_TCP_MAX_ADU_LENGTH,
 838     _modbus_set_slave,
 839     _modbus_tcp_build_request_basis,
 840     _modbus_tcp_build_response_basis,
 841     _modbus_tcp_get_response_tid,
 842     _modbus_tcp_send_msg_pre,
 843     _modbus_tcp_send,
 844     _modbus_tcp_receive,
 845     _modbus_tcp_recv,
 846     _modbus_tcp_check_integrity,
 847     _modbus_tcp_pre_check_confirmation,
 848     _modbus_tcp_connect,
 849     _modbus_tcp_is_connected,
 850     _modbus_tcp_close,
 851     _modbus_tcp_flush,
 852     _modbus_tcp_select,
 853     _modbus_tcp_free
 854 };
 855
 856 const modbus_backend_t _modbus_tcp_pi_backend = {
 857     _MODBUS_BACKEND_TYPE_TCP,
 858     _MODBUS_TCP_HEADER_LENGTH,
 859     _MODBUS_TCP_CHECKSUM_LENGTH,
 860     MODBUS_TCP_MAX_ADU_LENGTH,
 861     _modbus_set_slave,
 862     _modbus_tcp_build_request_basis,
 863     _modbus_tcp_build_response_basis,
 864     _modbus_tcp_get_response_tid,
 865     _modbus_tcp_send_msg_pre,
 866     _modbus_tcp_send,
 867     _modbus_tcp_receive,
 868     _modbus_tcp_recv,
 869     _modbus_tcp_check_integrity,
 870     _modbus_tcp_pre_check_confirmation,
 871     _modbus_tcp_pi_connect,
 872     _modbus_tcp_is_connected,
 873     _modbus_tcp_close,
 874     _modbus_tcp_flush,
 875     _modbus_tcp_select,
 876     _modbus_tcp_pi_free
 877 };
 878
 879 // clang-format on
 880
 881 modbus_t *modbus_new_tcp(const char *ip, int port)
 882 {
 883     modbus_t *ctx;
 884     modbus_tcp_t *ctx_tcp;
 885     size_t dest_size;
 886     size_t ret_size;
 887
 888 #if defined(OS_BSD)
 889     /* MSG_NOSIGNAL is unsupported on *BSD so we install an ignore
 890        handler for SIGPIPE. */
 891     struct sigaction sa;
 892
 893     sa.sa_handler = SIG_IGN;
 894     if (sigaction(SIGPIPE, &sa, NULL) < 0) {
 895         /* The debug flag can't be set here... */
 896         fprintf(stderr, "Could not install SIGPIPE handler.\n");
 897         return NULL;
 898     }
 899 #endif
 900
 901     ctx = (modbus_t *) malloc(sizeof(modbus_t));
 902     if (ctx == NULL) {
 903         return NULL;
 904     }
 905     _modbus_init_common(ctx);
 906
 907     /* Could be changed after to reach a remote serial Modbus device */
 908     ctx->slave = MODBUS_TCP_SLAVE;
 909
 910     ctx->backend = &_modbus_tcp_backend;
 911
 912     ctx->backend_data = (modbus_tcp_t *) malloc(sizeof(modbus_tcp_t));
 913     if (ctx->backend_data == NULL) {
 914         modbus_free(ctx);
 915         errno = ENOMEM;
 916         return NULL;
 917     }
 918     ctx_tcp = (modbus_tcp_t *) ctx->backend_data;
 919
 920     if (ip != NULL) {
 921         dest_size = sizeof(char) * 16;
 922         ret_size = strlcpy(ctx_tcp->ip, ip, dest_size);
 923         if (ret_size == 0) {
 924             fprintf(stderr, "The IP string is empty\n");
 925             modbus_free(ctx);
 926             errno = EINVAL;
 927             return NULL;
 928         }
 929
 930         if (ret_size >= dest_size) {
 931             fprintf(stderr, "The IP string has been truncated\n");
 932             modbus_free(ctx);
 933             errno = EINVAL;
 934             return NULL;
 935         }
 936     } else {
 937         ctx_tcp->ip[0] = '0';
 938     }
 939     ctx_tcp->port = port;
 940     ctx_tcp->t_id = 0;
 941
 942     return ctx;
 943 }
 944
 945 modbus_t *modbus_new_tcp_pi(const char *node, const char *service)
 946 {
 947     modbus_t *ctx;
 948     modbus_tcp_pi_t *ctx_tcp_pi;
 949
 950     ctx = (modbus_t *) malloc(sizeof(modbus_t));
 951     if (ctx == NULL) {
 952         return NULL;
 953     }
 954     _modbus_init_common(ctx);
 955
 956     /* Could be changed after to reach a remote serial Modbus device */
 957     ctx->slave = MODBUS_TCP_SLAVE;
 958
 959     ctx->backend = &_modbus_tcp_pi_backend;
 960
 961     ctx->backend_data = (modbus_tcp_pi_t *) malloc(sizeof(modbus_tcp_pi_t));
 962     if (ctx->backend_data == NULL) {
 963         modbus_free(ctx);
 964         errno = ENOMEM;
 965         return NULL;
 966     }
 967     ctx_tcp_pi = (modbus_tcp_pi_t *) ctx->backend_data;
 968     ctx_tcp_pi->node = NULL;
 969     ctx_tcp_pi->service = NULL;
 970
 971     if (node != NULL) {
 972         ctx_tcp_pi->node = strdup(node);
 973     } else {
 974         /* The node argument can be empty to indicate any hosts */
 975         ctx_tcp_pi->node = strdup("");
 976     }
 977
 978     if (ctx_tcp_pi->node == NULL) {
 979         modbus_free(ctx);
 980         errno = ENOMEM;
 981         return NULL;
 982     }
 983
 984     if (service != NULL && service[0] != '\0') {
 985         ctx_tcp_pi->service = strdup(service);
 986     } else {
 987         /* Default Modbus port number */
 988         ctx_tcp_pi->service = strdup("502");
 989     }
 990
 991     if (ctx_tcp_pi->service == NULL) {
 992         modbus_free(ctx);
 993         errno = ENOMEM;
 994         return NULL;
 995     }
 996
 997     ctx_tcp_pi->t_id = 0;
 998
 999     return ctx;
1000 }