slp/sobredirect
1
0
Fork 0
Code Issues Pull requests Projects Releases Wiki Activity
sobredirect/redirector.c

887 lines
23 KiB
C
Raw Blame History

/*
Copyright (C) 2006 AbsoluteValue Systems, Inc. All Rights Reserved.
Originally written and maintained by: Solomon Peachy <pizza@shaftnet.org>
Local redirection & Debugging code by: Mark Mathews <mark@linux-wlan.com>
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "common.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 <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 MAX_PACKET_LEN 1470
#define BUF_LEN 4096
/* ================================================================ */
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");
}
}
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 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
{
// 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, (int) (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;
}
Reference in a new issue View git blame Copy permalink