src/modbus.c

   1 /*
   2  * Copyright © 2001-2011 Stéphane Raimbault <stephane.raimbault@gmail.com>
   3  *
   4  * This library is free software; you can redistribute it and/or
   5  * modify it under the terms of the GNU Lesser General Public
   6  * License as published by the Free Software Foundation; either
   7  * version 2.1 of the License, or (at your option) any later version.
   8  *
   9  * This library is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  12  * Lesser General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU Lesser General Public
  15  * License along with this library; if not, write to the Free Software
  16  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
  17  *
  18  *
  19  * This library implements the Modbus protocol.
  20  * http://libmodbus.org/
  21  */
  22
  23 #include <stdio.h>
  24 #include <string.h>
  25 #include <stdlib.h>
  26 #include <errno.h>
  27 #include <limits.h>
  28 #include <time.h>
  29
  30 #include <config.h>
  31
  32 #include "modbus.h"
  33 #include "modbus-private.h"
  34
  35 /* Internal use */
  36 #define MSG_LENGTH_UNDEFINED -1
  37
  38 /* Exported version */
  39 const unsigned int libmodbus_version_major = LIBMODBUS_VERSION_MAJOR;
  40 const unsigned int libmodbus_version_minor = LIBMODBUS_VERSION_MINOR;
  41 const unsigned int libmodbus_version_micro = LIBMODBUS_VERSION_MICRO;
  42
  43 /* Max between RTU and TCP max adu length (so TCP) */
  44 #define MAX_MESSAGE_LENGTH 260
  45
  46 /* 3 steps are used to parse the query */
  47 typedef enum {
  48     _STEP_FUNCTION,
  49     _STEP_META,
  50     _STEP_DATA
  51 } _step_t;
  52
  53 const char *modbus_strerror(int errnum) {
  54     switch (errnum) {
  55     case EMBXILFUN:
  56         return "Illegal function";
  57     case EMBXILADD:
  58         return "Illegal data address";
  59     case EMBXILVAL:
  60         return "Illegal data value";
  61     case EMBXSFAIL:
  62         return "Slave device or server failure";
  63     case EMBXACK:
  64         return "Acknowledge";
  65     case EMBXSBUSY:
  66         return "Slave device or server is busy";
  67     case EMBXNACK:
  68         return "Negative acknowledge";
  69     case EMBXMEMPAR:
  70         return "Memory parity error";
  71     case EMBXGPATH:
  72         return "Gateway path unavailable";
  73     case EMBXGTAR:
  74         return "Target device failed to respond";
  75     case EMBBADCRC:
  76         return "Invalid CRC";
  77     case EMBBADDATA:
  78         return "Invalid data";
  79     case EMBBADEXC:
  80         return "Invalid exception code";
  81     case EMBMDATA:
  82         return "Too many data";
  83     case EMBBADSLAVE:
  84         return "Response not from requested slave";
  85     default:
  86         return strerror(errnum);
  87     }
  88 }
  89
  90 void _error_print(modbus_t *ctx, const char *context)
  91 {
  92     if (ctx->debug) {
  93         fprintf(stderr, "ERROR %s", modbus_strerror(errno));
  94         if (context != NULL) {
  95             fprintf(stderr, ": %s\n", context);
  96         } else {
  97             fprintf(stderr, "\n");
  98         }
  99     }
 100 }
 101
 102 int _sleep_and_flush(modbus_t *ctx)
 103 {
 104 #ifdef _WIN32
 105     /* usleep doesn't exist on Windows */
 106     Sleep((ctx->response_timeout.tv_sec * 1000) +
 107           (ctx->response_timeout.tv_usec / 1000));
 108 #else
 109     /* usleep source code */
 110     struct timespec request, remaining;
 111     request.tv_sec = ctx->response_timeout.tv_sec;
 112     request.tv_nsec = ((long int)ctx->response_timeout.tv_usec % 1000000)
 113         * 1000;
 114     while (nanosleep(&request, &remaining) == -1 && errno == EINTR)
 115         request = remaining;
 116 #endif
 117     return modbus_flush(ctx);
 118 }
 119
 120 int modbus_flush(modbus_t *ctx)
 121 {
 122     int rc = ctx->backend->flush(ctx);
 123     if (rc != -1 && ctx->debug) {
 124         printf("%d bytes flushed\n", rc);
 125     }
 126     return rc;
 127 }
 128
 129 /* Computes the length of the expected response */
 130 static unsigned int compute_response_length_from_request(modbus_t *ctx, uint8_t *req)
 131 {
 132     int length;
 133     const int offset = ctx->backend->header_length;
 134
 135     switch (req[offset]) {
 136     case _FC_READ_COILS:
 137     case _FC_READ_DISCRETE_INPUTS: {
 138         /* Header + nb values (code from write_bits) */
 139         int nb = (req[offset + 3] << 8) | req[offset + 4];
 140         length = 2 + (nb / 8) + ((nb % 8) ? 1 : 0);
 141     }
 142         break;
 143     case _FC_WRITE_AND_READ_REGISTERS:
 144     case _FC_READ_HOLDING_REGISTERS:
 145     case _FC_READ_INPUT_REGISTERS:
 146         /* Header + 2 * nb values */
 147         length = 2 + 2 * (req[offset + 3] << 8 | req[offset + 4]);
 148         break;
 149     case _FC_READ_EXCEPTION_STATUS:
 150         length = 3;
 151         break;
 152     case _FC_REPORT_SLAVE_ID:
 153         /* The response is device specific (the header provides the
 154            length) */
 155         return MSG_LENGTH_UNDEFINED;
 156     default:
 157         length = 5;
 158     }
 159
 160     return offset + length + ctx->backend->checksum_length;
 161 }
 162
 163 /* Sends a request/response */
 164 static int send_msg(modbus_t *ctx, uint8_t *msg, int msg_length)
 165 {
 166     int rc;
 167     int i;
 168
 169     msg_length = ctx->backend->send_msg_pre(msg, msg_length);
 170
 171     if (ctx->debug) {
 172         for (i = 0; i < msg_length; i++)
 173             printf("[%.2X]", msg[i]);
 174         printf("\n");
 175     }
 176
 177     /* In recovery mode, the write command will be issued until to be
 178        successful! Disabled by default. */
 179     do {
 180         rc = ctx->backend->send(ctx, msg, msg_length);
 181         if (rc == -1) {
 182             _error_print(ctx, NULL);
 183             if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_LINK) {
 184                 int saved_errno = errno;
 185
 186                 if ((errno == EBADF || errno == ECONNRESET || errno == EPIPE)) {
 187                     modbus_close(ctx);
 188                     modbus_connect(ctx);
 189                 } else {
 190                     _sleep_and_flush(ctx);
 191                 }
 192                 errno = saved_errno;
 193             }
 194         }
 195     } while ((ctx->error_recovery & MODBUS_ERROR_RECOVERY_LINK) &&
 196              rc == -1);
 197
 198     if (rc > 0 && rc != msg_length) {
 199         errno = EMBBADDATA;
 200         return -1;
 201     }
 202
 203     return rc;
 204 }
 205
 206 int modbus_send_raw_request(modbus_t *ctx, uint8_t *raw_req, int raw_req_length)
 207 {
 208     sft_t sft;
 209     uint8_t req[MAX_MESSAGE_LENGTH];
 210     int req_length;
 211
 212     if (raw_req_length < 2) {
 213         /* The raw request must contain function and slave at least */
 214         errno = EINVAL;
 215         return -1;
 216     }
 217
 218     sft.slave = raw_req[0];
 219     sft.function = raw_req[1];
 220     /* The t_id is left to zero */
 221     sft.t_id = 0;
 222     /* This response function only set the header so it's convenient here */
 223     req_length = ctx->backend->build_response_basis(&sft, req);
 224
 225     if (raw_req_length > 2) {
 226         /* Copy data after function code */
 227         memcpy(req + req_length, raw_req + 2, raw_req_length - 2);
 228         req_length += raw_req_length - 2;
 229     }
 230
 231     return send_msg(ctx, req, req_length);
 232 }
 233
 234 /*
 235     ---------- Request     Indication ----------
 236     | Client | ---------------------->| Server |
 237     ---------- Confirmation  Response ----------
 238 */
 239
 240 typedef enum {
 241     /* Request message on the server side */
 242     MSG_INDICATION,
 243     /* Request message on the client side */
 244     MSG_CONFIRMATION
 245 } msg_type_t;
 246
 247 /* Computes the length to read after the function received */
 248 static uint8_t compute_meta_length_after_function(int function,
 249                                                   msg_type_t msg_type)
 250 {
 251     int length;
 252
 253     if (msg_type == MSG_INDICATION) {
 254         if (function <= _FC_WRITE_SINGLE_REGISTER) {
 255             length = 4;
 256         } else if (function == _FC_WRITE_MULTIPLE_COILS ||
 257                    function == _FC_WRITE_MULTIPLE_REGISTERS) {
 258             length = 5;
 259         } else if (function == _FC_WRITE_AND_READ_REGISTERS) {
 260             length = 9;
 261         } else {
 262             /* _FC_READ_EXCEPTION_STATUS, _FC_REPORT_SLAVE_ID */
 263             length = 0;
 264         }
 265     } else {
 266         /* MSG_CONFIRMATION */
 267         switch (function) {
 268         case _FC_WRITE_SINGLE_COIL:
 269         case _FC_WRITE_SINGLE_REGISTER:
 270         case _FC_WRITE_MULTIPLE_COILS:
 271         case _FC_WRITE_MULTIPLE_REGISTERS:
 272             length = 4;
 273             break;
 274         default:
 275             length = 1;
 276         }
 277     }
 278
 279     return length;
 280 }
 281
 282 /* Computes the length to read after the meta information (address, count, etc) */
 283 static int compute_data_length_after_meta(modbus_t *ctx, uint8_t *msg,
 284                                           msg_type_t msg_type)
 285 {
 286     int function = msg[ctx->backend->header_length];
 287     int length;
 288
 289     if (msg_type == MSG_INDICATION) {
 290         switch (function) {
 291         case _FC_WRITE_MULTIPLE_COILS:
 292         case _FC_WRITE_MULTIPLE_REGISTERS:
 293             length = msg[ctx->backend->header_length + 5];
 294             break;
 295         case _FC_WRITE_AND_READ_REGISTERS:
 296             length = msg[ctx->backend->header_length + 9];
 297             break;
 298         default:
 299             length = 0;
 300         }
 301     } else {
 302         /* MSG_CONFIRMATION */
 303         if (function <= _FC_READ_INPUT_REGISTERS ||
 304             function == _FC_REPORT_SLAVE_ID ||
 305             function == _FC_WRITE_AND_READ_REGISTERS) {
 306             length = msg[ctx->backend->header_length + 1];
 307         } else {
 308             length = 0;
 309         }
 310     }
 311
 312     length += ctx->backend->checksum_length;
 313
 314     return length;
 315 }
 316
 317
 318 /* Waits a response from a modbus server or a request from a modbus client.
 319    This function blocks if there is no replies (3 timeouts).
 320
 321    The function shall return the number of received characters and the received
 322    message in an array of uint8_t if successful. Otherwise it shall return -1
 323    and errno is set to one of the values defined below:
 324    - ECONNRESET
 325    - EMBBADDATA
 326    - EMBUNKEXC
 327    - ETIMEDOUT
 328    - read() or recv() error codes
 329 */
 330
 331 static int receive_msg(modbus_t *ctx, uint8_t *msg, msg_type_t msg_type)
 332 {
 333     int rc;
 334     fd_set rfds;
 335     struct timeval tv;
 336     struct timeval *p_tv;
 337     int length_to_read;
 338     int msg_length = 0;
 339     _step_t step;
 340
 341     if (ctx->debug) {
 342         if (msg_type == MSG_INDICATION) {
 343             printf("Waiting for a indication...\n");
 344         } else {
 345             printf("Waiting for a confirmation...\n");
 346         }
 347     }
 348
 349     /* Add a file descriptor to the set */
 350     FD_ZERO(&rfds);
 351     FD_SET(ctx->s, &rfds);
 352
 353     /* We need to analyse the message step by step.  At the first step, we want
 354      * to reach the function code because all packets contain this
 355      * information. */
 356     step = _STEP_FUNCTION;
 357     length_to_read = ctx->backend->header_length + 1;
 358
 359     if (msg_type == MSG_INDICATION) {
 360         /* Wait for a message, we don't know when the message will be
 361          * received */
 362         p_tv = NULL;
 363     } else {
 364         tv.tv_sec = ctx->response_timeout.tv_sec;
 365         tv.tv_usec = ctx->response_timeout.tv_usec;
 366         p_tv = &tv;
 367     }
 368
 369     while (length_to_read != 0) {
 370         rc = ctx->backend->select(ctx, &rfds, p_tv, length_to_read);
 371         if (rc == -1) {
 372             _error_print(ctx, "select");
 373             if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_LINK) {
 374                 int saved_errno = errno;
 375
 376                 if (errno == ETIMEDOUT) {
 377                     _sleep_and_flush(ctx);
 378                 } else if (errno == EBADF) {
 379                     modbus_close(ctx);
 380                     modbus_connect(ctx);
 381                 }
 382                 errno = saved_errno;
 383             }
 384             return -1;
 385         }
 386
 387         rc = ctx->backend->recv(ctx, msg + msg_length, length_to_read);
 388         if (rc == 0) {
 389             errno = ECONNRESET;
 390             rc = -1;
 391         }
 392
 393         if (rc == -1) {
 394             _error_print(ctx, "read");
 395             if ((ctx->error_recovery & MODBUS_ERROR_RECOVERY_LINK) &&
 396                 (errno == ECONNRESET || errno == ECONNREFUSED ||
 397                  errno == EBADF)) {
 398                 int saved_errno = errno;
 399                 modbus_close(ctx);
 400                 modbus_connect(ctx);
 401                 /* Could be removed by previous calls */
 402                 errno = saved_errno;
 403             }
 404             return -1;
 405         }
 406
 407         /* Display the hex code of each character received */
 408         if (ctx->debug) {
 409             int i;
 410             for (i=0; i < rc; i++)
 411                 printf("<%.2X>", msg[msg_length + i]);
 412         }
 413
 414         /* Sums bytes received */
 415         msg_length += rc;
 416         /* Computes remaining bytes */
 417         length_to_read -= rc;
 418
 419         if (length_to_read == 0) {
 420             switch (step) {
 421             case _STEP_FUNCTION:
 422                 /* Function code position */
 423                 length_to_read = compute_meta_length_after_function(
 424                     msg[ctx->backend->header_length],
 425                     msg_type);
 426                 if (length_to_read != 0) {
 427                     step = _STEP_META;
 428                     break;
 429                 } /* else switches straight to the next step */
 430             case _STEP_META:
 431                 length_to_read = compute_data_length_after_meta(
 432                     ctx, msg, msg_type);
 433                 if ((msg_length + length_to_read) > ctx->backend->max_adu_length) {
 434                     errno = EMBBADDATA;
 435                     _error_print(ctx, "too many data");
 436                     return -1;
 437                 }
 438                 step = _STEP_DATA;
 439                 break;
 440             default:
 441                 break;
 442             }
 443         }
 444
 445         if (length_to_read > 0 && ctx->byte_timeout.tv_sec != -1) {
 446             /* If there is no character in the buffer, the allowed timeout
 447                interval between two consecutive bytes is defined by
 448                byte_timeout */
 449             tv.tv_sec = ctx->byte_timeout.tv_sec;
 450             tv.tv_usec = ctx->byte_timeout.tv_usec;
 451             p_tv = &tv;
 452         }
 453     }
 454
 455     if (ctx->debug)
 456         printf("\n");
 457
 458     return ctx->backend->check_integrity(ctx, msg, msg_length);
 459 }
 460
 461 /* Receive the request from a modbus master */
 462 int modbus_receive(modbus_t *ctx, uint8_t *req)
 463 {
 464     return receive_msg(ctx, req, MSG_INDICATION);
 465 }
 466
 467 /* Receives the confirmation.
 468
 469    The function shall store the read response in rsp and return the number of
 470    values (bits or words). Otherwise, its shall return -1 and errno is set.
 471
 472    The function doesn't check the confirmation is the expected response to the
 473    initial request.
 474 */
 475 int modbus_receive_confirmation(modbus_t *ctx, uint8_t *rsp)
 476 {
 477     return receive_msg(ctx, rsp, MSG_CONFIRMATION);
 478 }
 479
 480 static int check_confirmation(modbus_t *ctx, uint8_t *req,
 481                               uint8_t *rsp, int rsp_length)
 482 {
 483     int rc;
 484     int rsp_length_computed;
 485     const int offset = ctx->backend->header_length;
 486
 487     if (ctx->backend->pre_check_confirmation) {
 488         rc = ctx->backend->pre_check_confirmation(ctx, req, rsp, rsp_length);
 489         if (rc == -1) {
 490             if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_PROTOCOL) {
 491                 _sleep_and_flush(ctx);
 492             }
 493             return -1;
 494         }
 495     }
 496
 497     rsp_length_computed = compute_response_length_from_request(ctx, req);
 498
 499     /* Check length */
 500     if (rsp_length == rsp_length_computed ||
 501         rsp_length_computed == MSG_LENGTH_UNDEFINED) {
 502         int req_nb_value;
 503         int rsp_nb_value;
 504         const int function = rsp[offset];
 505
 506         /* Check function code */
 507         if (function != req[offset]) {
 508             if (ctx->debug) {
 509                 fprintf(stderr,
 510                         "Received function not corresponding to the request (%d != %d)\n",
 511                         function, req[offset]);
 512             }
 513             if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_PROTOCOL) {
 514                 _sleep_and_flush(ctx);
 515             }
 516             errno = EMBBADDATA;
 517             return -1;
 518         }
 519
 520         /* Check the number of values is corresponding to the request */
 521         switch (function) {
 522         case _FC_READ_COILS:
 523         case _FC_READ_DISCRETE_INPUTS:
 524             /* Read functions, 8 values in a byte (nb
 525              * of values in the request and byte count in
 526              * the response. */
 527             req_nb_value = (req[offset + 3] << 8) + req[offset + 4];
 528             req_nb_value = (req_nb_value / 8) + ((req_nb_value % 8) ? 1 : 0);
 529             rsp_nb_value = rsp[offset + 1];
 530             break;
 531         case _FC_WRITE_AND_READ_REGISTERS:
 532         case _FC_READ_HOLDING_REGISTERS:
 533         case _FC_READ_INPUT_REGISTERS:
 534             /* Read functions 1 value = 2 bytes */
 535             req_nb_value = (req[offset + 3] << 8) + req[offset + 4];
 536             rsp_nb_value = (rsp[offset + 1] / 2);
 537             break;
 538         case _FC_WRITE_MULTIPLE_COILS:
 539         case _FC_WRITE_MULTIPLE_REGISTERS:
 540             /* N Write functions */
 541             req_nb_value = (req[offset + 3] << 8) + req[offset + 4];
 542             rsp_nb_value = (rsp[offset + 3] << 8) | rsp[offset + 4];
 543             break;
 544         case _FC_REPORT_SLAVE_ID:
 545             /* Report slave ID (bytes received) */
 546             req_nb_value = rsp_nb_value = rsp[offset + 1];
 547             break;
 548         default:
 549             /* 1 Write functions & others */
 550             req_nb_value = rsp_nb_value = 1;
 551         }
 552
 553         if (req_nb_value == rsp_nb_value) {
 554             rc = rsp_nb_value;
 555         } else {
 556             if (ctx->debug) {
 557                 fprintf(stderr,
 558                         "Quantity not corresponding to the request (%d != %d)\n",
 559                         rsp_nb_value, req_nb_value);
 560             }
 561
 562             if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_PROTOCOL) {
 563                 _sleep_and_flush(ctx);
 564             }
 565
 566             errno = EMBBADDATA;
 567             rc = -1;
 568         }
 569     } else if (rsp_length == (offset + 2 + ctx->backend->checksum_length) &&
 570                req[offset] == (rsp[offset] - 0x80)) {
 571         /* EXCEPTION CODE RECEIVED */
 572
 573         int exception_code = rsp[offset + 1];
 574         if (exception_code < MODBUS_EXCEPTION_MAX) {
 575             errno = MODBUS_ENOBASE + exception_code;
 576         } else {
 577             errno = EMBBADEXC;
 578         }
 579         _error_print(ctx, NULL);
 580         rc = -1;
 581     } else {
 582         if (ctx->debug) {
 583             fprintf(stderr,
 584                     "Message length not corresponding to the computed length (%d != %d)\n",
 585                     rsp_length, rsp_length_computed);
 586         }
 587         if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_PROTOCOL) {
 588             _sleep_and_flush(ctx);
 589         }
 590         errno = EMBBADDATA;
 591         rc = -1;
 592     }
 593
 594     return rc;
 595 }
 596
 597 static int response_io_status(int address, int nb,
 598                               uint8_t *tab_io_status,
 599                               uint8_t *rsp, int offset)
 600 {
 601     int shift = 0;
 602     int byte = 0;
 603     int i;
 604
 605     for (i = address; i < address+nb; i++) {
 606         byte |= tab_io_status[i] << shift;
 607         if (shift == 7) {
 608             /* Byte is full */
 609             rsp[offset++] = byte;
 610             byte = shift = 0;
 611         } else {
 612             shift++;
 613         }
 614     }
 615
 616     if (shift != 0)
 617         rsp[offset++] = byte;
 618
 619     return offset;
 620 }
 621
 622 /* Build the exception response */
 623 static int response_exception(modbus_t *ctx, sft_t *sft,
 624                               int exception_code, uint8_t *rsp)
 625 {
 626     int rsp_length;
 627
 628     sft->function = sft->function + 0x80;
 629     rsp_length = ctx->backend->build_response_basis(sft, rsp);
 630
 631     /* Positive exception code */
 632     rsp[rsp_length++] = exception_code;
 633
 634     return rsp_length;
 635 }
 636
 637 /* Send a response to the received request.
 638    Analyses the request and constructs a response.
 639
 640    If an error occurs, this function construct the response
 641    accordingly.
 642 */
 643 int modbus_reply(modbus_t *ctx, const uint8_t *req,
 644                  int req_length, modbus_mapping_t *mb_mapping)
 645 {
 646     int offset = ctx->backend->header_length;
 647     int slave = req[offset - 1];
 648     int function = req[offset];
 649     uint16_t address = (req[offset + 1] << 8) + req[offset + 2];
 650     uint8_t rsp[MAX_MESSAGE_LENGTH];
 651     int rsp_length = 0;
 652     sft_t sft;
 653
 654     if (ctx->backend->filter_request(ctx, slave) == 1) {
 655         /* Filtered */
 656         return 0;
 657     }
 658
 659     sft.slave = slave;
 660     sft.function = function;
 661     sft.t_id = ctx->backend->prepare_response_tid(req, &req_length);
 662
 663     switch (function) {
 664     case _FC_READ_COILS: {
 665         int nb = (req[offset + 3] << 8) + req[offset + 4];
 666
 667         if ((address + nb) > mb_mapping->nb_bits) {
 668             if (ctx->debug) {
 669                 fprintf(stderr, "Illegal data address %0X in read_bits\n",
 670                         address + nb);
 671             }
 672             rsp_length = response_exception(
 673                 ctx, &sft,
 674                 MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp);
 675         } else {
 676             rsp_length = ctx->backend->build_response_basis(&sft, rsp);
 677             rsp[rsp_length++] = (nb / 8) + ((nb % 8) ? 1 : 0);
 678             rsp_length = response_io_status(address, nb,
 679                                             mb_mapping->tab_bits,
 680                                             rsp, rsp_length);
 681         }
 682     }
 683         break;
 684     case _FC_READ_DISCRETE_INPUTS: {
 685         /* Similar to coil status (but too many arguments to use a
 686          * function) */
 687         int nb = (req[offset + 3] << 8) + req[offset + 4];
 688
 689         if ((address + nb) > mb_mapping->nb_input_bits) {
 690             if (ctx->debug) {
 691                 fprintf(stderr, "Illegal data address %0X in read_input_bits\n",
 692                         address + nb);
 693             }
 694             rsp_length = response_exception(
 695                 ctx, &sft,
 696                 MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp);
 697         } else {
 698             rsp_length = ctx->backend->build_response_basis(&sft, rsp);
 699             rsp[rsp_length++] = (nb / 8) + ((nb % 8) ? 1 : 0);
 700             rsp_length = response_io_status(address, nb,
 701                                             mb_mapping->tab_input_bits,
 702                                             rsp, rsp_length);
 703         }
 704     }
 705         break;
 706     case _FC_READ_HOLDING_REGISTERS: {
 707         int nb = (req[offset + 3] << 8) + req[offset + 4];
 708
 709         if ((address + nb) > mb_mapping->nb_registers) {
 710             if (ctx->debug) {
 711                 fprintf(stderr, "Illegal data address %0X in read_registers\n",
 712                         address + nb);
 713             }
 714             rsp_length = response_exception(
 715                 ctx, &sft,
 716                 MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp);
 717         } else {
 718             int i;
 719
 720             rsp_length = ctx->backend->build_response_basis(&sft, rsp);
 721             rsp[rsp_length++] = nb << 1;
 722             for (i = address; i < address + nb; i++) {
 723                 rsp[rsp_length++] = mb_mapping->tab_registers[i] >> 8;
 724                 rsp[rsp_length++] = mb_mapping->tab_registers[i] & 0xFF;
 725             }
 726         }
 727     }
 728         break;
 729     case _FC_READ_INPUT_REGISTERS: {
 730         /* Similar to holding registers (but too many arguments to use a
 731          * function) */
 732         int nb = (req[offset + 3] << 8) + req[offset + 4];
 733
 734         if ((address + nb) > mb_mapping->nb_input_registers) {
 735             if (ctx->debug) {
 736                 fprintf(stderr, "Illegal data address %0X in read_input_registers\n",
 737                         address + nb);
 738             }
 739             rsp_length = response_exception(
 740                 ctx, &sft,
 741                 MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp);
 742         } else {
 743             int i;
 744
 745             rsp_length = ctx->backend->build_response_basis(&sft, rsp);
 746             rsp[rsp_length++] = nb << 1;
 747             for (i = address; i < address + nb; i++) {
 748                 rsp[rsp_length++] = mb_mapping->tab_input_registers[i] >> 8;
 749                 rsp[rsp_length++] = mb_mapping->tab_input_registers[i] & 0xFF;
 750             }
 751         }
 752     }
 753         break;
 754     case _FC_WRITE_SINGLE_COIL:
 755         if (address >= mb_mapping->nb_bits) {
 756             if (ctx->debug) {
 757                 fprintf(stderr, "Illegal data address %0X in write_bit\n",
 758                         address);
 759             }
 760             rsp_length = response_exception(
 761                 ctx, &sft,
 762                 MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp);
 763         } else {
 764             int data = (req[offset + 3] << 8) + req[offset + 4];
 765
 766             if (data == 0xFF00 || data == 0x0) {
 767                 mb_mapping->tab_bits[address] = (data) ? ON : OFF;
 768                 memcpy(rsp, req, req_length);
 769                 rsp_length = req_length;
 770             } else {
 771                 if (ctx->debug) {
 772                     fprintf(stderr,
 773                             "Illegal data value %0X in write_bit request at address %0X\n",
 774                             data, address);
 775                 }
 776                 rsp_length = response_exception(
 777                     ctx, &sft,
 778                     MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp);
 779             }
 780         }
 781         break;
 782     case _FC_WRITE_SINGLE_REGISTER:
 783         if (address >= mb_mapping->nb_registers) {
 784             if (ctx->debug) {
 785                 fprintf(stderr, "Illegal data address %0X in write_register\n",
 786                         address);
 787             }
 788             rsp_length = response_exception(
 789                 ctx, &sft,
 790                 MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp);
 791         } else {
 792             int data = (req[offset + 3] << 8) + req[offset + 4];
 793
 794             mb_mapping->tab_registers[address] = data;
 795             memcpy(rsp, req, req_length);
 796             rsp_length = req_length;
 797         }
 798         break;
 799     case _FC_WRITE_MULTIPLE_COILS: {
 800         int nb = (req[offset + 3] << 8) + req[offset + 4];
 801
 802         if ((address + nb) > mb_mapping->nb_bits) {
 803             if (ctx->debug) {
 804                 fprintf(stderr, "Illegal data address %0X in write_bits\n",
 805                         address + nb);
 806             }
 807             rsp_length = response_exception(
 808                 ctx, &sft,
 809                 MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp);
 810         } else {
 811             /* 6 = byte count */
 812             modbus_set_bits_from_bytes(mb_mapping->tab_bits, address, nb, &req[offset + 6]);
 813
 814             rsp_length = ctx->backend->build_response_basis(&sft, rsp);
 815             /* 4 to copy the bit address (2) and the quantity of bits */
 816             memcpy(rsp + rsp_length, req + rsp_length, 4);
 817             rsp_length += 4;
 818         }
 819     }
 820         break;
 821     case _FC_WRITE_MULTIPLE_REGISTERS: {
 822         int nb = (req[offset + 3] << 8) + req[offset + 4];
 823
 824         if ((address + nb) > mb_mapping->nb_registers) {
 825             if (ctx->debug) {
 826                 fprintf(stderr, "Illegal data address %0X in write_registers\n",
 827                         address + nb);
 828             }
 829             rsp_length = response_exception(
 830                 ctx, &sft,
 831                 MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp);
 832         } else {
 833             int i, j;
 834             for (i = address, j = 6; i < address + nb; i++, j += 2) {
 835                 /* 6 and 7 = first value */
 836                 mb_mapping->tab_registers[i] =
 837                     (req[offset + j] << 8) + req[offset + j + 1];
 838             }
 839
 840             rsp_length = ctx->backend->build_response_basis(&sft, rsp);
 841             /* 4 to copy the address (2) and the no. of registers */
 842             memcpy(rsp + rsp_length, req + rsp_length, 4);
 843             rsp_length += 4;
 844         }
 845     }
 846         break;
 847     case _FC_REPORT_SLAVE_ID: {
 848         int str_len;
 849         int byte_count_pos;
 850
 851         rsp_length = ctx->backend->build_response_basis(&sft, rsp);
 852         /* Skip byte count for now */
 853         byte_count_pos = rsp_length++;
 854         rsp[rsp_length++] = _REPORT_SLAVE_ID;
 855         /* Run indicator status to ON */
 856         rsp[rsp_length++] = 0xFF;
 857         /* LMB + length of LIBMODBUS_VERSION_STRING */
 858         str_len = 3 + strlen(LIBMODBUS_VERSION_STRING);
 859         memcpy(rsp + rsp_length, "LMB" LIBMODBUS_VERSION_STRING, str_len);
 860         rsp_length += str_len;
 861         rsp[byte_count_pos] = rsp_length - byte_count_pos - 1;
 862     }
 863         break;
 864     case _FC_READ_EXCEPTION_STATUS:
 865         if (ctx->debug) {
 866             fprintf(stderr, "FIXME Not implemented\n");
 867         }
 868         errno = ENOPROTOOPT;
 869         return -1;
 870         break;
 871
 872     case _FC_WRITE_AND_READ_REGISTERS: {
 873         int nb = (req[offset + 3] << 8) + req[offset + 4];
 874         uint16_t address_write = (req[offset + 5] << 8) + req[offset + 6];
 875         int nb_write = (req[offset + 7] << 8) + req[offset + 8];
 876
 877         if ((address + nb) > mb_mapping->nb_registers ||
 878             (address_write + nb_write) > mb_mapping->nb_registers) {
 879             if (ctx->debug) {
 880                 fprintf(stderr,
 881                         "Illegal data read address %0X or write address %0X write_and_read_registers\n",
 882                         address + nb, address_write + nb_write);
 883             }
 884             rsp_length = response_exception(ctx, &sft,
 885                                             MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp);
 886         } else {
 887             int i, j;
 888             rsp_length = ctx->backend->build_response_basis(&sft, rsp);
 889             rsp[rsp_length++] = nb << 1;
 890
 891             /* Write first.
 892                10 and 11 are the offset of the first values to write */
 893             for (i = address_write, j = 10; i < address_write + nb_write; i++, j += 2) {
 894                 mb_mapping->tab_registers[i] =
 895                     (req[offset + j] << 8) + req[offset + j + 1];
 896             }
 897
 898             /* and read the data for the response */
 899             for (i = address; i < address + nb; i++) {
 900                 rsp[rsp_length++] = mb_mapping->tab_registers[i] >> 8;
 901                 rsp[rsp_length++] = mb_mapping->tab_registers[i] & 0xFF;
 902             }
 903         }
 904     }
 905         break;
 906
 907     default:
 908         rsp_length = response_exception(ctx, &sft,
 909                                         MODBUS_EXCEPTION_ILLEGAL_FUNCTION,
 910                                         rsp);
 911         break;
 912     }
 913
 914     return send_msg(ctx, rsp, rsp_length);
 915 }
 916
 917 int modbus_reply_exception(modbus_t *ctx, const uint8_t *req,
 918                            unsigned int exception_code)
 919 {
 920     int offset = ctx->backend->header_length;
 921     int slave = req[offset - 1];
 922     int function = req[offset];
 923     uint8_t rsp[MAX_MESSAGE_LENGTH];
 924     int rsp_length;
 925     int dummy_length = 99;
 926     sft_t sft;
 927
 928     if (ctx->backend->filter_request(ctx, slave) == 1) {
 929         /* Filtered */
 930         return 0;
 931     }
 932
 933     sft.slave = slave;
 934     sft.function = function + 0x80;;
 935     sft.t_id = ctx->backend->prepare_response_tid(req, &dummy_length);
 936     rsp_length = ctx->backend->build_response_basis(&sft, rsp);
 937
 938     /* Positive exception code */
 939     if (exception_code < MODBUS_EXCEPTION_MAX) {
 940         rsp[rsp_length++] = exception_code;
 941         return send_msg(ctx, rsp, rsp_length);
 942     } else {
 943         errno = EINVAL;
 944         return -1;
 945     }
 946 }
 947
 948 /* Reads IO status */
 949 static int read_io_status(modbus_t *ctx, int function,
 950                           int addr, int nb, uint8_t *dest)
 951 {
 952     int rc;
 953     int req_length;
 954
 955     uint8_t req[_MIN_REQ_LENGTH];
 956     uint8_t rsp[MAX_MESSAGE_LENGTH];
 957
 958     req_length = ctx->backend->build_request_basis(ctx, function, addr, nb, req);
 959
 960     rc = send_msg(ctx, req, req_length);
 961     if (rc > 0) {
 962         int i, temp, bit;
 963         int pos = 0;
 964         int offset;
 965         int offset_end;
 966
 967         rc = receive_msg(ctx, rsp, MSG_CONFIRMATION);
 968         if (rc == -1)
 969             return -1;
 970
 971         rc = check_confirmation(ctx, req, rsp, rc);
 972         if (rc == -1)
 973             return -1;
 974
 975         offset = ctx->backend->header_length + 2;
 976         offset_end = offset + rc;
 977         for (i = offset; i < offset_end; i++) {
 978             /* Shift reg hi_byte to temp */
 979             temp = rsp[i];
 980
 981             for (bit = 0x01; (bit & 0xff) && (pos < nb);) {
 982                 dest[pos++] = (temp & bit) ? TRUE : FALSE;
 983                 bit = bit << 1;
 984             }
 985
 986         }
 987     }
 988
 989     return rc;
 990 }
 991
 992 /* Reads the boolean status of bits and sets the array elements
 993    in the destination to TRUE or FALSE (single bits). */
 994 int modbus_read_bits(modbus_t *ctx, int addr, int nb, uint8_t *dest)
 995 {
 996     int rc;
 997
 998     if (nb > MODBUS_MAX_READ_BITS) {
 999         if (ctx->debug) {
1000             fprintf(stderr,
1001                     "ERROR Too many bits requested (%d > %d)\n",
1002                     nb, MODBUS_MAX_READ_BITS);
1003         }
1004         errno = EMBMDATA;
1005         return -1;
1006     }
1007
1008     rc = read_io_status(ctx, _FC_READ_COILS, addr, nb, dest);
1009
1010     if (rc == -1)
1011         return -1;
1012     else
1013         return nb;
1014 }
1015
1016
1017 /* Same as modbus_read_bits but reads the remote device input table */
1018 int modbus_read_input_bits(modbus_t *ctx, int addr, int nb, uint8_t *dest)
1019 {
1020     int rc;
1021
1022     if (nb > MODBUS_MAX_READ_BITS) {
1023         if (ctx->debug) {
1024             fprintf(stderr,
1025                     "ERROR Too many discrete inputs requested (%d > %d)\n",
1026                     nb, MODBUS_MAX_READ_BITS);
1027         }
1028         errno = EMBMDATA;
1029         return -1;
1030     }
1031
1032     rc = read_io_status(ctx, _FC_READ_DISCRETE_INPUTS, addr, nb, dest);
1033
1034     if (rc == -1)
1035         return -1;
1036     else
1037         return nb;
1038 }
1039
1040 /* Reads the data from a remove device and put that data into an array */
1041 static int read_registers(modbus_t *ctx, int function, int addr, int nb,
1042                           uint16_t *dest)
1043 {
1044     int rc;
1045     int req_length;
1046     uint8_t req[_MIN_REQ_LENGTH];
1047     uint8_t rsp[MAX_MESSAGE_LENGTH];
1048
1049     if (nb > MODBUS_MAX_READ_REGISTERS) {
1050         if (ctx->debug) {
1051             fprintf(stderr,
1052                     "ERROR Too many registers requested (%d > %d)\n",
1053                     nb, MODBUS_MAX_READ_REGISTERS);
1054         }
1055         errno = EMBMDATA;
1056         return -1;
1057     }
1058
1059     req_length = ctx->backend->build_request_basis(ctx, function, addr, nb, req);
1060
1061     rc = send_msg(ctx, req, req_length);
1062     if (rc > 0) {
1063         int offset;
1064         int i;
1065
1066         rc = receive_msg(ctx, rsp, MSG_CONFIRMATION);
1067         if (rc == -1)
1068             return -1;
1069
1070         rc = check_confirmation(ctx, req, rsp, rc);
1071         if (rc == -1)
1072             return -1;
1073
1074         offset = ctx->backend->header_length;
1075
1076         for (i = 0; i < rc; i++) {
1077             /* shift reg hi_byte to temp OR with lo_byte */
1078             dest[i] = (rsp[offset + 2 + (i << 1)] << 8) |
1079                 rsp[offset + 3 + (i << 1)];
1080         }
1081     }
1082
1083     return rc;
1084 }
1085
1086 /* Reads the holding registers of remote device and put the data into an
1087    array */
1088 int modbus_read_registers(modbus_t *ctx, int addr, int nb, uint16_t *dest)
1089 {
1090     int status;
1091
1092     if (nb > MODBUS_MAX_READ_REGISTERS) {
1093         if (ctx->debug) {
1094             fprintf(stderr,
1095                     "ERROR Too many registers requested (%d > %d)\n",
1096                     nb, MODBUS_MAX_READ_REGISTERS);
1097         }
1098         errno = EMBMDATA;
1099         return -1;
1100     }
1101
1102     status = read_registers(ctx, _FC_READ_HOLDING_REGISTERS,
1103                             addr, nb, dest);
1104     return status;
1105 }
1106
1107 /* Reads the input registers of remote device and put the data into an array */
1108 int modbus_read_input_registers(modbus_t *ctx, int addr, int nb,
1109                                 uint16_t *dest)
1110 {
1111     int status;
1112
1113     if (nb > MODBUS_MAX_READ_REGISTERS) {
1114         fprintf(stderr,
1115                 "ERROR Too many input registers requested (%d > %d)\n",
1116                 nb, MODBUS_MAX_READ_REGISTERS);
1117         errno = EMBMDATA;
1118         return -1;
1119     }
1120
1121     status = read_registers(ctx, _FC_READ_INPUT_REGISTERS,
1122                             addr, nb, dest);
1123
1124     return status;
1125 }
1126
1127 /* Write a value to the specified register of the remote device.
1128    Used by write_bit and write_register */
1129 static int write_single(modbus_t *ctx, int function, int addr, int value)
1130 {
1131     int rc;
1132     int req_length;
1133     uint8_t req[_MIN_REQ_LENGTH];
1134
1135     req_length = ctx->backend->build_request_basis(ctx, function, addr, value, req);
1136
1137     rc = send_msg(ctx, req, req_length);
1138     if (rc > 0) {
1139         /* Used by write_bit and write_register */
1140         uint8_t rsp[_MIN_REQ_LENGTH];
1141
1142         rc = receive_msg(ctx, rsp, MSG_CONFIRMATION);
1143         if (rc == -1)
1144             return -1;
1145
1146         rc = check_confirmation(ctx, req, rsp, rc);
1147     }
1148
1149     return rc;
1150 }
1151
1152 /* Turns ON or OFF a single bit of the remote device */
1153 int modbus_write_bit(modbus_t *ctx, int addr, int status)
1154 {
1155     return write_single(ctx, _FC_WRITE_SINGLE_COIL, addr,
1156                         status ? 0xFF00 : 0);
1157 }
1158
1159 /* Writes a value in one register of the remote device */
1160 int modbus_write_register(modbus_t *ctx, int addr, int value)
1161 {
1162     return write_single(ctx, _FC_WRITE_SINGLE_REGISTER, addr, value);
1163 }
1164
1165 /* Write the bits of the array in the remote device */
1166 int modbus_write_bits(modbus_t *ctx, int addr, int nb, const uint8_t *src)
1167 {
1168     int rc;
1169     int i;
1170     int byte_count;
1171     int req_length;
1172     int bit_check = 0;
1173     int pos = 0;
1174
1175     uint8_t req[MAX_MESSAGE_LENGTH];
1176
1177     if (nb > MODBUS_MAX_WRITE_BITS) {
1178         if (ctx->debug) {
1179             fprintf(stderr, "ERROR Writing too many bits (%d > %d)\n",
1180                     nb, MODBUS_MAX_WRITE_BITS);
1181         }
1182         errno = EMBMDATA;
1183         return -1;
1184     }
1185
1186     req_length = ctx->backend->build_request_basis(ctx,
1187                                                    _FC_WRITE_MULTIPLE_COILS,
1188                                                    addr, nb, req);
1189     byte_count = (nb / 8) + ((nb % 8) ? 1 : 0);
1190     req[req_length++] = byte_count;
1191
1192     for (i = 0; i < byte_count; i++) {
1193         int bit;
1194
1195         bit = 0x01;
1196         req[req_length] = 0;
1197
1198         while ((bit & 0xFF) && (bit_check++ < nb)) {
1199             if (src[pos++])
1200                 req[req_length] |= bit;
1201             else
1202                 req[req_length] &=~ bit;
1203
1204             bit = bit << 1;
1205         }
1206         req_length++;
1207     }
1208
1209     rc = send_msg(ctx, req, req_length);
1210     if (rc > 0) {
1211         uint8_t rsp[MAX_MESSAGE_LENGTH];
1212
1213         rc = receive_msg(ctx, rsp, MSG_CONFIRMATION);
1214         if (rc == -1)
1215             return -1;
1216
1217         rc = check_confirmation(ctx, req, rsp, rc);
1218     }
1219
1220
1221     return rc;
1222 }
1223
1224 /* Write the values from the array to the registers of the remote device */
1225 int modbus_write_registers(modbus_t *ctx, int addr, int nb, const uint16_t *src)
1226 {
1227     int rc;
1228     int i;
1229     int req_length;
1230     int byte_count;
1231
1232     uint8_t req[MAX_MESSAGE_LENGTH];
1233
1234     if (nb > MODBUS_MAX_WRITE_REGISTERS) {
1235         if (ctx->debug) {
1236             fprintf(stderr,
1237                     "ERROR Trying to write to too many registers (%d > %d)\n",
1238                     nb, MODBUS_MAX_WRITE_REGISTERS);
1239         }
1240         errno = EMBMDATA;
1241         return -1;
1242     }
1243
1244     req_length = ctx->backend->build_request_basis(ctx,
1245                                                    _FC_WRITE_MULTIPLE_REGISTERS,
1246                                                    addr, nb, req);
1247     byte_count = nb * 2;
1248     req[req_length++] = byte_count;
1249
1250     for (i = 0; i < nb; i++) {
1251         req[req_length++] = src[i] >> 8;
1252         req[req_length++] = src[i] & 0x00FF;
1253     }
1254
1255     rc = send_msg(ctx, req, req_length);
1256     if (rc > 0) {
1257         uint8_t rsp[MAX_MESSAGE_LENGTH];
1258
1259         rc = receive_msg(ctx, rsp, MSG_CONFIRMATION);
1260         if (rc == -1)
1261             return -1;
1262
1263         rc = check_confirmation(ctx, req, rsp, rc);
1264     }
1265
1266     return rc;
1267 }
1268
1269 /* Write multiple registers from src array to remote device and read multiple
1270    registers from remote device to dest array. */
1271 int modbus_write_and_read_registers(modbus_t *ctx,
1272                                     int write_addr, int write_nb, const uint16_t *src,
1273                                     int read_addr, int read_nb, uint16_t *dest)
1274
1275 {
1276     int rc;
1277     int req_length;
1278     int i;
1279     int byte_count;
1280     uint8_t req[MAX_MESSAGE_LENGTH];
1281     uint8_t rsp[MAX_MESSAGE_LENGTH];
1282
1283     if (write_nb > MODBUS_MAX_RW_WRITE_REGISTERS) {
1284         if (ctx->debug) {
1285             fprintf(stderr,
1286                     "ERROR Too many registers to write (%d > %d)\n",
1287                     write_nb, MODBUS_MAX_RW_WRITE_REGISTERS);
1288         }
1289         errno = EMBMDATA;
1290         return -1;
1291     }
1292
1293     if (read_nb > MODBUS_MAX_READ_REGISTERS) {
1294         if (ctx->debug) {
1295             fprintf(stderr,
1296                     "ERROR Too many registers requested (%d > %d)\n",
1297                     read_nb, MODBUS_MAX_READ_REGISTERS);
1298         }
1299         errno = EMBMDATA;
1300         return -1;
1301     }
1302     req_length = ctx->backend->build_request_basis(ctx,
1303                                                    _FC_WRITE_AND_READ_REGISTERS,
1304                                                    read_addr, read_nb, req);
1305
1306     req[req_length++] = write_addr >> 8;
1307     req[req_length++] = write_addr & 0x00ff;
1308     req[req_length++] = write_nb >> 8;
1309     req[req_length++] = write_nb & 0x00ff;
1310     byte_count = write_nb * 2;
1311     req[req_length++] = byte_count;
1312
1313     for (i = 0; i < write_nb; i++) {
1314         req[req_length++] = src[i] >> 8;
1315         req[req_length++] = src[i] & 0x00FF;
1316     }
1317
1318     rc = send_msg(ctx, req, req_length);
1319     if (rc > 0) {
1320         int offset;
1321
1322         rc = receive_msg(ctx, rsp, MSG_CONFIRMATION);
1323         if (rc == -1)
1324             return -1;
1325
1326         rc = check_confirmation(ctx, req, rsp, rc);
1327         if (rc == -1)
1328             return -1;
1329
1330         offset = ctx->backend->header_length;
1331
1332         /* If rc is negative, the loop is jumped ! */
1333         for (i = 0; i < rc; i++) {
1334             /* shift reg hi_byte to temp OR with lo_byte */
1335             dest[i] = (rsp[offset + 2 + (i << 1)] << 8) |
1336                 rsp[offset + 3 + (i << 1)];
1337         }
1338     }
1339
1340     return rc;
1341 }
1342
1343 /* Send a request to get the slave ID of the device (only available in serial
1344    communication). */
1345 int modbus_report_slave_id(modbus_t *ctx, uint8_t *dest)
1346 {
1347     int rc;
1348     int req_length;
1349     uint8_t req[_MIN_REQ_LENGTH];
1350
1351     req_length = ctx->backend->build_request_basis(ctx, _FC_REPORT_SLAVE_ID,
1352                                                    0, 0, req);
1353
1354     /* HACKISH, addr and count are not used */
1355     req_length -= 4;
1356
1357     rc = send_msg(ctx, req, req_length);
1358     if (rc > 0) {
1359         int i;
1360         int offset;
1361         uint8_t rsp[MAX_MESSAGE_LENGTH];
1362
1363         rc = receive_msg(ctx, rsp, MSG_CONFIRMATION);
1364         if (rc == -1)
1365             return -1;
1366
1367         rc = check_confirmation(ctx, req, rsp, rc);
1368         if (rc == -1)
1369             return -1;
1370
1371         offset = ctx->backend->header_length + 2;
1372
1373         /* Byte count, slave id, run indicator status,
1374            additional data */
1375         for (i=0; i < rc; i++) {
1376             dest[i] = rsp[offset + i];
1377         }
1378     }
1379
1380     return rc;
1381 }
1382
1383 void _modbus_init_common(modbus_t *ctx)
1384 {
1385     /* Slave and socket are initialized to -1 */
1386     ctx->slave = -1;
1387     ctx->s = -1;
1388
1389     ctx->debug = FALSE;
1390     ctx->error_recovery = MODBUS_ERROR_RECOVERY_NONE;
1391
1392     ctx->response_timeout.tv_sec = 0;
1393     ctx->response_timeout.tv_usec = _RESPONSE_TIMEOUT;
1394
1395     ctx->byte_timeout.tv_sec = 0;
1396     ctx->byte_timeout.tv_usec = _BYTE_TIMEOUT;
1397 }
1398
1399 /* Define the slave number */
1400 int modbus_set_slave(modbus_t *ctx, int slave)
1401 {
1402     return ctx->backend->set_slave(ctx, slave);
1403 }
1404
1405 int modbus_set_error_recovery(modbus_t *ctx,
1406                               modbus_error_recovery_mode error_recovery)
1407 {
1408     if (error_recovery >= 0) {
1409         ctx->error_recovery = (uint8_t) error_recovery;
1410     } else {
1411         errno = EINVAL;
1412         return -1;
1413     }
1414
1415     return 0;
1416 }
1417
1418 void modbus_set_socket(modbus_t *ctx, int socket)
1419 {
1420     ctx->s = socket;
1421 }
1422
1423 int modbus_get_socket(modbus_t *ctx)
1424 {
1425     return ctx->s;
1426 }
1427
1428 /* Get the timeout interval used to wait for a response */
1429 void modbus_get_response_timeout(modbus_t *ctx, struct timeval *timeout)
1430 {
1431     *timeout = ctx->response_timeout;
1432 }
1433
1434 void modbus_set_response_timeout(modbus_t *ctx, const struct timeval *timeout)
1435 {
1436     ctx->response_timeout = *timeout;
1437 }
1438
1439 /* Get the timeout interval between two consecutive bytes of a message */
1440 void modbus_get_byte_timeout(modbus_t *ctx, struct timeval *timeout)
1441 {
1442     *timeout = ctx->byte_timeout;
1443 }
1444
1445 void modbus_set_byte_timeout(modbus_t *ctx, const struct timeval *timeout)
1446 {
1447     ctx->byte_timeout = *timeout;
1448 }
1449
1450 int modbus_get_header_length(modbus_t *ctx)
1451 {
1452     return ctx->backend->header_length;
1453 }
1454
1455 int modbus_connect(modbus_t *ctx)
1456 {
1457     return ctx->backend->connect(ctx);
1458 }
1459
1460 void modbus_close(modbus_t *ctx)
1461 {
1462     if (ctx == NULL)
1463         return;
1464
1465     ctx->backend->close(ctx);
1466 }
1467
1468 void modbus_free(modbus_t *ctx)
1469 {
1470     if (ctx == NULL)
1471         return;
1472
1473     free(ctx->backend_data);
1474     free(ctx);
1475 }
1476
1477 void modbus_set_debug(modbus_t *ctx, int boolean)
1478 {
1479     ctx->debug = boolean;
1480 }
1481
1482 /* Allocates 4 arrays to store bits, input bits, registers and inputs
1483    registers. The pointers are stored in modbus_mapping structure.
1484
1485    The modbus_mapping_new() function shall return the new allocated structure if
1486    successful. Otherwise it shall return NULL and set errno to ENOMEM. */
1487 modbus_mapping_t* modbus_mapping_new(int nb_bits, int nb_input_bits,
1488                                      int nb_registers, int nb_input_registers)
1489 {
1490     modbus_mapping_t *mb_mapping;
1491
1492     mb_mapping = (modbus_mapping_t *)malloc(sizeof(modbus_mapping_t));
1493     if (mb_mapping == NULL) {
1494         return NULL;
1495     }
1496
1497     /* 0X */
1498     mb_mapping->nb_bits = nb_bits;
1499     if (nb_bits == 0) {
1500         mb_mapping->tab_bits = NULL;
1501     } else {
1502         /* Negative number raises a POSIX error */
1503         mb_mapping->tab_bits =
1504             (uint8_t *) malloc(nb_bits * sizeof(uint8_t));
1505         if (mb_mapping->tab_bits == NULL) {
1506             free(mb_mapping);
1507             return NULL;
1508         }
1509         memset(mb_mapping->tab_bits, 0, nb_bits * sizeof(uint8_t));
1510     }
1511
1512     /* 1X */
1513     mb_mapping->nb_input_bits = nb_input_bits;
1514     if (nb_input_bits == 0) {
1515         mb_mapping->tab_input_bits = NULL;
1516     } else {
1517         mb_mapping->tab_input_bits =
1518             (uint8_t *) malloc(nb_input_bits * sizeof(uint8_t));
1519         if (mb_mapping->tab_input_bits == NULL) {
1520             free(mb_mapping->tab_bits);
1521             free(mb_mapping);
1522             return NULL;
1523         }
1524         memset(mb_mapping->tab_input_bits, 0, nb_input_bits * sizeof(uint8_t));
1525     }
1526
1527     /* 4X */
1528     mb_mapping->nb_registers = nb_registers;
1529     if (nb_registers == 0) {
1530         mb_mapping->tab_registers = NULL;
1531     } else {
1532         mb_mapping->tab_registers =
1533             (uint16_t *) malloc(nb_registers * sizeof(uint16_t));
1534         if (mb_mapping->tab_registers == NULL) {
1535             free(mb_mapping->tab_input_bits);
1536             free(mb_mapping->tab_bits);
1537             free(mb_mapping);
1538             return NULL;
1539         }
1540         memset(mb_mapping->tab_registers, 0, nb_registers * sizeof(uint16_t));
1541     }
1542
1543     /* 3X */
1544     mb_mapping->nb_input_registers = nb_input_registers;
1545     if (nb_input_registers == 0) {
1546         mb_mapping->tab_input_registers = NULL;
1547     } else {
1548         mb_mapping->tab_input_registers =
1549             (uint16_t *) malloc(nb_input_registers * sizeof(uint16_t));
1550         if (mb_mapping->tab_input_registers == NULL) {
1551             free(mb_mapping->tab_registers);
1552             free(mb_mapping->tab_input_bits);
1553             free(mb_mapping->tab_bits);
1554             free(mb_mapping);
1555             return NULL;
1556         }
1557         memset(mb_mapping->tab_input_registers, 0,
1558                nb_input_registers * sizeof(uint16_t));
1559     }
1560
1561     return mb_mapping;
1562 }
1563
1564 /* Frees the 4 arrays */
1565 void modbus_mapping_free(modbus_mapping_t *mb_mapping)
1566 {
1567     if (mb_mapping != NULL) {
1568         return;
1569     }
1570
1571     free(mb_mapping->tab_input_registers);
1572     free(mb_mapping->tab_registers);
1573     free(mb_mapping->tab_input_bits);
1574     free(mb_mapping->tab_bits);
1575     free(mb_mapping);
1576 }
1577
1578 #ifndef HAVE_STRLCPY
1579 /*
1580  * Function strlcpy was originally developed by
1581  * Todd C. Miller <Todd.Miller@courtesan.com> to simplify writing secure code.
1582  * See ftp://ftp.openbsd.org/pub/OpenBSD/src/lib/libc/string/strlcpy.3
1583  * for more information.
1584  *
1585  * Thank you Ulrich Drepper... not!
1586  *
1587  * Copy src to string dest of size dest_size.  At most dest_size-1 characters
1588  * will be copied.  Always NUL terminates (unless dest_size == 0).  Returns
1589  * strlen(src); if retval >= dest_size, truncation occurred.
1590  */
1591 size_t strlcpy(char *dest, const char *src, size_t dest_size)
1592 {
1593     register char *d = dest;
1594     register const char *s = src;
1595     register size_t n = dest_size;
1596
1597     /* Copy as many bytes as will fit */
1598     if (n != 0 && --n != 0) {
1599         do {
1600             if ((*d++ = *s++) == 0)
1601                 break;
1602         } while (--n != 0);
1603     }
1604
1605     /* Not enough room in dest, add NUL and traverse rest of src */
1606     if (n == 0) {
1607         if (dest_size != 0)
1608             *d = '\0'; /* NUL-terminate dest */
1609         while (*s++)
1610             ;
1611     }
1612
1613     return (s - src - 1); /* count does not include NUL */
1614 }
1615 #endif