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

static inline u16 mb_hton(u16 w) {

   168

  register u16 tmp;

   169

  tmp =  (w & 0x00FF);

   170

  tmp = ((w & 0xFF00) >> 0x08) | (tmp << 0x08);

   171

  return(tmp);

   172

}

   173

#define mb_ntoh(a) mb_hton(a)

   174

   175

static inline void mb_hton_count(u16 *w, int count) {

   176

  int i;

   177

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

   178

    /* swap the bytes around... 

   179

     *  a = a ^ b;

   180

     *  b = a ^ b;

   181

     *  a = a ^ b;

   182

     */

   183

    ((u8 *)(w+i))[0] ^= ((u8 *)(w+i))[1]; 

   184

    ((u8 *)(w+i))[1] ^= ((u8 *)(w+i))[0]; 

   185

    ((u8 *)(w+i))[0] ^= ((u8 *)(w+i))[1]; 

   186

  }

   187

}

   188

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

   189

   190

   191

   192

# else

   193

#  if __BYTE_ORDER == __BIG_ENDIAN

   194

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

   195

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

   196

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

   197

   198

 /* We do not need to swap the bytes around!  */

   199

#define mb_ntoh(val) (val)

   200

#define mb_hton(val) (val)

   201

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

   202

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

   203

   204

   205

#  else

   206

   207

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

   208

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

   209

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

   210

   211

 /* We don't know the byte order, so we revert to the 

   212

  * standard htons() and ntohs() ... 

   213

  */

   214

static inline u16 mb_hton(u16 h_value) 

   215

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

   216

   217

static inline u16 mb_ntoh(u16 m_value) 

   218

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

   219

   220

static inline void mb_hton_count(u16 *w, int count)

   221

  {int i; for (i = 0; i < count; i++) {w[i] = mb_hton(w[i]);}}

   222

   223

static inline void mb_ntoh_count(u16 *w, int count)

   224

  {int i; for (i = 0; i < count; i++) {w[i] = mb_ntoh(w[i]);}}

   225

   226

#  endif

   227

# endif

   228

#endif /* __BYTE_ORDER */

   229

   230

   231

   232

   233

/* Safe versions of the conversion functions!

   234

 *

   235

 * Note that these functions always work, whatever the endiannes

   236

 * of the machine that executes it!

   237

 *

   238

 * It is also safe because the resulting value may be stored

   239

 * on an odd address even on machines that do not allow directly

   240

 * accessing u16 bit words on odd addresses.

   241

 */

   242

static inline int mb_hton_safe(u16 from, u16 *to_ptr) {

   243

  ((u8 *)to_ptr)[1] =  (from & 0x00FF);

   244

  ((u8 *)to_ptr)[0] = ((from & 0xFF00) >> 0x08);

   245

  return 0;

   246

}

   247

   248

#define mb_ntoh_safe(a, b) mb_hton_safe(a, b)

   249

   250

   251

/*  return Most Significant Byte of value; */

   252

static inline u8 msb(u16 value) 

   253

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

   254

   255

/*  return Least Significant Byte of value; */

   256

static inline u8 lsb(u16 value) 

   257

  {return value & 0xFF;}

   258

   259

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

   260

   261

   262

   263

   264

   265

   266

   267

   268

   269

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

   270

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

   271

/**                                           **/

   272

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

   273

/**                                           **/

   274

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

   275

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

   276

   277

   278

/* Handle functions 0x01 and 0x02 */

   279

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

   280

static int handle_read_bits (u8 *query_packet,

   281

                             u8 **resp_packet_ptr,

   282

                             u8 *error_code,

   283

                             read_bits_callback_t read_bits_callback,

   284

                             void *callback_arg

   285

                            ) {

   286

  u16 start_addr, count;

   287

  int res;

   288

  u8 *resp_packet;

   289

   290

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

   291

  if (read_bits_callback == NULL) 

   292

    {*error_code = ERR_ILLEGAL_FUNCTION; return -1;}

   293

   294

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

   295

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

   296

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

   297

   */ 

   298

  (*resp_packet_ptr)++;

   299

  resp_packet = *resp_packet_ptr;

   300

   301

    /* NOTE:

   302

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

   303

     *  this is sent as 0 on the wire!

   304

     *  We could expect the user to specify high level addressing 

   305

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

   306

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

   307

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

   308

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

   309

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

   310

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

   311

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

   312

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

   313

     */

   314

  /* start_addr = mb_ntoh(u16_v(query_packet[2])) + 1; */

   319

   320

  #ifdef DEBUG

   321

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

   322

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

   323

  #endif

   324

   325

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

   326

    {*error_code = ERR_ILLEGAL_DATA_VALUE; return -1;}

   327

   328

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

   329

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

   330

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

   331

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

   332

  if (start_addr + count > 65536) 

   333

    {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;}

   334

   335

  /* start building response frame... */

   336

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

   337

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

   338

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

   339

   340

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

   341

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

   342

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

   343

   344

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

   345

}

   346

   347

   348

   349

/* Handle function 0x01 */

   350

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

   351

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

   352

   353

/* Handle function 0x02 */

   354

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

   355

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

   356

   357

   358

   359

   360

/* Handle functions 0x03 and 0x04 */

   361

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

   362

static int handle_read_words (u8 *query_packet, 

   363

                              u8 **resp_packet_ptr, 

   364

                              u8 *error_code, 

   365

                              read_words_callback_t read_words_callback,

   366

                              void *callback_arg

   367

                             ) {

   368

  u16 start_addr, count;

   369

  int res;

   370

  u8 *resp_packet;

   371

   372

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

   373

  if (read_words_callback == NULL) 

   374

    {*error_code = ERR_ILLEGAL_FUNCTION; return -1;}

   375

   376

  /* See equivalent comment in handle_read_bits() */ 

   377

  (*resp_packet_ptr)++;

   378

  resp_packet = *resp_packet_ptr;

   379

   380

  /* See equivalent comment in handle_read_bits() */ 

   385

   386

  #ifdef DEBUG

   387

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

   388

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

   389

  #endif

   390

   391

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

   392

    {*error_code = ERR_ILLEGAL_DATA_VALUE; return -1;}

   393

   394

  /* See equivalent comment in handle_read_bits() */ 

   395

  if (start_addr + count > 65536)

   396

    {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;}

   397

   398

  /* start building response frame... */

   399

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

   400

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

   401

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

   402

   403

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

   404

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

   405

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

   406

   407

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

   408

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

   409

   410

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

   411

}

   412

   413

   414

   415

   416

/* Handle function 0x03 */

   417

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

   418

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

   419

   420

/* Handle function 0x04 */

   421

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

   422

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

   423

   424

   425

   426

/* Handle function 0x05 */

   427

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

   428

  u16 start_addr;

   429

  int res;

   430

  u8 *resp_packet;

   431

   432

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

   433

  if (callbacks->write_outbits == NULL)

   434

    {*error_code = ERR_ILLEGAL_FUNCTION; return -1;}

   435

   436

  resp_packet = *resp_packet_ptr;

   437

   438

  /* See equivalent comment in handle_read_bits() */ 

   441

   442

  #ifdef DEBUG

   443

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

   444

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

   445

  #endif

   446

   447

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

   448

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

   449

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

   450

    {*error_code = ERR_ILLEGAL_DATA_VALUE; return -1;}

   451

   452

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

   453

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

   454

   455

  /* start building response frame... */

   456

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

   457

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

   458

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

   459

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

   460

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

   461

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

   462

   463

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

   464

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

   465

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

   466

   467

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

   468

}

   469

   470

   471

   472

/* Handle function 0x06 */

   473

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

   474

  u16 start_addr;

   475

  int res;

   476

  u8 *resp_packet;

   477

   478

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

   479

  if (callbacks->write_outwords == NULL)

   480

    {*error_code = ERR_ILLEGAL_FUNCTION; return -1;}

   481

   482

  resp_packet = *resp_packet_ptr;

   483

   484

  /* See equivalent comment in handle_read_bits() */ 

   488

  #ifdef DEBUG

   489

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

   490

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

   491

  #endif

   492

   493

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

   494

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

   495

   496

  /* start building response frame... */

   497

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

   498

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

   499

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

   500

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

   501

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

   502

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

   503

   504

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

   505

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

   506

   507

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

   508

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

   509

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

   510

   511

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

   512

}

   513

   514

   515

   516

/* Handle function 0x0F */

   517

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

   518

  u16 start_addr, count;

   519

  int res;

   520

  u8 *resp_packet;

   521

   522

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

   523

  if (callbacks->write_outbits == NULL)

   524

    {*error_code = ERR_ILLEGAL_FUNCTION; return -1;}

   525

   526

  resp_packet = *resp_packet_ptr;

   527

   528

  /* See equivalent comment in handle_read_bits() */