1 /*

     2  * Copyright (c) 2001,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 #include <fcntl.h>      /* File control definitions */

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

    27 #include <string.h>

    28 #include <stdlib.h>

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

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

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

    32 #include <assert.h>

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

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

    35 #include <limits.h>     /* required for INT_MAX */

    36 

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

    38 

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

    40 #include "mb_rtu_private.h"

    41 

    42 

    43 #define ERRMSG

    44 #define ERRMSG_HEAD "ModbusRTU: "

    45 

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

    47 

    48 #ifdef DEBUG

    49 #ifndef ERRMSG

    50 #define ERRMSG

    51 #endif

    52 #endif

    53 

    54 

    55 #define SAFETY_MARGIN 10

    56 

    57 /************************************/

    58 /**                                **/

    59 /** Include common code...         **/

    60 /**                                **/

    61 /************************************/

    62 

    63 #include "mb_ds_util.h"    /* data structures... */

    64 #include "mb_time_util.h"  /* time conversion routines... */

    65 

    66 

    67 

    68 /**************************************************************/

    69 /**************************************************************/

    70 /****                                                      ****/

    71 /****                                                      ****/

    72 /****                Forward Declarations                  ****/

    73 /****                    and Defaults                      ****/

    74 /****                                                      ****/

    75 /**************************************************************/

    76 /**************************************************************/

    77 

    78  /* CRC funtions... */

    79 typedef u16 (*crc_func_t)(u8 *buf, int cnt);

    80 static u16 crc_slow(u8 *buf, int cnt);

    81 static u16 crc_fast(u8 *buf, int cnt);

    82 

    83  /* slow version does not need to be initialised, so we use it as default. */

    84 #define DEF_CRC_FUNCTION crc_slow

    85 

    86 

    87 /**************************************************************/

    88 /**************************************************************/

    89 /****                                                      ****/

    90 /****                                                      ****/

    91 /****              Local Utility functions...              ****/

    92 /****                                                      ****/

    93 /****                                                      ****/

    94 /**************************************************************/

    95 /**************************************************************/

    96 

    97 /************************************/

    98 /**                                **/

    99 /** Miscelaneous Utility functions **/

   100 /**                                **/

   101 /************************************/

   102 

   103 /*

   104  * Functions to convert u16 variables

   105  * between network and host byte order

   106  *

   107  * NOTE: Modbus uses MSByte first, just like

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

   109  *       ntoh() functions to guarantee

   110  *       code portability.

   111  */

   112 static inline u16 mb_hton(u16 h_value) 

   113   {return htons(h_value);} /*  return h_value; */

   114 

   115 static inline u16 mb_ntoh(u16 m_value)

   116   {return ntohs(m_value);} /*  return m_value; */

   117 

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

   119 static inline u8 msb(u16 value)

   120   {return (value >> 8) & 0xFF;}

   121 

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

   123 static inline u8 lsb(u16 value)

   124   {return value & 0xFF;}

   125 

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

   127 

   128 

   129 

   130 /**************************************/

   131 /**                                  **/

   132 /**    Initialise a termios struct   **/

   133 /**                                  **/

   134 /**************************************/

   135 static int termios_init(struct termios *tios,

   136                         int baud,

   137                         int parity,

   138                         int data_bits,

   139                         int stop_bits) {

   140   speed_t baud_rate;

   141 

   142   if (tios == NULL)

   143     return -1;

   144 

   145   /* reset all the values... */

   146   /* NOTE: the following are initialised later on...

   147   tios->c_iflag = 0;

   148   tios->c_oflag = 0;

   149   tios->c_cflag = 0;

   150   tios->c_lflag = 0;

   151   */

   152   tios->c_line  = 0;

   153 

   154  /* The minimum number of characters that should be received

   155   * to satisfy a call to read().

   156   */

   157   tios->c_cc[VMIN ] = 0;

   158 

   159  /* The maximum inter-arrival interval between two characters,

   160   * in deciseconds.

   161   *

   162   * NOTE: we could use this to detect the end of RTU frames,

   163   *       but we prefer to use select() that has higher resolution,

   164   *       even though this higher resolution is most probably not

   165   *       supported, and the effective resolution is 10ms,

   166   *       one tenth of a decisecond.

   167   */

   168   tios->c_cc[VTIME] = 0;

   169 

   170   /* configure the input modes... */

   171   tios->c_iflag =  IGNBRK |  /* ignore BREAK condition on input         */

   172                    IGNPAR |  /* ignore framing errors and parity errors */

   173                    IXANY;    /* enable any character to restart output  */

   174   /*               BRKINT       Only active if IGNBRK is not set.

   175    *                            generate SIGINT on BREAK condition,

   176    *                            otherwise read BREAK as character \0.

   177    *               PARMRK       Only active if IGNPAR is not set.

   178    *                            replace bytes with parity errors with

   179    *                            \377 \0, instead of \0.

   180    *               INPCK        enable input parity checking

   181    *               ISTRIP       strip off eighth bit

   182    *               IGNCR        ignore carriage return on input

   183    *               INLCR        only active if IGNCR is not set.

   184    *                            translate newline to carriage return  on input

   185    *               ICRNL        only active if IGNCR is not set.

   186    *                            translate carriage return to newline on input

   187    *               IUCLC        map uppercase characters to lowercase on input

   188    *               IXON         enable XON/XOFF flow control on output

   189    *               IXOFF        enable XON/XOFF flow control on input

   190    *               IMAXBEL      ring bell when input queue is full

   191    */

   192 

   193   /* configure the output modes... */

   194   tios->c_oflag = OPOST;     /* enable implementation-defined output processing */

   195   /*              ONOCR         don't output CR at column 0

   196    *              OLCUC         map lowercase characters to uppercase on output

   197    *              ONLCR         map NL to CR-NL on output

   198    *              OCRNL         map CR to NL on output

   199    *              OFILL         send fill characters for a delay, rather than

   200    *                            using a timed delay

   201    *              OFDEL         fill character is ASCII DEL. If unset, fill

   202    *                            character is ASCII NUL

   203    *              ONLRET        don't output CR

   204    *              NLDLY         NL delay mask. Values are NL0 and NL1.

   205    *              CRDLY         CR delay mask. Values are CR0, CR1, CR2, or CR3.

   206    *              TABDLY        horizontal tab delay mask. Values are TAB0, TAB1,

   207    *                            TAB2, TAB3, or XTABS. A value of XTABS expands

   208    *                            tabs to spaces (with tab stops every eight columns).

   209    *              BSDLY         backspace delay mask. Values are BS0 or BS1.

   210    *              VTDLY         vertical tab delay mask. Values are VT0 or VT1.

   211    *              FFDLY         form feed delay mask. Values are FF0 or FF1.

   212    */

   213 

   214   /* configure the control modes... */

   215   tios->c_cflag = CREAD |    /* enable receiver.               */

   216                   CLOCAL;    /* ignore modem control lines     */

   217   /*              HUPCL         lower modem control lines after last process

   218    *                            closes the device (hang up).

   219    *              CRTSCTS       flow control (Request/Clear To Send).

   220    */

   221        if (data_bits == 5) tios->c_cflag |= CS5;

   222   else if (data_bits == 6) tios->c_cflag |= CS6;

   223   else if (data_bits == 7) tios->c_cflag |= CS7;

   224   else if (data_bits == 8) tios->c_cflag |= CS8;

   225   else return -1;

   226 

   227        if (stop_bits == 1) tios->c_cflag &=~ CSTOPB;

   228   else if (stop_bits == 2) tios->c_cflag |= CSTOPB;

   229   else return -1;

   230 

   231          if(parity == 0) { /* none */

   232     tios->c_cflag &=~ PARENB;

   233     tios->c_cflag &=~ PARODD;

   234   } else if(parity == 2)  { /* even */

   235     tios->c_cflag |=  PARENB;

   236     tios->c_cflag &=~ PARODD;

   237   } else if(parity == 1)  { /* odd */

   238     tios->c_cflag |=  PARENB;

   239     tios->c_cflag |=  PARODD;

   240   } else return -1;

   241 

   242 

   243   /* configure the local modes... */

   244   tios->c_lflag = IEXTEN;    /* enable implementation-defined input processing   */

   245   /*              ISIG          when any of the characters INTR, QUIT, SUSP, or DSUSP

   246    *                            are received, generate the corresponding signal.

   247    *              ICANON        enable canonical mode. This enables the special

   248    *                            characters EOF, EOL, EOL2, ERASE, KILL, REPRINT,

   249    *                            STATUS, and WERASE, and buffers by lines.

   250    *              ECHO          echo input characters.

   251    */

   252 

   253   /* Set the baud rate */

   254   /* Must be done before reseting all the values to 0! */

   255   switch(baud) {

   256     case 110:    baud_rate = B110;    break;

   257     case 300:    baud_rate = B300;    break;

   258     case 600:    baud_rate = B600;    break;

   259     case 1200:   baud_rate = B1200;   break;

   260     case 2400:   baud_rate = B2400;   break;

   261     case 4800:   baud_rate = B4800;   break;

   262     case 9600:   baud_rate = B9600;   break;

   263     case 19200:  baud_rate = B19200;  break;

   264     case 38400:  baud_rate = B38400;  break;

   265     case 57600:  baud_rate = B57600;  break;

   266     case 115200: baud_rate = B115200; break;

   267     default: return -1;

   268   } /* switch() */

   269 

   270   if ((cfsetispeed(tios, baud_rate) < 0) ||

   271       (cfsetospeed(tios, baud_rate) < 0))

   272     return -1;;

   273 

   274   return 0;

   275 }

   276 

   277 

   278 /************************************/

   279 /**                                **/

   280 /** A data structure - recv buffer **/

   281 /**                                **/

   282 /************************************/

   283 

   284 /* A data structutre used for the receive buffer, i.e. the buffer

   285  * that stores the bytes we receive from the bus.

   286  *

   287  * What we realy needed here is an unbounded buffer. This may be

   288  * implemented by:

   289  *   - a circular buffer the size of the maximum frame length

   290  *   - a linear buffer somewhat larger than the maximum frame length

   291  *

   292  * Due to the fact that this library's API hands over the frame data

   293  * in a linear buffer, and also reads the data (i,e, calls to read())

   294  * into a linear buffer:

   295  *  - the circular buffer would be more efficient in aborted frame

   296  *    situations

   297  *  - the linear is more efficient when no aborted frames are recieved.

   298  *

   299  * I have decided to optimize for the most often encountered situation,

   300  * i.e. when no aborted frames are received.

   301  *

   302  * The linear buffer has a size larger than the maximum

   303  * number of bytes we intend to store in it. We simply start ignoring

   304  * the first bytes in the buffer in which we are not interested in, and

   305  * continue with the extra bytes of the buffer. When we reach the limit

   306  * of these extra bytes, we shift the data down so it once again

   307  * uses the first bytes of the buffer. The more number of extra bytes,

   308  * the more efficient it will be.

   309  *

   310  * Note that if we don't receive any aborted frames, it will work as a

   311  * simple linear buffer, and no memory shifts will be required!

   312  */

   313 

   314 typedef struct {

   315         lb_buf_t data_buf;

   316           /* Flag:

   317             *  1 => We have detected a frame boundary using 3.5 character silence

   318             *  0 => We have not yet detected any frame boundary

   319            */

   320         int found_frame_boundary; /* ==1 => valid data ends at a frame boundary. */

   321           /* Flag:

   322             *  Used in the call to search_for_frame() as the history parameter!

   323            */

   324         int frame_search_history;

   325         } recv_buf_t;

   326 

   327 /* A small auxiliary function... */

   328 static inline u8 *recv_buf_init(recv_buf_t *buf, int size, int max_data_start) {

   329   buf->found_frame_boundary = 0;

   330   buf->frame_search_history = 0;

   331   return lb_init(&buf->data_buf, size, max_data_start);

   332 }

   333 

   334 

   335 /* A small auxiliary function... */

   336 static inline void recv_buf_done(recv_buf_t *buf) {

   337   buf->found_frame_boundary = 0;

   338   buf->frame_search_history = 0;

   339   lb_done(&buf->data_buf);

   340 }

   341 

   342 

   343 /* A small auxiliary function... */

   344 static inline void recv_buf_reset(recv_buf_t *buf) {

   345   buf->found_frame_boundary = 0;

   346   buf->frame_search_history = 0;

   347   lb_data_purge_all(&buf->data_buf);

   348 }

   349 

   350 

   351 /************************************/

   352 /**                                **/

   353 /**  A data structure - nd entry   **/

   354 /**                                **/

   355 /************************************/

   356 

   357 /* NOTE: nd = node descriptor */

   358 

   359 typedef struct {

   360          /* The file descriptor associated with this node */

   361          /* NOTE: if the node is not yet in use, i.e. if the node is free,

   362           *       then fd will be set to -1

   363           */

   364         int    fd;

   365 

   366          /* the time it takes to transmit 1.5 characters at the current baud rate */

   367         struct timeval time_15_char_;

   368          /* the time it takes to transmit 3.5 characters at the current baud rate */

   369         struct timeval time_35_char_;

   370 

   371          /* Due to the algorithm used to work around aborted frames, the modbus_read()

   372           * function might read beyond the current modbus frame. The extra bytes

   373           * must be stored for the subsequent call to modbus_read().

   374           */

   375         recv_buf_t recv_buf_;

   376 

   377           /* The old settings of the serial port, to be reset when the library is closed... */

   378         struct termios old_tty_settings_;

   379 

   380           /* ignore echo flag.

   381            * If set to 1, then it means that we will be reading every byte we

   382            * ourselves write out to the bus, so we must ignore those bytes read

   383            * before we really read the data sent by remote nodes.

   384            *

   385            * This comes in useful when using a RS232-RS485 converter that does

   386            * not correctly control the RTS-CTS lines...

   387            */

   388         int ignore_echo;

   389  } nd_entry_t;

   390 

   391 

   392 static inline void nd_entry_init(nd_entry_t *nde) {

   393   nde->fd = -1; /* The node is free... */

   394 }

   395 

   396 

   397 

   398 static int nd_entry_connect(nd_entry_t *nde,

   399                             node_addr_t *node_addr,

   400                             optimization_t opt) {

   401 

   402   int parity_bits, start_bits, char_bits;

   403   struct termios settings;

   404   int buf_size;

   405 

   406   /*

   407   if (nde == NULL)

   408     goto error_exit_0;

   409   */

   410   if (nde->fd >= 0)

   411     goto error_exit_0;

   412 

   413   /* initialise the termios data structure */

   414   if (termios_init(&settings,

   415                    node_addr->addr.rtu.baud,

   416                    node_addr->addr.rtu.parity,

   417                    node_addr->addr.rtu.data_bits,

   418                    node_addr->addr.rtu.stop_bits)

   419       < 0) {

   420 #ifdef ERRMSG

   421     fprintf(stderr, ERRMSG_HEAD "Invalid serial line settings"

   422                     "(baud=%d, parity=%d, data_bits=%d, stop_bits=%d)\n",

   423                     node_addr->addr.rtu.baud,

   424                     node_addr->addr.rtu.parity,

   425                     node_addr->addr.rtu.data_bits,

   426                     node_addr->addr.rtu.stop_bits);

   427 #endif

   428     goto error_exit_1;

   429   }

   430 

   431     /* set the ignore_echo flag */

   432   nde->ignore_echo = node_addr->addr.rtu.ignore_echo;

   433 

   434     /* initialise recv buffer */

   435   buf_size = (opt == optimize_size)?RECV_BUFFER_SIZE_SMALL:

   436                                     RECV_BUFFER_SIZE_LARGE;

   437   if (recv_buf_init(&nde->recv_buf_, buf_size, buf_size - MAX_RTU_FRAME_LENGTH)

   438       == NULL) {

   439 #ifdef ERRMSG

   440     fprintf(stderr, ERRMSG_HEAD "Out of memory: error initializing receive buffer\n");

   441 #endif

   442     goto error_exit_2;

   443   }

   444 

   445     /* open the serial port */

   446   if((nde->fd = open(node_addr->addr.rtu.device, O_RDWR | O_NOCTTY | O_NDELAY))

   447      < 0) {

   448 #ifdef ERRMSG

   449     perror("open()");

   450     fprintf(stderr, ERRMSG_HEAD "Error opening device %s\n",

   451                    node_addr->addr.rtu.device);

   452 #endif

   453     goto error_exit_3;

   454   }

   455 

   456   if(tcgetattr(nde->fd, &nde->old_tty_settings_) < 0) {

   457 #ifdef ERRMSG

   458     perror("tcgetattr()");

   459     fprintf(stderr, ERRMSG_HEAD "Error reading device's %s original settings.\n",

   460                       node_addr->addr.rtu.device);

   461 #endif

   462     goto error_exit_4;

   463   }

   464 

   465   if(tcsetattr(nde->fd, TCSANOW, &settings) < 0) {

   466 #ifdef ERRMSG

   467     perror("tcsetattr()");

   468     fprintf(stderr, ERRMSG_HEAD "Error configuring device %s "

   469                     "(baud=%d, parity=%d, data_bits=%d, stop_bits=%d)\n",

   470                     node_addr->addr.rtu.device,

   471                     node_addr->addr.rtu.baud,

   472                     node_addr->addr.rtu.parity,

   473                     node_addr->addr.rtu.data_bits,

   474                     node_addr->addr.rtu.stop_bits);

   475 #endif

   476     goto error_exit_4;

   477   }

   478 

   479   parity_bits   = (node_addr->addr.rtu.parity == 0)?0:1;

   480   start_bits    = 1;

   481   char_bits     = start_bits  + node_addr->addr.rtu.data_bits +

   482                   parity_bits + node_addr->addr.rtu.stop_bits;

   483   nde->time_15_char_ = d_to_timeval(SAFETY_MARGIN*1.5*char_bits/node_addr->addr.rtu.baud);

   484   nde->time_35_char_ = d_to_timeval(SAFETY_MARGIN*3.5*char_bits/node_addr->addr.rtu.baud);

   485 

   486 #ifdef DEBUG

   487   fprintf(stderr, "nd_entry_connect(): %s ope{.node=NULL, .node_count=0};n\n", node_addr->addr.rtu.device );

   488   fprintf(stderr, "nd_entry_connect(): returning fd=%d\n", nde->fd);

   489 #endif

   490   return nde->fd;

   491 

   492   error_exit_4:

   493     close(nde->fd);

   494   error_exit_3:

   495     recv_buf_done(&nde->recv_buf_);

   496   error_exit_2:

   497   error_exit_1:

   498     nde->fd = -1; /* set the node as free... */

   499   error_exit_0:

   500     return -1;

   501 }

   502 

   503 

   504 

   505 static int nd_entry_free(nd_entry_t *nde) {

   506   if (nde->fd < 0)

   507     /* already free */

   508     return -1;

   509 

   510   /* reset the tty device old settings... */

   511 #ifdef ERRMSG

   512   int res =

   513 #endif

   514             tcsetattr(nde->fd, TCSANOW, &nde->old_tty_settings_);

   515 #ifdef ERRMSG

   516   if(res < 0)

   517     fprintf(stderr, ERRMSG_HEAD "Error reconfiguring serial port to it's original settings.\n");

   518 #endif

   519 

   520   recv_buf_done(&nde->recv_buf_);

   521   close(nde->fd);

   522   nde->fd = -1;

   523 

   524   return 0;

   525 }

   526 

   527 

   528 

   529 

   530 static inline int nd_entry_is_free(nd_entry_t *nde) {

   531   return (nde->fd < 0);

   532 }

   533 

   534 

   535 

   536 

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

   538 /**                                **/

   539 /**  A data structure - nd table   **/

   540 /**                                **/

   541 /************************************/

   542 

   543 typedef struct {

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

   545     nd_entry_t *node;

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

   547 } nd_table_t;

   548 

   549 

   550 #if 1

   551 /* nd_table_init()

   552  *   Version 1 of the nd_table_init() function. 

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

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

   555  *   initialized with the requested number of nodes.

   556  */

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

   558   int count;

   559 

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

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

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

   563   }

   564 

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

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

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

   568 #ifdef ERRMSG

   569     fprintf(stderr, ERRMSG_HEAD "Out of memory: error initializing node address buffer\n");

   570 #endif

   571     return -1;

   572   }

   573   ndt->node_count = nd_count;

   574 

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

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

   577     nd_entry_init(&ndt->node[count]);

   578   } /* for() */

   579 

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

   581 }

   582 #else

   583 /* nd_table_init()

   584  *   Version 2 of the nd_table_init() function. 

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

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

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

   588  */

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

   590   int count;

   591 

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

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

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

   595 #ifdef ERRMSG

   596     fprintf(stderr, ERRMSG_HEAD "Out of memory: error initializing node address buffer\n");

   597 #endif

   598     return -1;

   599   }

   600 

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

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

   603     nd_entry_init(&ndt->node[count]);

   604   } /* for() */

   605   ndt->node_count += new_nd_count;

   606 

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

   608 }

   609 #endif

   610 

   611 

   612 static inline nd_entry_t *nd_table_get_nd(nd_table_t *ndt, int nd) {

   613   if ((nd < 0) || (nd >= ndt->node_count))

   614     return NULL;

   615 

   616   return &ndt->node[nd];

   617 }

   618 

   619 

   620 static inline void nd_table_done(nd_table_t *ndt) {

   621   int i;

   622 

   623   if (ndt->node == NULL) 

   624     return;

   625 

   626     /* close all the connections... */

   627   for (i = 0; i < ndt->node_count; i++)

   628     nd_entry_free(&ndt->node[i]);

   629 

   630   /* Free memory... */

   631   free(ndt->node);

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

   633 }

   634 

   635 

   636 

   637 static inline int nd_table_get_free_nd(nd_table_t *ndt) {

   638   int count;

   639 

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

   641     if (nd_entry_is_free(&ndt->node[count]))

   642       return count;

   643   }

   644 

   645    /* none found... */

   646   return -1;

   647 }

   648 

   649 

   650 static inline int nd_table_free_nd(nd_table_t *ndt, int nd) {

   651   if ((nd < 0) || (nd >= ndt->node_count))

   652     return -1;

   653 

   654   return nd_entry_free(&ndt->node[nd]);

   655 }

   656 

   657 

   658 

   659 /**************************************************************/

   660 /**************************************************************/

   661 /****                                                      ****/

   662 /****                                                      ****/

   663 /****                Global Library State                  ****/

   664 /****                                                      ****/

   665 /****                                                      ****/

   666 /**************************************************************/

   667 /**************************************************************/

   668 

   669  /* The node descriptor table... */

   670  /* NOTE: This variable must be correctly initialised here!! */

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

   672 

   673  /* The optimization choice... */

   674 static optimization_t optimization_;

   675 

   676  /* the crc function currently in use... */

   677  /* This will depend on the optimisation choice... */

   678 crc_func_t crc_calc = DEF_CRC_FUNCTION;

   679 

   680 

   681 

   682 /**************************************************************/

   683 /**************************************************************/

   684 /****                                                      ****/

   685 /****                                                      ****/

   686 /****                      CRC functions                   ****/

   687 /****                                                      ****/

   688 /****                                                      ****/

   689 /**************************************************************/

   690 /**************************************************************/

   691 

   692 #if     RTU_FRAME_CRC_LENGTH < 2

   693 #error  The CRC on modbus RTU frames requires at least 2 bytes in the frame length.

   694 #endif

   695 

   696 

   697 /************************************/

   698 /**                                **/

   699 /**     Read the CRC of a frame    **/

   700 /**                                **/

   701 /************************************/

   702 

   703 /* NOTE: cnt is number of bytes in the frame _excluding_ CRC! */

   704 static inline u16 crc_read(u8 *buf, int cnt) {

   705   /* For some strange reason, the crc is transmited

   706    * LSB first, unlike all other values...

   707    */

   708   return (buf[cnt + 1] << 8) | buf[cnt];

   709 }

   710 

   711 

   712 /************************************/

   713 /**                                **/

   714 /**    Write the CRC of a frame    **/

   715 /**                                **/

   716 /************************************/

   717 

   718 /* NOTE: cnt is number of bytes in the frame _excluding_ CRC! */

   719 static inline void crc_write(u8 *buf, int cnt) {

   720   /* For some strange reason, the crc is transmited

   721    * LSB first, unlike all other values...

   722    *

   723    * u16_v(query[string_length]) = mb_hton(temp_crc);  -> This is wrong !!

   724    */

   725   /* NOTE: We have already checked above that RTU_FRAME_CRC_LENGTH is >= 2 */

   726   u16 crc = crc_calc(buf, cnt);

   727   buf[cnt]   = lsb(crc);

   728   buf[cnt+1] = msb(crc);

   729 }

   730 

   731 

   732 

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

   734 /**                                **/

   735 /**     A slow version of the      **/

   736 /**          CRC function          **/

   737 /**                                **/

   738 /************************************/

   739 

   740 /* crc optimized for smallest memory footprint */

   741 static u16 crc_slow(u8 *buf, int cnt)

   742 {

   743   int bit;

   744   u16 temp,flag;

   745 

   746   temp=0xFFFF;

   747 

   748   while (cnt-- != 0) {

   749     temp=temp ^ *buf++;

   750     for (bit=1; bit<=8; bit++) {

   751       flag = temp & 0x0001;

   752       /* NOTE:

   753        *  - since temp is unsigned, we are guaranteed a zero in MSbit;

   754        *  - if it were signed, the value placed in the MSbit would be

   755        *      compiler dependent!

   756        */

   757       temp >>= 1;

   758       if (flag)

   759         temp=temp ^ 0xA001;

   760     }

   761   }

   762   return(temp);

   763 }

   764 

   765 

   766 

   767 

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

   769 /**                                **/

   770 /**     A fast version of the      **/

   771 /**          CRC function          **/

   772 /**                                **/

   773 /************************************/

   774 static u8 *crc_fast_buf = NULL;

   775 

   776 /* crc optimized for speed */

   777 static u16 crc_fast(u8 *buf, int cnt)

   778 {

   779 /* NOTE: The following arrays have been replaced by an equivalent

   780  *      array (crc_fast_buf[]) initialised at run-time.

   781  */

   782 /*

   783   static u8 buf_lsb[] = {0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41,

   784                          0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1, 0x81, 0x40,

   785                          0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1, 0x81, 0x40,

   786                          0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41,

   787                          0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1, 0x81, 0x40,

   788                          0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41,

   789                          0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41,

   790                          0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1, 0x81, 0x40,

   791                          0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1, 0x81, 0x40,

   792                          0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41,

   793                          0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41,

   794                          0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1, 0x81, 0x40,

   795                          0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41,

   796                          0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1, 0x81, 0x40,

   797                          0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1, 0x81, 0x40,

   798                          0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41,

   799                          0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1, 0x81, 0x40,

   800                          0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41,

   801                          0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41,

   802                          0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1, 0x81, 0x40,

   803                          0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41,

   804                          0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1, 0x81, 0x40,

   805                          0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1, 0x81, 0x40,

   806                          0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41,

   807                          0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41,

   808                          0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1, 0x81, 0x40,

   809                          0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1, 0x81, 0x40,

   810                          0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41,

   811                          0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1, 0x81, 0x40,

   812                          0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41,

   813                          0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41,

   814                          0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1, 0x81, 0x40

   815                         };

   816 

   817   static u8 buf_msb[] = {0x00, 0xc0, 0xc1, 0x01, 0xc3, 0x03, 0x02, 0xc2,

   818                          0xc6, 0x06, 0x07, 0xc7, 0x05, 0xc5, 0xc4, 0x04,

   819                          0xcc, 0x0c, 0x0d, 0xcd, 0x0f, 0xcf, 0xce, 0x0e,

   820                          0x0a, 0xca, 0xcb, 0x0b, 0xc9, 0x09, 0x08, 0xc8,

   821                          0xd8, 0x18, 0x19, 0xd9, 0x1b, 0xdb, 0xda, 0x1a,

   822                          0x1e, 0xde, 0xdf, 0x1f, 0xdd, 0x1d, 0x1c, 0xdc,

   823                          0x14, 0xd4, 0xd5, 0x15, 0xd7, 0x17, 0x16, 0xd6,

   824                          0xd2, 0x12, 0x13, 0xd3, 0x11, 0xd1, 0xd0, 0x10,

   825                          0xf0, 0x30, 0x31, 0xf1, 0x33, 0xf3, 0xf2, 0x32,

   826                          0x36, 0xf6, 0xf7, 0x37, 0xf5, 0x35, 0x34, 0xf4,

   827                          0x3c, 0xfc, 0xfd, 0x3d, 0xff, 0x3f, 0x3e, 0xfe,

   828                          0xfa, 0x3a, 0x3b, 0xfb, 0x39, 0xf9, 0xf8, 0x38,

   829                          0x28, 0xe8, 0xe9, 0x29, 0xeb, 0x2b, 0x2a, 0xea,

   830                          0xee, 0x2e, 0x2f, 0xef, 0x2d, 0xed, 0xec, 0x2c,

   831                          0xe4, 0x24, 0x25, 0xe5, 0x27, 0xe7, 0xe6, 0x26,

   832                          0x22, 0xe2, 0xe3, 0x23, 0xe1, 0x21, 0x20, 0xe0,

   833                          0xa0, 0x60, 0x61, 0xa1, 0x63, 0xa3, 0xa2, 0x62,

   834                          0x66, 0xa6, 0xa7, 0x67, 0xa5, 0x65, 0x64, 0xa4,

   835                          0x6c, 0xac, 0xad, 0x6d, 0xaf, 0x6f, 0x6e, 0xae,

   836                          0xaa, 0x6a, 0x6b, 0xab, 0x69, 0xa9, 0xa8, 0x68,

   837                          0x78, 0xb8, 0xb9, 0x79, 0xbb, 0x7b, 0x7a, 0xba,

   838                          0xbe, 0x7e, 0x7f, 0xbf, 0x7d, 0xbd, 0xbc, 0x7c,

   839                          0xb4, 0x74, 0x75, 0xb5, 0x77, 0xb7, 0xb6, 0x76,

   840                          0x72, 0xb2, 0xb3, 0x73, 0xb1, 0x71, 0x70, 0xb0,

   841                          0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92,

   842                          0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54,

   843                          0x9c, 0x5c, 0x5d, 0x9d, 0x5f, 0x9f, 0x9e, 0x5e,

   844                          0x5a, 0x9a, 0x9b, 0x5b, 0x99, 0x59, 0x58, 0x98,

   845                          0x88, 0x48, 0x49, 0x89, 0x4b, 0x8b, 0x8a, 0x4a,

   846                          0x4e, 0x8e, 0x8f, 0x4f, 0x8d, 0x4d, 0x4c, 0x8c,

   847                          0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86,

   848                          0x82, 0x42, 0x43, 0x83, 0x41, 0x81, 0x80, 0x40

   849                         };

   850 */

   851   u8 crc_msb = 0xFF;

   852   u8 crc_lsb = 0xFF;

   853   int index;

   854 

   855   if (cnt <= 0) {

   856     fprintf(stderr, "\nInternal program error in file %s at line %d\n\n\n", __FILE__, __LINE__);

   857     exit(EXIT_FAILURE);

   858   }

   859 

   860   while (cnt-- != 0) {

   861     index = 2 * (crc_lsb ^ *buf++);

   862     crc_lsb = crc_msb ^ crc_fast_buf[index]/* buf_lsb[index/2] */;

   863     crc_msb = crc_fast_buf[index + 1] /* buf_msb[index/2] */;

   864   }

   865 

   866   return crc_msb*0x0100 + crc_lsb;

   867 }

   868 

   869 

   870 /************************************/

   871 /**                                **/

   872 /**  init() and done() functions   **/

   873 /**      of fast CRC version       **/

   874 /**                                **/

   875 /************************************/

   876 

   877 static inline int crc_fast_init(void) {

   878   int i;

   879   u8  data[2];

   880   u16 tmp_crc;

   881 

   882   if ((crc_fast_buf = (u8 *)malloc(256 * 2)) == NULL)

   883     return -1;

   884 

   885   for (i = 0x00; i < 0x100; i++) {

   886     data[0] = 0xFF;

   887     data[1] = i;

   888     data[1] = ~data[1];

   889     tmp_crc = crc_slow(data, 2);

   890     crc_fast_buf[2*i    ] = lsb(tmp_crc);

   891     crc_fast_buf[2*i + 1] = msb(tmp_crc);

   892   }

   893 

   894   return 0;

   895 }

   896 

   897 

   898 static inline void crc_fast_done(void) {

   899   free(crc_fast_buf);

   900 }

   901 

   902 

   903 /************************************/

   904 /**                                **/

   905 /**  init() and done() functions   **/

   906 /**         of generic CRC         **/

   907 /**                                **/

   908 /************************************/

   909 

   910 static inline int crc_init(optimization_t opt) {

   911   switch (opt) {

   912     case optimize_speed:

   913       if (crc_fast_init() < 0)

   914         return -1;

   915       crc_calc = crc_fast;

   916       return 0;

   917     case optimize_size :

   918       crc_calc = crc_slow;

   919       return 0;

   920     default:

   921       return -1;

   922   }

   923 

   924   /* humour the compiler */

   925   return -1;

   926 }

   927 

   928 

   929 static inline int crc_done(void) {

   930   if (crc_calc == crc_fast)

   931     crc_fast_done();

   932 

   933   crc_calc = DEF_CRC_FUNCTION;

   934   return 0;

   935 }

   936 

   937 

   938 

   939 /**************************************************************/

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

   941 /****                                                      ****/

   942 /****                                                      ****/

   943 /****              Sending of Modbus RTU Frames            ****/

   944 /****                                                      ****/

   945 /****                                                      ****/

   946 /**************************************************************/

   947 /**************************************************************/

   948 

   949 /*  W A R N I N G

   950  *  =============

   951  *     The modbus_rtu_write() function assumes that the caller

   952  *     has allocated a few bytes extra for the buffer containing

   953  *     the data. These bytes will be used to write the crc.

   954  *

   955  *     The caller of this function MUST make sure that the data

   956  *     buffer, although only containing data_length bytes, has

   957  *     been allocated with a size equal to or larger than

   958  *     data_length + RTU_FRAME_CRC_LENGTH bytes

   959  *

   960  *     I know, this is a very ugly hack, but we don't have much

   961  *     choice (please read other comments further on for more

   962  *     explanations)

   963  *

   964  *     We will nevertheless try and make this explicit by having the

   965  *     library initialisation function (modbus_rtu_init() ) return a

   966  *     value specifying how many extra bytes this buffer should have.

   967  *     Maybe this way this very ugly hack won't go unnoticed, and we

   968  *     won't be having any segmentation faults...!

   969  * 

   970  *     NOTE: for now the transmit_timeout is silently ignored in RTU version!

   971  */

   972 int modbus_rtu_write(int    nd,

   973                      u8    *data,

   974                      size_t data_length,

   975                      u16    transaction_id,

   976                      const struct timespec *transmit_timeout

   977                     )

   978 {

   979   fd_set rfds;

   980   struct timeval timeout;

   981   int res, send_retries;

   982   nd_entry_t *nd_entry;

   983 

   984 #ifdef DEBUG

   985   fprintf(stderr, "modbus_rtu_write(fd=%d) called...\n", nd);

   986 #endif

   987   /* check if nd is correct... */

   988   if ((nd_entry = nd_table_get_nd(&nd_table_, nd)) == NULL)

   989     return -1;

   990 

   991   /* check if nd is initialzed... */

   992   if (nd_entry->fd < 0)

   993     return -1;

   994 

   995   /**************************

   996   * append crc to frame... *

   997   **************************/

   998 /* WARNING:

   999  *     The crc_write() function assumes that we have an extra

  1000  *     RTU_FRAME_CRC_LENGTH free bytes at the end of the *data

  1001  *     buffer.

  1002  *     The caller of this function had better make sure he has

  1003  *     allocated those extra bytes, or a segmentation fault will

  1004  *     occur.

  1005  *     Please read on why we leave this as it is...

  1006  *

  1007  * REASONS:

  1008  *     We want to write the data and the crc in a single call to

  1009  *     the OS. This is the only way we can minimally try to gurantee

  1010  *     that we will not be introducing a silence of more than 1.5

  1011  *     character transmission times between any two characters.

  1012  *

  1013  *     We could do the above using one of two methods:

  1014  *       (a) use a special writev() call in which the data

  1015  *           to be sent is stored in two buffers (one for the

  1016  *           data and the other for the crc).

  1017  *       (b) place all the data in a single linear buffer and

  1018  *           use the normal write() function.

  1019  *

  1020  *     We cannot use (a) since the writev(2) function does not seem

  1021  *     to be POSIX compliant...

  1022  *     (b) has the drawback that we would need to allocate a new buffer,

  1023  *      and copy all the data into that buffer. We have enough copying of

  1024  *      data between buffers as it is, so we won't be doing it here

  1025  *      yet again!

  1026  *

  1027  *      The only option that seems left over is to have the caller

  1028  *      of this function allocate a few extra bytes. Let's hope he

  1029  *      does not forget!

  1030 */

  1031   crc_write(data, data_length);

  1032   data_length += RTU_FRAME_CRC_LENGTH;

  1033 

  1034 #ifdef DEBUG

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

  1036  * sent over the bus.

  1037  */

  1038   { int i;

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

  1040       fprintf(stderr, "[0x%2X]", data[i]);

  1041     fprintf(stderr, "\n");

  1042   }

  1043 #endif

  1044    /* THE MAIN LOOP!!! */

  1045   /* NOTE: The modbus standard specifies that the message must

  1046    *       be sent continuosly over the wire with maximum

  1047    *       inter-character delays of 1.5 character intervals.

  1048    *

  1049    *       If the write() call is interrupted by a signal, then

  1050    *       this delay will most probably be exceeded. We should then

  1051    *       re-start writing the query from the begining.

  1052    *

  1053    *       BUT, can we really expect the write() call to return

  1054    *       query_length on every platform when no error occurs?

  1055    *       The write call would still be correct if it only wrote

  1056    *       1 byte at a time!

  1057    *

  1058    *       To protect ourselves getting into an infinte loop in the

  1059    *       above cases, we specify a maximum number of retries, and

  1060    *       hope for the best...! The worst will now be we simply do

  1061    *       not get to send out a whole frame, and will therefore always

  1062    *       fail on writing a modbus frame!

  1063    */

  1064   send_retries = RTU_FRAME_SEND_RETRY + 1; /* must try at least once... */

  1065   while (send_retries > 0) {

  1066 

  1067     /*******************************

  1068      * synchronise with the bus... *

  1069      *******************************/

  1070     /* Remember that a RS485 bus is half-duplex, so we have to wait until

  1071      * nobody is transmitting over the bus for our turn to transmit.

  1072      * This will never happen on a modbus network if the master and

  1073      * slave state machines never get out of synch (granted, it probably

  1074      * only has two states, but a state machine nonetheless), but we want

  1075      * to make sure we can re-synchronise if they ever do get out of synch.

  1076      *

  1077      * The following lines will guarantee that we will re-synchronise our

  1078      * state machine with the current state of the bus.

  1079      *

  1080      * We first wait until the bus has been silent for at least

  1081      * char_interval_timeout (i.e. 3.5 character interval). We then flush

  1082      * any input and output that might be on the cache.

  1083      */

  1084       /* NOTES:

  1085        *   - we do not need to reset the rfds with FD_SET(ttyfd, &rfds)

  1086        *     before every call to select! We only wait on one file descriptor,

  1087        *     so if select returns succesfully, it must have that same file

  1088        *     decriptor set in the rdfs!

  1089        *     If select returns with a timeout, then we do not get to call

  1090        *     select again!

  1091        *   -  On Linux, timeout (i.e. timeout) is modified by select() to

  1092        *      reflect the amount of time not slept; most other implementations

  1093        *      do not do this. In the cases in which timeout is not modified,

  1094        *      we will simply have to wait for longer periods if select is

  1095        *      interrupted by a signal.

  1096        */

  1097     FD_ZERO(&rfds);

  1098     FD_SET(nd_entry->fd, &rfds);

  1099     timeout = nd_entry->time_35_char_;

  1100     while ((res = select(nd_entry->fd+1, &rfds, NULL, NULL, &timeout)) != 0) {

  1101       if (res > 0) {

  1102         /* we are receiving data over the serial port! */

  1103         /* Throw the data away!                        */

  1104         tcflush(nd_entry->fd, TCIFLUSH); /* flush the input stream */

  1105         /* reset the timeout value! */

  1106         timeout = nd_entry->time_35_char_;

  1107         /* We do not need to reset the FD SET here! */

  1108       } else {

  1109         /* some kind of error ocurred */

  1110         if (errno != EINTR)

  1111           /* we were not interrupted by a signal */

  1112           return -1;

  1113         /* We will be calling select() again.

  1114          * We need to reset the FD SET !

  1115          */

  1116         FD_ZERO(&rfds);

  1117         FD_SET(nd_entry->fd, &rfds);

  1118       }

  1119     } /* while (select()) */

  1120 

  1121     /* Flush both input and output streams... */

  1122     /* NOTE: Due to the nature of the modbus protocol,

  1123      *       when a frame is sent all previous

  1124      *       frames that may have arrived at the sending node become

  1125      *       irrelevant.

  1126      */

  1127     tcflush(nd_entry->fd, TCIOFLUSH);       /* flush the input & output streams */

  1128     recv_buf_reset(&nd_entry->recv_buf_);   /* reset the recv buffer            */

  1129 

  1130     /**********************

  1131      * write to output... *

  1132      **********************/

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

  1134     if ((res = write(nd_entry->fd, data, data_length)) != data_length) {

  1135       if ((res < 0) && (errno != EAGAIN ) && (errno != EINTR ))

  1136         return -1;