/* * Copyright © 2001-2011 Stéphane Raimbault * * This library is free software; you can redistribute it and/or * modify it under the terms of the GNU Lesser General Public * License as published by the Free Software Foundation; either * version 2.1 of the License, or (at your option) any later version. * * This library is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * Lesser General Public License for more details. * * You should have received a copy of the GNU Lesser General Public * License along with this library; if not, write to the Free Software * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA * * * This library implements the Modbus protocol. * http://libmodbus.org/ */ #include #include #include #include #include #include #include #include "modbus.h" #include "modbus-private.h" /* Internal use */ #define MSG_LENGTH_UNDEFINED -1 /* Exported version */ const unsigned int libmodbus_version_major = LIBMODBUS_VERSION_MAJOR; const unsigned int libmodbus_version_minor = LIBMODBUS_VERSION_MINOR; const unsigned int libmodbus_version_micro = LIBMODBUS_VERSION_MICRO; /* Max between RTU and TCP max adu length (so TCP) */ #define MAX_MESSAGE_LENGTH 260 /* 3 steps are used to parse the query */ typedef enum { _STEP_FUNCTION, _STEP_META, _STEP_DATA } _step_t; const char *modbus_strerror(int errnum) { switch (errnum) { case EMBXILFUN: return "Illegal function"; case EMBXILADD: return "Illegal data address"; case EMBXILVAL: return "Illegal data value"; case EMBXSFAIL: return "Slave device or server failure"; case EMBXACK: return "Acknowledge"; case EMBXSBUSY: return "Slave device or server is busy"; case EMBXNACK: return "Negative acknowledge"; case EMBXMEMPAR: return "Memory parity error"; case EMBXGPATH: return "Gateway path unavailable"; case EMBXGTAR: return "Target device failed to respond"; case EMBBADCRC: return "Invalid CRC"; case EMBBADDATA: return "Invalid data"; case EMBBADEXC: return "Invalid exception code"; case EMBMDATA: return "Too many data"; default: return strerror(errnum); } } void _error_print(modbus_t *ctx, const char *context) { if (ctx->debug) { fprintf(stderr, "ERROR %s", modbus_strerror(errno)); if (context != NULL) { fprintf(stderr, ": %s\n", context); } else { fprintf(stderr, "\n"); } } } int _sleep_and_flush(modbus_t *ctx) { #ifdef _WIN32 /* usleep doesn't exist on Windows */ Sleep((ctx->response_timeout.tv_sec * 1000) + (ctx->response_timeout.tv_usec / 1000)); #else /* usleep source code */ struct timespec request, remaining; request.tv_sec = ctx->response_timeout.tv_sec; request.tv_nsec = ((long int)ctx->response_timeout.tv_usec % 1000000) * 1000; while (nanosleep(&request, &remaining) == -1 && errno == EINTR) request = remaining; #endif return modbus_flush(ctx); } int modbus_flush(modbus_t *ctx) { int rc = ctx->backend->flush(ctx); if (rc != -1 && ctx->debug) { printf("%d bytes flushed\n", rc); } return rc; } /* Computes the length of the expected response */ static unsigned int compute_response_length_from_request(modbus_t *ctx, uint8_t *req) { int length; const int offset = ctx->backend->header_length; switch (req[offset]) { case _FC_READ_COILS: case _FC_READ_DISCRETE_INPUTS: { /* Header + nb values (code from write_bits) */ int nb = (req[offset + 3] << 8) | req[offset + 4]; length = 2 + (nb / 8) + ((nb % 8) ? 1 : 0); } break; case _FC_WRITE_AND_READ_REGISTERS: case _FC_READ_HOLDING_REGISTERS: case _FC_READ_INPUT_REGISTERS: /* Header + 2 * nb values */ length = 2 + 2 * (req[offset + 3] << 8 | req[offset + 4]); break; case _FC_READ_EXCEPTION_STATUS: length = 3; break; case _FC_REPORT_SLAVE_ID: /* The response is device specific (the header provides the length) */ return MSG_LENGTH_UNDEFINED; default: length = 5; } return offset + length + ctx->backend->checksum_length; } /* Sends a request/response */ static int send_msg(modbus_t *ctx, uint8_t *msg, int msg_length) { int rc; int i; msg_length = ctx->backend->send_msg_pre(msg, msg_length); if (ctx->debug) { for (i = 0; i < msg_length; i++) printf("[%.2X]", msg[i]); printf("\n"); } /* In recovery mode, the write command will be issued until to be successful! Disabled by default. */ do { rc = ctx->backend->send(ctx, msg, msg_length); if (rc == -1) { _error_print(ctx, NULL); if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_LINK) { int saved_errno = errno; if ((errno == EBADF || errno == ECONNRESET || errno == EPIPE)) { modbus_close(ctx); modbus_connect(ctx); } else { _sleep_and_flush(ctx); } errno = saved_errno; } } } while ((ctx->error_recovery & MODBUS_ERROR_RECOVERY_LINK) && rc == -1); if (rc > 0 && rc != msg_length) { errno = EMBBADDATA; return -1; } return rc; } int modbus_send_raw_request(modbus_t *ctx, uint8_t *raw_req, int raw_req_length) { sft_t sft; uint8_t req[MAX_MESSAGE_LENGTH]; int req_length; if (raw_req_length < 2) { /* The raw request must contain function and slave at least */ errno = EINVAL; return -1; } sft.slave = raw_req[0]; sft.function = raw_req[1]; /* The t_id is left to zero */ sft.t_id = 0; /* This response function only set the header so it's convenient here */ req_length = ctx->backend->build_response_basis(&sft, req); if (raw_req_length > 2) { /* Copy data after function code */ memcpy(req + req_length, raw_req + 2, raw_req_length - 2); req_length += raw_req_length - 2; } return send_msg(ctx, req, req_length); } /* ---------- Request Indication ---------- | Client | ---------------------->| Server | ---------- Confirmation Response ---------- */ typedef enum { /* Request message on the server side */ MSG_INDICATION, /* Request message on the client side */ MSG_CONFIRMATION } msg_type_t; /* Computes the length to read after the function received */ static uint8_t compute_meta_length_after_function(int function, msg_type_t msg_type) { int length; if (msg_type == MSG_INDICATION) { if (function <= _FC_WRITE_SINGLE_REGISTER) { length = 4; } else if (function == _FC_WRITE_MULTIPLE_COILS || function == _FC_WRITE_MULTIPLE_REGISTERS) { length = 5; } else if (function == _FC_WRITE_AND_READ_REGISTERS) { length = 9; } else { /* _FC_READ_EXCEPTION_STATUS, _FC_REPORT_SLAVE_ID */ length = 0; } } else { /* MSG_CONFIRMATION */ switch (function) { case _FC_WRITE_SINGLE_COIL: case _FC_WRITE_SINGLE_REGISTER: case _FC_WRITE_MULTIPLE_COILS: case _FC_WRITE_MULTIPLE_REGISTERS: length = 4; break; default: length = 1; } } return length; } /* Computes the length to read after the meta information (address, count, etc) */ static int compute_data_length_after_meta(modbus_t *ctx, uint8_t *msg, msg_type_t msg_type) { int function = msg[ctx->backend->header_length]; int length; if (msg_type == MSG_INDICATION) { switch (function) { case _FC_WRITE_MULTIPLE_COILS: case _FC_WRITE_MULTIPLE_REGISTERS: length = msg[ctx->backend->header_length + 5]; break; case _FC_WRITE_AND_READ_REGISTERS: length = msg[ctx->backend->header_length + 9]; break; default: length = 0; } } else { /* MSG_CONFIRMATION */ if (function <= _FC_READ_INPUT_REGISTERS || function == _FC_REPORT_SLAVE_ID || function == _FC_WRITE_AND_READ_REGISTERS) { length = msg[ctx->backend->header_length + 1]; } else { length = 0; } } length += ctx->backend->checksum_length; return length; } /* Waits a response from a modbus server or a request from a modbus client. This function blocks if there is no replies (3 timeouts). The function shall return the number of received characters and the received message in an array of uint8_t if successful. Otherwise it shall return -1 and errno is set to one of the values defined below: - ECONNRESET - EMBBADDATA - EMBUNKEXC - ETIMEDOUT - read() or recv() error codes */ static int receive_msg(modbus_t *ctx, uint8_t *msg, msg_type_t msg_type) { int rc; fd_set rfds; struct timeval tv; struct timeval *p_tv; int length_to_read; int msg_length = 0; _step_t step; if (ctx->debug) { if (msg_type == MSG_INDICATION) { printf("Waiting for a indication...\n"); } else { printf("Waiting for a confirmation...\n"); } } /* Add a file descriptor to the set */ FD_ZERO(&rfds); FD_SET(ctx->s, &rfds); /* We need to analyse the message step by step. At the first step, we want * to reach the function code because all packets contain this * information. */ step = _STEP_FUNCTION; length_to_read = ctx->backend->header_length + 1; if (msg_type == MSG_INDICATION) { /* Wait for a message, we don't know when the message will be * received */ p_tv = NULL; } else { tv.tv_sec = ctx->response_timeout.tv_sec; tv.tv_usec = ctx->response_timeout.tv_usec; p_tv = &tv; } while (length_to_read != 0) { rc = ctx->backend->select(ctx, &rfds, p_tv, length_to_read); if (rc == -1) { _error_print(ctx, "select"); if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_LINK) { int saved_errno = errno; if (errno == ETIMEDOUT) { _sleep_and_flush(ctx); } else if (errno == EBADF) { modbus_close(ctx); modbus_connect(ctx); } errno = saved_errno; } return -1; } rc = ctx->backend->recv(ctx, msg + msg_length, length_to_read); if (rc == 0) { errno = ECONNRESET; rc = -1; } if (rc == -1) { _error_print(ctx, "read"); if ((ctx->error_recovery & MODBUS_ERROR_RECOVERY_LINK) && (errno == ECONNRESET || errno == ECONNREFUSED || errno == EBADF)) { int saved_errno = errno; modbus_close(ctx); modbus_connect(ctx); /* Could be removed by previous calls */ errno = saved_errno; } return -1; } /* Display the hex code of each character received */ if (ctx->debug) { int i; for (i=0; i < rc; i++) printf("<%.2X>", msg[msg_length + i]); } /* Sums bytes received */ msg_length += rc; /* Computes remaining bytes */ length_to_read -= rc; if (length_to_read == 0) { switch (step) { case _STEP_FUNCTION: /* Function code position */ length_to_read = compute_meta_length_after_function( msg[ctx->backend->header_length], msg_type); if (length_to_read != 0) { step = _STEP_META; break; } /* else switches straight to the next step */ case _STEP_META: length_to_read = compute_data_length_after_meta( ctx, msg, msg_type); if ((msg_length + length_to_read) > ctx->backend->max_adu_length) { errno = EMBBADDATA; _error_print(ctx, "too many data"); return -1; } step = _STEP_DATA; break; default: break; } } if (length_to_read > 0 && ctx->byte_timeout.tv_sec != -1) { /* If there is no character in the buffer, the allowed timeout interval between two consecutive bytes is defined by byte_timeout */ tv.tv_sec = ctx->byte_timeout.tv_sec; tv.tv_usec = ctx->byte_timeout.tv_usec; p_tv = &tv; } } if (ctx->debug) printf("\n"); return ctx->backend->check_integrity(ctx, msg, msg_length); } /* Receive the request from a modbus master */ int modbus_receive(modbus_t *ctx, uint8_t *req) { return receive_msg(ctx, req, MSG_INDICATION); } /* Receives the confirmation. The function shall store the read response in rsp and return the number of values (bits or words). Otherwise, its shall return -1 and errno is set. The function doesn't check the confirmation is the expected response to the initial request. */ int modbus_receive_confirmation(modbus_t *ctx, uint8_t *rsp) { return receive_msg(ctx, rsp, MSG_CONFIRMATION); } static int check_confirmation(modbus_t *ctx, uint8_t *req, uint8_t *rsp, int rsp_length) { int rc; int rsp_length_computed; const int offset = ctx->backend->header_length; if (ctx->backend->pre_check_confirmation) { rc = ctx->backend->pre_check_confirmation(ctx, req, rsp, rsp_length); if (rc == -1) { if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_PROTOCOL) { _sleep_and_flush(ctx); } return -1; } } rsp_length_computed = compute_response_length_from_request(ctx, req); /* Check length */ if (rsp_length == rsp_length_computed || rsp_length_computed == MSG_LENGTH_UNDEFINED) { int req_nb_value; int rsp_nb_value; const int function = rsp[offset]; /* Check function code */ if (function != req[offset]) { if (ctx->debug) { fprintf(stderr, "Received function not corresponding to the request (%d != %d)\n", function, req[offset]); } if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_PROTOCOL) { _sleep_and_flush(ctx); } errno = EMBBADDATA; return -1; } /* Check the number of values is corresponding to the request */ switch (function) { case _FC_READ_COILS: case _FC_READ_DISCRETE_INPUTS: /* Read functions, 8 values in a byte (nb * of values in the request and byte count in * the response. */ req_nb_value = (req[offset + 3] << 8) + req[offset + 4]; req_nb_value = (req_nb_value / 8) + ((req_nb_value % 8) ? 1 : 0); rsp_nb_value = rsp[offset + 1]; break; case _FC_WRITE_AND_READ_REGISTERS: case _FC_READ_HOLDING_REGISTERS: case _FC_READ_INPUT_REGISTERS: /* Read functions 1 value = 2 bytes */ req_nb_value = (req[offset + 3] << 8) + req[offset + 4]; rsp_nb_value = (rsp[offset + 1] / 2); break; case _FC_WRITE_MULTIPLE_COILS: case _FC_WRITE_MULTIPLE_REGISTERS: /* N Write functions */ req_nb_value = (req[offset + 3] << 8) + req[offset + 4]; rsp_nb_value = (rsp[offset + 3] << 8) | rsp[offset + 4]; break; case _FC_REPORT_SLAVE_ID: /* Report slave ID (bytes received) */ req_nb_value = rsp_nb_value = rsp[offset + 1]; break; default: /* 1 Write functions & others */ req_nb_value = rsp_nb_value = 1; } if (req_nb_value == rsp_nb_value) { rc = rsp_nb_value; } else { if (ctx->debug) { fprintf(stderr, "Quantity not corresponding to the request (%d != %d)\n", rsp_nb_value, req_nb_value); } if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_PROTOCOL) { _sleep_and_flush(ctx); } errno = EMBBADDATA; rc = -1; } } else if (rsp_length == (offset + 2 + ctx->backend->checksum_length) && req[offset] == (rsp[offset] - 0x80)) { /* EXCEPTION CODE RECEIVED */ int exception_code = rsp[offset + 1]; if (exception_code < MODBUS_EXCEPTION_MAX) { errno = MODBUS_ENOBASE + exception_code; } else { errno = EMBBADEXC; } _error_print(ctx, NULL); rc = -1; } else { if (ctx->debug) { fprintf(stderr, "Message length not corresponding to the computed length (%d != %d)\n", rsp_length, rsp_length_computed); } if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_PROTOCOL) { _sleep_and_flush(ctx); } errno = EMBBADDATA; rc = -1; } return rc; } static int response_io_status(int address, int nb, uint8_t *tab_io_status, uint8_t *rsp, int offset) { int shift = 0; int byte = 0; int i; for (i = address; i < address+nb; i++) { byte |= tab_io_status[i] << shift; if (shift == 7) { /* Byte is full */ rsp[offset++] = byte; byte = shift = 0; } else { shift++; } } if (shift != 0) rsp[offset++] = byte; return offset; } /* Build the exception response */ static int response_exception(modbus_t *ctx, sft_t *sft, int exception_code, uint8_t *rsp) { int rsp_length; sft->function = sft->function + 0x80; rsp_length = ctx->backend->build_response_basis(sft, rsp); /* Positive exception code */ rsp[rsp_length++] = exception_code; return rsp_length; } /* Send a response to the received request. Analyses the request and constructs a response. If an error occurs, this function construct the response accordingly. */ int modbus_reply(modbus_t *ctx, const uint8_t *req, int req_length, modbus_mapping_t *mb_mapping) { int offset = ctx->backend->header_length; int slave = req[offset - 1]; int function = req[offset]; uint16_t address = (req[offset + 1] << 8) + req[offset + 2]; uint8_t rsp[MAX_MESSAGE_LENGTH]; int rsp_length = 0; sft_t sft; if (ctx->backend->filter_request(ctx, slave) == 1) { /* Filtered */ return 0; } sft.slave = slave; sft.function = function; sft.t_id = ctx->backend->prepare_response_tid(req, &req_length); switch (function) { case _FC_READ_COILS: { int nb = (req[offset + 3] << 8) + req[offset + 4]; if (nb < 1 || MODBUS_MAX_READ_BITS < nb) { if (ctx->debug) { fprintf(stderr, "Illegal nb of values %d in read_bits (max %d)\n", nb, MODBUS_MAX_READ_BITS); } rsp_length = response_exception( ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp); } else if ((address + nb) > mb_mapping->nb_bits) { if (ctx->debug) { fprintf(stderr, "Illegal data address %0X in read_bits\n", address + nb); } rsp_length = response_exception( ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp); } else { rsp_length = ctx->backend->build_response_basis(&sft, rsp); rsp[rsp_length++] = (nb / 8) + ((nb % 8) ? 1 : 0); rsp_length = response_io_status(address, nb, mb_mapping->tab_bits, rsp, rsp_length); } } break; case _FC_READ_DISCRETE_INPUTS: { /* Similar to coil status (but too many arguments to use a * function) */ int nb = (req[offset + 3] << 8) + req[offset + 4]; if (nb < 1 || MODBUS_MAX_READ_BITS < nb) { if (ctx->debug) { fprintf(stderr, "Illegal nb of values %d in read_input_bits (max %d)\n", nb, MODBUS_MAX_READ_BITS); } rsp_length = response_exception( ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp); } else if ((address + nb) > mb_mapping->nb_input_bits) { if (ctx->debug) { fprintf(stderr, "Illegal data address %0X in read_input_bits\n", address + nb); } rsp_length = response_exception( ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp); } else { rsp_length = ctx->backend->build_response_basis(&sft, rsp); rsp[rsp_length++] = (nb / 8) + ((nb % 8) ? 1 : 0); rsp_length = response_io_status(address, nb, mb_mapping->tab_input_bits, rsp, rsp_length); } } break; case _FC_READ_HOLDING_REGISTERS: { int nb = (req[offset + 3] << 8) + req[offset + 4]; if (nb < 1 || MODBUS_MAX_READ_REGISTERS < nb) { if (ctx->debug) { fprintf(stderr, "Illegal nb of values %d in read_holding_registers (max %d)\n", nb, MODBUS_MAX_READ_REGISTERS); } rsp_length = response_exception( ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp); } else if ((address + nb) > mb_mapping->nb_registers) { if (ctx->debug) { fprintf(stderr, "Illegal data address %0X in read_registers\n", address + nb); } rsp_length = response_exception( ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp); } else { int i; rsp_length = ctx->backend->build_response_basis(&sft, rsp); rsp[rsp_length++] = nb << 1; for (i = address; i < address + nb; i++) { rsp[rsp_length++] = mb_mapping->tab_registers[i] >> 8; rsp[rsp_length++] = mb_mapping->tab_registers[i] & 0xFF; } } } break; case _FC_READ_INPUT_REGISTERS: { /* Similar to holding registers (but too many arguments to use a * function) */ int nb = (req[offset + 3] << 8) + req[offset + 4]; if (nb < 1 || MODBUS_MAX_READ_REGISTERS < nb) { if (ctx->debug) { fprintf(stderr, "Illegal number of values %d in read_input_registers (max %d)\n", nb, MODBUS_MAX_READ_REGISTERS); } rsp_length = response_exception( ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp); } else if ((address + nb) > mb_mapping->nb_input_registers) { if (ctx->debug) { fprintf(stderr, "Illegal data address %0X in read_input_registers\n", address + nb); } rsp_length = response_exception( ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp); } else { int i; rsp_length = ctx->backend->build_response_basis(&sft, rsp); rsp[rsp_length++] = nb << 1; for (i = address; i < address + nb; i++) { rsp[rsp_length++] = mb_mapping->tab_input_registers[i] >> 8; rsp[rsp_length++] = mb_mapping->tab_input_registers[i] & 0xFF; } } } break; case _FC_WRITE_SINGLE_COIL: if (address >= mb_mapping->nb_bits) { if (ctx->debug) { fprintf(stderr, "Illegal data address %0X in write_bit\n", address); } rsp_length = response_exception( ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp); } else { int data = (req[offset + 3] << 8) + req[offset + 4]; if (data == 0xFF00 || data == 0x0) { mb_mapping->tab_bits[address] = (data) ? ON : OFF; memcpy(rsp, req, req_length); rsp_length = req_length; } else { if (ctx->debug) { fprintf(stderr, "Illegal data value %0X in write_bit request at address %0X\n", data, address); } rsp_length = response_exception( ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp); } } break; case _FC_WRITE_SINGLE_REGISTER: if (address >= mb_mapping->nb_registers) { if (ctx->debug) { fprintf(stderr, "Illegal data address %0X in write_register\n", address); } rsp_length = response_exception( ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp); } else { int data = (req[offset + 3] << 8) + req[offset + 4]; mb_mapping->tab_registers[address] = data; memcpy(rsp, req, req_length); rsp_length = req_length; } break; case _FC_WRITE_MULTIPLE_COILS: { int nb = (req[offset + 3] << 8) + req[offset + 4]; if ((address + nb) > mb_mapping->nb_bits) { if (ctx->debug) { fprintf(stderr, "Illegal data address %0X in write_bits\n", address + nb); } rsp_length = response_exception( ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp); } else { /* 6 = byte count */ modbus_set_bits_from_bytes(mb_mapping->tab_bits, address, nb, &req[offset + 6]); rsp_length = ctx->backend->build_response_basis(&sft, rsp); /* 4 to copy the bit address (2) and the quantity of bits */ memcpy(rsp + rsp_length, req + rsp_length, 4); rsp_length += 4; } } break; case _FC_WRITE_MULTIPLE_REGISTERS: { int nb = (req[offset + 3] << 8) + req[offset + 4]; if ((address + nb) > mb_mapping->nb_registers) { if (ctx->debug) { fprintf(stderr, "Illegal data address %0X in write_registers\n", address + nb); } rsp_length = response_exception( ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp); } else { int i, j; for (i = address, j = 6; i < address + nb; i++, j += 2) { /* 6 and 7 = first value */ mb_mapping->tab_registers[i] = (req[offset + j] << 8) + req[offset + j + 1]; } rsp_length = ctx->backend->build_response_basis(&sft, rsp); /* 4 to copy the address (2) and the no. of registers */ memcpy(rsp + rsp_length, req + rsp_length, 4); rsp_length += 4; } } break; case _FC_REPORT_SLAVE_ID: { int str_len; int byte_count_pos; rsp_length = ctx->backend->build_response_basis(&sft, rsp); /* Skip byte count for now */ byte_count_pos = rsp_length++; rsp[rsp_length++] = _REPORT_SLAVE_ID; /* Run indicator status to ON */ rsp[rsp_length++] = 0xFF; /* LMB + length of LIBMODBUS_VERSION_STRING */ str_len = 3 + strlen(LIBMODBUS_VERSION_STRING); memcpy(rsp + rsp_length, "LMB" LIBMODBUS_VERSION_STRING, str_len); rsp_length += str_len; rsp[byte_count_pos] = rsp_length - byte_count_pos - 1; } break; case _FC_READ_EXCEPTION_STATUS: if (ctx->debug) { fprintf(stderr, "FIXME Not implemented\n"); } errno = ENOPROTOOPT; return -1; break; case _FC_WRITE_AND_READ_REGISTERS: { int nb = (req[offset + 3] << 8) + req[offset + 4]; uint16_t address_write = (req[offset + 5] << 8) + req[offset + 6]; int nb_write = (req[offset + 7] << 8) + req[offset + 8]; int nb_write_bytes = req[offset + 9]; if (nb_write < 1 || MODBUS_MAX_RW_WRITE_REGISTERS < nb_write || nb < 1 || MODBUS_MAX_READ_REGISTERS < nb || nb_write_bytes != nb_write * 2) { if (ctx->debug) { fprintf(stderr, "Illegal nb of values (W%d, R%d) in write_and_read_registers (max W%d, R%d)\n", nb_write, nb, MODBUS_MAX_RW_WRITE_REGISTERS, MODBUS_MAX_READ_REGISTERS); } rsp_length = response_exception( ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp); } else if ((address + nb) > mb_mapping->nb_registers || (address_write + nb_write) > mb_mapping->nb_registers) { if (ctx->debug) { fprintf(stderr, "Illegal data read address %0X or write address %0X write_and_read_registers\n", address + nb, address_write + nb_write); } rsp_length = response_exception(ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp); } else { int i, j; rsp_length = ctx->backend->build_response_basis(&sft, rsp); rsp[rsp_length++] = nb << 1; /* Write first. 10 and 11 are the offset of the first values to write */ for (i = address_write, j = 10; i < address_write + nb_write; i++, j += 2) { mb_mapping->tab_registers[i] = (req[offset + j] << 8) + req[offset + j + 1]; } /* and read the data for the response */ for (i = address; i < address + nb; i++) { rsp[rsp_length++] = mb_mapping->tab_registers[i] >> 8; rsp[rsp_length++] = mb_mapping->tab_registers[i] & 0xFF; } } } break; default: rsp_length = response_exception(ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_FUNCTION, rsp); break; } return send_msg(ctx, rsp, rsp_length); } int modbus_reply_exception(modbus_t *ctx, const uint8_t *req, unsigned int exception_code) { int offset = ctx->backend->header_length; int slave = req[offset - 1]; int function = req[offset]; uint8_t rsp[MAX_MESSAGE_LENGTH]; int rsp_length; int dummy_length = 99; sft_t sft; if (ctx->backend->filter_request(ctx, slave) == 1) { /* Filtered */ return 0; } sft.slave = slave; sft.function = function + 0x80;; sft.t_id = ctx->backend->prepare_response_tid(req, &dummy_length); rsp_length = ctx->backend->build_response_basis(&sft, rsp); /* Positive exception code */ if (exception_code < MODBUS_EXCEPTION_MAX) { rsp[rsp_length++] = exception_code; return send_msg(ctx, rsp, rsp_length); } else { errno = EINVAL; return -1; } } /* Reads IO status */ static int read_io_status(modbus_t *ctx, int function, int addr, int nb, uint8_t *dest) { int rc; int req_length; uint8_t req[_MIN_REQ_LENGTH]; uint8_t rsp[MAX_MESSAGE_LENGTH]; req_length = ctx->backend->build_request_basis(ctx, function, addr, nb, req); rc = send_msg(ctx, req, req_length); if (rc > 0) { int i, temp, bit; int pos = 0; int offset; int offset_end; rc = receive_msg(ctx, rsp, MSG_CONFIRMATION); if (rc == -1) return -1; rc = check_confirmation(ctx, req, rsp, rc); if (rc == -1) return -1; offset = ctx->backend->header_length + 2; offset_end = offset + rc; for (i = offset; i < offset_end; i++) { /* Shift reg hi_byte to temp */ temp = rsp[i]; for (bit = 0x01; (bit & 0xff) && (pos < nb);) { dest[pos++] = (temp & bit) ? TRUE : FALSE; bit = bit << 1; } } } return rc; } /* Reads the boolean status of bits and sets the array elements in the destination to TRUE or FALSE (single bits). */ int modbus_read_bits(modbus_t *ctx, int addr, int nb, uint8_t *dest) { int rc; if (nb > MODBUS_MAX_READ_BITS) { if (ctx->debug) { fprintf(stderr, "ERROR Too many bits requested (%d > %d)\n", nb, MODBUS_MAX_READ_BITS); } errno = EMBMDATA; return -1; } rc = read_io_status(ctx, _FC_READ_COILS, addr, nb, dest); if (rc == -1) return -1; else return nb; } /* Same as modbus_read_bits but reads the remote device input table */ int modbus_read_input_bits(modbus_t *ctx, int addr, int nb, uint8_t *dest) { int rc; if (nb > MODBUS_MAX_READ_BITS) { if (ctx->debug) { fprintf(stderr, "ERROR Too many discrete inputs requested (%d > %d)\n", nb, MODBUS_MAX_READ_BITS); } errno = EMBMDATA; return -1; } rc = read_io_status(ctx, _FC_READ_DISCRETE_INPUTS, addr, nb, dest); if (rc == -1) return -1; else return nb; } /* Reads the data from a remove device and put that data into an array */ static int read_registers(modbus_t *ctx, int function, int addr, int nb, uint16_t *dest) { int rc; int req_length; uint8_t req[_MIN_REQ_LENGTH]; uint8_t rsp[MAX_MESSAGE_LENGTH]; if (nb > MODBUS_MAX_READ_REGISTERS) { if (ctx->debug) { fprintf(stderr, "ERROR Too many registers requested (%d > %d)\n", nb, MODBUS_MAX_READ_REGISTERS); } errno = EMBMDATA; return -1; } req_length = ctx->backend->build_request_basis(ctx, function, addr, nb, req); rc = send_msg(ctx, req, req_length); if (rc > 0) { int offset; int i; rc = receive_msg(ctx, rsp, MSG_CONFIRMATION); if (rc == -1) return -1; rc = check_confirmation(ctx, req, rsp, rc); if (rc == -1) return -1; offset = ctx->backend->header_length; for (i = 0; i < rc; i++) { /* shift reg hi_byte to temp OR with lo_byte */ dest[i] = (rsp[offset + 2 + (i << 1)] << 8) | rsp[offset + 3 + (i << 1)]; } } return rc; } /* Reads the holding registers of remote device and put the data into an array */ int modbus_read_registers(modbus_t *ctx, int addr, int nb, uint16_t *dest) { int status; if (nb > MODBUS_MAX_READ_REGISTERS) { if (ctx->debug) { fprintf(stderr, "ERROR Too many registers requested (%d > %d)\n", nb, MODBUS_MAX_READ_REGISTERS); } errno = EMBMDATA; return -1; } status = read_registers(ctx, _FC_READ_HOLDING_REGISTERS, addr, nb, dest); return status; } /* Reads the input registers of remote device and put the data into an array */ int modbus_read_input_registers(modbus_t *ctx, int addr, int nb, uint16_t *dest) { int status; if (nb > MODBUS_MAX_READ_REGISTERS) { fprintf(stderr, "ERROR Too many input registers requested (%d > %d)\n", nb, MODBUS_MAX_READ_REGISTERS); errno = EMBMDATA; return -1; } status = read_registers(ctx, _FC_READ_INPUT_REGISTERS, addr, nb, dest); return status; } /* Write a value to the specified register of the remote device. Used by write_bit and write_register */ static int write_single(modbus_t *ctx, int function, int addr, int value) { int rc; int req_length; uint8_t req[_MIN_REQ_LENGTH]; req_length = ctx->backend->build_request_basis(ctx, function, addr, value, req); rc = send_msg(ctx, req, req_length); if (rc > 0) { /* Used by write_bit and write_register */ uint8_t rsp[MAX_MESSAGE_LENGTH]; rc = receive_msg(ctx, rsp, MSG_CONFIRMATION); if (rc == -1) return -1; rc = check_confirmation(ctx, req, rsp, rc); } return rc; } /* Turns ON or OFF a single bit of the remote device */ int modbus_write_bit(modbus_t *ctx, int addr, int status) { return write_single(ctx, _FC_WRITE_SINGLE_COIL, addr, status ? 0xFF00 : 0); } /* Writes a value in one register of the remote device */ int modbus_write_register(modbus_t *ctx, int addr, int value) { return write_single(ctx, _FC_WRITE_SINGLE_REGISTER, addr, value); } /* Write the bits of the array in the remote device */ int modbus_write_bits(modbus_t *ctx, int addr, int nb, const uint8_t *src) { int rc; int i; int byte_count; int req_length; int bit_check = 0; int pos = 0; uint8_t req[MAX_MESSAGE_LENGTH]; if (nb > MODBUS_MAX_WRITE_BITS) { if (ctx->debug) { fprintf(stderr, "ERROR Writing too many bits (%d > %d)\n", nb, MODBUS_MAX_WRITE_BITS); } errno = EMBMDATA; return -1; } req_length = ctx->backend->build_request_basis(ctx, _FC_WRITE_MULTIPLE_COILS, addr, nb, req); byte_count = (nb / 8) + ((nb % 8) ? 1 : 0); req[req_length++] = byte_count; for (i = 0; i < byte_count; i++) { int bit; bit = 0x01; req[req_length] = 0; while ((bit & 0xFF) && (bit_check++ < nb)) { if (src[pos++]) req[req_length] |= bit; else req[req_length] &=~ bit; bit = bit << 1; } req_length++; } rc = send_msg(ctx, req, req_length); if (rc > 0) { uint8_t rsp[MAX_MESSAGE_LENGTH]; rc = receive_msg(ctx, rsp, MSG_CONFIRMATION); if (rc == -1) return -1; rc = check_confirmation(ctx, req, rsp, rc); } return rc; } /* Write the values from the array to the registers of the remote device */ int modbus_write_registers(modbus_t *ctx, int addr, int nb, const uint16_t *src) { int rc; int i; int req_length; int byte_count; uint8_t req[MAX_MESSAGE_LENGTH]; if (nb > MODBUS_MAX_WRITE_REGISTERS) { if (ctx->debug) { fprintf(stderr, "ERROR Trying to write to too many registers (%d > %d)\n", nb, MODBUS_MAX_WRITE_REGISTERS); } errno = EMBMDATA; return -1; } req_length = ctx->backend->build_request_basis(ctx, _FC_WRITE_MULTIPLE_REGISTERS, addr, nb, req); byte_count = nb * 2; req[req_length++] = byte_count; for (i = 0; i < nb; i++) { req[req_length++] = src[i] >> 8; req[req_length++] = src[i] & 0x00FF; } rc = send_msg(ctx, req, req_length); if (rc > 0) { uint8_t rsp[MAX_MESSAGE_LENGTH]; rc = receive_msg(ctx, rsp, MSG_CONFIRMATION); if (rc == -1) return -1; rc = check_confirmation(ctx, req, rsp, rc); } return rc; } /* Write multiple registers from src array to remote device and read multiple registers from remote device to dest array. */ int modbus_write_and_read_registers(modbus_t *ctx, int write_addr, int write_nb, const uint16_t *src, int read_addr, int read_nb, uint16_t *dest) { int rc; int req_length; int i; int byte_count; uint8_t req[MAX_MESSAGE_LENGTH]; uint8_t rsp[MAX_MESSAGE_LENGTH]; if (write_nb > MODBUS_MAX_RW_WRITE_REGISTERS) { if (ctx->debug) { fprintf(stderr, "ERROR Too many registers to write (%d > %d)\n", write_nb, MODBUS_MAX_RW_WRITE_REGISTERS); } errno = EMBMDATA; return -1; } if (read_nb > MODBUS_MAX_READ_REGISTERS) { if (ctx->debug) { fprintf(stderr, "ERROR Too many registers requested (%d > %d)\n", read_nb, MODBUS_MAX_READ_REGISTERS); } errno = EMBMDATA; return -1; } req_length = ctx->backend->build_request_basis(ctx, _FC_WRITE_AND_READ_REGISTERS, read_addr, read_nb, req); req[req_length++] = write_addr >> 8; req[req_length++] = write_addr & 0x00ff; req[req_length++] = write_nb >> 8; req[req_length++] = write_nb & 0x00ff; byte_count = write_nb * 2; req[req_length++] = byte_count; for (i = 0; i < write_nb; i++) { req[req_length++] = src[i] >> 8; req[req_length++] = src[i] & 0x00FF; } rc = send_msg(ctx, req, req_length); if (rc > 0) { int offset; rc = receive_msg(ctx, rsp, MSG_CONFIRMATION); if (rc == -1) return -1; rc = check_confirmation(ctx, req, rsp, rc); if (rc == -1) return -1; offset = ctx->backend->header_length; /* If rc is negative, the loop is jumped ! */ for (i = 0; i < rc; i++) { /* shift reg hi_byte to temp OR with lo_byte */ dest[i] = (rsp[offset + 2 + (i << 1)] << 8) | rsp[offset + 3 + (i << 1)]; } } return rc; } /* Send a request to get the slave ID of the device (only available in serial communication). */ int modbus_report_slave_id(modbus_t *ctx, uint8_t *dest) { int rc; int req_length; uint8_t req[_MIN_REQ_LENGTH]; req_length = ctx->backend->build_request_basis(ctx, _FC_REPORT_SLAVE_ID, 0, 0, req); /* HACKISH, addr and count are not used */ req_length -= 4; rc = send_msg(ctx, req, req_length); if (rc > 0) { int i; int offset; uint8_t rsp[MAX_MESSAGE_LENGTH]; rc = receive_msg(ctx, rsp, MSG_CONFIRMATION); if (rc == -1) return -1; rc = check_confirmation(ctx, req, rsp, rc); if (rc == -1) return -1; offset = ctx->backend->header_length + 2; /* Byte count, slave id, run indicator status, additional data */ for (i=0; i < rc; i++) { dest[i] = rsp[offset + i]; } } return rc; } void _modbus_init_common(modbus_t *ctx) { /* Slave and socket are initialized to -1 */ ctx->slave = -1; ctx->s = -1; ctx->debug = FALSE; ctx->error_recovery = MODBUS_ERROR_RECOVERY_NONE; ctx->response_timeout.tv_sec = 0; ctx->response_timeout.tv_usec = _RESPONSE_TIMEOUT; ctx->byte_timeout.tv_sec = 0; ctx->byte_timeout.tv_usec = _BYTE_TIMEOUT; } /* Define the slave number */ int modbus_set_slave(modbus_t *ctx, int slave) { return ctx->backend->set_slave(ctx, slave); } int modbus_set_error_recovery(modbus_t *ctx, modbus_error_recovery_mode error_recovery) { /* The type of modbus_error_recovery_mode is unsigned enum */ ctx->error_recovery = (uint8_t) error_recovery; return 0; } void modbus_set_socket(modbus_t *ctx, int socket) { ctx->s = socket; } int modbus_get_socket(modbus_t *ctx) { return ctx->s; } /* Get the timeout interval used to wait for a response */ void modbus_get_response_timeout(modbus_t *ctx, struct timeval *timeout) { *timeout = ctx->response_timeout; } void modbus_set_response_timeout(modbus_t *ctx, const struct timeval *timeout) { ctx->response_timeout = *timeout; } /* Get the timeout interval between two consecutive bytes of a message */ void modbus_get_byte_timeout(modbus_t *ctx, struct timeval *timeout) { *timeout = ctx->byte_timeout; } void modbus_set_byte_timeout(modbus_t *ctx, const struct timeval *timeout) { ctx->byte_timeout = *timeout; } int modbus_get_header_length(modbus_t *ctx) { return ctx->backend->header_length; } int modbus_connect(modbus_t *ctx) { return ctx->backend->connect(ctx); } void modbus_close(modbus_t *ctx) { if (ctx == NULL) return; ctx->backend->close(ctx); } void modbus_free(modbus_t *ctx) { if (ctx == NULL) return; free(ctx->backend_data); free(ctx); } void modbus_set_debug(modbus_t *ctx, int boolean) { ctx->debug = boolean; } /* Allocates 4 arrays to store bits, input bits, registers and inputs registers. The pointers are stored in modbus_mapping structure. The modbus_mapping_new() function shall return the new allocated structure if successful. Otherwise it shall return NULL and set errno to ENOMEM. */ modbus_mapping_t* modbus_mapping_new(int nb_bits, int nb_input_bits, int nb_registers, int nb_input_registers) { modbus_mapping_t *mb_mapping; mb_mapping = (modbus_mapping_t *)malloc(sizeof(modbus_mapping_t)); if (mb_mapping == NULL) { return NULL; } /* 0X */ mb_mapping->nb_bits = nb_bits; if (nb_bits == 0) { mb_mapping->tab_bits = NULL; } else { /* Negative number raises a POSIX error */ mb_mapping->tab_bits = (uint8_t *) malloc(nb_bits * sizeof(uint8_t)); if (mb_mapping->tab_bits == NULL) { free(mb_mapping); return NULL; } memset(mb_mapping->tab_bits, 0, nb_bits * sizeof(uint8_t)); } /* 1X */ mb_mapping->nb_input_bits = nb_input_bits; if (nb_input_bits == 0) { mb_mapping->tab_input_bits = NULL; } else { mb_mapping->tab_input_bits = (uint8_t *) malloc(nb_input_bits * sizeof(uint8_t)); if (mb_mapping->tab_input_bits == NULL) { free(mb_mapping->tab_bits); free(mb_mapping); return NULL; } memset(mb_mapping->tab_input_bits, 0, nb_input_bits * sizeof(uint8_t)); } /* 4X */ mb_mapping->nb_registers = nb_registers; if (nb_registers == 0) { mb_mapping->tab_registers = NULL; } else { mb_mapping->tab_registers = (uint16_t *) malloc(nb_registers * sizeof(uint16_t)); if (mb_mapping->tab_registers == NULL) { free(mb_mapping->tab_input_bits); free(mb_mapping->tab_bits); free(mb_mapping); return NULL; } memset(mb_mapping->tab_registers, 0, nb_registers * sizeof(uint16_t)); } /* 3X */ mb_mapping->nb_input_registers = nb_input_registers; if (nb_input_registers == 0) { mb_mapping->tab_input_registers = NULL; } else { mb_mapping->tab_input_registers = (uint16_t *) malloc(nb_input_registers * sizeof(uint16_t)); if (mb_mapping->tab_input_registers == NULL) { free(mb_mapping->tab_registers); free(mb_mapping->tab_input_bits); free(mb_mapping->tab_bits); free(mb_mapping); return NULL; } memset(mb_mapping->tab_input_registers, 0, nb_input_registers * sizeof(uint16_t)); } return mb_mapping; } /* Frees the 4 arrays */ void modbus_mapping_free(modbus_mapping_t *mb_mapping) { if (mb_mapping == NULL) { return; } free(mb_mapping->tab_input_registers); free(mb_mapping->tab_registers); free(mb_mapping->tab_input_bits); free(mb_mapping->tab_bits); free(mb_mapping); } #ifndef HAVE_STRLCPY /* * Function strlcpy was originally developed by * Todd C. Miller to simplify writing secure code. * See ftp://ftp.openbsd.org/pub/OpenBSD/src/lib/libc/string/strlcpy.3 * for more information. * * Thank you Ulrich Drepper... not! * * Copy src to string dest of size dest_size. At most dest_size-1 characters * will be copied. Always NUL terminates (unless dest_size == 0). Returns * strlen(src); if retval >= dest_size, truncation occurred. */ size_t strlcpy(char *dest, const char *src, size_t dest_size) { register char *d = dest; register const char *s = src; register size_t n = dest_size; /* Copy as many bytes as will fit */ if (n != 0 && --n != 0) { do { if ((*d++ = *s++) == 0) break; } while (--n != 0); } /* Not enough room in dest, add NUL and traverse rest of src */ if (n == 0) { if (dest_size != 0) *d = '\0'; /* NUL-terminate dest */ while (*s++) ; } return (s - src - 1); /* count does not include NUL */ } #endif