/* * Copyright © Stéphane Raimbault * * SPDX-License-Identifier: LGPL-2.1-or-later */ #include #include #include #include #include #ifndef _MSC_VER #include #endif #if defined(_WIN32) # define OS_WIN32 /* ws2_32.dll has getaddrinfo and freeaddrinfo on Windows XP and later. * minwg32 headers check WINVER before allowing the use of these */ # ifndef WINVER # define WINVER 0x0501 # endif /* Already set in modbus-tcp.h but it seems order matters in VS2005 */ # include # include # define SHUT_RDWR 2 # define close closesocket # define strdup _strdup #else # include # include #if defined(__OpenBSD__) || (defined(__FreeBSD__) && __FreeBSD__ < 5) # define OS_BSD # include #endif # include # include # include # include # include #endif #if !defined(MSG_NOSIGNAL) #define MSG_NOSIGNAL 0 #endif #if defined(_AIX) && !defined(MSG_DONTWAIT) #define MSG_DONTWAIT MSG_NONBLOCK #endif #include "modbus-private.h" #include #include "modbus-rtuoverudp-private.h" #include "modbus-rtuoverudp.h" #if HAVE_DECL_TIOCSRS485 || HAVE_DECL_TIOCM_RTS #include #endif #if HAVE_DECL_TIOCSRS485 #include #endif #ifdef OS_WIN32 static int _modbus_tcpoverudp_init_win32(void) { /* Initialise Windows Socket API */ WSADATA wsaData; if (WSAStartup(MAKEWORD(2, 2), &wsaData) != 0) { fprintf(stderr, "WSAStartup() returned error code %d\n", (unsigned int)GetLastError()); errno = EIO; return -1; } return 0; } #endif /* Table of CRC values for high-order byte */ static const uint8_t table_crc_hi[] = { 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40}; /* Table of CRC values for low-order byte */ static const uint8_t table_crc_lo[] = { 0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, 0x07, 0xC7, 0x05, 0xC5, 0xC4, 0x04, 0xCC, 0x0C, 0x0D, 0xCD, 0x0F, 0xCF, 0xCE, 0x0E, 0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09, 0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9, 0x1B, 0xDB, 0xDA, 0x1A, 0x1E, 0xDE, 0xDF, 0x1F, 0xDD, 0x1D, 0x1C, 0xDC, 0x14, 0xD4, 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3, 0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3, 0xF2, 0x32, 0x36, 0xF6, 0xF7, 0x37, 0xF5, 0x35, 0x34, 0xF4, 0x3C, 0xFC, 0xFD, 0x3D, 0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A, 0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38, 0x28, 0xE8, 0xE9, 0x29, 0xEB, 0x2B, 0x2A, 0xEA, 0xEE, 0x2E, 0x2F, 0xEF, 0x2D, 0xED, 0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26, 0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60, 0x61, 0xA1, 0x63, 0xA3, 0xA2, 0x62, 0x66, 0xA6, 0xA7, 0x67, 0xA5, 0x65, 0x64, 0xA4, 0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F, 0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB, 0x69, 0xA9, 0xA8, 0x68, 0x78, 0xB8, 0xB9, 0x79, 0xBB, 0x7B, 0x7A, 0xBA, 0xBE, 0x7E, 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5, 0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71, 0x70, 0xB0, 0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92, 0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54, 0x9C, 0x5C, 0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, 0x5A, 0x9A, 0x9B, 0x5B, 0x99, 0x59, 0x58, 0x98, 0x88, 0x48, 0x49, 0x89, 0x4B, 0x8B, 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C, 0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42, 0x43, 0x83, 0x41, 0x81, 0x80, 0x40}; /* Define the slave ID of the remote device to talk in master mode or set the * internal slave ID in slave mode */ static int _modbus_set_slave(modbus_t *ctx, int slave) { int max_slave = 255; /* Broadcast address is 0 (MODBUS_BROADCAST_ADDRESS) */ if (slave >= 0 && slave <= max_slave) { ctx->slave = slave; } else { errno = EINVAL; return -1; } return 0; } /* Builds a RTU request header */ static int _modbus_rtuoverudp_build_request_basis( modbus_t *ctx, int function, int addr, int nb, uint8_t *req) { assert(ctx->slave != -1); req[0] = ctx->slave; req[1] = function; req[2] = addr >> 8; req[3] = addr & 0x00ff; req[4] = nb >> 8; req[5] = nb & 0x00ff; return _MODBUS_RTU_PRESET_REQ_LENGTH; } /* Builds a RTU response header */ static int _modbus_rtuoverudp_build_response_basis(sft_t *sft, uint8_t *rsp) { /* In this case, the slave is certainly valid because a check is already * done in _modbus_rtuoverudp_listen */ rsp[0] = sft->slave; rsp[1] = sft->function; return _MODBUS_RTU_PRESET_RSP_LENGTH; } static uint16_t crc16(uint8_t *buffer, uint16_t buffer_length) { uint8_t crc_hi = 0xFF; /* high CRC byte initialized */ uint8_t crc_lo = 0xFF; /* low CRC byte initialized */ unsigned int i; /* will index into CRC lookup */ /* pass through message buffer */ while (buffer_length--) { i = crc_lo ^ *buffer++; /* calculate the CRC */ crc_lo = crc_hi ^ table_crc_hi[i]; crc_hi = table_crc_lo[i]; } return (crc_hi << 8 | crc_lo); } static int _modbus_rtuoverudp_prepare_response_tid(const uint8_t *req, int *req_length) { (*req_length) -= _MODBUS_RTU_CHECKSUM_LENGTH; /* No TID */ return 0; } static int _modbus_rtuoverudp_send_msg_pre(uint8_t *req, int req_length) { uint16_t crc = crc16(req, req_length); /* According to the MODBUS specs (p. 14), the low order byte of the CRC comes * first in the RTU message */ req[req_length++] = crc & 0x00FF; req[req_length++] = crc >> 8; return req_length; } static ssize_t _modbus_rtuoverudp_send(modbus_t *ctx, const uint8_t *req, int req_length) { /* MSG_NOSIGNAL Requests not to send SIGPIPE on errors on stream oriented sockets when the other end breaks the connection. The EPIPE error is still returned. */ return send(ctx->s, (const char *) req, req_length, MSG_NOSIGNAL); } static int _modbus_rtuoverudp_receive(modbus_t *ctx, uint8_t *req) { return _modbus_receive_msg(ctx, req, MSG_INDICATION); } static int _modbus_rtuoverudp_flush(modbus_t *); static ssize_t _modbus_rtuoverudp_recv(modbus_t *ctx, uint8_t *rsp, int rsp_length) { modbus_rtuoverudp_t *ctx_tcp; ctx_tcp = ctx->backend_data; if (ctx_tcp->offset == ctx_tcp->msg_len) { ctx_tcp->msg_len = recv(ctx->s, (char *)ctx_tcp->msg, 512, 0); ctx_tcp->offset = 0; if (ctx_tcp->msg_len < 0) { ctx_tcp->msg_len = 0; return -1; } } int offset = ctx_tcp->offset; int msg_len = ctx_tcp->msg_len; int n_left = msg_len - offset; int n_copy = (n_left > rsp_length) ? rsp_length : n_left; if (n_copy <= 0) { _modbus_rtuoverudp_flush(ctx); return -1; } memcpy((char *)rsp, ctx_tcp->msg + offset, n_copy); ctx_tcp->offset += n_copy; return n_copy; } static int _modbus_rtuoverudp_pre_check_confirmation(modbus_t *ctx, const uint8_t *req, const uint8_t *rsp, int rsp_length) { /* Check responding slave is the slave we requested (except for broacast * request) */ if (req[0] != rsp[0] && req[0] != MODBUS_BROADCAST_ADDRESS) { if (ctx->debug) { fprintf(stderr, "The responding slave %d isn't the requested slave %d\n", rsp[0], req[0]); } errno = EMBBADSLAVE; _modbus_rtuoverudp_flush(ctx); return -1; } else { return 0; } } /* The check_crc16 function shall return 0 if the message is ignored and the message length if the CRC is valid. Otherwise it shall return -1 and set errno to EMBBADCRC. */ static int _modbus_rtuoverudp_check_integrity(modbus_t *ctx, uint8_t *msg, const int msg_length) { uint16_t crc_calculated; uint16_t crc_received; int slave = msg[0]; /* Filter on the Modbus unit identifier (slave) in RTU mode to avoid useless * CRC computing. */ if (slave != ctx->slave && slave != MODBUS_BROADCAST_ADDRESS) { if (ctx->debug) { printf("Request for slave %d ignored (not %d)\n", slave, ctx->slave); } /* Following call to check_confirmation handles this error */ return 0; } crc_calculated = crc16(msg, msg_length - 2); crc_received = (msg[msg_length - 1] << 8) | msg[msg_length - 2]; /* Check CRC of msg */ if (crc_calculated == crc_received) { return msg_length; } else { if (ctx->debug) { fprintf(stderr, "ERROR CRC received 0x%0X != CRC calculated 0x%0X\n", crc_received, crc_calculated); } if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_PROTOCOL) { _modbus_rtuoverudp_flush(ctx); } errno = EMBBADCRC; return -1; } } static int _modbus_tcpoverudp_set_ipv4_options(int s) { return 0; int rc; int option; /* Set the TCP no delay flag */ /* SOL_TCP = IPPROTO_TCP */ option = 1; rc = setsockopt(s, IPPROTO_TCP, TCP_NODELAY, (const void *) &option, sizeof(int)); if (rc == -1) { return -1; } /* If the OS does not offer SOCK_NONBLOCK, fall back to setting FIONBIO to * make sockets non-blocking */ /* Do not care about the return value, this is optional */ #if !defined(SOCK_NONBLOCK) && defined(FIONBIO) #ifdef OS_WIN32 { /* Setting FIONBIO expects an unsigned long according to MSDN */ u_long loption = 1; ioctlsocket(s, FIONBIO, &loption); } #else option = 1; ioctl(s, FIONBIO, &option); #endif #endif #ifndef OS_WIN32 /** * Cygwin defines IPTOS_LOWDELAY but can't handle that flag so it's * necessary to workaround that problem. **/ /* Set the IP low delay option */ option = IPTOS_LOWDELAY; rc = setsockopt(s, IPPROTO_IP, IP_TOS, (const void *) &option, sizeof(int)); if (rc == -1) { return -1; } #endif return 0; } /* POSIX */ static int _connect(int sockfd, const struct sockaddr *addr, socklen_t addrlen, const struct timeval *ro_tv) { int rc = connect(sockfd, addr, addrlen); #ifdef OS_WIN32 int wsaError = 0; if (rc == -1) { wsaError = WSAGetLastError(); } if (wsaError == WSAEWOULDBLOCK || wsaError == WSAEINPROGRESS) { #else if (rc == -1 && errno == EINPROGRESS) { #endif fd_set wset; int optval; socklen_t optlen = sizeof(optval); struct timeval tv = *ro_tv; /* Wait to be available in writing */ FD_ZERO(&wset); FD_SET(sockfd, &wset); rc = select(sockfd + 1, NULL, &wset, NULL, &tv); if (rc <= 0) { /* Timeout or fail */ return -1; } /* The connection is established if SO_ERROR and optval are set to 0 */ rc = getsockopt(sockfd, SOL_SOCKET, SO_ERROR, (void *) &optval, &optlen); if (rc == 0 && optval == 0) { return 0; } else { errno = ECONNREFUSED; return -1; } } return rc; } static int _modbus_rtuoverudp_connect(modbus_t *ctx) { int rc; /* Specialized version of sockaddr for Internet socket address (same size) */ struct sockaddr_in addr; modbus_rtuoverudp_t *ctx_tcp = ctx->backend_data; // int flags = SOCK_STREAM; int flags = SOCK_DGRAM; #ifdef OS_WIN32 if (_modbus_tcpoverudp_init_win32() == -1) { return -1; } #endif #ifdef SOCK_CLOEXEC flags |= SOCK_CLOEXEC; #endif #ifdef SOCK_NONBLOCK flags |= SOCK_NONBLOCK; #endif ctx->s = socket(PF_INET, flags, 0); if (ctx->s < 0) { return -1; } rc = _modbus_tcpoverudp_set_ipv4_options(ctx->s); if (rc == -1) { close(ctx->s); ctx->s = -1; return -1; } if (ctx->debug) { printf("Connecting to %s:%d\n", ctx_tcp->ip, ctx_tcp->port); } addr.sin_family = AF_INET; addr.sin_port = htons(ctx_tcp->port); rc = inet_pton(addr.sin_family, ctx_tcp->ip, &(addr.sin_addr)); if (rc <= 0) { if (ctx->debug) { fprintf(stderr, "Invalid IP address: %s\n", ctx_tcp->ip); } close(ctx->s); ctx->s = -1; return -1; } rc = _connect(ctx->s, (struct sockaddr *) &addr, sizeof(addr), &ctx->response_timeout); if (rc == -1) { close(ctx->s); ctx->s = -1; return -1; } return 0; } static void _modbus_rtuoverudp_close(modbus_t *ctx) { if (ctx->s >= 0) { //shutdown(ctx->s, SHUT_RDWR); close(ctx->s); ctx->s = -1; } } static int _modbus_rtuoverudp_flush(modbus_t *ctx) { int rc; int rc_sum = 0; do { /* Extract the garbage from the socket */ char devnull[MODBUS_TCP_MAX_ADU_LENGTH]; #ifndef OS_WIN32 rc = recv(ctx->s, devnull, MODBUS_TCP_MAX_ADU_LENGTH, MSG_DONTWAIT); #else /* On Win32, it's a bit more complicated to not wait */ fd_set rset; struct timeval tv; tv.tv_sec = 0; tv.tv_usec = 0; FD_ZERO(&rset); FD_SET(ctx->s, &rset); rc = select(ctx->s + 1, &rset, NULL, NULL, &tv); if (rc == -1) { return -1; } if (rc == 1) { /* There is data to flush */ rc = recv(ctx->s, devnull, MODBUS_TCP_MAX_ADU_LENGTH, 0); } #endif if (rc > 0) { rc_sum += rc; } } while (rc == MODBUS_TCP_MAX_ADU_LENGTH); modbus_rtuoverudp_t *ctx_tcp; ctx_tcp = ctx->backend_data; ctx_tcp->msg_len = 0; ctx_tcp->offset = 0; memset(ctx_tcp->msg, 0, sizeof(ctx_tcp->msg)); return rc_sum; } static int _modbus_rtuoverudp_select(modbus_t *ctx, fd_set *rset, struct timeval *tv, int length_to_read) { modbus_rtuoverudp_t *ctx_tcp; ctx_tcp = ctx->backend_data; if (ctx_tcp->offset < ctx_tcp->msg_len) { return 1; } int s_rc; while ((s_rc = select(ctx->s + 1, rset, NULL, NULL, tv)) == -1) { if (errno == EINTR) { if (ctx->debug) { fprintf(stderr, "A non blocked signal was caught\n"); } /* Necessary after an error */ FD_ZERO(rset); FD_SET(ctx->s, rset); } else { return -1; } } if (s_rc == 0) { /* Timeout */ errno = ETIMEDOUT; return -1; } return s_rc; } static void _modbus_rtuoverudp_free(modbus_t *ctx) { if (ctx->backend_data) { free(ctx->backend_data); } free(ctx); } // clang-format off const modbus_backend_t _modbus_rtuoverudp_backend = { _MODBUS_BACKEND_TYPE_RTU, _MODBUS_RTU_HEADER_LENGTH, _MODBUS_RTU_CHECKSUM_LENGTH, MODBUS_RTU_MAX_ADU_LENGTH, _modbus_set_slave, _modbus_rtuoverudp_build_request_basis, _modbus_rtuoverudp_build_response_basis, _modbus_rtuoverudp_prepare_response_tid, _modbus_rtuoverudp_send_msg_pre, _modbus_rtuoverudp_send, _modbus_rtuoverudp_receive, _modbus_rtuoverudp_recv, NULL, _modbus_rtuoverudp_check_integrity, _modbus_rtuoverudp_pre_check_confirmation, _modbus_rtuoverudp_connect, _modbus_rtuoverudp_close, _modbus_rtuoverudp_flush, _modbus_rtuoverudp_select, _modbus_rtuoverudp_free }; // clang-format on modbus_t *modbus_new_rtuoverudp(const char *ip, int port) { modbus_t *ctx; modbus_rtuoverudp_t *ctx_tcp; size_t dest_size; size_t ret_size; #if defined(OS_BSD) /* MSG_NOSIGNAL is unsupported on *BSD so we install an ignore handler for SIGPIPE. */ struct sigaction sa; sa.sa_handler = SIG_IGN; if (sigaction(SIGPIPE, &sa, NULL) < 0) { /* The debug flag can't be set here... */ fprintf(stderr, "Could not install SIGPIPE handler.\n"); return NULL; } #endif ctx = (modbus_t *) malloc(sizeof(modbus_t)); if (ctx == NULL) { return NULL; } _modbus_init_common(ctx); /* Could be changed after to reach a remote serial Modbus device */ ctx->slave = MODBUS_TCP_SLAVE; ctx->backend = &_modbus_rtuoverudp_backend; ctx->backend_data = (modbus_rtuoverudp_t *) malloc(sizeof(modbus_rtuoverudp_t)); if (ctx->backend_data == NULL) { modbus_free(ctx); errno = ENOMEM; return NULL; } ctx_tcp = (modbus_rtuoverudp_t *) ctx->backend_data; if (ip != NULL) { dest_size = sizeof(char) * 16; ret_size = strlcpy(ctx_tcp->ip, ip, dest_size); if (ret_size == 0) { fprintf(stderr, "The IP string is empty\n"); modbus_free(ctx); errno = EINVAL; return NULL; } if (ret_size >= dest_size) { fprintf(stderr, "The IP string has been truncated\n"); modbus_free(ctx); errno = EINVAL; return NULL; } } else { ctx_tcp->ip[0] = '0'; } ctx_tcp->port = port; ctx_tcp->t_id = 0; ctx_tcp->msg_len = 0; ctx_tcp->offset = 0; return ctx; }