From b2873df66df1b9a41603c06afaf40d2af4467567 Mon Sep 17 00:00:00 2001 From: Solomon Peachy Date: Sun, 25 Aug 2013 08:19:52 -0400 Subject: [PATCH] Initial AVS SoB code. --- Makefile | 47 +++ redirector.c | 1072 +++++++++++++++++++++++++++++++++++++++++++++++++ serialstats.c | 52 +++ 3 files changed, 1171 insertions(+) create mode 100644 Makefile create mode 100644 redirector.c create mode 100644 serialstats.c diff --git a/Makefile b/Makefile new file mode 100644 index 0000000..cfa538c --- /dev/null +++ b/Makefile @@ -0,0 +1,47 @@ +# Makefile +# +#-------------------------------------------------------------------------- +# +# Copyright (C) 2006 AbsoluteValue Systems, Inc. All Rights Reserved. +# +# The contents of this file are subject to the licensing terms +# between AbsoluteValue Systems, Inc., and licensees of +# AbsoluteValue Systems linux-wlan(tm). You may not use this file +# except in compliance with the said License. When in doubt, do not +# distribute the contents of this file without permission from +# AbsoluteValue Sytems, Inc. +# +# Software distributed under the License is distributed on an "AS +# IS" basis, WITHOUT WARRANTY OF ANY KIND, either express or +# implied. See the License for the specific language governing +# rights and limitations under the License. +# +# Any inquiries regarding this software may be directed to: +# +# AbsoluteValue Systems, Inc. +# info@linux-wlan.com +# http://www.linux-wlan.com +#-------------------------------------------------------------------------- + +CPPFLAGS= -Wall + +all: sobredird serialstats + +sobredird: redirector.o + $(CC) -o $@ $^ + +serialstats: serialstats.o + $(CC) -o $@ $^ + +%.o: %.c + $(CC) -c -Wall $(CFLAGS) $(CPPFLAGS) $< -o $@ + +install: + mkdir -p $(PREFIX)/bin + cp -f sobredird $(PREFIX)/bin + cp -f serialstats $(PREFIX)/bin + +distclean: clean + +clean: + rm -f core core.* *.o .*.o *.s *.a .depend tmp_make *~ sobredird serialstats diff --git a/redirector.c b/redirector.c new file mode 100644 index 0000000..b45a442 --- /dev/null +++ b/redirector.c @@ -0,0 +1,1072 @@ +/* +*-------------------------------------------------------------------------- +* +* Copyright (C) 2006 AbsoluteValue Systems, Inc. All Rights Reserved. +* +* The contents of this file are subject to the licensing terms +* between AbsoluteValue Systems, Inc., and licensees of +* AbsoluteValue Systems linux-wlan(tm). You may not use this file +* except in compliance with the said License. When in doubt, do not +* distribute the contents of this file without permission from +* AbsoluteValue Sytems, Inc. +* +* Software distributed under the License is distributed on an "AS +* IS" basis, WITHOUT WARRANTY OF ANY KIND, either express or +* implied. See the License for the specific language governing +* rights and limitations under the License. +* +* Any inquiries regarding this software may be directed to: +* +* AbsoluteValue Systems, Inc. +* info@linux-wlan.com +* http://www.linux-wlan.com +*-------------------------------------------------------------------------- +*/ + +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include + +/* Basic over-the-wire serial packet */ + +struct s_msg { + u_int16_t magic; + u_int16_t flags; + u_int16_t seqnum_pkt; + u_int16_t seqnum_dat; + u_int16_t pktlen; + u_int8_t data[]; +} __attribute__ ((packed)); + +#define SOB_MAGIC 0x0584 + +struct sob_config { + u_int32_t baud; + u_int32_t stopbits; + u_int32_t databits; + u_int8_t parity; + u_int8_t flow; + + struct in_addr ip; + u_int16_t port; +} __attribute__ ((packed)); + +#define MSG_FLAG_BREAK 0x0001 +#define MSG_FLAG_NONDATA 0x8000 +#define MSG_FLAG_CONFIG 0x4000 + +#define SERIAL_ESC_CHAR 0xff +#define SERIAL_ESC_CHAR2 0x00 +#define SERIAL_ESC_BREAK 0x00 + +#define min_t(type,x,y) \ + ({ type __x = (x); type __y = (y); __x < __y ? __x: __y; }) +#define max_t(type,x,y) \ + ({ type __x = (x); type __y = (y); __x > __y ? __x: __y; }) + +#define MAX_PACKET_LEN 1470 +#define BUF_LEN 4096 + +#define SERIAL_BAUD_CASE(__b__) case __b__: cfsetispeed(cfg, B ## __b__) ; \ + cfsetospeed(cfg, B ## __b__) ; \ + break + +#define SERIAL_DATA_CASE(__b__) case __b__: cfg->c_cflag |= CS ## __b__ ; \ + break + +/* ================================================================ */ + +static void dump_config(char *prefix, struct sob_config *cfg) +{ + syslog(LOG_INFO, "%s Far Host %s:%d Serial %d-%d%c%d Flow %c", + prefix, + inet_ntoa(cfg->ip), cfg->port, + cfg->baud, cfg->databits, cfg->parity, cfg->stopbits, + cfg->flow); +} + +static void process_msg(struct s_msg *msg_in, struct in_addr *far_ip) +{ + if (msg_in->flags & MSG_FLAG_CONFIG) { + struct sob_config *far_cfg = (struct sob_config *) msg_in->data; + + far_cfg->baud = ntohl(far_cfg->baud); + far_cfg->stopbits = ntohl(far_cfg->stopbits); + far_cfg->databits = ntohl(far_cfg->databits); + far_cfg->port = ntohs(far_cfg->port); + + syslog(LOG_INFO, "Rx Config from %s", inet_ntoa(*far_ip)); + dump_config("Rx Config: ", far_cfg); + } +} + +static void send_config(int far_sock, struct sob_config *far_cfg, + struct sockaddr_in *far_addr) +{ + static u_int8_t pkt_buf[MAX_PACKET_LEN]; + struct s_msg *msg_out = (struct s_msg *) pkt_buf; + int i; + + msg_out->flags = MSG_FLAG_NONDATA | MSG_FLAG_CONFIG; + msg_out->seqnum_pkt = 0; + msg_out->pktlen = sizeof(*far_cfg); + msg_out->magic = SOB_MAGIC; + + memcpy(msg_out->data, far_cfg, sizeof(*far_cfg)); + far_cfg = (struct sob_config *) msg_out->data; + + far_cfg->baud = htonl(far_cfg->baud); + far_cfg->stopbits = htonl(far_cfg->stopbits); + far_cfg->databits = htonl(far_cfg->databits); + far_cfg->port = htons(far_cfg->port); + + msg_out->magic = htons(msg_out->magic); + msg_out->flags = htons(msg_out->flags); + msg_out->seqnum_pkt = htons(msg_out->seqnum_pkt); + msg_out->seqnum_dat = htons(msg_out->seqnum_dat); + msg_out->pktlen = htons(msg_out->pktlen); + + i = sendto(far_sock, pkt_buf, + ntohs(msg_out->pktlen) + sizeof(struct s_msg), + MSG_DONTWAIT, (struct sockaddr*) far_addr, sizeof(*far_addr)); + if (i < 0) { + syslog(LOG_ERR, "cfg sendto failed: %m"); + } +} + +int setserial(struct termios *cfg, int ser_fd, int baud, + int databits, char parity, int stopbits, + int hw_flow, int sw_flow, int modemlines) +{ + int rval = 0; + + rval = tcgetattr(ser_fd, cfg); + if (rval) goto done; + + cfmakeraw(cfg); + + cfg->c_cc[VMIN] = 1; /* 1 byte minimum */ + cfg->c_cc[VTIME] = 0; /* No timeout */ + + switch(baud) { + SERIAL_BAUD_CASE(50); + SERIAL_BAUD_CASE(75); + SERIAL_BAUD_CASE(110); + SERIAL_BAUD_CASE(134); + SERIAL_BAUD_CASE(150); + SERIAL_BAUD_CASE(200); + SERIAL_BAUD_CASE(300); + SERIAL_BAUD_CASE(600); + SERIAL_BAUD_CASE(1200); + SERIAL_BAUD_CASE(1800); + SERIAL_BAUD_CASE(2400); + SERIAL_BAUD_CASE(4800); + SERIAL_BAUD_CASE(9600); + SERIAL_BAUD_CASE(19200); + SERIAL_BAUD_CASE(38400); + SERIAL_BAUD_CASE(57600); + SERIAL_BAUD_CASE(115200); +// SERIAL_BAUD_CASE(230400); + default: + fprintf(stderr, "Invalid serial baud rate specified (%d)\n", + baud); + rval = 1; + goto done; + } + + parity &= ~0x20; /* make things case insensitive */ + switch (parity) { + case 'N': + cfg->c_cflag &= ~PARENB; + break; + case 'E': + cfg->c_cflag |= PARENB; + cfg->c_cflag &= ~PARODD; + break; + case 'O': + cfg->c_cflag |= PARENB; + cfg->c_cflag |= PARODD; + break; + default: + fprintf(stderr, "Invalid serial parity specified (%c), should be E/O/N\n", + parity); + rval = 2; + goto done; + } + + cfg->c_cflag &= ~CSIZE; + switch(databits) { + SERIAL_DATA_CASE(5); + SERIAL_DATA_CASE(6); + SERIAL_DATA_CASE(7); + SERIAL_DATA_CASE(8); + default: + fprintf(stderr, "Invalid number of data bits specified (%d), should be between 5 and 8.\n", + databits); + rval = 3; + goto done; + } + + switch (stopbits) { + case 1: + cfg->c_cflag &= ~CSTOPB; + break; + case 2: + cfg->c_cflag |= CSTOPB; + break; + default: + fprintf(stderr, "Invalid number of stop bits specified (%d), should be 1 or 2.\n", + stopbits); + rval = 4; + goto done; + } + + cfg->c_iflag &= ~CRTSCTS; + cfg->c_iflag &= ~(IXON|IXOFF|IXANY); + + if (hw_flow) { + cfg->c_iflag |= CRTSCTS; + } + if (sw_flow) { + cfg->c_iflag |= (IXON|IXOFF|IXANY); + cfg->c_cc[VSTART] = 17; + cfg->c_cc[VSTOP] = 19; + } + + if (modemlines) { + cfg->c_cflag |= HUPCL; + cfg->c_cflag &= ~CLOCAL; + } else { + cfg->c_cflag |= CLOCAL; + cfg->c_cflag &= ~HUPCL; + } + + cfg->c_cflag |= CREAD; + + cfg->c_iflag |= PARMRK; + cfg->c_iflag &= ~(IGNPAR | BRKINT | IGNBRK); + + rval = tcsetattr(ser_fd, TCSANOW, cfg); + if (rval) goto done; + +done: + if (rval) fprintf(stderr, "ERROR.. %d\n", rval); + + { + struct termios cfg2; + tcgetattr(ser_fd, &cfg2); + if (cfg->c_iflag != cfg2.c_iflag) { + fprintf(stderr, "ERROR! c_iflag %x vs %x\n", + cfg->c_iflag, cfg2.c_iflag); + } + if (cfg->c_oflag != cfg2.c_oflag) { + fprintf(stderr, "ERROR! c_oflag %x vs %x\n", + cfg->c_oflag, cfg2.c_oflag); + } + if (cfg->c_cflag != cfg2.c_cflag) { + fprintf(stderr, "ERROR! c_cflag %x vs %x\n", + cfg->c_cflag, cfg2.c_cflag); + } + if (cfg->c_lflag != cfg2.c_lflag) { + fprintf(stderr, "ERROR! c_lflag %x vs %x\n", + cfg->c_lflag, cfg2.c_lflag); + } + } + + return rval; +} + +void printhelp() { + fprintf(stderr, " Serial Over Broadband Redirector\n (c) AbsoluteValue Systems 2006\n\n"); + fprintf(stderr, "\t-d -s -i -p \n"); + fprintf(stderr, "\t[ -b ] [ -t ]\n"); + fprintf(stderr, "\t[ -1 [ -2 ] ]\n"); + fprintf(stderr, "\t[ -x | -r ] [ -h ] [ -l ttl ] [ -L ] [ -D ]\n\n"); + fprintf(stderr, "\t-h (this message)\n\n"); + fprintf(stderr, "\t-x Enable XON/XOFF (software) flow control\n"); + fprintf(stderr, "\t-r Enable RTS/CTS (hardware) flow control\n"); + fprintf(stderr, "\t-D Run as a Daemon\n"); + fprintf(stderr, "\t-L Enable modem control lines\n"); + + fprintf(stderr, "\t-t # Max time to wait for full buffer (in ms, default 250)\n"); + fprintf(stderr, "\t-b # Min bytes to buffer (default 1)\n"); + fprintf(stderr, "\t-l # TTL for multicast packets (default 1)\n\n"); + + fprintf(stderr, "commsparams: -\n"); + fprintf(stderr, " examples include: 9600-8N1, 19200-6E2\n"); + fprintf(stderr, "delimeters: Specified in hex, ie 0x20 or 0xfe\n"); + fprintf(stderr, "\n"); +} + +static int run = 1; + +int main (int argc, char **argv) +{ + struct sob_config sob_cfg; + + /* Basic communications parameters */ + char *comm_params = NULL; + + char delim1 = 0; + char delim2 = 0; + int delim1_enable = 0; + int delim2_enable = 0; + + int modem_lines = 0; + + int multicast = 0; + + int hw_flow = 0; + int sw_flow = 0; + + int min_buf_size = 1; /* bytes */ + int max_buf_time = 1000; /* ms */ + + unsigned char mult_ttl = 1; + + struct sockaddr_in far_addr; + struct sockaddr_in my_addr; + + int far_sock; + + char *ser_dev = NULL; + char *far_host = NULL; + + int ser_fd; + struct termios ser_cfg; + + int daemonize = 0; + + int i; + + memset(&far_addr, 0, sizeof(far_addr)); + memset(&my_addr, 0, sizeof(my_addr)); + far_addr.sin_family = AF_INET; + my_addr.sin_family = AF_INET; + + /* Parse command line */ + while(1) { + int c; + c = getopt(argc, argv, "hd:s:i:p:b:t:xrl:D1:2:L"); + if (c == -1) break; + switch (c) { + case 'h': + printhelp(); + exit(0); + case 'd': + ser_dev = optarg; + break; + case 's': + comm_params = optarg; + break; + case 'i': + far_host = optarg; + break; + case 'p': + sob_cfg.port = atoi(optarg); + break; + case 'b': + min_buf_size = atoi(optarg); + break; + case 't': + max_buf_time = atoi(optarg); + break; + case 'x': + sw_flow = 1; + break; + case 'r': + hw_flow = 1; + break; + case 'D': + daemonize = 1; + break; + case 'l': + mult_ttl = atoi(optarg); + break; + case 'L': + modem_lines = 1; + break; + case '1': + delim1 = strtol(optarg, NULL, 16); + delim1_enable = 1; + break; + case '2': + delim2 = strtol(optarg, NULL, 16); + delim2_enable = 1; + break; + default: + fprintf(stderr, "getopt returned bogus option '%c'\n", c); + return 1; + } + } + if (!ser_dev || !far_host || !sob_cfg.port || !comm_params) { + fprintf(stderr, "ERROR: missing command-line arguments.\n\n"); + printhelp(); + return 2; + } + if (hw_flow && sw_flow) { + fprintf(stderr, "ERROR: can't use both hardware and software flow control.\n\n"); + printhelp(); + return 10; + } + if (delim2_enable && !delim1_enable) { + fprintf(stderr, "ERROR: Can't specify delim2 without delim1\n\n"); + return 11; + } + + if (hw_flow) + sob_cfg.flow = 'H'; + else if (sw_flow) + sob_cfg.flow = 'S'; + else + sob_cfg.flow = 'N'; + + /* Parse hostname */ + i = inet_aton(far_host, &far_addr.sin_addr); + if (!i) { + struct hostent *host; + host = gethostbyname(far_host); + if (!host) { + fprintf(stderr, "host lookup failed for '%s'\n", + far_host); + return 3; + } + memcpy(&far_addr.sin_addr.s_addr, host->h_addr, sizeof(far_addr.sin_addr.s_addr)); + } + + /* Find out if address is multicast */ + { + unsigned long int addr = ntohl(far_addr.sin_addr.s_addr); + + if ((addr >= 3758096384UL) && (addr < 4026531840UL)) { + multicast = 1; + } + + } + memcpy(&sob_cfg.ip, &far_addr.sin_addr.s_addr, sizeof(sob_cfg.ip)); + + /* Parse serial parameters */ + i = sscanf(comm_params, "%d-%d%c%d", &sob_cfg.baud, &sob_cfg.databits, + &sob_cfg.parity, &sob_cfg.stopbits); + if (i < 4) { + fprintf(stderr, "Invalid commparameters (%s)\n", comm_params); + printhelp(); + return 6; + } + sob_cfg.parity &= ~0x20; /* Case insensitive */ + + /* Open socket, bind it. */ + far_sock = socket(AF_INET, SOCK_DGRAM, 0); + if (far_sock < 0) { + perror("Could not open socket"); + return 4; + } + + /* TTL */ + if (multicast) { + struct ip_mreq mreq; + int flag = 0; + + mreq.imr_multiaddr.s_addr = far_addr.sin_addr.s_addr; + mreq.imr_interface.s_addr = htonl( INADDR_ANY ); + + setsockopt(far_sock,IPPROTO_IP,IP_ADD_MEMBERSHIP,&mreq,sizeof(mreq)); + +// my_addr.sin_addr = far_addr.sin_addr; + + i = setsockopt(far_sock, IPPROTO_IP, IP_MULTICAST_TTL, + &mult_ttl, sizeof(mult_ttl)); + if (i < 0) { + perror("Could not set TTL"); + return 10; + } + + i = setsockopt( far_sock, IPPROTO_IP, IP_MULTICAST_LOOP, + &flag, sizeof(flag)); + if(i < 0) { + perror("Could not turn off multicast loopback"); + return 11; + } + } + + /* Set non-block mode */ + i = fcntl(far_sock, F_GETFL, 0); + if (i < 0) { + perror("Couldn't get fcntl"); + return 11; + } + i |= O_NONBLOCK; + i = fcntl(far_sock, F_SETFL, i); + if (i < 0) { + perror("Couldn't set fcntl"); + return 12; + } + + far_addr.sin_port = htons(sob_cfg.port); + my_addr.sin_port = far_addr.sin_port; + + i = bind(far_sock, (struct sockaddr *) &my_addr, sizeof(my_addr)); + if (i < 0) { + perror("Could not bind socket to address"); + return 9; + } + + /* Open serial port */ + ser_fd = open(ser_dev,O_RDWR|O_NONBLOCK|O_NOCTTY|O_ASYNC); + if (ser_fd < 0) { + perror("Could not open serial device"); + return 5; + } + + if (!isatty(ser_fd)) { + perror("Not a TTY\n"); + return 999; + } + + /* Set serial Parameters */ + i = setserial(&ser_cfg, ser_fd, sob_cfg.baud, sob_cfg.databits, + sob_cfg.parity, sob_cfg.stopbits, + hw_flow, sw_flow, modem_lines); + if (i > 0) { + printhelp(); + return 7; + } + + if (i < 0) { + perror("Could not initialize serial device"); + return 8; + } + + /* Turn off Break */ + i = ioctl(ser_fd, TIOCCBRK); + if (i < 0) { + perror("Could not turn off BREAK"); + return 18; + } + + /* Ignore sigpipes */ + signal(SIGPIPE, SIG_IGN); + + if (daemonize) daemon(0, 0); + + /* And at this point, start! */ + openlog("sob", LOG_PID, LOG_DAEMON); + + /* Serial State */ + int rx_delim = 0; // 0 == no, 1 == rx1, 2 == rx2 + int rx_esc = 0; // 0 == no, 1 == yes, 2 == next is special. + int ser_brk = 0; /* Received a break on the serial port */ + + u_int16_t sequence_pkt_rx = 0; + u_int16_t sequence_pkt_tx = 0; + u_int16_t sequence_dat_rx = 0; + u_int16_t sequence_dat_tx = 0; + + static u_int8_t pkt_buf[MAX_PACKET_LEN]; + struct pollfd pfds[2]; + + static u_int8_t ser_out_buf[BUF_LEN]; + u_int16_t ser_out_head = 0; + u_int16_t ser_out_tail = 0; + + static u_int8_t net_out_buf[BUF_LEN]; + u_int16_t net_out_head = 0; + u_int16_t net_out_tail = 0; + + struct timeval last_rx_ser; + struct timeval last_rx_net; + + /* Set up the poll */ + pfds[0].fd = far_sock; + pfds[0].revents = 0; + pfds[0].events = POLLIN|POLLPRI; + pfds[1].fd = ser_fd; + pfds[1].revents = 0; + pfds[1].events = POLLIN|POLLPRI; + + syslog(LOG_INFO, "sobredird WIP3 bound and active"); + dump_config("Local config: ", &sob_cfg); + + send_config(far_sock, &sob_cfg, &far_addr); + +#define TV_FLATTEN(__tv__) ((__tv__.tv_sec * 1000000) + __tv__.tv_usec) + + /* Now we can run */ + while (run) { + struct timeval now; + int64_t delta1, delta2; + u_int16_t len, len2; + + /* Compute the actual time we need to sleep; + min of (max_buf_time, max_buf_time - (now-last_rx_net), + max_buf_time - (now-last_rx_ser) */ + + gettimeofday(&now, NULL); + delta1 = delta2 = 1000 * max_buf_time; + + if (net_out_head - net_out_tail) { + delta1 += TV_FLATTEN(last_rx_ser) - TV_FLATTEN(now); + if (delta1 < 0) { + // fprintf(stderr, "pre time watermark, setting net out\n"); + pfds[0].events |= POLLOUT; + delta1 = 0; + } + + } + if (ser_out_head - ser_out_tail) { + delta2 += TV_FLATTEN(last_rx_net) - TV_FLATTEN(now); + if (delta2 < 0) { + pfds[1].events |= POLLOUT; + delta2 = 0; + } + } + + pfds[0].revents = 0; + pfds[1].revents = 0; + + i = poll(pfds, 2, min_t(int64_t, delta1, delta2) / 1000); + +#if 0 + { + fd_set rfds; + fd_set wfds; + int v = 0, j = 0; + FD_ZERO(&rfds); + FD_ZERO(&wfds); + for (j = 0; j < 2 ; j++) { + FD_SET(pfds[j].fd, &rfds); + FD_SET(pfds[j].fd, &wfds); + if (pfds[j].fd > v) + v = pfds[j].fd; + } + v = select(v+1, &rfds, &wfds, NULL, NULL); + for (j = 0; j < 2 ; j++) { + if (FD_ISSET(pfds[j].fd, &rfds)) + fprintf(stderr, "rfd %d %d set\n", j, pfds[i].fd); + if (FD_ISSET(pfds[j].fd, &wfds)) + fprintf(stderr, "wfd %d %d set\n", j, pfds[i].fd); + + } + } +#endif + +#if 0 + { +// XXX this lets us get serial line twiddles: +// TIOCM_CD TIOCM_RI TIOCM_DSR TIOCM_CTS // TIOCM_DTR TIOCM_RTS + int val = 0, j = 0; + j = ioctl(ser_fd, TIOCMGET, &val); + if (j < 0) { + fprintf(stderr, "TIOCMGET failed %m\n"); + } else { + fprintf(stderr, "mflag %d %#x\n", j, val); + } + } +#endif + + /* If we got a BREAK signal, do the right thing. */ + if (ser_brk && !(pfds[0].events & POLLOUT)) { + // fprintf(stderr, "ser_break, setting net out\n"); + pfds[0].events |= POLLOUT; + continue; + } + + gettimeofday(&now, NULL); + + /* If the poll timed out, see if we hit a watermark point */ + if (i == 0) { + delta1 = delta2 = 1000 * max_buf_time; + if (net_out_head - net_out_tail) { + delta1 += TV_FLATTEN(last_rx_ser) - TV_FLATTEN(now); + if (delta1 < 0) { + // fprintf(stderr, "pre time watermark, setting net out\n"); + pfds[0].events |= POLLOUT; + } + + } + if (ser_out_head - ser_out_tail) { + delta2 += TV_FLATTEN(last_rx_net) - TV_FLATTEN(now); + if (delta2 < 0) { + pfds[1].events |= POLLOUT; + } + } + /* And turn around immediately and re-poll */ + continue; + } + + /* Handle serial port reset */ + if (pfds[1].revents & (POLLERR|POLLHUP|POLLNVAL)) { + syslog(LOG_ERR, "Serial port reset, purging buffers (%d)\n", pfds[1].revents); + + tcflush(ser_fd, TCIOFLUSH); + + close(ser_fd); + ser_fd = open(ser_dev,O_RDWR|O_NONBLOCK|O_NOCTTY|O_ASYNC); + + ser_out_head = ser_out_tail = 0; + net_out_head = net_out_tail = 0; + + if (ser_fd < 0) { + syslog(LOG_CRIT, "Could not open serial device, terminating: %m"); + return 5; + } + + i = tcsetattr(ser_fd, TCSANOW, &ser_cfg); + if (i < 0) { + syslog(LOG_CRIT, "Could not initialize serial device: %m"); + } + pfds[1].fd = ser_fd; + continue; + } + + /* Handle the serial port OUT */ + if ((pfds[1].revents & POLLOUT) && (pfds[1].events & POLLOUT)) { + len = min_t(u_int16_t, BUF_LEN - (ser_out_tail % BUF_LEN), + ser_out_head - ser_out_tail); + i = write(ser_fd, &ser_out_buf[ser_out_tail % BUF_LEN], len); + if (i < 0) { + syslog(LOG_ERR, "write failed: %m"); + } else { + ser_out_tail += i; + } + + if (ser_out_head == ser_out_tail) { + pfds[1].events &= ~POLLOUT; + } + // fprintf(stdout, "ser out, %d bytes, ser_qlen %d\n", i, ser_out_head - ser_out_tail); + } + + /* Handle the network socket OUT */ + if ((pfds[0].revents & POLLOUT) && (pfds[0].events & POLLOUT)) { + struct s_msg *msg_out = (struct s_msg *) pkt_buf; + + msg_out->magic = SOB_MAGIC; + msg_out->flags = 0; + msg_out->seqnum_pkt = sequence_pkt_tx++; + msg_out->pktlen = 0; + msg_out->seqnum_dat = sequence_dat_tx; + + if (ser_brk) { + // fprintf(stderr, "BREAK, sending to far end\n"); + msg_out->flags |= MSG_FLAG_BREAK; + ser_out_head = ser_out_tail = 0; + net_out_head = net_out_tail = 0; + tcflush(ser_fd, TCIOFLUSH); + ser_brk = 0; + } + + /* Copy over to buffer; handle wraps! */ + len = min_t(u_int16_t, BUF_LEN - (net_out_tail % BUF_LEN), + net_out_head - net_out_tail); + len = min_t(u_int16_t, len, MAX_PACKET_LEN - sizeof(struct s_msg)); + + memcpy(&msg_out->data[msg_out->pktlen], + &net_out_buf[net_out_tail % BUF_LEN], len); + + net_out_tail += len; + msg_out->pktlen += len; + sequence_dat_tx += len; + + /* Handle wrap */ + len2 = min_t(u_int16_t, BUF_LEN - (net_out_tail % BUF_LEN), + net_out_head - net_out_tail); + len2 = min_t(u_int16_t, len2, MAX_PACKET_LEN - sizeof(struct s_msg) - msg_out->pktlen); + memcpy(&msg_out->data[msg_out->pktlen], + &net_out_buf[net_out_tail % BUF_LEN], len2); + net_out_tail += len2; + msg_out->pktlen += len2; + sequence_dat_tx += len2; + + /* Byteswap and send */ + msg_out->magic = htons(msg_out->magic); + msg_out->flags = htons(msg_out->flags); + msg_out->seqnum_pkt = htons(msg_out->seqnum_pkt); + msg_out->seqnum_dat = htons(msg_out->seqnum_dat); + msg_out->pktlen = htons(msg_out->pktlen); + + i = sendto(far_sock, pkt_buf, + ntohs(msg_out->pktlen) + sizeof(struct s_msg), + MSG_DONTWAIT, (struct sockaddr*) &far_addr, sizeof(far_addr)); + if (i < 0) { + syslog(LOG_ERR, "data sendto failed: %m"); + } + + /* Mark outbound Network to include OUT */ + if ((net_out_head - net_out_tail) < min_buf_size) { + // fprintf(stderr, "clearing net out\n"); + pfds[0].events &= ~POLLOUT; + } + + // fprintf(stdout, "net out (%d bytes), net_qlen %d\n", ntohs(msg_out->pktlen), net_out_head - net_out_tail); + } + + /* Handle the serial port IN */ + if (pfds[1].revents & (POLLIN|POLLPRI)) { +// fprintf(stdout, "ser in\n"); + u_int8_t c; + + len = net_out_head - net_out_tail; /* size of data in buffer */ + if (len >= BUF_LEN) { + syslog(LOG_ERR, "Rx Serial Buffer Full... (%d bytes)", net_out_head - net_out_tail); + continue; + } + len = BUF_LEN - len; + + while ((len > 0) && ((i = read(ser_fd, &c, sizeof(c))) > 0)) { + last_rx_ser = now; + + switch (rx_esc) { + case 0: + if (c == SERIAL_ESC_CHAR) { + /* Don't pass up */ + rx_esc++; + continue; + } else { + /* Normal data */ + net_out_buf[net_out_head++ % BUF_LEN] = c; + len--; + } + break; + case 1: + /* SERIAL_ESC_CHAR 0x?? */ + if (c == SERIAL_ESC_CHAR) { + /* This is an escaped SERIAL_ESC_CHAR! */ + net_out_buf[net_out_head++ % BUF_LEN] = c; + len--; + rx_esc = 0; + } else if (c == SERIAL_ESC_CHAR2) { + /* Don't pass up */ + rx_esc++; + } + break; + case 2: + /* SERIAL_ESC_CHAR SERIAL_ESC_CHAR2 0x?? */ + if (c == SERIAL_ESC_BREAK) { + /* Break! */ + ser_brk++; + rx_esc = 0; + syslog(LOG_INFO, "Rx serial break!\n"); + continue; + } else { + /* All others are parity errors, ignore */ + continue; + } + break; + + default: + rx_esc = 0; + // ERROR. + break; + } + + /* If we got here, the character was legit data */ + // Check for and handle delimeter detection + + switch (rx_delim) { + case 0: + if (delim1_enable && (c == delim1)) + rx_delim++; + else + rx_delim = 0; + break; + case 1: + if (delim2_enable && (c == delim2)) + rx_delim++; + else + rx_delim = 0; + break; + default: + rx_delim = 0; + // ERROR; + break; + } + + if ((delim1_enable && !delim2_enable && (rx_delim == 1)) || + (delim1_enable && delim2_enable && (rx_delim == 2))) { + pfds[0].events |= POLLOUT; + rx_delim = 0; + } + } + + /* Merk outbound network to include OUT */ + if ((net_out_head - net_out_tail) >= min_buf_size) { + // fprintf(stderr, "marking net out\n"); + pfds[0].events |= POLLOUT; + } +// fprintf(stdout, "ser in, net_qlen %d\n", net_out_head - net_out_tail); + } + + /* Handle the network socket IN */ + if (pfds[0].revents & (POLLIN|POLLPRI)) { /* Read from Network */ + struct sockaddr_in remote_addr; + struct s_msg *msg_in = (struct s_msg *) pkt_buf; + socklen_t socklen = sizeof(remote_addr); + + i = recvfrom(far_sock, pkt_buf, MAX_PACKET_LEN, MSG_DONTWAIT, + (struct sockaddr*) &remote_addr, &socklen); + // fprintf(stdout, "net in len %d\n", i); + + last_rx_net = now; + + if (i < 0) { + perror("recvfrom failed"); + } else if (i < sizeof(struct s_msg)) { + syslog(LOG_WARNING, "Short read! (%d bytes)\n", i); + } else { + if (!multicast && (remote_addr.sin_addr.s_addr != far_addr.sin_addr.s_addr)) { + syslog(LOG_INFO, "discarding pkt from non-configured host"); + continue; + } + + msg_in->magic = ntohs(msg_in->magic); + msg_in->flags = ntohs(msg_in->flags); + msg_in->seqnum_pkt = ntohs(msg_in->seqnum_pkt); + msg_in->pktlen = ntohs(msg_in->pktlen); + msg_in->seqnum_dat = htons(msg_in->seqnum_dat); + + if (msg_in->magic != SOB_MAGIC) { + syslog(LOG_INFO, "discarding pkt w/o magic"); + continue; + } + + if (msg_in->pktlen > MAX_PACKET_LEN) { + syslog(LOG_INFO, "discarding pktlen %d > maxlen", msg_in->pktlen); + continue; + } + + if (msg_in->flags & MSG_FLAG_NONDATA) { + process_msg(msg_in, &remote_addr.sin_addr); + continue; + } + + /* Packet Sequence number validation */ + if (!multicast && sequence_pkt_rx) { + if (msg_in->seqnum_pkt <= sequence_pkt_rx) { + if ((msg_in->seqnum_pkt == 0) && + ((msg_in->seqnum_pkt - sequence_pkt_rx) > 1)) { + syslog(LOG_INFO, "pkt sequence reset (was %d)", sequence_pkt_rx); + } else { + syslog(LOG_WARNING, "pkt sequence %d replay (curr %d)\n", msg_in->seqnum_pkt, sequence_pkt_rx); + // continue; + } + } + + if ((msg_in->seqnum_pkt - sequence_pkt_rx) > 1) { + syslog(LOG_WARNING, "pkt sequence gap of %d packets!\n", msg_in->seqnum_pkt - sequence_pkt_rx - 1); + } + } + + /* Data sequence number validation */ + if (!multicast && sequence_dat_rx) { + if (msg_in->seqnum_dat < sequence_dat_rx) { + if ((msg_in->seqnum_dat == 0) && + ((msg_in->seqnum_dat - sequence_dat_rx) > 1)) { + syslog(LOG_INFO, "data sequence reset (was %d)", sequence_dat_rx); + } else { + syslog(LOG_WARNING, "data sequence %d replay (curr %d)\n", msg_in->seqnum_dat, sequence_dat_rx); + // continue; + } + } + + if (msg_in->seqnum_dat > sequence_dat_rx) { + syslog(LOG_WARNING, "data sequence gap of %d bytes!\n", msg_in->seqnum_dat - sequence_dat_rx); + } + } + + /* Update Sequence counters */ + sequence_pkt_rx = msg_in->seqnum_pkt; + sequence_dat_rx = msg_in->seqnum_dat + msg_in->pktlen; + + if ((i - sizeof(struct s_msg)) < msg_in->pktlen) { + syslog(LOG_WARNING, "Short read! (%d vs %d bytes) truncating\n", msg_in->pktlen, i - sizeof(struct s_msg)); + msg_in->pktlen = i - sizeof(struct s_msg); + } + + if ((ser_out_head - ser_out_tail) >= BUF_LEN) { + syslog(LOG_WARNING, "Network Rx Buffer Full (seq_dat %d, seq_pkt %d, buflen %d, discarding %d bytes)\n", + msg_in->seqnum_dat, + msg_in->seqnum_pkt, + ser_out_head - ser_out_tail, + msg_in->pktlen); + pfds[1].events |= POLLOUT; + continue; + } + + /* Copy over to buffer; handle wraps */ + len = min_t(u_int16_t, BUF_LEN - (ser_out_head % BUF_LEN), + BUF_LEN - (ser_out_head - ser_out_tail)); + len = min_t(u_int16_t, len, msg_in->pktlen); + + memcpy(&ser_out_buf[ser_out_head % BUF_LEN], msg_in->data, len); + ser_out_head += len; + + /* Handle wrap */ + len2 = min_t(u_int16_t, BUF_LEN - (ser_out_head % BUF_LEN), + BUF_LEN - (ser_out_head - ser_out_tail)); + len2 = min_t(u_int16_t, len2, msg_in->pktlen - len); + + memcpy(&ser_out_buf[ser_out_head % BUF_LEN], msg_in->data + len, len2); + ser_out_head += len2; + + if ((len + len2) < msg_in->pktlen) { + syslog(LOG_WARNING, "Network Rx Buffer Overflow (lost %d bytes)!\n", + msg_in->pktlen - (len + len2)); + } + + /* Mark outbound serial to include OUT */ + if (ser_out_head != ser_out_tail) { + pfds[1].events |= POLLOUT; + } + + /* If we get a break */ + if (msg_in->flags & MSG_FLAG_BREAK) { + /* Reset our buffers */ + ser_out_head = ser_out_tail = 0; + net_out_head = net_out_tail = 0; + tcflush(ser_fd, TCIOFLUSH); + syslog(LOG_INFO, "sending break to serial port\n"); + tcsendbreak(ser_fd, 0); + } + } + + // fprintf(stdout, "ser_qlen %d\n", ser_out_head - ser_out_tail); + } + + } + + /* Clean up */ + syslog(LOG_INFO, "sobredird shutting down"); + + tcflush(ser_fd, TCIOFLUSH); + close(ser_fd); + close(far_sock); + + closelog(); + + return 0; +} + + diff --git a/serialstats.c b/serialstats.c new file mode 100644 index 0000000..8fc1e11 --- /dev/null +++ b/serialstats.c @@ -0,0 +1,52 @@ +#include +#include +#include + +#include +#include +#include + +#include + +int main(int argc, char **argv) { + int fd; + int i; + int rval = 0; + char *devname; + + struct serial_icounter_struct stats; + + if (argc != 2) { + fprintf(stderr, "usage: serialstats devicename\n"); + rval = 1; + goto done; + } + devname = argv[1]; + + fd = open(devname, O_RDWR|O_NONBLOCK|O_NOCTTY); + + if (fd <= 0) { + perror("couldn't open device"); + rval = fd; + goto done; + } + + + i = ioctl(fd, TIOCGICOUNT, &stats); + if (i != 0) { + perror("couldn't issue ioctl"); + rval = i; + goto done; + } + + printf("cts=%d\ndsr=%d\nrng=%d\ndcd=%d\nrx=%d\ntx=%d\nframe=%d\noverrun=%d\nparity=%d\nbrk=%d\nbuf_overrun=%d\n", + stats.cts, stats.dsr, stats.rng, stats.dcd, + stats.rx, stats.tx, + stats.frame, stats.overrun, stats.parity, stats.brk, + stats.buf_overrun); + + close(fd); + +done: + return rval; +}