#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <errno.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netinet/tcp.h>
#include <fcntl.h>
#include <errno.h>
#include <termios.h>

/*
 * ssplitd.c
 *
 * Mux/demux incoming and outgoing bytes from a serial port which has MSB
 * byte stream multiplexing enabled.  Streams are sent to local tcp sockets.
 * 
 * High bit set stream is sent/received to tcp port set with -h option.
 * High bit unset stream is sent/received to tcp port set with -l option.
 *
 * % ssplitd -t /dev/ttyS0 -b 115200 -h 4014 -l 4015
 *
 */

#define MAXNAMLEN       64
#define BUFSZ           64

#define ERROR(fmt, ...)    \
            fprintf(stderr, "%s:%d ", __FILE__, __LINE__);  \
            fprintf(stderr, fmt, ##__VA_ARGS__)

#define MAX(a,b)      ((a)>(b)?(a):(b))


/*
 * Private Data Types.
 */
struct options
{
    int         h_port;
    int         l_port;
    int         baud;
    char        tty_name[MAXNAMLEN];
};

enum stream
{
    STREAM_LO,
    STREAM_HI,
    STREAM_NONE
};

enum stream_direction
{
    STREAM_TO_HOST,
    STREAM_FROM_HOST,
    STREAM_DIR_NONE
};


/*
 * Global Data
 */
static int h_sock = -1;         /* high bit set tcp socket */
static int h_data_sock = -1;    /* high bit set data socket */
static int l_sock = -1;         /* high bit unset tcp socket */
static int l_data_sock = -1;    /* high bit unset data socket */
static int tty_fd = -1;         /* tty file descriptor */
static struct options   opts;
static int gdb_print = 0;
static int gdb_print_stream = STREAM_NONE;

static void print_usage(char *prog)
{
    ERROR("Usage: %s <-t /dev/ttySX> <-b baud> <-h high bit set stream port> <-l high bit unset stream port>\n", prog);
}

static void debug_print(char ch, enum stream_direction dir, enum stream str)
{
    static enum stream_direction last_dir = STREAM_DIR_NONE;

    if ( !gdb_print || str != gdb_print_stream )
        return;

    if ( dir != last_dir )
    {
        /* 
         * GDB packet communication has changed direction, 
         * print a message header. 
         */
        switch (dir)
        {
            case STREAM_TO_HOST:
                printf("\ngdb ==> host: ");
                break;
            case STREAM_FROM_HOST:
                printf("\nhost ==> gdb: ");
                break;
            default:
                break;
        }
        last_dir = dir;
    }
    printf("%c", ch);
}

static int setup_socket(int port, int *sfd)
{
    int                 sockfd;
    int                 sopt = 1;
    struct sockaddr_in  sin;

    if ( (sockfd = socket(AF_INET, SOCK_STREAM, 0)) < 0 )
    {
        ERROR("Cannot open socket on port '%d'\n", port);
        perror("Error: ");
        return -1;
    }

    if ( setsockopt(sockfd, SOL_SOCKET, SO_REUSEADDR, &sopt, sizeof (int)) < 0 )
    {
        ERROR("Cannot set socket option (SO_REUSEADDR)\n");
    }

    memset(&sin, 0, sizeof(sin));
    sin.sin_family      = AF_INET;
    sin.sin_port        = htons(port);
    sin.sin_addr.s_addr = INADDR_ANY;

    if ( bind(sockfd, (struct sockaddr*)&sin, sizeof(struct sockaddr_in)) < 0 )
    {
        ERROR("Cannot bind to port '%d'\n", port);
        perror("Error: ");
        return -1;
    }

    if ( listen(sockfd, 1) < 0 )
    {
        ERROR("Cannot listen on port '%d'\n", port);
        return -1;
    }

    /* 
     * Set the socket as non-blocking so the later call to 'accept' 
     * wont block.
     */
#if 0
    if ( fcntl(sockfd, F_SETFL, O_NONBLOCK) < 0 )
    {
        ERROR("Cannot set O_NONBLOCK flags\n");
        return -1;
    }
#endif
    *sfd = sockfd;
    return 0;
}

static int setup_tty(char *tty_name, int baud, int *nfd)
{
    struct termios  options;
    int             fd;
    speed_t         speed;

    /*
     * Ensure a valid baud rate has been provided.
     */
    switch (baud)
    {
        case 9600:
            speed = B9600;
            break;
        case 19200:
            speed = B19200;
            break;
        case 38400:
            speed = B38400;
            break;
        case 57600:
            speed = B57600;
            break;
        case 115200:
            speed = B115200;
            break;
        default:
            ERROR("Baud rate (%d) is not supported\n", baud);
            return -1;
    }

    fd = open(tty_name, O_RDWR | O_NDELAY);
    if ( fd < 0 )
    {
        ERROR("setup_tty: Unable to open '%s' - ", tty_name);
        perror("Error: ");
        return -1;
    }

    if ( fcntl(fd, F_SETFL, O_NONBLOCK) < 0 )
    {
        ERROR("Cannot set O_NONBLOCK flags\n");
        return -1;
    }

    /* 
     * Configure serial port 
     */
    tcgetattr(fd, &options);

    /*
     * Set the baud rates
     */
    cfsetispeed(&options, speed);
    cfsetospeed(&options, speed);

    /*
     * Enable the receiver and set local mode.
     */
    options.c_cflag |= (CLOCAL | CREAD);

    /* 8n1 is assumed */
    options.c_cflag &= ~PARENB;
    options.c_cflag &= ~CSTOPB;
    options.c_cflag &= ~CSIZE;
    options.c_cflag |= CS8;

    /* Disable hardware and software flow control */
    options.c_cflag &= ~CRTSCTS;
    options.c_iflag &= ~(IXON | IXOFF | IXANY);

    /* Ensure we are in raw mode for input and output. */
    options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
    options.c_oflag &= ~OPOST;

    /*
     * Set the new options for the port.
     */
    if ( tcsetattr(fd, TCSANOW, &options) )
    {
        perror("Failed to set serial port options: ");
        return -1;
    }

    *nfd = fd;
    return 0;
}

static int process_options(int argc, char **argv, struct options *opts)
{
    char    c;

    while ( (c = getopt(argc, argv, "t:b:h:l:g:")) != -1 )
    {
        switch (c)
        {
            case 't':
                strncpy(opts->tty_name, optarg, MAXNAMLEN);
                break;
            case 'b':
                opts->baud = atoi(optarg);
                break;
            case 'h':
                opts->h_port = atoi(optarg);
                break;
            case 'l':
                opts->l_port = atoi(optarg);
                break;
            case 'g':
                gdb_print_stream = atoi(optarg);
                gdb_print = 1;
                break;
            default:
                ERROR("Invalid option '%s'\n", optarg);
                return -1;
        }
    }
    return 0;
}

static int mux_stream(char *buf, int len, int highbit)
{
    int                 i;
    enum stream         str = STREAM_LO;
    unsigned char       chval = 0;
   
    if ( highbit )
    {
        chval = 0x80;
        str = STREAM_HI;
    }
 
    for ( i=0; i < len; i++ )
    {
        debug_print(buf[i], STREAM_TO_HOST, str);
        buf[i] |= chval;
    }

    if ( write(tty_fd, buf, len) < 0 )
    {
        ERROR("Cannot write to tty device\n");
        return -1;
    }
    return 0;
}

static int demux_stream(char *buf, int len)
{
    char    l_buf[BUFSZ];
    char    h_buf[BUFSZ];
    int     l_len = 0;
    int     h_len = 0;
    int     i;

    /*
     * Process each byte, sending bytes with the MSB set to the
     * h_data_sock and ones with the MSB not set to the
     * l_data_sock.
     */
    if ( h_data_sock < 0 && l_data_sock < 0 )
    {
        /* Neither socket is open, dont bother */
        return 0;
    }

    for ( i=0; i < len; i++ )
    {
        if ( buf[i] & 0x80 )
        {
            h_buf[h_len] = buf[i] & 0x7f;
            debug_print(h_buf[h_len], STREAM_FROM_HOST, STREAM_HI);
            h_len++;
        }
        else
        {
            l_buf[l_len] = buf[i];
            debug_print(l_buf[l_len], STREAM_FROM_HOST, STREAM_LO);
            l_len++;
        }
    }

    if ( h_len > 0 && h_data_sock > 0 )
    {
        if ( write(h_data_sock, h_buf, h_len) < 0 )
        {
            ERROR("Cannot write to high-bit-set socket\n");
        }
    }
    if ( l_len > 0 && l_data_sock > 0 )
    {
        if ( write(l_data_sock, l_buf, l_len) < 0 )
        {
            ERROR("Cannot write to high-bit-unset socket\n");
        }
    }

    return 0;
}

static int check_socket_connect(int sockfd, fd_set *rdset, int *newsock)
{
    struct sockaddr_in  from;
    socklen_t           from_len = sizeof(struct sockaddr_in);
    int                 new_socket, rv = -1;

    if ( FD_ISSET(sockfd, rdset) )
    {
        /* There is a connection ready to be accepted */
        if ( (new_socket = accept(sockfd, (struct sockaddr*)&from, &from_len)) < 0 )
        {
            ERROR("Cannot accept connection\n");
            return -1;
        }

        fprintf(stdout, "Accepted connection\n");

        *newsock = new_socket;
        rv = 0;
    }

    return rv;
}

static int check_socket_read(int *sfd, fd_set *rdset, int highbit)
{
    char    buf[BUFSZ];
    int     len = 0;
    int     sockfd = *sfd;

    if ( sockfd > 0 && FD_ISSET(sockfd, rdset) )
    {
        /* Characters available at socket */
        if ( (len = read(sockfd, buf, BUFSZ)) <= 0 )
        {
            ERROR("socket closed unexpectedly\n");
            /* Clear the bit in our I/O mask */
            FD_CLR(sockfd, rdset);
            close(sockfd);
            *sfd = -1;
            return 0;
        }

        if ( mux_stream(buf, len, highbit) )
        {
            ERROR("Cannot mux stream\n");
        }
        FD_SET(sockfd, rdset);
    }

    return len;
}

static int process_streams(void)
{
    fd_set              rdset, exset;
    int                 fd_count, len, nfds;
    char                buf[BUFSZ];

    /*
     * Initialize fd_sets
     */
    FD_ZERO(&rdset);
    FD_ZERO(&exset);

    nfds = MAX(nfds, tty_fd);
    nfds = MAX(nfds, h_sock);
    nfds = MAX(nfds, l_sock);
    nfds++;

    while (1)
    {
        if ( tty_fd > 0 )
            FD_SET(tty_fd, &rdset);
        if ( h_sock > 0 )
            FD_SET(h_sock, &rdset);
        if ( l_sock > 0 )
            FD_SET(l_sock, &rdset);
        if ( h_data_sock > 0 )
            FD_SET(h_data_sock, &rdset);
        if ( l_data_sock > 0 )
            FD_SET(l_data_sock, &rdset);

        fd_count = select(nfds, &rdset, NULL, NULL, NULL);
        if ( fd_count < 0 )
        {
            perror("select: ");
            return -1;
        }
   
        if ( FD_ISSET(tty_fd, &rdset) )
        {
            /* Characters available at tty device for reading */
            if ( (len = read(tty_fd, buf, BUFSZ)) <= 0 )
            {
                /* tty device has closed */
                ERROR("tty device (%s) closed unexpectedly\n", opts.tty_name);
                return -1;
            }
    
            if ( demux_stream(buf, len) )
            {
                ERROR("Cannot demux stream\n");
                return -1;
            }
        }

        check_socket_read(&h_data_sock, &rdset, 1);
        check_socket_read(&l_data_sock, &rdset, 0);

        if ( !check_socket_connect(h_sock, &rdset, &h_data_sock) )
        {
            /* Add the new data socket to our read fd_set */
            FD_SET(h_data_sock, &rdset);
            /* re-compute our nfds value for 'select' */
            nfds = MAX(nfds, h_data_sock) + 1;
        }
    
        if ( !check_socket_connect(l_sock, &rdset, &l_data_sock) )
        {
            /* Add the new data socket to our read fd_set */
            FD_SET(l_data_sock, &rdset);
            /* re-compute our nfds value for 'select' */
            nfds = MAX(nfds, l_data_sock) + 1;
        }
    }
    return 0;
}

int main(int argc, char **argv)
{
    /*
     * Set some default options.
     */
    strcpy(opts.tty_name, "/dev/ttyS0");
    opts.baud = 115200;
    opts.h_port = 4000;
    opts.l_port = 4001;

    if ( process_options(argc, argv, &opts) )
    {
        print_usage(argv[0]);
        return 1;
    }

    /*
     * Initialize and configure serial port device.
     */
    if ( setup_tty(opts.tty_name, opts.baud, &tty_fd) )
    {
        ERROR("Cannot initialize tty '%s'\n", opts.tty_name);
        return 1;
    }

    /*
     * Initialize high bit set socket.
     */
    if ( setup_socket(opts.h_port, &h_sock) )
    {
        ERROR("Cannot initialize high bit set socket on port (%d)\n", opts.h_port);
        return 1;
    }

    /*
     * Initialize high bit clear socket.
     */
    if ( setup_socket(opts.l_port, &l_sock) )
    {
        ERROR("Cannot initialize high bit clear socket on port (%d)\n", opts.l_port);
        return 1;
    }

    //daemonize();

    process_streams();
   
    return 0;
}