1 /*

     2  * Copyright (c) 2002,2016 Mario de Sousa (msousa@fe.up.pt)

     3  *

     4  * This file is part of the Modbus library for Beremiz and matiec.

     5  *

     6  * This Modbus library is free software: you can redistribute it and/or modify

     7  * it under the terms of the GNU Lesser General Public License as published by

     8  * the Free Software Foundation, either version 3 of the License, or

     9  * (at your option) any later version.

    10  *

    11  * This program is distributed in the hope that it will be useful, but

    12  * WITHOUT ANY WARRANTY; without even the implied warranty of

    13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser 

    14  * General Public License for more details.

    15  *

    16  * You should have received a copy of the GNU Lesser General Public License

    17  * along with this Modbus library.  If not, see <http://www.gnu.org/licenses/>.

    18  *

    19  * This code is made available on the understanding that it will not be

    20  * used in safety-critical situations without a full and competent review.

    21  */

    22 

    23 

    24 

    25 

    26 

    27 #include <fcntl.h>      /* File control definitions */

    28 #include <stdio.h>      /* Standard input/output */

    29 #include <string.h>

    30 #include <stdlib.h>

    31 #include <termio.h>     /* POSIX terminal control definitions */

    32 #include <sys/time.h>   /* Time structures for select() */

    33 #include <unistd.h>     /* POSIX Symbolic Constants */

    34 #include <assert.h>

    35 #include <errno.h>      /* Error definitions */

    36 #include <time.h>       /* clock_gettime()   */

    37 #include <sys/types.h>

    38 #include <sys/socket.h>

    39 #include <netinet/in.h>  /* required for htons() and ntohs() */

    40 #include <netinet/tcp.h> /* TCP level socket options */

    41 #include <netinet/ip.h>  /* IP  level socket options */

    42 

    43 #include <pthread.h>

    44 #include <sched.h>       /* sched_yield() */

    45 

    46 

    47 

    48 #include "sin_util.h"   /* internet socket utility functions... */

    49 #include "mb_layer1.h"  /* The public interface this file implements... */

    50 #include "mb_tcp_private.h"

    51 

    52 

    53 

    54 /************************************/

    55 /**                                **/

    56 /** Include common code...         **/

    57 /**                                **/

    58 /************************************/

    59 

    60 #include "mb_time_util.h"

    61 

    62 

    63 //#define ERRMSG

    64 #define ERRMSG_HEAD "Modbus/TCP: "

    65 

    66 

    67 // #define DEBUG       /* uncomment to see the data sent and received */

    68 

    69 

    70 #ifdef DEBUG

    71 #ifndef ERRMSG

    72 #define ERRMSG

    73 #endif

    74 #endif

    75 

    76 

    77 

    78 /**************************************************************/

    79 /**************************************************************/

    80 /****                                                      ****/

    81 /****                                                      ****/

    82 /****                Forward Declarations                  ****/

    83 /****                    and Defaults                      ****/

    84 /****                                                      ****/

    85 /**************************************************************/

    86 /**************************************************************/

    87 

    88 

    89   /* A Node Descriptor metadata,

    90    *   Due to the fact that modbus TCP is connection oriented,

    91    *   and that if the client detects an error the connection

    92    *   must be shut down and re-established automatically,

    93    *   the modbus TCP layer needs to keep the address of the remote server.

    94    *

    95    * We do this by implementing a node descriptor table, in which each

    96    *   entry will have the remote address, and the file descriptor

    97    *   of the socket currently in use.

    98    *

    99    * We do not pass the file descriptor up to the next higher layer. We

   100    *   send them the node descriptor instead...

   101    */

   102 #define MB_MASTER_NODE 12

   103 #define MB_LISTEN_NODE 14

   104 #define MB_SLAVE_NODE  16

   105 #define MB_FREE_NODE   18

   106 typedef sa_family_t nd_type_t;

   107 

   108 typedef struct {

   109     int    fd;                 /* socket descriptor == file descriptor */

   110                                /* NOTE:

   111                                 *   Modbus TCP says that on error, we should close

   112                                 *   a connection and retry with a new connection.

   113                                 *   Since it takes time for a socket to close

   114                                 *   a connection if the remote server is down,

   115                                 *   we close the connection on the socket, close the

   116                                 *   socket itself, and create a new one for the new

   117                                 *   connection. There will be times when the node will

   118                                 *   not have any valid socket, and it will have to

   119                                 *   be created on the fly.

   120                                 *   When the node does not have a valid socket,

   121                                 *   fd will be set to -1

   122                                 */

   123     int    node_type;          /*   What kind of use we are giving to this node...

   124                                 *   If node_type == MB_MASTER_NODE

   125                                 *      The node descriptor was initialised by the

   126                                 *      modbus_connect() function.

   127                                 *      The node descriptor is being used by a master

   128                                 *      device, and the addr contains the address of the slave.

   129                                 *      Remember that in this case fd may be >= 0 while

   130                                 *      we have a valid connection, or it may be < 0 when

   131                                 *      the connection needs to be reset.

   132                                 *   If node_type == MB_LISTEN_NODE

   133                                 *      The node descriptor was initialised by the

   134                                 *      modbus_listen() function.

   135                                 *      The node is merely used to accept() new connection

   136                                 *      requests. The new slave connections will use another

   137                                 *      node to transfer data.

   138                                 *      In this case fd must be >= 0.

   139                                 *      fd < 0 is an ilegal state and should never occur.

   140                                 *   If node_type == MB_SLAVE_NODE

   141                                 *      The node descriptor was initialised when a new

   142                                 *      connection request arrived on a MB_LISTEN type node.

   143                                 *      The node descriptor is being used by a slave device,

   144                                 *      and is currently being used to connect to a master.

   145                                 *      In this case fd must be >= 0.

   146                                 *      fd < 0 is an ilegal state and should never occur.

   147                                 *   If node_type == FREE_ND

   148                                 *      The node descriptor is currently not being used.

   149                                 *      In this case fd is set to -1, but is really irrelevant.

   150                                 */

   151     struct sockaddr_in addr;   /* The internet adress we are using.

   152                                 *   If node_type == MB_MASTER_NODE

   153                                 *      addr will be the address of the remote slave

   154                                 *   If node_type == MB_LISTEN_NODE

   155                                 *      addr will be the address of the local listening port and network interface

   156                                 *   If node_type == MB_SLAVE_NODE

   157                                 *      addr will be the address of the local port and network interface

   158                                 *       of the connection to the specific client.

   159                                 */

   160     int listen_node;           /* When a slave accepts a connection through a MB_LISTEN_NODE, it will

   161                                 * will use an empty node for the new connection, and configure this new node

   162                                 * to use the type MB_SLAVE_NODE.

   163                                 * The listen_node entry is only used by nodes of type MB_SLAVE_NODE.

   164                                 * In this case, listen_node will be the node of type MB_LISTEN_NODE through

   165                                 * which the connection request came through...

   166                                 */ 

   167     int close_on_silence;      /* A flag used only by Master Nodes.

   168                                 * When (close_on_silence > 0), then the connection to the

   169                                 * slave device will be shut down whenever the

   170                                 * modbus_tcp_silence_init() function is called.

   171                                 * Remember that the connection will be automatically

   172                                 * re-established the next time the user wishes to communicate

   173                                 * with the same slave (using this same node descripto).

   174                                 * If the user wishes to comply with the sugestion

   175                                 * in the OpenModbus Spec, (s)he should set this flag

   176                                 * if a silence interval longer than 1 second is expected.

   177                                 */

   178     int print_connect_error;   /* flag to guarantee we only print an error the first time we

   179                                 * attempt to connect to a emote server.

   180                                 * Stops us from generting a cascade of errors while the slave

   181                                 * is down.

   182                                 * Flag will get reset every time we successfully

   183                                 * establish a connection, so a message is once again generated 

   184                                 * on the next error.

   185                                 */

   186     u8 *recv_buf;              /* This node's receive buffer

   187                                 * The library supports multiple simultaneous connections,

   188                                 * and may need to receive multiple frames through mutiple nodes concurrently.

   189                                 * To make the library thread-safe, we use one buffer for each node.

   190                                 */

   191 } nd_entry_t;

   192 

   193 

   194 /* please make sure to lock the node table mutex before calling this function */

   195 static int nd_entry_init(nd_entry_t *nde) {

   196   nde->addr.sin_family = AF_INET  ;

   197   nde->node_type = MB_FREE_NODE;

   198   nde->fd = -1; /* not currently connected... */

   199   /* initialise recv buffer */

   200   nde->recv_buf = malloc(sizeof(u8) * RECV_BUFFER_SIZE);

   201   if (nde->recv_buf == NULL)

   202     return -1;

   203   return 0;

   204 }

   205 

   206 /* please make sure to lock the node table mutex before calling this function */

   207 static int nd_entry_done(nd_entry_t *nde) {

   208   free(nde->recv_buf);

   209   return 0;

   210 }

   211 

   212 

   213 

   214 typedef struct {

   215       /* the array of node descriptors, and current size... */

   216     nd_entry_t      *node;           /* array of node entries. if NULL => node table not initialized */

   217     int             node_count;      /* total number of nodes in the node[] array */

   218     int             free_node_count; /* number of free nodes in the node[] array */

   219     pthread_mutex_t mutex;

   220 } nd_table_t;

   221 

   222 

   223 

   224 static int nd_table_done(nd_table_t *ndt) {

   225   int count;

   226 

   227   if (ndt->node == NULL) 

   228     return 0;

   229 

   230   /* lock the mutex */

   231   while (pthread_mutex_lock(&ndt->mutex) != 0) sched_yield();

   232   

   233   /* initialise the state of each node in the array... */

   234   for (count = 0; count < ndt->node_count; count++) {

   235     nd_entry_done(&ndt->node[count]);

   236   } /* for() */

   237 

   238   free(ndt->node);

   239   pthread_mutex_unlock (&ndt->mutex);

   240   pthread_mutex_destroy(&ndt->mutex);

   241   *ndt = (nd_table_t){.node=NULL, .node_count=0, .free_node_count=0};

   242   

   243   return 0;

   244 }

   245 

   246 

   247 

   248 

   249 #if 1

   250 /* nd_table_init()

   251  *   Version 1 of the nd_table_init() function. 

   252  *   If called more than once, 2nd and any subsequent calls will

   253  *   be interpreted as a request to confirm that it was already correctly 

   254  *   initialized with the requested number of nodes.

   255  */

   256 static int nd_table_init(nd_table_t *ndt, int nd_count) {

   257   int count;

   258 

   259   if (ndt->node != NULL) {

   260     /* this function has already been called, and the node table is already initialised */

   261     return (ndt->node_count == nd_count)?0:-1;

   262   }

   263   

   264   /* initialise the node table mutex... */

   265   pthread_mutex_init(&ndt->mutex, NULL);

   266   if (pthread_mutex_lock(&ndt->mutex) != 0) {

   267 #ifdef DEBUG

   268     perror("pthread_mutex_lock()");

   269     fprintf(stderr, "[%lu] Unable to lock newly crated mutex while creating new node table!\n", pthread_self());

   270 #endif

   271     pthread_mutex_destroy(&ndt->mutex);

   272     return -1;

   273   }

   274   

   275   /* initialise the node descriptor metadata array... */

   276   ndt->node = malloc(sizeof(nd_entry_t) * nd_count);

   277   if (ndt->node == NULL) {

   278 #ifdef DEBUG

   279     perror("malloc()");

   280     fprintf(stderr, "[%lu] Out of memory: error initializing node address buffer\n", pthread_self());

   281 #endif

   282 #ifdef ERRMSG

   283     perror("malloc()");

   284     fprintf(stderr, ERRMSG_HEAD "Out of memory. Error initializing node address buffer\n");

   285 #endif

   286     pthread_mutex_unlock (&ndt->mutex);

   287     pthread_mutex_destroy(&ndt->mutex);

   288     return -1;

   289   }

   290 

   291   /* initialise the state of each node in the array... */

   292   for (count = 0; count < nd_count; count++) {

   293     if (nd_entry_init(&ndt->node[count]) < 0) {

   294       pthread_mutex_unlock(&ndt->mutex);

   295       nd_table_done(ndt);

   296       return -1;

   297     }

   298     ndt->node_count = count+1;

   299     ndt->free_node_count = count+1;

   300   } /* for() */

   301 

   302   ndt->node_count = nd_count;

   303   ndt->free_node_count = nd_count;

   304 

   305   pthread_mutex_unlock(&ndt->mutex);

   306   return nd_count; /* number of succesfully created nodes! */

   307 }

   308 

   309 

   310 #else

   311 /* nd_table_init()

   312  *   Version 2 of the nd_table_init() function. 

   313  *   If called more than once, 2nd and any subsequent calls will

   314  *   be interpreted as a request to reserve an extra new_nd_count

   315  *   number of nodes. This will be done using realloc().

   316  */

   317 static int nd_table_init(nd_table_t *ndt, int new_nd_count) {

   318   int count;

   319 

   320   if (ndt->node == NULL) {

   321     /* Node table nt yet initialized => we must initialise the node table mutex... */

   322     pthread_mutex_init(&ndt->mutex, NULL);

   323   }

   324   

   325   /* lock the mutex */

   326   while (pthread_mutex_lock(&ndt->mutex) != 0) sched_yield();

   327   

   328   /* initialise the node descriptor metadata array... */

   329   ndt->node = realloc(ndt->node, sizeof(nd_entry_t) * (ndt->node_count + new_nd_count));

   330   if (ndt->node == NULL) {

   331 #ifdef DEBUG

   332     perror("malloc()");

   333     fprintf(stderr, "[%lu] Out of memory: error initializing node address buffer\n", pthread_self());

   334 #endif

   335 #ifdef ERRMSG

   336     perror("malloc()");

   337     fprintf(stderr, ERRMSG_HEAD "Out of memory. Error initializing node address buffer\n");

   338 #endif

   339     pthread_mutex_unlock (&ndt->mutex);

   340     pthread_mutex_destroy(&ndt->mutex);

   341     return -1;

   342   }

   343 

   344   /* initialise the state of each newly added node in the array... */

   345   for (count = ndt->node_count; count < ndt->node_count + new_nd_count; count++) {

   346     if (nd_entry_init(&ndt->node[count]) < 0) {

   347       pthread_mutex_unlock(&ndt->mutex);

   348       return -1;

   349     }

   350   } /* for() */

   351   ndt->node_count      += new_nd_count;

   352   ndt->free_node_count += new_nd_count;

   353 

   354   pthread_mutex_unlock(&ndt->mutex);

   355   return new_nd_count; /* number of succesfully created nodes! */

   356 }

   357 #endif

   358 

   359 

   360 static int nd_table_get_free_node(nd_table_t *ndt, nd_type_t nd_type) {

   361   int count;

   362 

   363   /* lock the mutex */

   364   while (pthread_mutex_lock(&ndt->mutex) != 0) sched_yield();

   365 

   366   /* check for free nodes... */

   367   if (ndt->free_node_count <= 0) {

   368     /* no free nodes... */

   369     pthread_mutex_unlock(&ndt->mutex);

   370     return -1;

   371   }

   372 

   373   /* Decrement the free node counter...*/

   374   ndt->free_node_count--;

   375 

   376   /* search for a free node... */

   377   for (count = 0; count < ndt->node_count; count++) {

   378     if(ndt->node[count].node_type == MB_FREE_NODE) {

   379       /* found one!! Allocate it to the new type! */

   380       ndt->node[count].node_type = nd_type;

   381       pthread_mutex_unlock(&ndt->mutex);

   382       return count;

   383     }

   384   } /* for() */

   385 

   386   /* Strange... We should have free nodes, but we didn't finda any! */

   387   /* Let's try to get into a consistent state, and return an error! */

   388   ndt->free_node_count = 0;

   389   pthread_mutex_unlock(&ndt->mutex);

   390   return -1;

   391 }

   392 

   393 

   394 

   395 static void nd_table_close_node(nd_table_t *ndt, int nd) {

   396 

   397   /* lock the mutex */

   398   while (pthread_mutex_lock(&ndt->mutex) != 0) sched_yield();

   399 

   400   if(ndt->node[nd].node_type == MB_FREE_NODE) {

   401     /* Node already free... */

   402     pthread_mutex_unlock(&ndt->mutex);

   403     return;

   404   }

   405 

   406   /* Increment the free node counter...*/

   407   ndt->free_node_count++;

   408   /* Mark the node as being free. */

   409   ndt->node[nd].node_type = MB_FREE_NODE;

   410 

   411   pthread_mutex_unlock(&ndt->mutex);

   412   return;

   413 }

   414 

   415 

   416 

   417 /**************************************************************/

   418 /**************************************************************/

   419 /****                                                      ****/

   420 /****                                                      ****/

   421 /****                Global Library State                  ****/

   422 /****                                                      ****/

   423 /****                                                      ****/

   424 /**************************************************************/

   425 /**************************************************************/

   426 

   427 

   428  /* The node descriptor table... */

   429  /* NOTE: The node_table_ Must be initialized correctly here! */

   430 static nd_table_t nd_table_ = {.node=NULL, .node_count=0, .free_node_count=0};

   431 

   432 

   433 /**************************************************************/

   434 /**************************************************************/

   435 /****                                                      ****/

   436 /****                                                      ****/

   437 /****              Local Utility functions...              ****/

   438 /****                                                      ****/

   439 /****                                                      ****/

   440 /**************************************************************/

   441 /**************************************************************/

   442 

   443 

   444 #define min(a,b) ((a<b)?a:b)

   445 #define max(a,b) ((a>b)?a:b)

   446 

   447 /************************************/

   448 /**                                **/

   449 /**  Configure socket for Modbus   **/

   450 /**                                **/

   451 /************************************/

   452 

   453 

   454 static int configure_socket(int socket_id) {

   455 

   456   /* configure the socket */

   457     /* Set it to be non-blocking. This is safe because we always use select() before reading from it! 

   458      * It is also required for the connect() call. The default timeout in the TCP stack is much too long

   459      * (typically blocks for 128 s ??) when the connect does not succedd imediately!

   460      */

   461   if (fcntl(socket_id, F_SETFL, O_NONBLOCK) < 0) {

   462 #ifdef ERRMSG

   463     perror("fcntl()");

   464     fprintf(stderr, ERRMSG_HEAD "Error configuring socket 'non-blocking' option.\n");

   465 #endif

   466     return -1;

   467   }

   468 

   469   /* configure the socket */

   470     /* set the TCP no delay flag. */

   471   {int bool_opt = 1;

   472   if (setsockopt(socket_id, SOL_TCP, TCP_NODELAY,

   473                  (const void *)&bool_opt, sizeof(bool_opt))

   474       < 0) {

   475 #ifdef ERRMSG

   476     perror("setsockopt()");

   477     fprintf(stderr, ERRMSG_HEAD "Error configuring socket 'TCP no delay' option.\n");

   478 #endif

   479     return -1;

   480   }

   481   }

   482 

   483     /* set the IP low delay option. */

   484   {int priority_opt = IPTOS_LOWDELAY;

   485   if (setsockopt(socket_id, SOL_IP, IP_TOS,

   486                  (const void *)&priority_opt, sizeof(priority_opt))

   487       < 0) {

   488 #ifdef ERRMSG

   489     perror("setsockopt()");

   490     fprintf(stderr, ERRMSG_HEAD "Error configuring socket 'IP low delay' option.\n");

   491 #endif

   492     return -1;

   493   }

   494   }

   495 

   496 #if 0

   497     /* send buffer */

   498     /* NOTE: For slave devices, that may be receiving multiple

   499      *       requests before they have a chance to reply to the first,

   500      *       it probably is a good idea to have a large receive buffer.

   501      *       So it is best to leave it with the default configuration, as it is

   502      *       larger than the largest Modbus TCP frame.

   503      *

   504      *       For the send buffer, a smaller buffer should suffice.

   505      *       However, it probably does not make sense to

   506      *       waste time asking for a smaller buffer, since the larger

   507      *       default buffer has already been allocated (the socket has already

   508      *       been created!)

   509      *

   510      *       We might just as well leave out the configuration of the socket

   511      *       buffer size...

   512      */

   513 #define SOCK_BUF_SIZE 300 /* The size proposed in the Modbus TCP spec. */

   514   {int sock_buf_size;

   515   sock_buf_size = SOCK_BUF_SIZE;

   516   if (setsockopt(socket_id, SOL_SOCKET, SO_SNDBUF,

   517                  (const void *)&sock_buf_size, sizeof(sock_buf_size))

   518       < 0)

   519     return -1;

   520     /* recv buffer */

   521   sock_buf_size = SOCK_BUF_SIZE;

   522   if (setsockopt(socket_id, SOL_SOCKET, SO_RCVBUF,

   523              (const void *)&sock_buf_size, sizeof(sock_buf_size))

   524       < 0)

   525     return -1;

   526   }

   527 #endif

   528 

   529   return 0;

   530 }

   531 

   532 

   533 /************************************/

   534 /**                                **/

   535 /** Connect socket to remote host  **/

   536 /**                                **/

   537 /************************************/

   538 

   539 /* This function will create a new socket, and connect it to a remote host... */

   540 static inline int open_connection(int nd, const struct timespec *timeout) {

   541   int socket_id, con_res;

   542 

   543 #ifdef DEBUG

   544         printf("[%lu] open_connection(): called, nd = %d\n", pthread_self(), nd);

   545 #endif

   546 

   547   if (nd_table_.node[nd].fd >= 0)

   548     /* nd already connected) */

   549     return nd_table_.node[nd].fd;

   550 

   551   if (nd_table_.node[nd].addr.sin_family != AF_INET)

   552     /* invalid remote address, or invalid nd */

   553     return -1;

   554 

   555   /* lets try to connect... */

   556     /* create the socket */

   557   if ((socket_id = socket(PF_INET, DEF_TYPE, 0 /* protocol_num */)) < 0) {

   558 #ifdef DEBUG

   559     perror("socket()");

   560     fprintf(stderr, "[%lu] Error creating socket\n", pthread_self());

   561 #endif

   562 #ifdef ERRMSG

   563     perror("socket()");

   564     fprintf(stderr, ERRMSG_HEAD "Error creating socket\n");

   565 #endif

   566     return -1;

   567   }

   568 

   569   /* configure the socket - includes setting non-blocking option! */

   570   if (configure_socket(socket_id) < 0) {

   571     close(socket_id);

   572     return -1;

   573   };

   574  

   575   /* establish the connection to remote host */

   576   con_res = connect(socket_id,

   577                     (struct sockaddr *)&(nd_table_.node[nd].addr),

   578                     sizeof(nd_table_.node[nd].addr));

   579 

   580   /* The following condition is not strictly necessary 

   581    * (we could let the code fall through)

   582    * but it does make the code easier to read/understand...

   583    */

   584   if (con_res >= 0)

   585     goto success_exit; /* connected succesfully on first try! */

   586     

   587   if (con_res < 0) {

   588     if ((errno != EINPROGRESS) && (errno != EALREADY))

   589       goto error_exit; /* error in connection request! */

   590 

   591     /* connection request is ongoing */

   592     /* EINPROGRESS -> first call to connect, EALREADY -> subsequent calls to connect */

   593     /* Must wait for connect to complete at most 'timeout' seconds */

   594     {fd_set fdset;

   595      int res, so_error;

   596      socklen_t len;

   597      struct timespec end_time, *et_ptr;

   598      

   599      et_ptr = NULL;

   600      if (timeout != NULL) {

   601         et_ptr = &end_time;

   602        *et_ptr = timespec_add_curtime(*timeout);

   603      }

   604       

   605      FD_ZERO(&fdset);

   606      FD_SET(socket_id, &fdset);

   607      

   608      res = my_select(socket_id+1, NULL, &fdset, et_ptr);

   609      if (res  < 0) goto error_exit; /* error on call to select */

   610      if (res == 0) goto error_exit; /* timeout */

   611      /* (res  > 0) -> connection attemt completed. May have been success or failure! */

   612      

   613      len = sizeof(so_error);

   614      res = getsockopt(socket_id, SOL_SOCKET, SO_ERROR, &so_error, &len);

   615      if (res  < 0)      goto error_exit; /* error on call to getsockopt */

   616      if (so_error != 0) goto error_exit; /* error on connection attempt */

   617      goto success_exit; /* succesfully completed connection attempt! */

   618                         /* goto sucess_exit is not strcitly necessary - we could let the code fall through! */

   619     }

   620   }

   621 

   622 success_exit:

   623   nd_table_.node[nd].fd = socket_id;

   624   /* Succesfully established connection => print a message next time we have error. */

   625   nd_table_.node[nd].print_connect_error = 1;  

   626 

   627 #ifdef DEBUG

   628   printf("[%lu] open_connection(): returning...\n", pthread_self());

   629 #endif

   630   return socket_id;

   631 

   632 error_exit:

   633 #ifdef ERRMSG

   634     if (nd_table_.node[nd].print_connect_error > 0) {

   635       perror("connect()");

   636       fprintf(stderr, ERRMSG_HEAD "Error establishing socket connection.\n");

   637       /* do not print more error messages for this node... */

   638       nd_table_.node[nd].print_connect_error = 0;

   639     }

   640 #endif

   641     close(socket_id);

   642     return -1;

   643 }

   644 

   645 

   646 /* This function will accept a new connection request, and attribute it to a new node... */

   647 static inline int accept_connection(int nd) {

   648   int socket_id, new_nd;

   649 

   650 #ifdef DEBUG

   651         printf("[%lu] accept_connection(): called, nd = %d\n", pthread_self(), nd);

   652 #endif

   653 

   654   /* NOTE: We MUST accccept8) all connection requests, even if no new node is available.

   655    *       => We first accept the connection request, and only later look for a node.

   656    *          If no node is free/available for this new connections request, the 

   657    *          connection will be accepted and immediately closed.

   658    *       Reason:

   659    *       When the library is used for a Modbus/TCP server and no free node is 

   660    *        available, if we do not accept() all newly arrived connection requests

   661    *        we would enter an infinite loop calling

   662    *           - select() (in modbus_tcp_read()) 

   663    *           - and accept_connection().

   664    *        Note that select() will continue to return immediately if the 

   665    *        connection request is not accept()ted!

   666    */

   667   /* lets accept new connection request... */

   668   if ((socket_id = accept(nd_table_.node[nd].fd, NULL, NULL)) < 0) {

   669 #ifdef ERRMSG

   670     perror("accept()");

   671     fprintf(stderr, ERRMSG_HEAD "Error while waiting for connection request from new client\n");

   672 #endif

   673     /* error establishing new connection... */

   674     return -1;

   675   }

   676 

   677   /* find a free node */ 

   678   if ((new_nd = nd_table_get_free_node(&nd_table_, MB_SLAVE_NODE)) < 0) {

   679     /* no available free nodes for the new connection... */

   680     close(socket_id);    

   681     return -1;

   682   }

   683 

   684   /* configure the socket - includes setting the non-blocking option! */

   685   if (configure_socket(socket_id) < 0) {

   686     nd_table_close_node(&nd_table_, new_nd);  /* first free up the un-used node. */

   687     close(socket_id);

   688     return -1;

   689   }

   690 

   691   /* set up the node entry and update the fd sets */

   692   nd_table_.node[new_nd].fd = socket_id;

   693   nd_table_.node[new_nd].listen_node = nd;

   694 

   695 #ifdef DEBUG

   696         printf("[%lu] accept_connection(): returning new_nd = %d\n", pthread_self(), new_nd);

   697 #endif

   698   return new_nd;

   699 }

   700 

   701 

   702 static inline void close_connection(int nd) {

   703   if (nd_table_.node[nd].fd >= 0) {

   704     /* disconnect the tcp connection */

   705     shutdown(nd_table_.node[nd].fd, SHUT_RDWR);

   706 #ifdef ERRMSG

   707     int res =

   708 #endif

   709     close(nd_table_.node[nd].fd);

   710 #ifdef ERRMSG

   711     if (res < 0) {

   712       perror("close()");

   713       fprintf(stderr, ERRMSG_HEAD "Error closing socket\n");

   714     }

   715 #endif

   716     nd_table_.node[nd].fd = -1;

   717   }

   718 

   719   if (nd_table_.node[nd].node_type == MB_SLAVE_NODE) {

   720     /* If it is a slave node, we will not be receiving any more data over this disconnected node,

   721      * (MB_SLAVE_NODE do not get re-connected!), so we free the node...

   722      */

   723     nd_table_close_node(&nd_table_, nd);

   724   }

   725 }

   726 

   727 

   728 

   729 /************************************/

   730 /**                                **/

   731 /**     Data format conversion     **/

   732 /**                                **/

   733 /************************************/

   734 

   735 /*

   736  * Functions to convert u16 variables

   737  * between network and host byte order

   738  *

   739  * NOTE: Modbus uses MSByte first, just like

   740  *       tcp/ip, so we use the htons() and

   741  *       ntoh() functions to guarantee

   742  *       code portability.

   743  */

   744 

   745 static inline u16 mb_hton(u16 h_value) {

   746 /*  return h_value; */

   747   return htons(h_value);

   748 }

   749 

   750 static inline u16 mb_ntoh(u16 m_value) {

   751 /*  return m_value; */

   752   return ntohs(m_value);

   753 }

   754 

   755 static inline u8 msb(u16 value) {

   756 /*  return Most Significant Byte of value; */

   757   return (value >> 8) & 0xFF;

   758 }

   759 

   760 static inline u8 lsb(u16 value) {

   761 /*  return Least Significant Byte of value; */

   762   return value & 0xFF;

   763 }

   764 

   765 #define u16_v(char_ptr)  (*((u16 *)(&(char_ptr))))

   766 

   767 

   768 /************************************/

   769 /**                                **/

   770 /**   Build/Check a frame header   **/

   771 /**                                **/

   772 /************************************/

   773 

   774 /* A modbus TCP frame header has 6 bytes...

   775  *   header[0-1] -> transaction id

   776  *   header[2-3] -> must be 0

   777  *   header[4-5] -> frame data length (must be <= 255)

   778  */

   779 #if TCP_HEADER_LENGTH < 6

   780 #error This code assumes a header size of 6 bytes, but TCP_HEADER_LENGTH < 6

   781 #endif

   782 

   783 static inline void build_header(u8 *header,

   784                                 u16 transaction_id,

   785                                 u16 byte_count)

   786 {

   787   u16_v(header[0]) = mb_hton(transaction_id);

   788   header[2] = 0;

   789   header[3] = 0;

   790   u16_v(header[4]) = mb_hton(byte_count);

   791 }

   792 

   793 

   794 static inline int check_header(u8  *header,

   795                                u16 *transaction_id,

   796                                u16 *byte_count)

   797 {

   798   if ((header[2] != 0) || (header[3] != 0))

   799     return -1;

   800 

   801   *transaction_id = mb_ntoh(*(u16 *)(header + 0));

   802   *byte_count     = mb_ntoh(*(u16 *)(header + 4));

   803 

   804   if (*byte_count > MAX_L2_FRAME_LENGTH)

   805     return -1;

   806 

   807   return 0;

   808 }

   809 

   810 

   811 

   812 

   813 

   814 /**************************************************************/

   815 /**************************************************************/

   816 /****                                                      ****/

   817 /****                                                      ****/

   818 /****              Sending of Modbus TCP Frames            ****/

   819 /****                                                      ****/

   820 /****                                                      ****/

   821 /**************************************************************/

   822 /**************************************************************/

   823 

   824 // pthread_mutex_t sendmsg_mutex = PTHREAD_MUTEX_INITIALIZER; 

   825 

   826 /* NOTE: this function MUST be thread safe!! */

   827 int modbus_tcp_write(int nd,  /* node descriptor */

   828                      u8 *data,

   829                      size_t data_length,

   830                      u16 transaction_id,

   831                      const struct timespec *transmit_timeout

   832                      )

   833 {

   834 #define data_vector_size 2

   835 

   836   u8            header[TCP_HEADER_LENGTH];

   837   struct iovec  data_vector[data_vector_size] = {

   838                          {(void *)header, TCP_HEADER_LENGTH},

   839                          {NULL, 0}};

   840   struct msghdr msg = {NULL, 0, data_vector, data_vector_size, NULL, 0, 0};

   841   int res, bytes_sent;

   842 

   843 #ifdef DEBUG

   844   printf("[%lu] modbus_tcp_write(): called...  nd=%d\n", pthread_self(), nd);

   845 #endif

   846 

   847   if ((nd >= nd_table_.node_count) || (nd < 0))

   848     /* invalid node descriptor... */

   849     return -1;

   850 

   851 #ifdef DEBUG

   852 //  printf("[%lu] locking mutex...\n", pthread_self());

   853 #endif

   854 //  while (pthread_mutex_lock(&sendmsg_mutex) != 0);

   855 

   856   /*************************

   857   * prepare the header...  *

   858   *************************/

   859   build_header(header, transaction_id, data_length);

   860 #ifdef DEBUG

   861 /* Print the hex value of each character that is about to be

   862  * sent over the bus.

   863  */

   864   { int i;

   865     printf("modbus_tcp_write(): sending data...\n");

   866     for(i = 0; i < TCP_HEADER_LENGTH; i++)

   867       printf("[0x%2X]", header[i]);

   868     for(i = 0; i < data_length; i++)

   869       printf("[0x%2X]", data[i]);

   870     printf("\n");

   871   }

   872 #endif

   873 

   874   /******************************************

   875    * do we need to re-establish connection? *

   876    ******************************************/

   877   if (open_connection(nd, transmit_timeout) < 0) {

   878 #ifdef DEBUG

   879     fprintf(stderr, "[%lu] modbus_tcp_write(): could not establish connection...\n", pthread_self());

   880 #endif

   881 #ifdef ERRMSG

   882     fprintf(stderr, ERRMSG_HEAD "could not establish connection...\n");

   883 #endif

   884     return -1;

   885   }

   886 

   887   /**********************

   888    * write to output... *

   889    **********************/

   890    /* TWO ALTERNATIVE IMPLEMENTATIONS !!! */

   891 #if 0

   892     /* write header */

   893   bytes_sent = 0;

   894   while (1) {

   895     res = write(nd_table_.node[nd].fd, header+bytes_sent, TCP_HEADER_LENGTH-bytes_sent);

   896     if (res < 0) {

   897       if ((errno != EAGAIN ) && (errno != EINTR )) {

   898         /* error sending message... */

   899         close_connection(nd);

   900         return -1;

   901       } else {

   902         continue;

   903       }

   904     } else {

   905       /* res >= 0 */

   906       bytes_sent += res;

   907       if (bytes_sent >= TCP_HEADER_LENGTH) {

   908         break;

   909       }

   910 	}

   911   }

   912 

   913       /* write data */

   914   bytes_sent = 0;

   915   while (1) {

   916     res = write(nd_table_.node[nd].fd, data+bytes_sent, data_length-bytes_sent);

   917     if (res < 0) {

   918       if ((errno != EAGAIN ) && (errno != EINTR )) {

   919         /* error sending message... */

   920         close_connection(nd);

   921         return -1;

   922       } else {

   923         continue;

   924       }

   925     } else {

   926       /* res >= 0 */

   927       bytes_sent += res;

   928       if (bytes_sent >= data_length) {

   929         /* query succesfully sent! */

   930 #ifdef DEBUG

   931         printf("[%lu] modbus_tcp_write(): sent %d bytes\n", pthread_self(), TCP_HEADER_LENGTH+data_length);

   932 #endif

   933         return data_length;

   934       }

   935 	}

   936   }

   937 

   938    /**********************

   939    * write to output... *

   940    **********************/

   941 #else

   942   /* We are optimising for the most likely case, and in doing that

   943    * we are making the least likely case have worse behaviour!

   944    * Read on for an explanation...

   945    *

   946    * - The optimised behaviour for the most likely case:

   947    * We have set the NO_DELAY flag on the socket, so the IP datagram

   948    * is not delayed and is therefore sent as soon as any data is written to

   949    * the socket.

   950    * In order to send the whole message in a single IP datagram, we have to

   951    * write both the the header and the data with a single call to write()

   952    * In order to not to have to copy the data around just to add the

   953    * message header, we use sendmsg() instead of write()!

   954    *

   955    * - The worse behaviour for the least likely case:

   956    * If for some reason only part of the data is sent with the first call to

   957    * write(), a datagram is sent right away, and the subsequent data will

   958    * be sent in another datagram. :-(

   959    */

   960    /* NOTE: since snedmsg() is not thread safe, we use a mutex to protect access to this function... */

   961 

   962   data_vector[data_vector_size - 1].iov_base = data;

   963   data_vector[data_vector_size - 1].iov_len  = data_length;

   964   data_vector[                   0].iov_base = header;

   965   data_vector[                   0].iov_len  = TCP_HEADER_LENGTH;

   966   bytes_sent = 0;

   967   while (1) {

   968     int sendmsg_errno;

   969      /* Please see the comment just above the main loop!! */

   970     res = sendmsg(nd_table_.node[nd].fd, &msg, 0);

   971     sendmsg_errno = errno;

   972     if (res < 0) {

   973       if ((sendmsg_errno != EAGAIN ) && (sendmsg_errno != EINTR )) {

   974         /* error sending message... */

   975         close_connection(nd);

   976         return -1;

   977       } else {

   978         continue;

   979       }

   980     } else {

   981       /* res >= 0 */

   982       bytes_sent += res;

   983       if (bytes_sent >= data_length + TCP_HEADER_LENGTH) {

   984         /* query succesfully sent! */

   985 #ifdef DEBUG

   986         printf("[%lu] modbus_tcp_write(): sent %d bytes\n", pthread_self(), bytes_sent);

   987 #endif

   988 //        pthread_mutex_unlock(&sendmsg_mutex);

   989 #ifdef DEBUG

   990 //        printf("[%lu] unlocked  mutex...\n", pthread_self());

   991 #endif

   992         return data_length;

   993       }

   994 

   995       /* adjust the data_vector... */

   996       if (res < data_vector[0].iov_len) {

   997         u8* tmp = data_vector[0].iov_base;

   998         tmp += res; 

   999         data_vector[0].iov_len -= res;

  1000         data_vector[0].iov_base = tmp;

  1001       } else {

  1002         u8* tmp = data_vector[1].iov_base;

  1003         tmp += res; 

  1004         res -= data_vector[0].iov_len;

  1005         data_vector[0].iov_len  = 0;

  1006         data_vector[1].iov_len -= res;

  1007         data_vector[1].iov_base = tmp;

  1008       }

  1009     }

  1010   } /* while (1) */

  1011 #endif

  1012 

  1013   /* humour the compiler... */

  1014 //  pthread_mutex_unlock(&sendmsg_mutex);

  1015 #ifdef DEBUG

  1016 //  printf("[%lu] unlocked  mutex...\n", pthread_self());

  1017 #endif

  1018   return -1;

  1019 }

  1020 

  1021 

  1022 

  1023 /**************************************************************/

  1024 /**************************************************************/

  1025 /****                                                      ****/

  1026 /****                                                      ****/

  1027 /****              Receiving Modbus TCP Frames             ****/

  1028 /****                                                      ****/

  1029 /****                                                      ****/

  1030 /**************************************************************/

  1031 /**************************************************************/

  1032 

  1033 

  1034 /* A helper function to modbus_tcp_read()

  1035  *

  1036  * WARNING: The semantics of this function are not what you would expect!

  1037  *

  1038  *          if (data_already_available != 0)

  1039  *          It assumes that select() has already been called before

  1040  *          this function got called, and we are therefore guaranteed

  1041  *          to have at least one byte to read off the socket (the fd).

  1042  *

  1043  *          if (data_already_available == 0)

  1044  *          it starts off by calling select()!

  1045  *

  1046  *

  1047  * NOTE: Ususal select semantics for (a: end_time == NULL) and

  1048  *       (b: *end_time == 0) also apply.

  1049  *

  1050  *       (a) Indefinite timeout

  1051  *       (b) Try once, and and quit if no data available.

  1052  */

  1053 /* RETURNS: number of bytes read

  1054  *          -1 read error!

  1055  *          -2 timeout

  1056  */

  1057 static int read_bytes(int fd,

  1058                       u8 *data,

  1059                       int max_data_count,

  1060                       const struct timespec *end_time,

  1061                       int data_already_available)

  1062 {

  1063   fd_set rfds;

  1064   int res, data_count;

  1065 

  1066   data_count = 0;

  1067 

  1068   while (data_count < max_data_count) {

  1069     /*============================*

  1070      * wait for data availability *

  1071      *============================*/

  1072     if (data_already_available == 0) {

  1073       int sel_res;

  1074       FD_ZERO(&rfds);

  1075       FD_SET(fd, &rfds);

  1076       sel_res = my_select(fd + 1, &rfds, NULL, end_time);

  1077       if (sel_res < 0)

  1078         return -1;

  1079       if (sel_res == 0)

  1080         /* timeout! */

  1081         return -2;

  1082     }

  1083 

  1084     /*============================*

  1085      * read the available data... *

  1086      *============================*/

  1087     res = read(fd, data + data_count, max_data_count - data_count);

  1088     if (res == 0) {

  1089       /* We are guaranteed to have data to read off the fd since we called

  1090        * select(), but read() returned 0 bytes.

  1091        * This means that the remote process has closed down the connection,

  1092        * so we return 0.

  1093        */

  1094       return 0;

  1095     }

  1096 

  1097     if (res < 0) {

  1098       if (errno != EINTR)

  1099         return -1;

  1100       else

  1101         res = 0;

  1102     }

  1103 #ifdef DEBUG

  1104     {/* display the hex code of each character received */

  1105       int i;

  1106       for (i=0; i < res; i++)

  1107         printf("<0x%2X>", *(data + data_count + i));

  1108     }

  1109 #endif

  1110     data_count += res;

  1111     data_already_available = 0;

  1112   } /* while ()*/

  1113 

  1114   /* data read succesfully... */

  1115   return data_count;

  1116 }

  1117 

  1118 

  1119 

  1120 /***************************************/

  1121 /**                                   **/

  1122 /**    Read a Modbus TCP frame        **/

  1123 /**    off a single identified node.  **/

  1124 /**                                   **/

  1125 /***************************************/

  1126 

  1127 /* This private function will read a Modbus TCP frame off a single identified node

  1128  * that we know before hand that has data ready to be read off it. The data may or may not be

  1129  * a valid Modbus TCP frame. It is up to this function to figure that out.

  1130  */

  1131 /* NOTES:

  1132  *  - We re-use the recv_buf_ to load the frame header, so we have to make

  1133  *    sure that the buffer is large enough to take it...

  1134  */

  1135  /* RETURNS: number of bytes read

  1136   *          -1 on read from file/node error