Qt-libmodus

/* * Copyright © 2001-2011 Stéphane Raimbault <stephane.raimbault@gmail.com> * * SPDX-License-Identifier: LGPL-2.1+ * * This library implements the Modbus protocol. * http://libmodbus.org/ */ #include <stdio.h> #include <string.h> #include <stdlib.h> #include <stdarg.h> #include <errno.h> #include <limits.h> #include <time.h> #ifndef _MSC_VER #include <unistd.h> #endif #include <libmodbus/config.h> #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"; case EMBBADSLAVE: return "Response not from requested slave"; 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"); } } } static void _sleep_response_timeout(modbus_t *ctx) { /* Response timeout is always positive */ #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) * 1000; while (nanosleep(&request, &remaining) == -1 && errno == EINTR) { request = remaining; } #endif } int modbus_flush(modbus_t *ctx) { int rc; if (ctx == NULL) { errno = EINVAL; return -1; } rc = ctx->backend->flush(ctx); if (rc != -1 && ctx->debug) { /* Not all backends are able to return the number of bytes flushed */ printf("Bytes flushed (%d)\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 MODBUS_FC_READ_COILS: case MODBUS_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 MODBUS_FC_WRITE_AND_READ_REGISTERS: case MODBUS_FC_READ_HOLDING_REGISTERS: case MODBUS_FC_READ_INPUT_REGISTERS: /* Header + 2 * nb values */ length = 2 + 2 * (req[offset + 3] << 8 | req[offset + 4]); break; case MODBUS_FC_READ_EXCEPTION_STATUS: length = 3; break; case MODBUS_FC_REPORT_SLAVE_ID: /* The response is device specific (the header provides the length) */ return MSG_LENGTH_UNDEFINED; case MODBUS_FC_MASK_WRITE_REGISTER: length = 7; break; 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); _sleep_response_timeout(ctx); modbus_connect(ctx); } else { _sleep_response_timeout(ctx); modbus_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 (ctx == NULL) { errno = EINVAL; return -1; } if (raw_req_length < 2 || raw_req_length > (MODBUS_MAX_PDU_LENGTH + 1)) { /* The raw request must contain function and slave at least and must not be longer than the maximum pdu length plus the slave address. */ 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 ---------- */ /* 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 <= MODBUS_FC_WRITE_SINGLE_REGISTER) { length = 4; } else if (function == MODBUS_FC_WRITE_MULTIPLE_COILS || function == MODBUS_FC_WRITE_MULTIPLE_REGISTERS) { length = 5; } else if (function == MODBUS_FC_MASK_WRITE_REGISTER) { length = 6; } else if (function == MODBUS_FC_WRITE_AND_READ_REGISTERS) { length = 9; } else { /* MODBUS_FC_READ_EXCEPTION_STATUS, MODBUS_FC_REPORT_SLAVE_ID */ length = 0; } } else { /* MSG_CONFIRMATION */ switch (function) { case MODBUS_FC_WRITE_SINGLE_COIL: case MODBUS_FC_WRITE_SINGLE_REGISTER: case MODBUS_FC_WRITE_MULTIPLE_COILS: case MODBUS_FC_WRITE_MULTIPLE_REGISTERS: length = 4; break; case MODBUS_FC_MASK_WRITE_REGISTER: length = 6; break; default: length = 1; }