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