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

  {

   471

   int optval;

   472

   socklen_t optlen = sizeof(optval);

   473

   optval = 1;

   474

   if(setsockopt(socket_id, SOL_SOCKET, SO_KEEPALIVE, &optval, optlen) < 0) {

   475

#ifdef ERRMSG

   476

    perror("setsockopt()");

   477

    fprintf(stderr, ERRMSG_HEAD "Error configuring socket 'KeepAlive' option.\n");

   478

#endif

   479

      return -1;

   480

   }

   481

  }

   482

    /* set the TCP no delay flag. */

   483

  {int bool_opt = 1;

   484

  if (setsockopt(socket_id, SOL_TCP, TCP_NODELAY,

   485

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

   486

      < 0) {

   487

#ifdef ERRMSG

   488

    perror("setsockopt()");

   489

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

   490

#endif

   491

    return -1;

   492

  }

   493

  }

   494

   495

    /* set the IP low delay option. */

   496

  {int priority_opt = IPTOS_LOWDELAY;

   497

  if (setsockopt(socket_id, SOL_IP, IP_TOS,

   498

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

   499

      < 0) {

   500

#ifdef ERRMSG

   501

    perror("setsockopt()");

   502

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

   503

#endif

   504

    return -1;

   505

  }

   506

  }

   507

   508

#if 0

   509

    /* send buffer */

   510

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

   511

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

   512

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

   513

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

   514

     *       larger than the largest Modbus TCP frame.

   515

     *

   516

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

   517

     *       However, it probably does not make sense to

   518

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

   519

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

   520

     *       been created!)

   521

     *

   522

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

   523

     *       buffer size...

   524

     */

   525

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

   526

  {int sock_buf_size;

   527

  sock_buf_size = SOCK_BUF_SIZE;

   528

  if (setsockopt(socket_id, SOL_SOCKET, SO_SNDBUF,

   529

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