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

/* mb_slave.c */

    25

    26

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

    27

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

    28

#include <string.h>

    29

#include <stdlib.h>

    30

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

    31

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

    32

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

    33

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

    34

    35

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

    36

#include "mb_layer1.h"

    37

#include "mb_slave.h"

    38

#include "mb_slave_private.h"

    39

    40

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

    41

    42

    43

#define modbus_write             fptr_[layer1_fin].modbus_write           

    44

#define modbus_read              fptr_[layer1_fin].modbus_read            

    45

#define modbus_init              fptr_[layer1_fin].modbus_init            

    46

#define modbus_done              fptr_[layer1_fin].modbus_done            

    47

#define modbus_connect           fptr_[layer1_fin].modbus_connect         

    48

#define modbus_listen            fptr_[layer1_fin].modbus_listen          

    49

#define modbus_close             fptr_[layer1_fin].modbus_close           

    50

#define modbus_silence_init      fptr_[layer1_fin].modbus_silence_init    

    51

#define modbus_get_min_timeout   fptr_[layer1_fin].modbus_get_min_timeout 

    52

    53

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

    54

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

    55

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

    56

                        if (fd < 0) {ttyfd = fd; layer1_fin = 0; /* use modbusTCP */}

    57

    58

    59

    60

    61

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

    62

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

    63

/**                                      **/

    64

/**         Global Variables...          **/

    65

/**                                      **/

    66

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

    67

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

    68

/* The layer 1 (RTU, ASCII, TCP) implementations will be adding some 

    69

 *  header and tail bytes (e.g. CRC) to the packet we build here. Since

    70

 *  layer1 will re-use the same buffer allocated in this slave layer 

    71

 *  (so as not to continuosly copy the same info from buffer to buffer),

    72

 *  we need to allocate more bytes than those strictly required for this

    73

 *  slave layer. Therefore, the extra_bytes parameter.

    74

 *

    75

 *  Note that we add one more extra byte to the response buffer.

    76

 *  This is because some response packets will not be starting off

    77

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

    78

 *  to guarantee that the data that is sent on the buffer is aligned

    79

 *  on even bytes (the 16 bit words!). This will allow the application

    80

 *  (layer above the one implemented in this file - i.e. the callback 

    81

 *  functions) to reference this memory as an u16 *, without producing

    82

 *  'bus error' messages in some embedded devices that do not allow

    83

 *   acessing u16 on odd numbered addresses.

    84

 */

    85

static int buff_extra_bytes_;

    86

#define RESP_BUFFER_SIZE       (MAX_L2_FRAME_LENGTH + buff_extra_bytes_ + 1)

    87

    88

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

    89

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

    90

/**                                      **/

    91

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

    92

/**                                      **/

    93

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

    94

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

    95

    96

    97

/*

    98

 * Function to determine next transaction id.

    99

 *

   100

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

   101

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

   102

 * be sending the request...

   103

 */

   104

static inline u16 next_transaction_id(void) {

   105

  static u16 next_id = 0;

   106

  return next_id++;

   107

}

   108

   109

   110

/*

   111

 * Functions to convert u16 variables

   112

 * between network and host byte order

   113

 *

   114

 * NOTE: Modbus uses MSByte first, just like

   115

 *       tcp/ip, so we could be tempted to use the htons() and

   116

 *       ntohs() functions to guarantee code portability.

   117

 *

   118

 *       However, on some embedded systems running Linux

   119

 *       these functions only work if the 16 bit words are 

   120

 *       stored on even addresses. This is not always the 

   121

 *       case in our code, so we have to define our own

   122

 *       conversion functions...

   123

 */

   124

   125

/* if using gcc, use it to determine byte order... */

   126

#ifndef __BYTE_ORDER

   127

#if defined(__GNUC__) 

   128

  /* We have GCC, which should define __LITTLE_ENDIAN__ */ 

   129

#  if defined(__LITTLE_ENDIAN__)

   130

#    define __BYTE_ORDER __LITTLE_ENDIAN

   131

#  else

   132

#    define __BYTE_ORDER __BIG_ENDIAN

   133

#  endif

   134

#endif /* __GNUC__ */ 

   135

#endif /* __BYTE_ORDER */

   136

   137

   138

/* If we still don't know byte order, try to get it from <sys/param.h> */

   139

#ifndef __BYTE_ORDER

   140

#include <sys/param.h>

   141

#endif

   142

   143

   144

#ifndef __BYTE_ORDER

   145

#  ifdef BYTE_ORDER

   146

#   if BYTE_ORDER == LITTLE_ENDIAN

   147

#    define __BYTE_ORDER __LITTLE_ENDIAN

   148

#   else

   149

#    if BYTE_ORDER == BIG_ENDIAN

   150

#      define __BYTE_ORDER __BIG_ENDIAN

   151

#    endif

   152

#   endif

   153

#  endif /* BYTE_ORDER */

   154

#endif /* __BYTE_ORDER */

   155

   156

   157

   158

   159

   160

#ifdef __BYTE_ORDER

   161

# if __BYTE_ORDER == __LITTLE_ENDIAN

   162

   163

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

   164

/* u16 conversion functions to use on little endian platforms */

   165

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

   166

   167

/* NOTE: The input parameter must be the address 

   168

 *          of an u16 passed as a pointer to u8

   169

 * 

   170

 *       We use u8 *ptr as input parameter and read both (ptr+0) and (ptr+1)

   171

 *       instead of using u16 *ptr because we sometimes receive data in packtes

   172

 *       that are not aligned on even addresses, so some compilers recognize that

   173

 *       the given odd address cannot be used as a pointer to u16 and therefore 

   174

 *       adjust the pointer by (+1) or (-1), basicaly breacking our code!

   175

 *       So, we revert to u8 pointers... for u16 values.

   176

 */

   177

static inline void mb_hton(u8 *u16_from_ptr, u8 *u16_to_ptr) {

   178

  u16_to_ptr[0] =  u16_from_ptr[1];

   179

  u16_to_ptr[1] =  u16_from_ptr[0];

   180

}

   181

#define mb_ntoh(a, b) mb_hton(a, b)

   182

   183

   184

static inline void mb_hton_count(u8 *u16_ptr, unsigned count) {

   185

  unsigned i;

   186

  for (i = 0; i < count*2; i += 2) {

   187

    /* swap the bytes around... 

   188

     *  a = a ^ b;

   189

     *  b = a ^ b;

   190

     *  a = a ^ b;

   191

     */

   192

    (u16_ptr+i)[0] ^= (u16_ptr+i)[1]; 

   193

    (u16_ptr+i)[1] ^= (u16_ptr+i)[0]; 

   194

    (u16_ptr+i)[0] ^= (u16_ptr+i)[1]; 

   195

  }

   196

}

   197

#define mb_ntoh_count(w, count) mb_hton_count(w, count)

   198

   199

   200

   201

# else

   202

#  if __BYTE_ORDER == __BIG_ENDIAN

   203

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

   204

/* u16 conversion functions to use on big endian platforms */

   205

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

   206

   207

/* We don't need to swap the bytes around! */

   208

static inline void mb_hton(u8 *u16_from_ptr, u8 *u16_to_ptr) {

   209

  u16_to_ptr[0] =  u16_from_ptr[0];

   210

  u16_to_ptr[1] =  u16_from_ptr[1];

   211

}

   212

#define mb_ntoh(a, b) mb_hton(a, b)

   213

   214

#define mb_hton_count(w, count) /* empty ! */

   215

#define mb_ntoh_count(w, count) /* empty ! */

   216

   217

   218

#  else

   219

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

   220

/* u16 conversion functions to use on generic platforms */

   221

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

   222

   223

   224

/* We can't determine endiannes at compile time, so we do it at runtime.

   225

 * With any luck the compiler will be able to determine the result of the

   226

 * comparison at compile time and end up discarding the non-used code

   227

 * and the 'if' itself from the final executable.

   228

 */

   229

   230

static union {u16 u16;

   231

              u8  u8[2];} endian_ = 0x0102;

   232

   233

   234

static inline void mb_hton(u8 *u16_from_ptr, u8 *u16_to_ptr) {

   235

  if (endian_.u8[0] == 0x01) {

   236

    /* machine is big endian -> no swapping */    

   237

    u16_to_ptr[0] =  u16_from_ptr[0];

   238

    u16_to_ptr[1] =  u16_from_ptr[1];

   239

  } else {

   240

    /* machine is little endian -> we swap bytes around */    

   241

    u16_to_ptr[0] =  u16_from_ptr[1];

   242

    u16_to_ptr[1] =  u16_from_ptr[0];

   243

  }

   244

}

   245

#define mb_ntoh(a, b) mb_hton(a, b)

   246

   247

   248

static inline void mb_hton_count(u8 *u16_ptr, unsigned count) {

   249

  unsigned i;

   250

   251

  if (endian_.u8[0] == 0x01)

   252

      /* machine is big endian. Nothing to do */

   253

      return;

   254

   255

  /* machine is little endian -> we swap bytes around */    

   256

  for (i = 0; i < count*2; i += 2) {

   257

    /* swap the bytes around... 

   258

     *  a = a ^ b;

   259

     *  b = a ^ b;

   260

     *  a = a ^ b;

   261

     */

   262

    (u16_ptr+i)[0] ^= (u16_ptr+i)[1]; 

   263

    (u16_ptr+i)[1] ^= (u16_ptr+i)[0]; 

   264

    (u16_ptr+i)[0] ^= (u16_ptr+i)[1]; 

   265

  }

   266

}

   267

#define mb_ntoh_count(w, count) mb_hton_count(w, count)

   268

   269

#  endif

   270

# endif

   271

#endif /* __BYTE_ORDER */

   272

   273

   274

   275

   276

   277

   278

   279

   280

   281

   282

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

   283

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

   284

/**                                           **/

   285

/**    Handle requests from master/client     **/

   286

/**                                           **/

   287

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

   288

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

   289

   290

   291

/* Handle functions 0x01 and 0x02 */

   292

typedef int (*read_bits_callback_t)(void *arg, u16 start_addr, u16 bit_count,  u8  *data_bytes);

   293

static int handle_read_bits (u8 *query_packet,

   294

                             u8 **resp_packet_ptr,

   295

                             u8 *error_code,

   296

                             read_bits_callback_t read_bits_callback,

   297

                             void *callback_arg

   298

                            ) {

   299

  u16 start_addr, count;

   300

  int res;

   301

  u8 *resp_packet;

   302

   303

  /* If no callback, handle as if function is not supported... */

   304

  if (read_bits_callback == NULL) 

   305

    {*error_code = ERR_ILLEGAL_FUNCTION; return -1;}

   306

   307

  /* in oprder for the data in this packet to be aligned on even numbered addresses, this 

   308

   *  response packet will start off at an odd numbered byte...

   309

   *  We therefore add 1 to the address where the packet starts.

   310

   */ 

   311

  (*resp_packet_ptr)++;

   312

  resp_packet = *resp_packet_ptr;

   313

   314

    /* NOTE:

   315

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

   316

     *  this is sent as 0 on the wire!

   317

     *  We could expect the user to specify high level addressing 

   318

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

   319

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

   320

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

   321

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

   322

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

   323

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

   324

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

   325

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

   326

     */

   327

  mb_ntoh(&(query_packet[2]), (u8 *)&start_addr);

   328

  mb_ntoh(&(query_packet[4]), (u8 *)&count); 

   329

   330

  #ifdef DEBUG

   331

  printf("handle_read_input_bits() called. slave=%d, function=%d, start_addr=%d, count=%d\n", 

   332

          query_packet[0], query_packet[1], start_addr, count);

   333

  #endif

   334

   335

  if ((count > MAX_READ_BITS) || (count < 1)) 

   336

    {*error_code = ERR_ILLEGAL_DATA_VALUE; return -1;}

   337

   338

  /* Remember, we are using addressing starting off at 0, in the start_addr variable! */

   339

  /*  This means that he highest acceptable address is 65535, when count=1 .... */

   340

  /* Note the use of 65536 in the comparison will force automatic upgrade of u16 variables! */

   341

  /*    => start_addr + count will nver overflow the u16 type!                              */

   342

  if (start_addr + count > 65536) 

   343

    {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;}

   344

   345

  /* start building response frame... */

   346

  resp_packet[0] = query_packet[0]; /* slave */ 

   347

  resp_packet[1] = query_packet[1]; /* function (either 0x01 or 0x02 ! */

   348

  resp_packet[2] = (count + 7) / 8; /* number of data bytes = ceil(count/8) */

   349

   350

  res = read_bits_callback(callback_arg, start_addr, count, &(resp_packet[3]));

   351

  if (res == -2) {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;}

   352

  if (res  <  0) {*error_code = ERR_SLAVE_DEVICE_FAILURE; return -1;}

   353

   354

  return resp_packet[2] + 3; /* packet size is data length + 3 bytes -> slave, function, count */

   355

}

   356

   357

   358

   359

/* Handle function 0x01 */

   360

int handle_read_output_bits  (u8 *query_packet, u8 **resp_packet_ptr, u8 *error_code, mb_slave_callback_t *callbacks)

   361

  {return handle_read_bits(query_packet, resp_packet_ptr, error_code, callbacks->read_outbits, callbacks->arg);}

   362

   363

/* Handle function 0x02 */

   364

int handle_read_input_bits   (u8 *query_packet, u8 **resp_packet_ptr, u8 *error_code, mb_slave_callback_t *callbacks)

   365

  {return handle_read_bits(query_packet, resp_packet_ptr, error_code, callbacks->read_inbits, callbacks->arg);}

   366

   367

   368

   369

   370

/* Handle functions 0x03 and 0x04 */

   371

typedef int (*read_words_callback_t)(void *arg, u16 start_addr, u16 word_count, u16 *data_words);

   372

static int handle_read_words (u8 *query_packet, 

   373

                              u8 **resp_packet_ptr, 

   374

                              u8 *error_code, 

   375

                              read_words_callback_t read_words_callback,

   376

                              void *callback_arg

   377

                             ) {

   378

  u16 start_addr, count;

   379

  int res;

   380

  u8 *resp_packet;

   381

   382

  /* If no callback, handle as if function is not supported... */

   383

  if (read_words_callback == NULL) 

   384

    {*error_code = ERR_ILLEGAL_FUNCTION; return -1;}

   385

   386

  /* See equivalent comment in handle_read_bits() */ 

   387

  (*resp_packet_ptr)++;

   388

  resp_packet = *resp_packet_ptr;

   389

   390

  /* See equivalent comment in handle_read_bits() */ 

   391

  mb_ntoh(&(query_packet[2]), (u8 *)&start_addr);

   392

  mb_ntoh(&(query_packet[4]), (u8 *)&count);

   393

   394

  #ifdef DEBUG

   395

  printf("handle_read_output_words() called. slave=%d, function=%d, start_addr=%d, count=%d\n", 

   396

         query_packet[0], query_packet[1], start_addr, count);

   397

  #endif

   398

   399

  if ((count > MAX_READ_REGS) || (count < 1))

   400

    {*error_code = ERR_ILLEGAL_DATA_VALUE; return -1;}

   401

   402

  /* See equivalent comment in handle_read_bits() */ 

   403

  if (start_addr + count > 65536)

   404

    {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;}

   405

   406

  /* start building response frame... */

   407

  resp_packet[0] = query_packet[0]; /* slave     */

   408

  resp_packet[1] = query_packet[1]; /* function code, either 0x03 or 0x04 !!!*/

   409

  resp_packet[2] = count * 2;       /* number of bytes of data... */

   410

   411

  res = read_words_callback(callback_arg, start_addr, count, (u16 *)&(resp_packet[3]));

   412

  if (res == -2) {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;}

   413

  if (res  <  0) {*error_code = ERR_SLAVE_DEVICE_FAILURE; return -1;}

   414

   415

  /* convert all data from host to network byte order. */

   416

  mb_hton_count(&(resp_packet[3]), count);

   417

   418

  return resp_packet[2] + 3; /* packet size is data length + 3 bytes -> slave, function, count */

   419

}

   420

   421

   422

   423

   424

/* Handle function 0x03 */

   425

int handle_read_output_words (u8 *query_packet, u8 **resp_packet_ptr, u8 *error_code, mb_slave_callback_t *callbacks) 

   426

  {return handle_read_words(query_packet, resp_packet_ptr, error_code, callbacks->read_outwords, callbacks->arg);}

   427

   428

/* Handle function 0x04 */

   429

int handle_read_input_words  (u8 *query_packet, u8 **resp_packet_ptr, u8 *error_code, mb_slave_callback_t *callbacks) 

   430

  {return handle_read_words(query_packet, resp_packet_ptr, error_code, callbacks->read_inwords, callbacks->arg);}

   431

   432

   433

   434

/* Handle function 0x05 */

   435

int handle_write_output_bit  (u8 *query_packet, u8 **resp_packet_ptr, u8 *error_code, mb_slave_callback_t *callbacks) {

   436

  u16 start_addr;

   437

  int res;

   438

  u8 *resp_packet;

   439

   440

  /* If no callback, handle as if function is not supported... */

   441

  if (callbacks->write_outbits == NULL)

   442

    {*error_code = ERR_ILLEGAL_FUNCTION; return -1;}

   443

   444

  resp_packet = *resp_packet_ptr;

   445

   446

  /* See equivalent comment in handle_read_bits() */ 

   447

  mb_ntoh(&(query_packet[2]), (u8 *)&start_addr);

   448

   449

  #ifdef DEBUG

   450

  printf("handle_write_output_bit() called. slave=%d, function=%d, start_addr=%d\n", 

   451

         query_packet[0], query_packet[1], start_addr);

   452

  #endif

   453

   454

  // byte 5 Must be 0x00, byte 4 must be 0x00 or 0xFF !!

   455

  if ( (query_packet[5] != 0) || 

   456

      ((query_packet[4] != 0) && (query_packet[4] != 0xFF)))

   457

    {*error_code = ERR_ILLEGAL_DATA_VALUE; return -1;}

   458

   459

  /* Address will always be valid, no need to check! */

   460

  // if (start_addr > 65535) {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;}

   461

   462

  /* start building response frame... */

   463

  resp_packet[0] = query_packet[0]; /* slave */ 

   464

  resp_packet[1] = query_packet[1]; /* function */ 

   465

  resp_packet[2] = query_packet[2]; /* start address - hi byte */

   466

  resp_packet[3] = query_packet[3]; /* start address - lo byte */

   467

  resp_packet[4] = query_packet[4]; /* value: 0x00 or 0xFF */

   468

  resp_packet[5] = query_packet[5]; /* value: must be 0x00 */

   469

   470

  res = (callbacks->write_outbits)(callbacks->arg, start_addr, 1, &(query_packet[4]));

   471

  if (res == -2) {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;}

   472

  if (res  <  0) {*error_code = ERR_SLAVE_DEVICE_FAILURE; return -1;}

   473

   474

  return 6; /* response packet size, including slave id in byte 0 */

   475

}

   476

   477

   478

   479

/* Handle function 0x06 */

   480

int handle_write_output_word (u8 *query_packet, u8 **resp_packet_ptr, u8 *error_code, mb_slave_callback_t *callbacks) {

   481

  u16 start_addr;

   482

  int res;

   483

  u8 *resp_packet;

   484

   485

  /* If no callback, handle as if function is not supported... */

   486

  if (callbacks->write_outwords == NULL)

   487

    {*error_code = ERR_ILLEGAL_FUNCTION; return -1;}

   488

   489

  resp_packet = *resp_packet_ptr;

   490

   491

  /* See equivalent comment in handle_read_bits() */ 

   492

  mb_ntoh(&(query_packet[2]), (u8 *)&start_addr);

   493

   494

  #ifdef DEBUG

   495

  printf("handle_write_output_word() called. slave=%d, function=%d, start_addr=%d\n", 

   496

         query_packet[0], query_packet[1], start_addr);

   497

  #endif

   498

   499

  /* Address will always be valid, no need to check! */

   500

  // if (start_addr > 65535) {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;}

   501

   502

  /* start building response frame... */

   503

  resp_packet[0] = query_packet[0]; /* slave           */

   504

  resp_packet[1] = query_packet[1]; /* function        */ 

   505

  resp_packet[2] = query_packet[2]; /* start address - hi byte */

   506

  resp_packet[3] = query_packet[3]; /* start address - lo byte */

   507

  resp_packet[4] = query_packet[4]; /* value - hi byte */

   508

  resp_packet[5] = query_packet[5]; /* value - lo byte */

   509

   510

  /* convert data from network to host byte order */

   511

  mb_ntoh_count(&(query_packet[4]), 1);

   512

   513

  res = (callbacks->write_outwords)(callbacks->arg, start_addr, 1, (u16 *)&(query_packet[4]));

   514

  if (res == -2) {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;}

   515

  if (res  <  0) {*error_code = ERR_SLAVE_DEVICE_FAILURE; return -1;}

   516

   517

  return 6; /* packet size is 6 -> slave, function, addr(2), value(2) */

   518

}

   519

   520

   521

   522

/* Handle function 0x0F */

   523

int handle_write_output_bits (u8 *query_packet, u8 **resp_packet_ptr, u8 *error_code, mb_slave_callback_t *callbacks) {