From 90b4a84e545279ca24b5064a6e8f1dcf4d749e94 Mon Sep 17 00:00:00 2001 From: Rick van Rein Date: Thu, 18 Jul 2019 16:35:56 +0200 Subject: [PATCH] Added mbusio, a client for Modbus TCP Communicate in terms of SLAVE,FUNCTION,DATA in binary - mbusio adds MBAP header except for the SLAVE address - Modbus TCP multiplexes, removes MBAP header, adds Modbus RTU checksums - mbusio expects its responses in the order of its queries - mbusio checks txnid and protoid but not SLAVE address in the response --- Makefile | 9 ++- mbusio.c | 109 +++++++++++++++++++++++++++++++++ socket.c | 207 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ socket.h | 81 ++++++++++++++++++++++++ 4 files changed, 405 insertions(+), 1 deletion(-) create mode 100644 mbusio.c create mode 100644 socket.c create mode 100644 socket.h diff --git a/Makefile b/Makefile index 42c7d11..a337f7e 100644 --- a/Makefile +++ b/Makefile @@ -1,7 +1,11 @@ PREFIX ?= /usr/local DESTDIR ?= -TARGETS=hexin hexout devio llcio pcscio +TARGETS=hexin hexout devio mbusio +# Dropped llcio, since LLC is not commonly used +# TARGETS += llcio +# Dropped pcscio, as it requires uncommon headers +# TARGETS += pcscio LITERAL=derdump all: $(TARGETS) @@ -25,6 +29,9 @@ llcio: llcio.c pcscio: pcscio.c $(CC) -I /usr/include/PCSC $(CFLAGS) $(PCSCFLAGS) -o pcscio pcscio.c -lpcsclite +mbusio: mbusio.c socket.c + $(CC) $(CFLAGS) -o mbusio mbusio.c socket.c + install: all install $(TARGETS) $(LITERAL) "$(DESTDIR)$(PREFIX)/sbin" diff --git a/mbusio.c b/mbusio.c new file mode 100644 index 0000000..d6e2a3b --- /dev/null +++ b/mbusio.c @@ -0,0 +1,109 @@ +/* mbusio.c -- input/output through Modbus TCP */ + +#include +#include +#include +#include + +#include +#include + +#include "socket.h" + + +int get16 (char *msg) { + return ((int) msg [0]) | (((int) msg [1]) << 8); +} + +void set16 (char *msg, int val) { + msg [0] = val & 0x00ff; + msg [1] = (val >> 8) & 0x00ff; +} + + +int main (int argc, char *argv []) { + fd_set sel; + int busy=1; + if ((argc < 1) || (argc > 3)) { + fprintf (stderr, "Usage: %s [addr [port]]\n where addr defaults to 127.0.0.1 and port defaults to 502\nSend and receive slave+PDU over Modbus TCP, so each message is formatted\nSLAVE/8,FUNCTION/8,DATA/n\nThe TCP headering is handled by this utility\n", argv [0]); + exit (1); + } + char *addr = "127.0.0.1"; + if (argc >= 2) { + addr = argv [1]; + } + char *port = "502"; + if (argc >= 3) { + port = argv [2]; + } + struct sockaddr_storage ss; + if (!socket_parse (addr, port, (struct sockaddr *) &ss)) { + perror ("Cannot parse address and/or port"); + exit (1); + } + int sox; + if (!socket_client ((struct sockaddr *) &ss, SOCK_STREAM, &sox)) { + perror ("Cannot connect to Modbus TCP"); + exit (1); + } + int txnid_send = 1; + int txnid_recv = 1; + while (busy) { + FD_ZERO (&sel); + FD_SET (sox, &sel); + FD_SET (0, &sel); + if (select (sox+1, &sel, NULL, NULL, NULL) < 0) { + perror ("Select failed"); + busy = 0; + } else { + if (FD_ISSET (sox, &sel)) { + char buf [6+256]; + int len = read (sox, buf, 6+256); + if (len < 0) { + perror ("Error reading"); + busy = 0; + } else if (len < 7) { + fprintf (stderr, "MBAP header too short\n"); + busy = 0; + } else if (get16 (buf+0) != txnid_recv % 65536) { + fprintf (stderr, "MBAP txnid is bad\n"); + busy = 0; + } else if (get16 (buf+2) != 0) { + fprintf (stderr, "MBAP protoid is bad\n"); + busy = 0; + } else if (get16 (buf+4) != len - 7) { + fprintf (stderr, "MBAP length is bad\n"); + busy = 0; + } else if (txnid_recv >= txnid_send) { + fprintf (stderr, "Reply without Query\n"); + busy = 0; + } else { + if (write (1, buf+6, len-6) < len-6) { + perror ("Partial write"); + busy = 0; + } + txnid_recv++; + } + } + if (FD_ISSET (0, &sel)) { + char buf [6+256]; + int len = read (0, buf+6, 256); + if (len < 0) { + perror ("Error on stdin"); + busy = 0; + } else { + set16 (buf+0, txnid_send % 65536); + set16 (buf+2, 0); + set16 (buf+4, len); + if (write (sox, buf, 6+len) < 6+len) { + perror ("Partial output"); + busy = 0; + } + txnid_send++; + } + } + } + } + close (sox); + return 0; +} diff --git a/socket.c b/socket.c new file mode 100644 index 0000000..97a4e5e --- /dev/null +++ b/socket.c @@ -0,0 +1,207 @@ +/* Socket utilities, including parsing and sockaddr juggling. + * + * From: Rick van Rein + */ + + +#include "socket.h" + +#include +#include +#include +#include +#include + +#include +#include + + +#ifdef DEBUG +# include +# define DPRINTF printf +#else +# define DPRINTF(...) +#endif + + +#ifndef PART_OF_KXOVER +#include +#define kxerrno errno +#endif + + +/* Given a socket address, determine its length. + * + * This function does not fail. + * + * TODO:inline + */ +socklen_t sockaddrlen (const struct sockaddr *sa) { + assert ((sa->sa_family == AF_INET6) || (sa->sa_family == AF_INET)); + if (sa->sa_family == AF_INET6) { + return sizeof (struct sockaddr_in6); + } else { + return sizeof (struct sockaddr_in ); + } +} + + +/* Store a raw address from a given family in a socket address, + * together with a port that may be set to 0 as a catch-all. + */ +bool socket_address (sa_family_t af, uint8_t *addr, uint16_t portnr, struct sockaddr *sa) { + sa->sa_family = af; + memset (sa, 0, sockaddrlen (sa)); + sa->sa_family = af; + switch (af) { + case AF_INET6: + memcpy (&((struct sockaddr_in6 *) sa)->sin6_addr, addr, 16); + ((struct sockaddr_in6 *) sa)->sin6_port = htons (portnr); + return true; + case AF_INET: +DPRINTF ("DEBUG: socket address (%d.%d.%d.%d, %d)\n", addr [0], addr [1], addr [2], addr [3], portnr); + memcpy (&((struct sockaddr_in *) sa)->sin_addr, addr, 4); + ((struct sockaddr_in *) sa)->sin_port = htons (portnr); + return true; + default: + break; + } + kxerrno = EINVAL; + return false; +} + + +/* Parse an address and port, and store them in a sockaddr of + * type AF_INET or AF_INET6. The space provided is large enough + * to hold either, as it is defined as a union. + * + * The opt_port may be NULL, in which case the port is set to 0 + * in the returned sockaddr; otherwise, its value is rejected + * if it is 0. + * + * We always try IPv6 address parsing first, but fallback to + * IPv4 if we have to, but that fallback is deprecated. The + * port will be syntax-checked and range-checked. + * + * Return true on success, or false with kxerrno set on error. + */ +bool socket_parse (char *addr, char *opt_port, struct sockaddr *out_sa) { + // + // Optional port parsing + uint16_t portnr = 0; + if (opt_port != NULL) { + long p = strtol (opt_port, &opt_port, 10); + if (*opt_port != '\0') { + kxerrno = EINVAL; + return false; + } + if ((p == LONG_MIN) || (p == LONG_MAX) || (p <= 0) || (p > 65535)) { + /* errno is ERANGE */ + kxerrno = errno; + return false; + } + portnr = (uint16_t) p; + } + // + // IPv6 address parsing + uint8_t raw_addr [16]; + switch (inet_pton (AF_INET6, addr, raw_addr)) { + case 1: + return socket_address (AF_INET6, raw_addr, portnr, out_sa); + case 0: + break; + default: + break; + } + // + // IPv4 address parsing + switch (inet_pton (AF_INET, addr, raw_addr)) { + case 1: + return socket_address (AF_INET, raw_addr, portnr, out_sa); + case 0: + break; + default: + break; + } + // + // Report EINVAL as an error condition + kxerrno = EINVAL; + return false; +} + + +/* Open a connection as a client, to the given address. Do not bind locally. + * + * Set contype to one SOCK_DGRAM, SOCK_STREAM or SOCK_SEQPACKET. + * + * The resulting socket is written to out_sox. + * + * Return true on success, or false with kxerrno set on failure. + * On error, *out_sox is set to -1. + */ +bool socket_client (const struct sockaddr *peer, int contype, int *out_sox) { + int sox = -1; + sox = socket (peer->sa_family, contype, 0); + if (sox < 0) { + goto fail; + } + if (connect (sox, peer, sockaddrlen (peer)) != 0) { + goto fail; + } +#ifdef PART_OF_KXOVER + int soxflags = fcntl (sox, F_GETFL, 0); + if (fcntl (sox, F_SETFL, soxflags | O_NONBLOCK) != 0) { + goto fail; + } +#endif + *out_sox = sox; + return true; +fail: + *out_sox = -1; + if (sox >= 0) { + close (sox); + } + return false; +} + + +/* Open a listening socket as a server, at the given address. + * + * Set contype to one of SOCK_DGRAM, SOCK_STREAM or SOCK_SEQPACKET. + * + * The resulting socket is written to out_sox. + * + * Return true on success, or false with kxerrno set on failure. + * On error, *out_sox is set to -1. + */ +bool socket_server (const struct sockaddr *mine, int contype, int *out_sox) { + int sox = -1; + sox = socket (mine->sa_family, contype, 0); + if (sox < 0) { + goto fail; + } + if (bind (sox, mine, sockaddrlen (mine)) != 0) { + goto fail; + } + if ((contype == SOCK_STREAM) || (contype == SOCK_SEQPACKET)) { + if (listen (sox, 10) != 0) { + goto fail; + } + } +#ifdef PART_OF_KXOVER + int soxflags = fcntl (sox, F_GETFL, 0); + if (fcntl (sox, F_SETFL, soxflags | O_NONBLOCK) != 0) { + goto fail; + } +#endif + *out_sox = sox; + return true; +fail: + *out_sox = -1; + if (sox >= 0) { + close (sox); + } + return false; +} + + diff --git a/socket.h b/socket.h new file mode 100644 index 0000000..1d38ae1 --- /dev/null +++ b/socket.h @@ -0,0 +1,81 @@ +/* Socket utilities, including parsing and sockaddr juggling. + * + * From: Rick van Rein + */ + + +#ifndef KXOVER_SOCKET_H +#define KXOVER_SOCKET_H + + +#include + +#include +#include + +#include + +#include + + +/* Error codes for the entire KXOVER package, for com_err(), see src/errors.et */ +typedef long kxerr_t; +extern kxerr_t kxerrno; + + +/* Given a socket address, determine its length. + * + * This function does not fail. + * + * TODO:inline + */ +socklen_t sockaddrlen (const struct sockaddr *sa); + + +/* Store a raw address from a given family in a socket address, + * together with a port that may be set to 0 as a catch-all. + */ +bool socket_address (sa_family_t af, uint8_t *addr, uint16_t portnr, struct sockaddr *sa); + + +/* Parse an address and port, and store them in a sockaddr of + * type AF_INET or AF_INET6. The space provided is large enough + * to hold either, as it is defined as a union. + * + * The opt_port may be NULL, in which case the port is set to 0 + * in the returned sockaddr; otherwise, its value is rejected + * if it is 0. + * + * We always try IPv6 address parsing first, but fallback to + * IPv4 if we have to, but that fallback is deprecated. The + * port will be syntax-checked and range-checked. + * + * Return true on success, or false with kxerrno set on error. + */ +bool socket_parse (char *addr, char *opt_port, struct sockaddr *out_sa); + + +/* Open a connection as a client, to the given address. Do not bind locally. + * + * Set contype to one SOCK_DGRAM, SOCK_STREAM or SOCK_SEQPACKET. + * + * The resulting socket is written to out_sox. + * + * Return true on success, or false with kxerrno set on failure. + */ +bool socket_client (const struct sockaddr *peer, int contype, int *out_sox); + + +/* Open a listening socket as a server, at the given address. + * + * Set contype to one of SOCK_DGRAM, SOCK_STREAM or SOCK_SEQPACKET. + * + * The resulting socket is written to out_sox. + * + * Return true on success, or false with kxerrno set on failure. + */ +bool socket_server (const struct sockaddr *mine, int contype, int *out_sox); + + +#endif /* KXOVER_SOCKET_H */ + -- 1.7.10.4