1

/* File generated by Beremiz (PlugGenerate_C method of Modbus plugin) */

     2

     3

/*

     4

 * Copyright (c) 2016 Mario de Sousa (msousa@fe.up.pt)

     5

 *

     6

 * This file is part of the Modbus library for Beremiz and matiec.

     7

 *

     8

 * This Modbus library is free software: you can redistribute it and/or modify

     9

 * it under the terms of the GNU Lesser General Public License as published by

    10

 * the Free Software Foundation, either version 2 of the License, or

    11

 * (at your option) any later version.

    12

 *

    13

 * This program is distributed in the hope that it will be useful, but

    14

 * WITHOUT ANY WARRANTY; without even the implied warranty of

    15

 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser 

    16

 * General Public License for more details.

    17

 *

    18

 * You should have received a copy of the GNU Lesser General Public License

    19

 * along with this Modbus library.  If not, see <http://www.gnu.org/licenses/>.

    20

 *

    21

 * This code is made available on the understanding that it will not be

    22

 * used in safety-critical situations without a full and competent review.

    23

 */

    24

    25

    26

#include <stdio.h>

    27

#include <string.h>  /* required for memcpy() */

    28

#include <errno.h>

    29

#include <time.h>

    30

#include <signal.h>

    31

#include <unistd.h>  /* required for pause() */

    32

#include "mb_slave_and_master.h"

    33

#include "MB_%(locstr)s.h"

    34

    35

    36

#define MAX_MODBUS_ERROR_CODE 11

    37

static const char *modbus_error_messages[MAX_MODBUS_ERROR_CODE+1] = {

    38

    /* 0 */ "",                             /* un-used -> no error! */

    39

    /* 1 */ "illegal/unsuported function",

    40

    /* 2 */ "illegal data address",

    41

    /* 3 */ "illegal data value",

    42

    /* 4 */ "slave device failure",

    43

    /* 5 */ "acknowledge -> slave intends to reply later",

    44

    /* 6 */ "slave device busy",

    45

    /* 7 */ "negative acknowledge",

    46

    /* 8 */ "memory parity error",

    47

    /* 9 */ "",                             /* undefined by Modbus */

    48

    /* 10*/ "gateway path unavalilable",

    49

    /* 11*/ "gateway target device failed to respond"

    50

};    

    51

    52

    53

/* Execute a modbus client transaction/request */

    54

static int __execute_mb_request(int request_id){

    55

	switch (client_requests[request_id].mb_function){

    56

    57

	case  1: /* read coils */

    58

		return read_output_bits(client_requests[request_id].slave_id,

    59

					client_requests[request_id].address,

    60

					client_requests[request_id].count,

    61

					client_requests[request_id].coms_buffer,

    62

					(int) client_requests[request_id].count,

    63

					client_nodes[client_requests[request_id].client_node_id].mb_nd,

    64

					client_requests[request_id].retries,

    65

					&(client_requests[request_id].mb_error_code),

    66

					&(client_requests[request_id].resp_timeout),

    67

					&(client_requests[request_id].coms_buf_mutex));

    68

    69

	case  2: /* read discrete inputs */

    70

		return read_input_bits( client_requests[request_id].slave_id,

    71

					client_requests[request_id].address,

    72

					client_requests[request_id].count,

    73

					client_requests[request_id].coms_buffer,

    74

					(int) client_requests[request_id].count,

    75

					client_nodes[client_requests[request_id].client_node_id].mb_nd,

    76

					client_requests[request_id].retries,

    77

					&(client_requests[request_id].mb_error_code),

    78

					&(client_requests[request_id].resp_timeout),

    79

					&(client_requests[request_id].coms_buf_mutex));

    80

    81

	case  3: /* read holding registers */

    82

		return read_output_words(client_requests[request_id].slave_id,

    83

					client_requests[request_id].address,

    84

					client_requests[request_id].count,

    85

					client_requests[request_id].coms_buffer,

    86

					(int) client_requests[request_id].count,

    87

					client_nodes[client_requests[request_id].client_node_id].mb_nd,

    88

					client_requests[request_id].retries,

    89

					&(client_requests[request_id].mb_error_code),

    90

					&(client_requests[request_id].resp_timeout),

    91

					&(client_requests[request_id].coms_buf_mutex));

    92

    93

	case  4: /* read input registers */

    94

		return read_input_words(client_requests[request_id].slave_id,

    95

					client_requests[request_id].address,

    96

					client_requests[request_id].count,

    97

					client_requests[request_id].coms_buffer,

    98

					(int) client_requests[request_id].count,

    99

					client_nodes[client_requests[request_id].client_node_id].mb_nd,

   100

					client_requests[request_id].retries,

   101

					&(client_requests[request_id].mb_error_code),

   102

					&(client_requests[request_id].resp_timeout),

   103

					&(client_requests[request_id].coms_buf_mutex));

   104

   105

	case  5: /* write single coil */

   106

		return write_output_bit(client_requests[request_id].slave_id,

   107

					client_requests[request_id].address,

   108

					client_requests[request_id].coms_buffer[0],

   109

					client_nodes[client_requests[request_id].client_node_id].mb_nd,

   110

					client_requests[request_id].retries,

   111

					&(client_requests[request_id].mb_error_code),

   112

					&(client_requests[request_id].resp_timeout),

   113

					&(client_requests[request_id].coms_buf_mutex));

   114

   115

	case  6: /* write single register */

   116

		return write_output_word(client_requests[request_id].slave_id,

   117

					client_requests[request_id].address,

   118

					client_requests[request_id].coms_buffer[0],

   119

					client_nodes[client_requests[request_id].client_node_id].mb_nd,

   120

					client_requests[request_id].retries,

   121

					&(client_requests[request_id].mb_error_code),

   122

					&(client_requests[request_id].resp_timeout),

   123

					&(client_requests[request_id].coms_buf_mutex));

   124

   125

	case  7: break; /* function not yet supported */

   126

	case  8: break; /* function not yet supported */

   127

	case  9: break; /* function not yet supported */

   128

	case 10: break; /* function not yet supported */

   129

	case 11: break; /* function not yet supported */

   130

	case 12: break; /* function not yet supported */

   131

	case 13: break; /* function not yet supported */

   132

	case 14: break; /* function not yet supported */

   133

   134

	case 15: /* write multiple coils */

   135

		return write_output_bits(client_requests[request_id].slave_id,

   136

					 client_requests[request_id].address,

   137

					 client_requests[request_id].count,

   138

					 client_requests[request_id].coms_buffer,

   139

					 client_nodes[client_requests[request_id].client_node_id].mb_nd,

   140

					 client_requests[request_id].retries,

   141

					 &(client_requests[request_id].mb_error_code),

   142

					 &(client_requests[request_id].resp_timeout),

   143

					 &(client_requests[request_id].coms_buf_mutex));

   144

   145

	case 16: /* write multiple registers */

   146

		return write_output_words(client_requests[request_id].slave_id,

   147

					client_requests[request_id].address,

   148

					client_requests[request_id].count,

   149

					client_requests[request_id].coms_buffer,

   150

					client_nodes[client_requests[request_id].client_node_id].mb_nd,

   151

					client_requests[request_id].retries,

   152

					&(client_requests[request_id].mb_error_code),

   153

					&(client_requests[request_id].resp_timeout),

   154

					&(client_requests[request_id].coms_buf_mutex));

   155

   156

	default: break;  /* should never occur, if file generation is correct */

   157

	}

   158

   159

	fprintf(stderr, "Modbus plugin: Modbus function %%d not supported\n", request_id); /* should never occur, if file generation is correct */

   160

	return -1;

   161

}

   162

   163

   164

   165

/* pack bits from unpacked_data to packed_data */

   166

static inline int __pack_bits(u16 *unpacked_data, u16 start_addr, u16 bit_count,  u8  *packed_data) {

   167

  u8 bit;

   168

  u16 byte, coils_processed;

   169

   170

  if ((0 == bit_count) || (65535-start_addr < bit_count-1))

   171

    return -ERR_ILLEGAL_DATA_ADDRESS; /* ERR_ILLEGAL_DATA_ADDRESS defined in mb_util.h */

   172

   173

  for( byte = 0, coils_processed = 0; coils_processed < bit_count; byte++) {

   174

    packed_data[byte] = 0;

   175

    for( bit = 0x01; (bit & 0xFF) && (coils_processed < bit_count); bit <<= 1, coils_processed++ ) {

   176

      if(unpacked_data[start_addr + coils_processed])

   177

            packed_data[byte] |=  bit; /*   set bit */

   178

      else  packed_data[byte] &= ~bit; /* reset bit */

   179

    }

   180

  }

   181

  return 0;

   182

}

   183

   184

   185

/* unpack bits from packed_data to unpacked_data */

   186

static inline int __unpack_bits(u16 *unpacked_data, u16 start_addr, u16 bit_count,  u8  *packed_data) {

   187

  u8  temp, bit;

   188

  u16 byte, coils_processed;

   189

   190

  if ((0 == bit_count) || (65535-start_addr < bit_count-1))

   191

    return -ERR_ILLEGAL_DATA_ADDRESS; /* ERR_ILLEGAL_DATA_ADDRESS defined in mb_util.h */

   192

   193

  for(byte = 0, coils_processed = 0; coils_processed < bit_count; byte++) {

   194

    temp = packed_data[byte] ;

   195

    for(bit = 0x01; (bit & 0xff) && (coils_processed < bit_count); bit <<= 1, coils_processed++) {

   196

      unpacked_data[start_addr + coils_processed] = (temp & bit)?1:0;

   197

    }

   198

  }

   199

  return 0;

   200

}

   201

   202

   203

static int __read_inbits   (void *mem_map, u16 start_addr, u16 bit_count, u8  *data_bytes) {

   204

  int res = __pack_bits(((server_mem_t *)mem_map)->ro_bits, start_addr, bit_count, data_bytes);

   205

   206

  if (res >= 0) {

   207

    /* update the flag and counter of Modbus requests we have processed. */

   208

    ((server_mem_t *)mem_map)->flag_read_req_counter++;

   209

    ((server_mem_t *)mem_map)->flag_read_req_flag = 1;

   210

  }

   211

   212

  return res;

   213

}

   214

   215

static int __read_outbits  (void *mem_map, u16 start_addr, u16 bit_count, u8  *data_bytes) {

   216

  int res = __pack_bits(((server_mem_t *)mem_map)->rw_bits, start_addr, bit_count, data_bytes);

   217

   218

  if (res >= 0) {

   219

    /* update the flag and counter of Modbus requests we have processed. */

   220

    ((server_mem_t *)mem_map)->flag_read_req_counter++;

   221

    ((server_mem_t *)mem_map)->flag_read_req_flag = 1;

   222

  }

   223

   224

  return res;

   225

}

   226

   227

static int __write_outbits (void *mem_map, u16 start_addr, u16 bit_count, u8  *data_bytes) {

   228

  int res = __unpack_bits(((server_mem_t *)mem_map)->rw_bits, start_addr, bit_count, data_bytes);

   229

   230

  if (res >= 0) {

   231

    /* update the flag and counter of Modbus requests we have processed. */

   232

    ((server_mem_t *)mem_map)->flag_write_req_counter++;

   233

    ((server_mem_t *)mem_map)->flag_write_req_flag = 1;

   234

  }

   235

   236

  return res;

   237

}

   238

   239

   240

   241

   242

static int __read_inwords  (void *mem_map, u16 start_addr, u16 word_count, u16 *data_words) {

   243

   244

  if ((start_addr + word_count) > MEM_AREA_SIZE)

   245

    return -ERR_ILLEGAL_DATA_ADDRESS; /* ERR_ILLEGAL_DATA_ADDRESS defined in mb_util.h */

   246

   247

  /* update the flag and counter of Modbus requests we have processed. */

   248

  ((server_mem_t *)mem_map)->flag_read_req_counter++;

   249

  ((server_mem_t *)mem_map)->flag_read_req_flag = 1;

   250

   251

  /* use memcpy() because loop with pointers (u16 *) caused alignment problems */

   252

  memcpy(/* dest */ (void *)data_words,

   253

         /* src  */ (void *)&(((server_mem_t *)mem_map)->ro_words[start_addr]),

   254

         /* size */ word_count * 2);

   255

  return 0;

   256

}

   257

   258

   259

   260

static int __read_outwords (void *mem_map, u16 start_addr, u16 word_count, u16 *data_words) {

   261

   262

  if ((start_addr + word_count) > MEM_AREA_SIZE)

   263

    return -ERR_ILLEGAL_DATA_ADDRESS; /* ERR_ILLEGAL_DATA_ADDRESS defined in mb_util.h */

   264

   265

  /* update the flag and counter of Modbus requests we have processed. */

   266

  ((server_mem_t *)mem_map)->flag_read_req_counter++;

   267

  ((server_mem_t *)mem_map)->flag_read_req_flag = 1;

   268

   269

  /* use memcpy() because loop with pointers (u16 *) caused alignment problems */

   270

  memcpy(/* dest */ (void *)data_words,

   271

         /* src  */ (void *)&(((server_mem_t *)mem_map)->rw_words[start_addr]),

   272

         /* size */ word_count * 2);

   273

  return 0;

   274

}

   275

   276

   277

   278

   279

static int __write_outwords(void *mem_map, u16 start_addr, u16 word_count, u16 *data_words) {

   280

   281

  if ((start_addr + word_count) > MEM_AREA_SIZE)

   282

    return -ERR_ILLEGAL_DATA_ADDRESS; /* ERR_ILLEGAL_DATA_ADDRESS defined in mb_util.h */

   283

   284

  /* update the flag and counter of Modbus requests we have processed. */

   285

  ((server_mem_t *)mem_map)->flag_write_req_counter++;

   286

  ((server_mem_t *)mem_map)->flag_write_req_flag = 1;

   287

   288

  /* WARNING: The data returned in the data_words[] array is not guaranteed to be 16 bit aligned.

   289

   *           It is not therefore safe to cast it to an u16 data type.

   290

   *           The following code cannot be used. memcpy() is used instead.

   291

   */

   292

  /*

   293

  for (count = 0; count < word_count ; count++)

   294

    ((server_mem_t *)mem_map)->rw_words[count + start_addr] = data_words[count];

   295

  */

   296

  memcpy(/* dest */ (void *)&(((server_mem_t *)mem_map)->rw_words[start_addr]),

   297

         /* src  */ (void *)data_words,

   298

         /* size */ word_count * 2);

   299

  return 0;

   300

}

   301

   302

   303

   304

   305

#include <pthread.h>

   306

   307

static void *__mb_server_thread(void *_server_node)  {

   308

	server_node_t *server_node = _server_node;

   309

	mb_slave_callback_t callbacks = { 

   310

			&__read_inbits,

   311

			&__read_outbits,

   312

			&__write_outbits,

   313

			&__read_inwords,

   314

			&__read_outwords,

   315

			&__write_outwords,

   316

			(void *)&(server_node->mem_area)

   317

			};  

   318

   319

	// Enable thread cancelation. Enabled is default, but set it anyway to be safe.

   320

	pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, NULL);

   321

   322

	// mb_slave_run() should never return!

   323

	mb_slave_run(server_node->mb_nd /* nd */, callbacks, server_node->slave_id);

   324

	fprintf(stderr, "Modbus plugin: Modbus server for node %%s died unexpectedly!\n", server_node->location); /* should never occur */

   325

	return NULL;

   326

}

   327

   328

   329

#define timespec_add(ts, sec, nsec) {		\

   330

	ts.tv_sec  +=  sec;			\

   331

	ts.tv_nsec += nsec;			\

   332

	if (ts.tv_nsec >= 1000000000) {		\

   333

		ts.tv_sec  ++;			\

   334

		ts.tv_nsec -= 1000000000;	\

   335

	}					\

   336

}

   337

   338

   339

static void *__mb_client_thread(void *_index)  {

   340

	int client_node_id = (char *)_index - (char *)NULL; // Use pointer arithmetic (more portable than cast)

   341

   342

	// Enable thread cancelation. Enabled is default, but set it anyway to be safe.

   343

	pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, NULL);

   344

   345

    /* loop the communication with the client

   346

     * 

   347

         * When the client thread has difficulty communicating with remote client and/or server (network issues, for example),

   348

         * then the communications get delayed and we will fall behind in the period. 

   349

         * 

   350

         * This is OK. Note that if the condition variable were to be signaled multiple times while the client thread is inside the same

   351

         * Modbus transaction, then all those signals would be ignored.

   352

         * However, and since we keep the mutex locked during the communication cycle, it is not possible to signal the condition variable

   353

         * during that time (it is only possible while the thread is blocked during the call to pthread_cond_wait().

   354

         * 

   355

         * This means that when network issues eventually get resolved, we will NOT have a bunch of delayed activations to handle

   356

         * in quick succession (which would goble up CPU time). 

   357

         * 

   358

         * Notice that the above property is valid whether the communication cycle is run with the mutex locked, or unlocked.

   359

         * Since it makes it easier to implement the correct semantics for the other activation methods if the communication cycle

   360

         * is run with the mutex locked, then that is what we do.

   361

         * 

   362

     * Note that during all the communication cycle we will keep locked the mutex 

   363

     * (i.e. the mutex used together with the condition variable that will activate a new communication cycle)

   364

     * 

   365

     * Note that we never get to explicitly unlock this mutex. It will only be unlocked by the pthread_cond_wait()

   366

     * call at the end of the cycle.

   367

     */

   368

    pthread_mutex_lock(&(client_nodes[client_node_id].mutex));

   369

   370

	while (1) {

   371

		/*

   372

		struct timespec cur_time;

   373

		clock_gettime(CLOCK_MONOTONIC, &cur_time);

   374

		fprintf(stderr, "Modbus client thread (%%d) - new cycle (%%ld:%%ld)!\n", client_node_id, cur_time.tv_sec, cur_time.tv_nsec);

   375

		*/

   384

		int req;

   385

		for (req=0; req < NUMBER_OF_CLIENT_REQTS; req ++){

   386

			/* just do the requests belonging to the client */

   387

			if (client_requests[req].client_node_id != client_node_id)

   388

				continue;

   389

   390

            /* only do the request if:

   391

             *   - this request was explictly asked to be executed by the client program

   392

             *  OR

   393

             *   - the client thread was activated periodically

   394

             *     (in which case we execute all the requests belonging to the client node)

   395

             */

   396

            if ((client_requests[req].flag_exec_req == 0) && (client_nodes[client_requests[req].client_node_id].periodic_act == 0))

   397

                continue;

   398

   399

            /*

   400

            fprintf(stderr, "Modbus client thread (%%d): RUNNING Modbus request %%d  (periodic = %%d  flag_exec_req = %%d)\n", 

   401

                    client_node_id, req, client_nodes[client_requests[req].client_node_id].periodic_act, client_requests[req].flag_exec_req );

   402

            */

   411

   412

			int res_tmp = __execute_mb_request(req);

   413

			client_requests[req].tn_error_code = 0; // assume success

   414

			switch (res_tmp) {

   415

			  case PORT_FAILURE: {

   416

				if (res_tmp != client_nodes[client_node_id].prev_error)

   417

					fprintf(stderr, "Modbus plugin: Error connecting Modbus client %%s to remote server.\n", client_nodes[client_node_id].location);

   418

				client_nodes[client_node_id].prev_error = res_tmp;

   419

				client_requests[req].tn_error_code = 1; // error accessing IP network, or serial interface

   420

				break;

   421

			  }

   422

			  case INVALID_FRAME: {

   423

				if ((res_tmp != client_requests[req].prev_error) && (0 == client_nodes[client_node_id].prev_error))

   424

					fprintf(stderr, "Modbus plugin: Modbus client request configured at location %%s was unsuccesful. Server/slave returned an invalid/corrupted frame.\n", client_requests[req].location);

   425

				client_requests[req].prev_error = res_tmp;

   426

				client_requests[req].tn_error_code = 2; // reply received from server was an invalid frame

   427

				break;

   428

			  }

   429

			  case TIMEOUT: {

   430

				if ((res_tmp != client_requests[req].prev_error) && (0 == client_nodes[client_node_id].prev_error))

   431

					fprintf(stderr, "Modbus plugin: Modbus client request configured at location %%s timed out waiting for reply from server.\n", client_requests[req].location);

   432

				client_requests[req].prev_error = res_tmp;

   433

				client_requests[req].tn_error_code = 3; // server did not reply before timeout expired

   434

				break;

   435

			  }

   436

			  case MODBUS_ERROR: {

   437

				if (client_requests[req].prev_error != client_requests[req].mb_error_code) {

   438

					fprintf(stderr, "Modbus plugin: Modbus client request configured at location %%s was unsuccesful. Server/slave returned error code 0x%%2x", client_requests[req].location, client_requests[req].mb_error_code);

   439

					if (client_requests[req].mb_error_code <= MAX_MODBUS_ERROR_CODE ) {

   440

						fprintf(stderr, "(%%s)", modbus_error_messages[client_requests[req].mb_error_code]);

   441

						fprintf(stderr, ".\n");

   442

					}

   443

				}

   444

				client_requests[req].prev_error = client_requests[req].mb_error_code;

   445

				client_requests[req].tn_error_code = 4; // server returned a valid Modbus error frame

   446

				break;

   447

			  }

   448

			  default: {

   449

				if ((res_tmp >= 0) && (client_nodes[client_node_id].prev_error != 0)) {

   450

					fprintf(stderr, "Modbus plugin: Modbus client %%s has reconnected to server/slave.\n", client_nodes[client_node_id].location);

   451

				}

   452

				if ((res_tmp >= 0) && (client_requests[req]        .prev_error != 0)) {

   453

					fprintf(stderr, "Modbus plugin: Modbus client request configured at location %%s has succesfully resumed comunication.\n", client_requests[req].location);

   454

				}

   455

				client_nodes[client_node_id].prev_error = 0;

   456

				client_requests[req]        .prev_error = 0;

   457

				break;

   458

			  }

   459

			}

   460

   461

			/* Set the flag_tn_error_code and flag_mb_error_code that are mapped onto

   462

             * located BYTE variables, so the user program

   463

             * knows how the communication is going.

   464

             */

   465

            client_requests[req].flag_mb_error_code = client_requests[req].mb_error_code;

   466

            client_requests[req].flag_tn_error_code = client_requests[req].tn_error_code;

   467

   469

             * If the current cycle was activated by user request we reset the flag used to ask to run it

   470

             */

   471

            if (0 != client_requests[req].flag_exec_req) {

   472

                client_requests[req].flag_exec_req     = 0;

   473

                client_requests[req].flag_exec_started = 0;   

   474

            }

   475

   482

            //fprintf(stderr, "Modbus plugin: RUNNING<---> of Modbus request %%d  (periodic = %%d  flag_exec_req = %%d)\n", 

   483

            //        req, client_nodes[client_requests[req].client_node_id].periodic_act, client_requests[req].flag_exec_req );

   484

        }

   485

   486

        // Wait for signal (from timer or explicit request from user program) before starting the next cycle

   487

        {

   488

            // No need to lock the mutex. Is is already locked just before the while(1) loop.

   489

            // Read the comment there to understand why.

   490

            // pthread_mutex_lock(&(client_nodes[client_node_id].mutex));