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 <time.h>

    29

#include <signal.h>

    30

#include "mb_slave_and_master.h"

    31

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

    32

    33

    34

#define MAX_MODBUS_ERROR_CODE 11

    35

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

    36

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

    37

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

    38

    /* 2 */ "illegal data address",

    39

    /* 3 */ "illegal data value",

    40

    /* 4 */ "slave device failure",

    41

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

    42

    /* 6 */ "slave device busy",

    43

    /* 7 */ "negative acknowledge",

    44

    /* 8 */ "memory parity error",

    45

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

    46

    /* 10*/ "gateway path unavalilable",

    47

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

    48

};    

    49

    50

    51

/* Execute a modbus client transaction/request */

    52

static int __execute_mb_request(int request_id){

    53

	switch (client_requests[request_id].mb_function){

    54

    55

	case  1: /* read coils */

    56

		return read_output_bits(client_requests[request_id].slave_id,

    57

					client_requests[request_id].address,

    58

					client_requests[request_id].count,

    59

					client_requests[request_id].coms_buffer,

    60

					(int) client_requests[request_id].count,

    61

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

    62

					client_requests[request_id].retries,

    63

					&(client_requests[request_id].error_code),

    64

					&(client_requests[request_id].resp_timeout),

    65

					&(client_requests[request_id].coms_buf_mutex));

    66

    67

	case  2: /* read discrete inputs */

    68

		return read_input_bits( client_requests[request_id].slave_id,

    69

					client_requests[request_id].address,

    70

					client_requests[request_id].count,

    71

					client_requests[request_id].coms_buffer,

    72

					(int) client_requests[request_id].count,

    73

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

    74

					client_requests[request_id].retries,

    75

					&(client_requests[request_id].error_code),

    76

					&(client_requests[request_id].resp_timeout),

    77

					&(client_requests[request_id].coms_buf_mutex));

    78

    79

	case  3: /* read holding registers */

    80

		return read_output_words(client_requests[request_id].slave_id,

    81

					client_requests[request_id].address,

    82

					client_requests[request_id].count,

    83

					client_requests[request_id].coms_buffer,

    84

					(int) client_requests[request_id].count,

    85

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

    86

					client_requests[request_id].retries,

    87

					&(client_requests[request_id].error_code),

    88

					&(client_requests[request_id].resp_timeout),

    89

					&(client_requests[request_id].coms_buf_mutex));

    90

    91

	case  4: /* read input registers */

    92

		return read_input_words(client_requests[request_id].slave_id,

    93

					client_requests[request_id].address,

    94

					client_requests[request_id].count,

    95

					client_requests[request_id].coms_buffer,

    96

					(int) client_requests[request_id].count,

    97

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

    98

					client_requests[request_id].retries,

    99

					&(client_requests[request_id].error_code),

   100

					&(client_requests[request_id].resp_timeout),

   101

					&(client_requests[request_id].coms_buf_mutex));

   102

   103

	case  5: /* write single coil */

   104

		return write_output_bit(client_requests[request_id].slave_id,

   105

					client_requests[request_id].address,

   106

					client_requests[request_id].coms_buffer[0],

   107

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

   108

					client_requests[request_id].retries,

   109

					&(client_requests[request_id].error_code),

   110

					&(client_requests[request_id].resp_timeout),

   111

					&(client_requests[request_id].coms_buf_mutex));

   112

   113

	case  6: /* write single register */

   114

		return write_output_word(client_requests[request_id].slave_id,

   115

					client_requests[request_id].address,

   116

					client_requests[request_id].coms_buffer[0],

   117

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

   118

					client_requests[request_id].retries,

   119

					&(client_requests[request_id].error_code),

   120

					&(client_requests[request_id].resp_timeout),

   121

					&(client_requests[request_id].coms_buf_mutex));

   122

   123

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

   124

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

   125

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

   126

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

   127

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

   128

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

   129

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

   130

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

   131

   132

	case 15: /* write multiple coils */

   133

		return write_output_bits(client_requests[request_id].slave_id,

   134

					 client_requests[request_id].address,

   135

					 client_requests[request_id].count,

   136

					 client_requests[request_id].coms_buffer,

   137

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

   138

					 client_requests[request_id].retries,

   139

					 &(client_requests[request_id].error_code),

   140

					 &(client_requests[request_id].resp_timeout),

   141

					 &(client_requests[request_id].coms_buf_mutex));

   142

   143

	case 16: /* write multiple registers */

   144

		return write_output_words(client_requests[request_id].slave_id,

   145

					client_requests[request_id].address,

   146

					client_requests[request_id].count,

   147

					client_requests[request_id].coms_buffer,

   148

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

   149

					client_requests[request_id].retries,

   150

					&(client_requests[request_id].error_code),

   151

					&(client_requests[request_id].resp_timeout),

   152

					&(client_requests[request_id].coms_buf_mutex));

   153

   154

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

   155

	}

   156

   157

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

   158

	return -1;

   159

}

   160

   161

   162

   163

/* pack bits from unpacked_data to packed_data */

   164

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

   165

  u8 bit;

   166

  u16 byte, coils_processed;

   167

   168

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

   169

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

   170

   171

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

   172

    packed_data[byte] = 0;

   173

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

   174

      if(unpacked_data[start_addr + coils_processed])

   175

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

   176

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

   177

    }

   178

  }

   179

  return 0;

   180

}

   181

   182

   183

/* unpack bits from packed_data to unpacked_data */

   184

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

   185

  u8  temp, bit;

   186

  u16 byte, coils_processed;

   187

   188

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

   189

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

   190

   191

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

   192

    temp = packed_data[byte] ;

   193

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

   194

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

   195

    }

   196

  }

   197

  return 0;

   198

}

   199

   200

   201

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

   202

  {return   __pack_bits(((server_mem_t *)mem_map)->ro_bits, start_addr, bit_count, data_bytes);}

   203

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

   204

  {return   __pack_bits(((server_mem_t *)mem_map)->rw_bits, start_addr, bit_count, data_bytes);}

   205

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

   206

  {return __unpack_bits(((server_mem_t *)mem_map)->rw_bits, start_addr, bit_count, data_bytes); }

   207

   208

   209

   210

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

   211

   212

  if ((start_addr + word_count) > MEM_AREA_SIZE)

   213

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

   214

   215

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

   216

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

   217

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

   218

         /* size */ word_count * 2);

   219

  return 0;

   220

}

   221

   222

   223

   224

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

   225

   226

  if ((start_addr + word_count) > MEM_AREA_SIZE)

   227

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

   228

   229

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

   230

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

   231

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

   232

         /* size */ word_count * 2);

   233

  return 0;

   234

}

   235

   236

   237

   238

   239

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

   240

   241

  if ((start_addr + word_count) > MEM_AREA_SIZE)

   242

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

   243

   244

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

   245

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

   246

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

   247

   */

   248

  /*

   249

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

   250

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

   251

  */

   252

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

   253

         /* src  */ (void *)data_words,

   254

         /* size */ word_count * 2);

   255

  return 0;

   256

}

   257

   258

   259

   260

   261

#include <pthread.h>

   262

   263

static void *__mb_server_thread(void *_server_node)  {

   264

	server_node_t *server_node = _server_node;

   265

	mb_slave_callback_t callbacks = { 

   266

			&__read_inbits,

   267

			&__read_outbits,

   268

			&__write_outbits,

   269

			&__read_inwords,

   270

			&__read_outwords,

   271

			&__write_outwords,

   272

			(void *)&(server_node->mem_area)

   273

			};  

   274

   275

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

   276

	pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, NULL);

   277

   278

	// mb_slave_run() should never return!

   279

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

   280

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

   281

	return NULL;

   282

}

   283

   284

   285

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

   286

	ts.tv_sec  +=  sec;			\

   287

	ts.tv_nsec += nsec;			\

   288

	if (ts.tv_nsec >= 1000000000) {		\

   289

		ts.tv_sec  ++;			\

   290

		ts.tv_nsec -= 1000000000;	\

   291

	}					\

   292

}

   293

   294

   295

static void *__mb_client_thread(void *_index)  {

   296

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

   297

	struct timespec next_cycle;

   298

	int period_sec  =  client_nodes[client_node_id].comm_period / 1000;          /* comm_period is in ms */

   299

	int period_nsec = (client_nodes[client_node_id].comm_period %%1000)*1000000; /* comm_period is in ms */

   300

   301

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

   302

	pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, NULL);

   303

   304

	// configure the timer for periodic activation

   305

    {

   306

      struct itimerspec timerspec;

   307

      timerspec.it_interval.tv_sec  = period_sec;

   308

      timerspec.it_interval.tv_nsec = period_nsec;

   309

      timerspec.it_value            = timerspec.it_interval;

   310

   311

      if (timer_settime(client_nodes[client_node_id].timer_id, 0 /* flags */, &timerspec, NULL) < 0)

   312

        fprintf(stderr, "Modbus plugin: Error configuring periodic activation timer for Modbus client %%s.\n", client_nodes[client_node_id].location);          

   313

    }

   314

   315

    /* loop the communication with the client

   316

     * 

   317

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

   318

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

   319

         * 

   320

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

   321

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

   322

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

   323

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

   324

         * 

   325

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

   326

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

   327

         * 

   328

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

   329

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

   330

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

   331

         * 

   332

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

   333

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

   334

     * 

   335

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

   336

     * call at the end of the cycle.

   337

     */

   338

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

   339

   340

	while (1) {

   341

		/*

   342

		struct timespec cur_time;

   343

		clock_gettime(CLOCK_MONOTONIC, &cur_time);

   344

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

   345

		*/

   346

		int req;

   347

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

   348

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

   349

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

   350

				continue;

   351

   352

            /* only do the request if:

   353

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

   354

             *  OR

   355

             *   - the client thread was activated periodically

   356

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

   357

             */

   358

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

   359

                continue;

   360

   361

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

   362

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

   363

   364

			int res_tmp = __execute_mb_request(req);

   365

			switch (res_tmp) {

   366

			  case PORT_FAILURE: {

   367

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

   368

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

   369

				client_nodes[client_node_id].prev_error = res_tmp;

   370

				break;

   371

			  }

   372

			  case INVALID_FRAME: {

   373

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

   374

					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);

   375

				client_requests[req].prev_error = res_tmp;

   376

				break;

   377

			  }

   378

			  case TIMEOUT: {

   379

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

   380

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

   381

				client_requests[req].prev_error = res_tmp;

   382

				break;

   383

			  }

   384

			  case MODBUS_ERROR: {

   385

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

   386

					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].error_code);

   387

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

   388

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

   389

						fprintf(stderr, ".\n");

   390

					}

   391

				}

   392

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

   393

				break;

   394

			  }

   395

			  default: {

   396

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

   397

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

   398

				}

   399

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

   400

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

   401

				}

   402

				client_nodes[client_node_id].prev_error = 0;

   403

				client_requests[req]        .prev_error = 0;

   404

				break;

   405

			  }

   406

			}

   407

   408

            /* We have just finished excuting a client transcation request.

   409

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

   410

             */

   411

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

   412

                client_requests[req].flag_exec_req     = 0;

   413

                client_requests[req].flag_exec_started = 0;   

   414

            }

   415

   416

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

   417

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

   418

        }

   419

   420

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

   421

        {

   422

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

   423

            // Read the comment there to understand why.

   424

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

   425

   426

            /* the client thread has just finished a cycle, so all the flags used to signal an activation

   427

             * and specify the activation source (periodic, user request, ...)

   428

             * get reset here, before waiting for a new activation.

   429

             */

   430

            client_nodes[client_node_id].periodic_act = 0;            

   431

            client_nodes[client_node_id].execute_req  = 0;

   432

   433

            while (client_nodes[client_node_id].execute_req == 0)

   434

                pthread_cond_wait(&(client_nodes[client_node_id].condv),

   435

                                &(client_nodes[client_node_id].mutex)); 

   436

   437

            // We run the communication cycle with the mutex locked.

   438

            // Read the comment just above the while(1) to understand why.

   439

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

   440

        }

   441

	}

   442

   443

	// humour the compiler.

   444

	return NULL;

   445

}

   446

   447

   448

   449

/* Function to activate a client node's thread */

   450

/* returns -1 if it could not send the signal */

   451

static int __signal_client_thread(int client_node_id) {

   452

    /* We TRY to signal the client thread.

   453

     * We do this because this function can be called at the end of the PLC scan cycle

   454

     * and we don't want it to block at that time.

   455

     */

   456

    if (pthread_mutex_trylock(&(client_nodes[client_node_id].mutex)) != 0)

   457

        return -1;

   458

    client_nodes[client_node_id].execute_req = 1; // tell the thread to execute

   459

    pthread_cond_signal (&(client_nodes[client_node_id].condv));

   460

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

   461

    return 0;

   462

}

   463

   464

   465

   466

/* Function that will be called whenever a client node's periodic timer expires. */

   467

/* The client node's thread will be waiting on a condition variable, so this function simply signals that 

   468

 * condition variable.

   469

 * 

   470

 * The same callback function is called by the timers of all client nodes. The id of the client node

   471

 * in question will be passed as a parameter to the call back function.

   472

 */

   473

void __client_node_timer_callback_function(union sigval sigev_value) {

   474

    /* signal the client node's condition variable on which the client node's thread should be waiting... */

   475

    /* Since the communication cycle is run with the mutex locked, we use trylock() instead of lock() */

   476

    //pthread_mutex_lock  (&(client_nodes[sigev_value.sival_int].mutex));

   477

    if (pthread_mutex_trylock (&(client_nodes[sigev_value.sival_int].mutex)) != 0)

   478

        /* we never get to signal the thread for activation. But that is OK.

   479

         * If it still in the communication cycle (during which the mutex is kept locked)

   480

         * then that means that the communication cycle is falling behing in the periodic 

   481

         * communication cycle, and we therefore need to skip a period.

   482

         */

   483

        return;

   484

    client_nodes[sigev_value.sival_int].execute_req  = 1; // tell the thread to execute

   485

    client_nodes[sigev_value.sival_int].periodic_act = 1; // tell the thread the activation was done by periodic timer   

   486

    pthread_cond_signal (&(client_nodes[sigev_value.sival_int].condv));

   487

    pthread_mutex_unlock(&(client_nodes[sigev_value.sival_int].mutex));

   488

}

   489

   490

   491

   492

int __cleanup_%(locstr)s ();

   493

int __init_%(locstr)s (int argc, char **argv){

   494

	int index;