1

/*

     2

 * Copyright (c) 2001-2003,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

/* mb_master.c */

    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 <errno.h>	/* Error definitions */

    35

    36

#include <pthread.h>    /* pthread_mutex_[un]lock() */

    37

    38

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

    39

#include "mb_layer1.h"

    40

#include "mb_master.h"

    41

#include "mb_master_private.h"

    42

    43

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

    44

    45

#define modbus_write             fptr_[layer1_fin].modbus_write           

    46

#define modbus_read              fptr_[layer1_fin].modbus_read            

    47

#define modbus_init              fptr_[layer1_fin].modbus_init            

    48

#define modbus_done              fptr_[layer1_fin].modbus_done            

    49

#define modbus_connect           fptr_[layer1_fin].modbus_connect         

    50

#define modbus_listen            fptr_[layer1_fin].modbus_listen          

    51

#define modbus_close             fptr_[layer1_fin].modbus_close           

    52

#define modbus_silence_init      fptr_[layer1_fin].modbus_silence_init    

    53

#define modbus_get_min_timeout   fptr_[layer1_fin].modbus_get_min_timeout 

    54

    55

/* the lower two bits of ttyfd are used to store the index to layer1 function pointers */

    56

/* layer1_fin index to fptr_[] is in lowest 2 bits of fd */

    57

#define get_ttyfd()     int layer1_fin = fd & 3; int ttyfd = fd / 4;

    58

    59

    60

    61

/******************************************/

    62

/******************************************/

    63

/**                                      **/

    64

/**         Global Variables...          **/

    65

/**                                      **/

    66

/******************************************/

    67

/******************************************/

    68

   /* The layer 1 (RTU, ASCII, TCP) implementation will be adding some headers and CRC (at the end)

    69

    *  of the packet we build here (actually currently it is only at the end). Since we want to 

    70

    *  re-use the same buffer so as not to continuosly copy the same info from buffer to buffer,

    71

    *  we need tp allocate more bytes than the ones we need for this layer. Therefore, the

    72

    *  extra_bytes parameter.

    73

    *

    74

    *  Note that we add one more extra byte. This is because some packets will not be 

    75

    *  starting off at byte 0, but rather at byte 1 of the buffer. This is in order to guarantee

    76

    *  that the data that is sent on the buffer is aligned on even bytes (the 16 bit words!).

    77

    *  This will allow us to reference this memory as an u16 *, without producing 'bus error'

    78

    *  messages in some embedded devices that do not allow acessing u16 on odd numbered addresses.

    79

    */ 

    80

static int buff_extra_bytes_;

    81

#define QUERY_BUFFER_SIZE       (MAX_L2_FRAME_LENGTH + buff_extra_bytes_ + 1)

    82

    83

    84

/******************************************/

    85

/******************************************/

    86

/**                                      **/

    87

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

    88

/**                                      **/

    89

/******************************************/

    90

/******************************************/

    91

    92

    93

/*

    94

 * Function to determine next transaction id.

    95

 *

    96

 * We use a library wide transaction id, which means that we

    97

 * use a new transaction id no matter what slave to which we will

    98

 * be sending the request...

    99

 */

   100

static inline u16 next_transaction_id(void) {

   101

  static u16 next_id = 0;

   102

  return next_id++;

   103

}

   104

   105

   106

/*

   107

 * Functions to convert u16 variables

   108

 * between network and host byte order

   109

 *

   110

 * NOTE: Modbus uses MSByte first, just like

   111

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

   112

 *       ntoh() functions to guarantee

   113

 *       code portability.

   114

 */

   115

static inline u16 mb_hton(u16 h_value) {return htons(h_value);}

   116

static inline u16 mb_ntoh(u16 m_value) {return ntohs(m_value);}

   117

static inline u8  msb    (u16   value) {return (value >> 8) & 0xFF;}

   118

static inline u8  lsb    (u16   value) {return  value & 0xFF;}

   119

   120

   121

   122

   123

/*************************************************/

   124

/*************************************************/

   125

/**                                             **/

   126

/**   Common functions for Modbus Protocol.     **/

   127

/**                                             **/

   128

/*************************************************/

   129

/*************************************************/

   130

   131

/* build the common elements of a query frame */

   132

static inline int build_packet(u8  slave,

   133

                               u8  function,

   134

                               u16 start_addr,

   135

                               u16 count,

   136

                               u8 *packet) {

   137

  union {

   138

      u16 u16;

   139

      u8  u8[2];

   140

  } tmp;

   141

   142

  packet[0] = slave,

   143

  packet[1] = function;

   144

    /* NOTE:

   145

     *  Modbus uses high level addressing starting off from 1, but

   146

     *  this is sent as 0 on the wire!

   147

     *  We could expect the user to specify high level addressing 

   148

     *   starting at 1, and do the conversion to start off at 0 here.

   149

     *   However, to do this we would then need to use an u32 data type

   150

     *   to correctly hold the address supplied by the user (which could

   151

     *   correctly be 65536, which does not fit in an u16), which would

   152

     *   in turn require us to check whether the address supplied by the user

   153

     *   is correct (i.e. <= 65536). 

   154

     *  I decided to go with the other option of using an u16, and 

   155

     *   requiring the user to use addressing starting off at 0! 

   156

     */

   157

  /* NOTE: we do not use up casting - i.e. the following

   158

   *       *((u16 *)(packet+2)) = mb_hton(start_addr);

   159

   *       because packet+2 is NOT aligned with an even address, and would

   160

   *       therefore result in 'bus error' when using compilers that do not 

   161

   *       automatically do the required decomposing of this supposedly 

   162

   *       single bus access into two distinct bus accesses.

   163

   *       (Note that some compilers do do this decomposing automatically

   164

   *       in which case the following is not necessary).

   165

   *       At the moment, I (Mario de Sousa) know of at least one cross-compiler

   166

   *       that does not do the decomposing automatically, i.e. the 

   167

   *       AVR32 cross-compiler.

   168

   */

   169

  tmp.u16 = mb_hton(start_addr);

   170

  packet[2] = tmp.u8[0];

   171

  packet[3] = tmp.u8[1];

   172

  tmp.u16 = mb_hton(count);

   173

  packet[4] = tmp.u8[0];

   174

  packet[5] = tmp.u8[1];

   175

   176

  return 6;

   177

}

   178

   179

   180

   181

/* Execute a Query/Response transaction between client and server */ 

   182

/* returns: <0    -> ERROR: error codes

   183

 *          >2    -> SUCCESS: frame length

   184

 *           0..2 -> will never be returned!

   185

 */

   186

static int mb_transaction(u8  *packet,

   187

                          int query_length,

   188

                          u8  **data,

   189

                          int fd,

   190

                          int send_retries,

   191

                          u8  *error_code,

   192

                          const struct timespec *response_timeout) {

   193

  int error = TIMEOUT;

   194

  int response_length = INTERNAL_ERROR;

   195

  u16 send_transaction_id, recv_transaction_id;

   196

  get_ttyfd(); /* declare the ttyfd variable, ... */

   197

   198

    /* We must also initialize the recv_transaction_id with the same value,

   199

     * since some layer 1 protocols do not support transaction id's, so

   200

     * simply return the recv_transaction_id variable without any changes...

   201

     */

   202

  /* NOTE: we re-use the same transaction id for all send re-tries., since, in truth, 

   203

   * it is still the same transaction. This will also simplify re-synchronising with

   204

   * some slaves that keep a buffer of outstanding requests, and will reply to all of

   205

   * them, in FIFO order. In this case, once an error occurs we will be swamping the

   206

   * slave with requests. By using the same transaction id, we may correctly consider

   207

   * the reply to the first request sent as the reply to the third request! This means

   208

   * we stop re-trying the sending of further requests, and no longer swamp the slave...

   209

   */

   210

  send_transaction_id = recv_transaction_id = next_transaction_id();

   211

   212

  for (send_retries++; send_retries > 0; send_retries--) {

   213

    error = TIMEOUT;

   214

   215

    if (modbus_write(ttyfd, packet, query_length, send_transaction_id, response_timeout) < 0)

   216

      {error = PORT_FAILURE; continue;}

   217

   218

      /* if we receive a correct response but with a wrong transaction id or wrong modbus function, we try to 

   219

       * receive another frame instead of returning an error or re-sending the request! This first frame could 

   220

       * have been a response to a previous request of ours that timed out waiting for a response, and the 

   221

       * response we are waiting for could be coming 'any minute now'.

   222

       */

   223

    do {

   224

      response_length = modbus_read(&ttyfd, data, &recv_transaction_id,

   225

                                    packet, query_length, response_timeout);

   226

   227

      /* TIMEOUT condition */

   228

      /* However, if we had previously received an invalid frame, or some other error,

   229

       * we return that error instead!

   230

       * Note that the 'error' variable was initialised with the TIMEOUT error

   231

       * condition, so if no previous error ocurred, we will be returning the

   232

       * TIMEOUT error condition here!

   233

       */

   234

      if(response_length == -2)  return error;

   235

      /* NOTE we want to break out of this while loop without even running the while()

   236

       * condition, as that condition is only valid if response_length > 3 !!

   237

       */

   238

      if(response_length  <  0)  {error = PORT_FAILURE; break;}

   239

      /* This should never occur! Modbus_read() should only return valid frames! */

   240

      if(response_length  <  3)  return INTERNAL_ERROR;

   241

   242

    } while (/* we have the wrong transaction id */

   243

             (send_transaction_id != recv_transaction_id)

   244

             /* not a response frame to _our_ query */

   245

            ||

   246

             (((*data)[1] & ~0x80) != packet[1])

   247

            /* NOTE: no need to check whether (*data)[0] = slave!              */

   248

            /*       This has already been done by the modbus_read() function! */

   249

            );

   250

   251

    if(response_length < 0)  {error = PORT_FAILURE; continue;}

   252

   253

    /* Now check whether we received a Modbus Exception frame */

   254

    if (((*data)[1] & 0x80) != 0) {       /* we have an exception frame! */

   255

      /* NOTE: we have already checked above that data[2] exists! */

   256

      if (error_code != NULL)  *error_code = (*data)[2];

   257

      return MODBUS_ERROR;

   258

    }

   259

    /* success! Let's get out of the send retry loop... */

   260

    return response_length;

   261

  }

   262

  /* reached the end of the retries... */

   263

  return error;

   264

}

   265

   266

   267

/**************************************/

   268

/**************************************/

   269

/**                                  **/

   270

/**   Modbus Protocol Functions.     **/

   271

/**                                  **/

   272

/**************************************/

   273

/**************************************/

   274

   275

   276

   277

/* Execute a transaction for functions that READ BITS.

   278

 * Bits are stored on an int array, one bit per int.

   279

 * Called by:  read_input_bits()

   280

 *             read_output_bits()

   281

 */

   282

static int read_bits(u8  function,

   283

                     u8  slave,

   284

                     u16 start_addr,

   285

                     u16 count,

   286

                     u16 *dest,

   287

                     int dest_size,

   288

                     int ttyfd,

   289

                     int send_retries,

   290

                     u8  *error_code,

   291

                     const struct timespec *response_timeout,

   292

                     pthread_mutex_t *data_access_mutex) {

   293

   294

  u8 packet[QUERY_BUFFER_SIZE];

   295

  u8 *data;

   296

  int response_length, query_length;

   297

  int temp, i, bit, dest_pos = 0;

   298

  int coils_processed = 0;

   299

   300

  query_length = build_packet(slave, function, start_addr, count, packet);

   301

  if (query_length < 0)  return INTERNAL_ERROR;

   302

   303

  response_length = mb_transaction(packet, query_length, &data, ttyfd,

   304

                                   send_retries, error_code, response_timeout);

   305

   306

  if (response_length  < 0)                  return response_length;

   307

  /* NOTE: Integer division. (count+7)/8 is equivalent to ceil(count/8) */

   308

  if (response_length != 3 + (count+7)/8)    return INVALID_FRAME;

   309

  if (data[2]         !=     (count+7)/8)    return INVALID_FRAME;

   310

   311

  if (NULL != data_access_mutex) pthread_mutex_lock(data_access_mutex);

   312

  for( i = 0; (i < data[2]) && (i < dest_size); i++ ) {

   313

    temp = data[3 + i];

   314

    for( bit = 0x01; (bit & 0xff) && (coils_processed < count); ) {

   315

      dest[dest_pos] = (temp & bit)?1:0;

   316

      coils_processed++;

   317

      dest_pos++;

   318

      bit = bit << 1;

   319

    }

   320

  }

   321

  if (NULL != data_access_mutex) pthread_mutex_unlock(data_access_mutex);

   322

   323

  return response_length;

   324

}

   325

   326

   327

   328

/* Execute a transaction for functions that READ BITS.

   329

 * Bits are stored on an u32 array, 32 bits per u32.

   330

 * Unused bits in last u32 are set to 0.

   331

 * Called by:  read_input_bits_u32()

   332

 *             read_output_bits_u32()

   333

 */

   334

static int read_bits_u32(u8  function,

   335

                         u8  slave,

   336

                         u16 start_addr,

   337

                         u16 count, /* number of bits !! */

   338

                         u32 *dest,

   339

                         int ttyfd,

   340

                         int send_retries,

   341

                         u8  *error_code,

   342

                         const struct timespec *response_timeout) {

   343

  u8 packet[QUERY_BUFFER_SIZE];

   344

  u8 *data;

   345

  int response_length, query_length;                         

   346

  int byte_count, i, dest_pos = 0;

   347

   348

  query_length = build_packet(slave, function, start_addr, count, packet);

   349

  if (query_length < 0)  return INTERNAL_ERROR;

   350

   351

  response_length = mb_transaction(packet, query_length, &data, ttyfd,

   352

                                   send_retries, error_code, response_timeout);

   353

   354

  if (response_length < 0)                   return response_length;  

   355

  /* NOTE: Integer division. (count+7)/8 is equivalent to ceil(count/8) */

   356

  if (response_length != 3 + (count+7)/8)    return INVALID_FRAME;

   357

  if (data[2]         !=     (count+7)/8)    return INVALID_FRAME;

   358

   359

  byte_count = data[2];

   360

  data += 3;

   361

  /* handle groups of 4 bytes... */

   362

  for(i = 0, dest_pos = 0; i + 3 < byte_count; i += 4, dest_pos++)

   363

    dest[dest_pos] = data[i] + data[i+1]*0x100 + data[i+2]*0x10000 + data[i+3]*0x1000000;

   364

  /* handle any remaining bytes... begining with the last! */

   365

  if (i < byte_count) dest[dest_pos] = 0;

   366

  for(byte_count--; i <= byte_count; byte_count--)

   367

    dest[dest_pos] = dest[dest_pos]*0x100 + data[byte_count];

   368

   369

  return response_length;

   370

}

   371

   372

   373

   374

/* FUNCTION 0x01   - Read Coils

   375

 * Bits are stored on an int array, one bit per int.

   376

 */

   377

int read_output_bits(u8  slave,

   378

                     u16 start_addr,

   379

                     u16 count,

   380

                     u16 *dest,

   381

                     int dest_size,

   382

                     int ttyfd,

   383

                     int send_retries,

   384

                     u8  *error_code,

   385

                     const struct timespec *response_timeout,

   386

                     pthread_mutex_t *data_access_mutex) {

   387

  if( count > MAX_READ_BITS ) {

   388

    count = MAX_READ_BITS;

   389

    #ifdef DEBUG

   390

    fprintf( stderr, "Too many coils requested.\n" );

   391

    #endif

   392

  }

   393

   394

  return read_bits(0x01 /* function */,

   395

                   slave, start_addr, count, dest, dest_size, ttyfd, 

   396

                   send_retries, error_code, response_timeout, data_access_mutex);

   397

}

   398

   399

   400

   401

/* FUNCTION 0x01   - Read Coils

   402

 * Bits are stored on an u32 array, 32 bits per u32.

   403

 * Unused bits in last u32 are set to 0.

   404

 */

   405

int read_output_bits_u32(u8  slave,

   406

                         u16 start_addr,

   407

                         u16 count,

   408

                         u32 *dest,

   409

                         int ttyfd,

   410

                         int send_retries,

   411

                         u8  *error_code,

   412

                         const struct timespec *response_timeout) {

   413

  if( count > MAX_READ_BITS ) {

   414

    count = MAX_READ_BITS;

   415

    #ifdef DEBUG

   416

    fprintf( stderr, "Too many coils requested.\n" );

   417

    #endif

   418

  }

   419

   420

  return read_bits_u32(0x01 /* function */,

   421

                       slave, start_addr, count, dest, ttyfd,

   422

                       send_retries, error_code, response_timeout);

   423

}

   424

   425

   426

/* FUNCTION 0x02   - Read Discrete Inputs

   427

 * Bits are stored on an int array, one bit per int.

   428

 */

   429

int read_input_bits(u8  slave,

   430

                    u16 start_addr,

   431

                    u16 count,

   432

                    u16 *dest,

   433

                    int dest_size,

   434

                    int ttyfd,

   435

                    int send_retries,

   436

                    u8  *error_code,

   437

                    const struct timespec *response_timeout,

   438

                    pthread_mutex_t *data_access_mutex) {

   439

  if( count > MAX_READ_BITS ) {

   440

    count = MAX_READ_BITS;

   441

    #ifdef DEBUG

   442

    fprintf( stderr, "Too many coils requested.\n" );

   443

    #endif

   444

  }

   445

   446

  return read_bits(0x02 /* function */,

   447

                   slave, start_addr, count, dest, dest_size, ttyfd,

   448

                   send_retries, error_code, response_timeout, data_access_mutex);

   449

}

   450

   451

   452

   453

/* FUNCTION 0x02   - Read Discrete Inputs

   454

 * Bits are stored on an u32 array, 32 bits per u32.

   455

 * Unused bits in last u32 are set to 0.

   456

 */

   457

int read_input_bits_u32(u8  slave,

   458

                        u16 start_addr,

   459

                        u16 count,

   460

                        u32 *dest,

   461

                        int ttyfd,

   462

                        int send_retries,

   463

                        u8  *error_code,

   464

                        const struct timespec *response_timeout) {

   465

  if( count > MAX_READ_BITS ) {

   466

    count = MAX_READ_BITS;

   467

    #ifdef DEBUG

   468

    fprintf( stderr, "Too many coils requested.\n" );

   469

    #endif

   470

  }

   471

   472

  return read_bits_u32(0x02 /* function */,

   473

                       slave, start_addr, count, dest, ttyfd,

   474

                       send_retries, error_code, response_timeout);

   475

}

   476

   477

   478

   479

   480

   481

   482

/* Execute a transaction for functions that READ REGISTERS.

   483

 * Called by:  read_input_words()

   484

 *             read_output_words()

   485

 */

   486

static int read_registers(u8  function,

   487

                          u8  slave,

   488

                          u16 start_addr,

   489

                          u16 count,

   490

                          u16 *dest,

   491

                          int dest_size,

   492

                          int ttyfd,

   493

                          int send_retries,

   494

                          u8  *error_code,

   495

                          const struct timespec *response_timeout,

   496

                          pthread_mutex_t *data_access_mutex) {

   497

  u8 *data;

   498

  u8 packet[QUERY_BUFFER_SIZE];

   499

  int response_length;

   500

  int query_length;

   501

  int temp,i;

   502

   503

  query_length = build_packet(slave, function, start_addr, count, packet);

   504

  if (query_length < 0)    return INTERNAL_ERROR;

   505

   506

  response_length = mb_transaction(packet, query_length, &data, ttyfd, 

   507

                                   send_retries, error_code, response_timeout);

   508

   509

  if (response_length  < 0)              return response_length;  

   510

  if (response_length != 3 + 2*count)    return INVALID_FRAME;  

   511

  if (data[2]         !=     2*count)    return INVALID_FRAME;

   512

   513

  if (NULL != data_access_mutex) pthread_mutex_lock(data_access_mutex);

   514

  for(i = 0; (i < (data[2]*2)) && (i < dest_size); i++ ) {

   515

    temp = data[3 + i *2] << 8;    /* copy reg hi byte to temp hi byte*/

   516

    temp = temp | data[4 + i * 2]; /* copy reg lo byte to temp lo byte*/

   517

    dest[i] = temp;

   518

  }

   519

  if (NULL != data_access_mutex) pthread_mutex_unlock(data_access_mutex);

   520

   521

  return response_length;

   522

}

   523

   524

   525

   526

/* Execute a transaction for functions that READ REGISTERS.

   527

 * return the array with the data to the calling function