summaryrefslogtreecommitdiffstats
path: root/redirector.c
diff options
context:
space:
mode:
authorSolomon Peachy <pizza@shaftnet.org>2013-08-25 08:19:52 -0400
committerSolomon Peachy <pizza@shaftnet.org>2013-08-25 08:19:52 -0400
commitb2873df66df1b9a41603c06afaf40d2af4467567 (patch)
tree97bd7741eca062b80affbc101535f7dc0131fbba /redirector.c
downloadsobredirect-b2873df66df1b9a41603c06afaf40d2af4467567.tar.gz
sobredirect-b2873df66df1b9a41603c06afaf40d2af4467567.tar.bz2
sobredirect-b2873df66df1b9a41603c06afaf40d2af4467567.zip
Initial AVS SoB code.
Diffstat (limited to 'redirector.c')
-rw-r--r--redirector.c1072
1 files changed, 1072 insertions, 0 deletions
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 <stdio.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <unistd.h>
+
+#include <errno.h>
+#include <sys/types.h>
+
+#include <sys/stat.h>
+#include <sys/poll.h>
+
+#include <sys/time.h>
+
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+#include <netdb.h>
+
+#include <sys/ioctl.h>
+#include <fcntl.h>
+#include <termios.h>
+#include <signal.h>
+
+#include <string.h>
+
+#include <syslog.h>
+
+/* 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 <serialdev> -s <commsparams> -i <farhost> -p <farport>\n");
+ fprintf(stderr, "\t[ -b <min_buf_size> ] [ -t <max_buf_time> ]\n");
+ fprintf(stderr, "\t[ -1 <delimeter1> [ -2 <delimeter2> ] ]\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: <baud>-<databits><parity><stopbits>\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;
+}
+
+