#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
#include <signal.h>
#include <netinet/in.h>
#include <sys/socket.h>
#include <sys/un.h>
#include <sys/select.h>
#include <sys/time.h>
#include <sys/ioctl.h>
#include <sys/prctl.h>
#include <errno.h>
#include <fcntl.h>

#include "dbp.h"
#include "jtag.h"
#include "rdb.h"

#include <netinet/tcp.h>
#include <arpa/inet.h>

/* mux/demux streams for:
 *
 *    data
 *    debug
 *    fault
 *    kdebug
 *    logging
 *    print
 *    profile
 */

#define DEV_MAIN    0
#define DEV_DATA    1
#define DEV_DEBUG   2
#define DEV_FAULT   3
#define DEV_KDEBUG  4
#define DEV_LOGGING 5
#define DEV_PRINT   6
#define DEV_PROFILE 7
#define DEV_JTAG    8
#define DEV_POKE    9

#define MAX_WRITE_SIZE  0x8000

static struct stream {
    char *name;            /* Name of the mux stream */
    
    /* Information about the socket */
    int port;           /* If 0 - UNIX_DOMAIN socket
                           Otherwise, INET TCP socket */

    int lfd;            /* Local file descriptor */
    int fd;             /* File descriptor connecting to this socket */

    int incoming_ct;
    int outgoing_ct;
    unsigned int message;
    int flag;
#define F_WRITE     1
#define F_READ      2
#define F_VALID     4  /* Does the program on the BBPlayer support this stream? */
    int write_cmd;
    int write_count;
    int write_ptr;
    unsigned char write_buf[MAX_WRITE_SIZE];
} stream[] = {
    { "/tmp/u64",             0, 0, 0, 0, 0, 0, 0, 0, 0, 0, {0} },
    { "/tmp/u64_data",        0, 0, 0, 0, 0, 0, 0, 0, 0, 0, {0} },
    { "/tmp/u64_debug",    8088, 0, 0, 0, 0, 0, 0, 0, 0, 0, {0} },
    { "/tmp/u64_fault",       0, 0, 0, 0, 0, 0, 0, 0, 0, 0, {0} },
    { "/tmp/u64_kdebug",      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, {0} },
    { "/tmp/u64_logging",     0, 0, 0, 0, 0, 0, 0, 0, 0, 0, {0} },
    { "/tmp/u64_print",       0, 0, 0, 0, 0, 0, 0, 0, 0, 0, {0} },
    { "/tmp/u64_profile",     0, 0, 0, 0, 0, 0, 0, 0, 0, 0, {0} },
    { "/tmp/u64_jtag",        0, 0, 0, 0, 0, 0, 0, 0, 0, 0, {0} },
    { "/tmp/u64_pokemux",     0, 0, 0, 0, 0, 0, 0, 0, 0, 0, {0} },
    { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, {0} }
};

static int ramrom_flag = 0;
static int write_avail = 1; /* BB can receive data */
static int write_stream = -1;   /* current writer */
static int verbose = 0;
static int vvv = 0;
static int high_speed = 1;
static int usb = 1;

#define USB_DEFAULT_PATH "/dev/usb/bbrdb0"
static char *usbpath;
static int usbfd = -1;

/* print server from gload */
char* progName = "mux";
extern int printServerInit(char *);
extern int printServer(void);
extern void printServerClose(void);

static int poke;

static int
send_cmd(int cmd)
{
    unsigned char data[4];
    int datalen = 0, rv = 0, count = 0;
    memset(data, 0, sizeof data);
    data[0] = (cmd << 2); /* no data - length bits are 0 */
    datalen = 1;

    if (usb) {
        if ((rv = write(usbfd, data, datalen)) < 0 || rv != datalen) {
            perror("WRITE DATA FAILED (USB)");
            (void) close(usbfd);
            usbfd = -1;
            count = rv;
        }
        if (verbose)
            fprintf(stderr, "wrote %d bytes to USB: %02x %02x %02x %02x ...\n", 
                    rv, data[0], data[1], data[2], data[3]);
    } else {
        unsigned char req;
        req = DBP_CTRL_SET_WR_PTR;
        if (dbp_ctrl(DBP_CTRL_WRITE, &req) < 0) {
            perror("DBP_SET_WR_PTR");
            exit(1);
        }
        if ((rv = dbp_write_data(data, datalen)) < 0 || rv != datalen) {
            printf("WRITE DATA FAILED\n");
            count = rv;
        }
        req = DBP_CTRL_SET_HOST_REQ|DBP_CTRL_CLR_BB_ACK;
        if ((rv = dbp_ctrl(DBP_CTRL_WRITE, &req)) < 0)
            count = rv;
    }
    return count;
}

/* Sends data to the USB in block mode with
   byte 1 = cmd, byte 2 = length, bytes 3-n = data */
static int
send_next_usb(unsigned char cmd) {
    int i, rv, count, wcount, wtotal, wstart;
    static unsigned char data[2048];
    unsigned char *datap;

    memset(data, 0, sizeof data);
    wcount = stream[write_stream].write_count;
/*    cmd = stream[write_stream].write_cmd; */
    if (wcount > 8*254) wcount = 8*254;
    
    wtotal = 0;
    wstart = 0;
    datap = &data[0];
    while (wtotal < wcount) {
        count = wcount - wtotal;
        if (count > 254) count = 254;
        wstart += 2;
        /* Let length bits be 0 since we are using the next byte for length
           For RDB_TYPE_HtoG_DEBUG, this is used to indicate that the length
           is stored in the next byte */
        *datap++ = (cmd << 2);
        *datap++ = count;
        for(i = 0; i < count; i++) {
            *datap++ = stream[write_stream].write_buf[stream[write_stream].write_ptr++];
            if (stream[write_stream].write_ptr >= MAX_WRITE_SIZE)
                stream[write_stream].write_ptr = 0;
        }
        stream[write_stream].write_count -= count;
        wtotal += count;
    }
    if (verbose)
        fprintf(stderr, "sending %d bytes of %s cmd 0x%x count %d\n", wcount, stream[write_stream].name, cmd, stream[write_stream].write_count);
    if ((rv = write(usbfd, data, wcount+wstart)) < 0 || rv != wcount+wstart) {
        perror("WRITE DATA FAILED (USB)");
        (void) close(usbfd);
        usbfd = -1;
        count = rv;
    }
    if (verbose)
        fprintf(stderr, "wrote %d bytes to USB: %02x %02x %02x %02x ...\n", rv, data[0], data[1], data[2], data[3]);
    write_avail = usb;  /* hack for now XXX */

    if (stream[write_stream].write_count == 0)
    {
        stream[write_stream].write_cmd = 0;

        /* For debug stream, block until we get RDB_TYPE_GTOH_DEBUG_READY */
        if (write_stream == DEV_DEBUG)
        {
            send_cmd(RDB_TYPE_HtoG_DEBUG_DONE);
            stream[write_stream].flag &= ~F_WRITE;
            if (verbose)
                fprintf(stderr, "block %s\n", stream[write_stream].name);
        }
    }
    return wcount;
}

static int
send_next(void) {
    int i, rv, count, ccount, start;
    unsigned char data[256], req, cmd;
    if (write_stream == -1) return 0;

    memset(data, 0, sizeof data);
    count = stream[write_stream].write_count;
    cmd = stream[write_stream].write_cmd;
    if (count == 0 && cmd == 0) goto next;

    if (usb && cmd == RDB_TYPE_HtoG_DATA && count > 16 && high_speed) 
    {
        if (send_next_usb(RDB_TYPE_HtoG_DATAB))
            return count;
        goto next;
    }
    else if ((usb && cmd == RDB_TYPE_HtoG_DEBUG) && count >= 4)
    {
        /* Use block transfer to send debug bytes if
           we have 4 or more bytes - otherwise use normal method
           where the length is encoded in the lower 2 bits of
           the cmd byte */
        if (send_next_usb(cmd))
            return count;
        goto next;
    }

    if (cmd == RDB_TYPE_HtoG_DATA && count > 16 && high_speed) {
        if (count > 254) count = 254;
        cmd = RDB_TYPE_HtoG_DATAB;
        ccount = 3;
        start = 2;
        data[1] = count;
    } else {
        if (count > 3) count = 3;
        ccount = count;
        start = 1;
    }
    if (verbose)
        fprintf(stderr, "sending %d bytes of %s cmd 0x%x count %d\n", count, stream[write_stream].name, cmd, stream[write_stream].write_count);
    data[0] = (cmd << 2) | (ccount&3);
    stream[write_stream].write_count -= count;
    for(i = 0; i < count; i++) {
        data[start+i] = stream[write_stream].write_buf[stream[write_stream].write_ptr++];
        if (stream[write_stream].write_ptr >= MAX_WRITE_SIZE)
            stream[write_stream].write_ptr = 0;
    }
    if (usb) {
        if ((rv = write(usbfd, data, count+start)) < 0 || rv != count+start) {
            perror("WRITE DATA FAILED (USB)");
            (void) close(usbfd);
            usbfd = -1;
            count = rv;
        }
        if (verbose)
            fprintf(stderr, "wrote %d bytes to USB: %02x %02x %02x %02x ...\n", rv, data[0], data[1], data[2], data[3]);
    } else {
        req = DBP_CTRL_SET_WR_PTR;
        if (dbp_ctrl(DBP_CTRL_WRITE, &req) < 0) {
            perror("DBP_SET_WR_PTR");
            exit(1);
        }
        if ((rv = dbp_write_data(data, count+start)) < 0 || rv != count+start) {
            printf("WRITE DATA FAILED\n");
            count = rv;
        }
        req = DBP_CTRL_SET_HOST_REQ|DBP_CTRL_CLR_BB_ACK;
        if ((rv = dbp_ctrl(DBP_CTRL_WRITE, &req)) < 0)
            count = rv;
    }
    write_avail = usb;  /* hack for now XXX */
    if (stream[write_stream].write_count) return count;

    /* No more data to send */
    stream[write_stream].write_cmd = 0;
    /* For debug stream, block until we get RDB_TYPE_GTOH_DEBUG_READY */
    if (write_stream == DEV_DEBUG)
    {
        send_cmd(RDB_TYPE_HtoG_DEBUG_DONE);
        stream[write_stream].flag &= ~F_WRITE;
        if (verbose)
            fprintf(stderr, "block %s\n", stream[write_stream].name);
    }

  next:
    /* pick new stream, starting at write_stream */
    for(i = write_stream;;) {
        if (stream[i].write_cmd) {
            write_stream = i;
            break;
        }
        if (!stream[++i].name) i = 0;
        if (i == write_stream) { /* wrapped */
            write_stream = -1;
            break;
        }
    }
    return count;
}

static int
empty_stream(int s, int warn) {
    char buf[256];
    if (warn)
        fprintf(stderr, "dropping unexpected input on stream %s\n", stream[s].name);
    //write(stream[s].fd, "bah humbug\n", sizeof "bah humbug\n"-1);
    return read(stream[s].fd, buf, sizeof buf);
}

static int
copy_in(int s, int cmd) {
    int rv, count = 0;
    int wp = stream[s].write_ptr, wc = stream[s].write_count;
    stream[s].write_cmd = cmd;
    /* if no space in buffer, return non-zero so we don't close the fd */
    if (wc == MAX_WRITE_SIZE) return 1;
    if (wp + wc < MAX_WRITE_SIZE) {
        int n = MAX_WRITE_SIZE-wp-wc;
        if (s != DEV_DEBUG && s != DEV_KDEBUG && s!= DEV_PROFILE && n > stream[s].outgoing_ct) 
            n = stream[s].outgoing_ct;
        rv = read(stream[s].fd, stream[s].write_buf+wp+wc, n);
        if (rv < 0) return rv;
        wc += rv;
        count += rv;
        stream[s].outgoing_ct -= rv;
    }
    if (wp + wc >= MAX_WRITE_SIZE) {
        int n = MAX_WRITE_SIZE-wc;
        if (s != DEV_DEBUG && s != DEV_KDEBUG && s != DEV_PROFILE && n > stream[s].outgoing_ct) 
            n = stream[s].outgoing_ct;
        rv = read(stream[s].fd, stream[s].write_buf+wc+wp-MAX_WRITE_SIZE, n);
        if (rv < 0) return rv;
        wc += rv;
        count += rv;
        stream[s].outgoing_ct -= rv;
    }
    stream[s].write_count = wc;
    if (verbose)
        fprintf(stderr, "got %d bytes from stream %s\n", count, stream[s].name);
    if (stream[s].outgoing_ct == 0 && s != DEV_PROFILE && s != DEV_DEBUG && s != DEV_KDEBUG ) {
        stream[s].flag &= ~F_WRITE;
        if (verbose) fprintf(stderr, "block %s\n", stream[s].name);
    }

    /* kick start if necessary */
    if (write_stream == -1)
        write_stream = s;

    return count;
}

static int
copy_cmd(int s, int cmd) {
    stream[s].write_cmd = cmd;
    if (write_stream == -1)
        write_stream = s;
    return 0;
}

int
copy_data(unsigned int data, int s) {
    if (stream[s].fd) {
        int len = data&3;
        char buf[3];
        buf[0] = data >> 8; buf[1] = data >> 16; buf[2] = data >> 24;
        if (verbose && s != DEV_PRINT)
            fprintf(stderr, "send %d bytes to stream %s\n", len, stream[s].name);
        if (stream[s].incoming_ct) {
            stream[s].incoming_ct -= len;
            if (!stream[s].incoming_ct)
                copy_cmd(s, stream[s].message);
        }
        return write(stream[s].fd, buf, len);
    }
    return 0;
}

static int
recv_next(unsigned int data) {
    int cmd = (data >> 2)&0x3f;
    int count = ntohl(data) & 0xffffff;
    switch(cmd) {
    case RDB_TYPE_GtoH_PRINT:
        copy_data(data, DEV_PRINT); break;
    case RDB_TYPE_GtoH_FAULT:
        copy_data(data, DEV_FAULT); break;
    case RDB_TYPE_GtoH_LOG_CT:
        stream[DEV_LOGGING].incoming_ct = count;
        stream[DEV_LOGGING].message = RDB_TYPE_HtoG_LOG_DONE;
        break;
    case RDB_TYPE_GtoH_LOG:
        copy_data(data, DEV_LOGGING); break;
    case RDB_TYPE_GtoH_READY_FOR_DATA:
        stream[DEV_DATA].flag |= F_WRITE;
        if (verbose) fprintf(stderr, "unblock %s\n", stream[DEV_DATA].name);
        break;
    case RDB_TYPE_GtoH_DATA_CT:
        stream[DEV_DATA].incoming_ct = count;
        stream[DEV_DATA].message = RDB_TYPE_HtoG_DATA_DONE;
        break;
    case RDB_TYPE_GtoH_DATA:
        copy_data(data, DEV_DATA); break;
    case RDB_TYPE_GtoH_DEBUG:
        copy_data(data, DEV_DEBUG); break;
    case RDB_TYPE_GtoH_RAMROM:
        ramrom_flag = F_WRITE;
        break;
    case RDB_TYPE_GtoH_DEBUG_DONE:
        stream[DEV_DEBUG].flag |= F_READ;
        break;
    case RDB_TYPE_GtoH_DEBUG_READY:
        stream[DEV_DEBUG].flag |= F_WRITE;
        stream[DEV_DEBUG].flag |= F_VALID;
        if (verbose) fprintf(stderr, "unblock %s\n", stream[DEV_DEBUG].name);
        break;
    case RDB_TYPE_GtoH_KDEBUG:
        copy_data(data, DEV_KDEBUG); break;
    case RDB_TYPE_GtoH_PROF_DATA:
        copy_data(data, DEV_PROFILE); break;
    case RDB_TYPE_GtoH_SYNC:
        copy_cmd(DEV_DATA, RDB_TYPE_HtoG_SYNC_DONE); break;
    default:
        fprintf(stderr, "mux: RDB_TYPE_UNKNOWN %x\n", data);
        break;
    }
    return 0;
}

void
dump(int n, int fd) {
    int i;
    unsigned short buf[16];
    unsigned char status, req = DBP_CTRL_SET_RD_PTR|(2016>>5);

    if (usb) return;
    if (dbp_ctrl(DBP_CTRL_WRITE, &req) < 0) {
        perror("dbp_ctrl DBP_CTRL_SET_RD_PTR");
        exit(1);
    }
    dbp_read_data((unsigned char*)buf, sizeof buf);
    if (dbp_ctrl(DBP_CTRL_READ, &status) < 0) {
        perror("dbp_ctrl read");
        exit(1);
    }
    req = DBP_CTRL_SET_HOST_ACK;
    if (dbp_ctrl(DBP_CTRL_WRITE, &req) < 0) {
        perror("DBP_CTRL_SET_HOST_ACK");
        exit(1);
    }
    for(i = 0; i < 16; i++)
        buf[i] = ntohs(buf[i]);
    buf[15] = status;
    if (fd)
        write(fd, buf, n*sizeof buf[0]);
    else {
        for(i = 0; i < n; i++)
            printf("%04x ", buf[i]);
        printf("\n");
    }
}

void
usb_event() {
    int rv;
    unsigned int data;

    /*
     * Process input until we get a timeout on read
     */
    while (1) {
        if (usbfd < 0) return;
        if ((rv = read(usbfd, (char*)&data, sizeof data)) < 0) {
            if (errno != ETIMEDOUT && errno != EAGAIN) {
                fprintf(stderr, "usb read returns errno %d\n", errno);
                perror(usbpath);
                (void) close(usbfd);
                usbfd = -1;
            }
            return;
        }
        if (rv != sizeof data) {
            fprintf(stderr, "usb short read %d\n", rv);
        }
        if (verbose) {
            char *dp = (char *)&data;
            fprintf(stderr, "Got %d bytes from USB: %02x %02x %02x %02x\n", rv, *dp, *(dp+1), *(dp+2), *(dp+3));
        }
        recv_next(data);
    }
}

void
pp_event(int ppfd) {
    unsigned char req;
    unsigned char status;
    int rv;
    unsigned int data;

    /* demux parallel port event */
    if (dbp_ctrl(DBP_CTRL_READ, &status) < 0) {
        perror("dbp_ctrl read");
        exit(1);
    }
    if (vvv) printf("%x\n", status);
    if (status & DBP_STATUS_BB_ACK) {
//printf("bb ack 0x%x\n", status);
//dump(4, 0);
        /* host req acknowledged, send next req */
#if 0
        req = DBP_CTRL_SET_WR_PTR;
        if (dbp_ctrl(DBP_CTRL_WRITE, &req) < 0) {
            perror("DBP_SET_WR_PTR");
            exit(1);
        }
#endif
        /*XXXblythe could do this when setting REQ in send_next */
        if (write_stream == -1) {
            req = DBP_CTRL_CLR_BB_ACK;
            if (dbp_ctrl(DBP_CTRL_WRITE, &req) < 0) {
                perror("DBP_CTRL_CLR_BB_ACK");
                exit(1);
            }
        }
        write_avail = 1;
    }
    if (status & DBP_STATUS_BB_REQ) {
        /* incoming request */
        req = DBP_CTRL_SET_RD_PTR|(0x400>>5);
        if (dbp_ctrl(DBP_CTRL_WRITE, &req) < 0) {
            perror("dbp_ctrl DBP_CTRL_SET_RD_PTR");
            exit(1);
        }
        if ((rv = dbp_read_data((char*)&data, sizeof data)) < 0) {
            perror("dbp_read");
            exit(1);
        }
        if (rv != sizeof data) {
            fprintf(stderr, "dbp_read short read %d\n", rv);
        }
        recv_next(data);
        req = DBP_CTRL_CLR_BB_REQ | DBP_CTRL_SET_HOST_ACK;
        if (dbp_ctrl(DBP_CTRL_WRITE, &req) < 0) {
            perror("DBP_CTRL_CLR_BB_REQ");
            exit(1);
        }
    }
    /* dismiss pp interrupt */
    data = 0;
    if (ioctl(ppfd, PPCLRIRQ, &data) < 0) {
        perror("PPCLRIRQ");
        exit(1);
    }
}

static void
usb_open()
{
    if ((usbfd = open(usbpath, O_RDWR)) < 0) {
        perror(usbpath);
        return;
    }
    printf("%s: open successful\n", usbpath);
    if (fcntl(usbfd, F_SETFL, O_NONBLOCK) < 0) {
        perror("fcntl usb");
    }
    
    /* Poke to see if we are ready to accept debug input*/
    send_cmd(RDB_TYPE_HtoG_DEBUG_DONE);
    stream[DEV_DEBUG].flag &= ~F_VALID;
    stream[DEV_DEBUG].flag &= ~F_WRITE;
    if (verbose)
        fprintf(stderr, "block %s\n", stream[DEV_DEBUG].name);
}

static void
handler(int sig) {
    int i;
    if (sig == SIGQUIT) {
        vvv ^= 1;
        return;
    }
    for(i = 0; stream[i].name; i++) {
        unlink(stream[i].name);
    }
    exit(0);
}

/* Closes stream i */
void close_stream(int i)
{
    /* Check that i is a valid stream index */
    if ((i < 0) || (i >= sizeof(stream)/sizeof(stream[0])))
    {
        fprintf(stderr, "Unable to close stream %d: invalid stream index.\n", i);
    }

    /* Make sure we have a stream to close */
    if (stream[i].fd)
    {
        close(stream[i].fd);
        stream[i].fd = 0;
        stream[i].flag &= ~F_WRITE;
        stream[i].outgoing_ct = stream[i].write_cmd =
            stream[i].write_count = stream[i].write_ptr = 0;
        if (verbose > 1)
            fprintf(stderr, "close %s\n", stream[i].name);
        if (i == DEV_POKE) poke = 0;
    }
}

void usage()
{
    fprintf(stderr, "Usage: mux [-v] [-e] [-h] [-i] [-l] [-u]\n");
    fprintf(stderr, "           [--debugport port]\n");
}

int
main(int argc, char* argv[]) {
    int i, nfd, ppfd, rv;
    fd_set rfd, efd, lfd;
    struct timeval tm;
    unsigned char status;
    int __epp = (int)getenv("DBP_EPP");
    int irq = 1;
    __epp = 1; putenv("DBP_EPP=1");
    while(argc > 1 && argv[1][0] == '-') {
        switch(argv[1][1]) {
        case 'v':
            verbose++; break;
        case 'u':
            usb = 1;
            high_speed = 1; /* USB requires -h */
            break;
        case 'e':
            __epp = 1;
            putenv("DBP_EPP=1");
            break;
        case 'h':
            high_speed = 1;
            break;
        case 'i':
            usb = 0;
            break;
        case 'l':
            high_speed = 0;
            usb = 0;
            break;
        case '-':
            /* Long option */
            if (strcmp(&argv[1][2], "debugport") == 0)
            {
                int port;
                argv++;
                argc--;
                port = atoi(argv[1]);
                if (port == 0)
                {
                    fprintf(stderr, "Invalid debug port %s\n", argv[1]);
                    return 1;
                }
                stream[DEV_DEBUG].port = port;
            }
            else 
            {
                usage();
                return 1;
            }
            break;
            
        default:
            usage();
            return 1;
        }
        argc--; argv++;
    }
    if (usb && ((usbpath = getenv("USB_RDB")) == NULL))
        usbpath = USB_DEFAULT_PATH;

    /* create fifos */
    FD_ZERO(&lfd);
    for(i = 0; stream[i].name; i++) {
        if (stream[i].port == 0) 
        {
            /* Port number not set: Open a Unix Domain Socket */
            struct sockaddr_un sa;
            if ((stream[i].lfd = socket(PF_UNIX, SOCK_STREAM, 0)) < 0) {
                perror(stream[i].name);
                exit(1);
            }
            FD_SET(stream[i].lfd, &lfd);
            sa.sun_family = AF_UNIX;
            strcpy(sa.sun_path, stream[i].name);
            unlink(stream[i].name);
            if (bind(stream[i].lfd, (struct sockaddr*)&sa, sizeof sa) < 0) {
                perror("bind");
                return 1;
            }
            if (listen(stream[i].lfd, 1) < 0) {
                perror("listen");
                return 1;
            }
        }
        else
        {
            struct sockaddr_in sa;
            int tmp;

            /* There is a valid port: Open a internet TCP socket */
            if ((stream[i].lfd = socket(PF_INET, SOCK_STREAM, 0)) < 0) {
                perror(stream[i].name);
                exit(1);
            }

            /* Allow rapid reuse of this port. */
            tmp = 1;
            setsockopt (stream[i].lfd, SOL_SOCKET, SO_REUSEADDR, (char *) &tmp,
                        sizeof (tmp));

            sa.sin_family = AF_INET;
            sa.sin_port = htons (stream[i].port);
            sa.sin_addr.s_addr = INADDR_ANY;

            if (bind(stream[i].lfd, (struct sockaddr*) &sa, sizeof sa) < 0) {
                fprintf(stdout, "Cannot bind stream %s to port %d\n", 
                        stream[i].name, stream[i].port);
                perror("bind");
                return 1;
            }

            if (listen(stream[i].lfd, 1) < 0) {
                perror("listen");
                return 1;
            }

            fprintf(stdout, "Stream %s opened on port %d\n", 
                    stream[i].name, stream[i].port);
        }
    }

    if (fork() == 0) {
        for(i = 0; stream[i].name; i++) close(stream[i].lfd);
        prctl(PR_SET_PDEATHSIG, SIGTERM);
        if (printServerInit(NULL) < 0) {
            fprintf(stderr, "printServerInit failed\n");
            exit(0);
        }
        printServer();
        printServerClose();
    }

    signal(SIGINT, handler);
    signal(SIGQUIT, handler);
    signal(SIGTERM, handler);
    signal(SIGHUP, handler);
    signal(SIGPIPE, SIG_IGN);

    /* open debug port */
    if (usb)
    {
        usb_open();
    }
    else {
#ifdef EPP_JTAG
        if ((ppfd = dbp_open(dbp_Rmon, 0)) < 0)
#else
            if ((ppfd = dbp_open(dbp_JTAG, 0)) < 0)
#endif
            {
                perror("dbp");
                return 1;
            }
        dbp_cr(1);
#if 0
#ifdef EPP_JTAG
        status = DBP_CTRL_RESET_HOST;
        if (dbp_ctrl(DBP_CTRL_WRITE, &status) < 0) {
            perror("dbp_ctrl DBP_RESET_HOST");
            return 1;
        }
#endif
#endif
#if 0
        if (dbp_ctrl(DBP_CTRL_READ, &status) < 0) {
            perror("dbp_ctrl read");
            return 1;
        }
        printf("status %x\n", status);
        dbp_write_data("", 1);
#endif
        dbp_ctrl(DBP_CTRL_READ, &status);
        printf("status 0x%x\n", status);
        { unsigned char req;
        req = DBP_CTRL_CLR_BB_ACK;
        if (dbp_ctrl(DBP_CTRL_WRITE, &req) < 0) {
            perror("DBP_CTRL_CLR_BB_ACK");
            exit(1);
        }
        }
    }

    for(;;) {
        FD_ZERO(&rfd);
        FD_ZERO(&efd);
        nfd = 0;
        for(i = 0; stream[i].name; i++) {
            if (stream[i].fd /*&& (stream[i].flag & F_WRITE)*/) {
                FD_SET(stream[i].fd, &rfd);
                FD_SET(stream[i].fd, &efd);
                if (stream[i].fd > nfd) nfd = stream[i].fd;
            } else {
                FD_SET(stream[i].lfd, &rfd);
                if (stream[i].lfd > nfd) nfd = stream[i].lfd;
            }
        }
        if (usb) {
            /*
             * The usb device can detach and reattach
             */
            while (usbfd < 0) {
                usb_open();
                if (usbfd >= 0)
                    break;
                sleep(1);
            }
            FD_SET(usbfd, &rfd);
            if (usbfd > nfd) nfd = usbfd;
        } else {
            FD_SET(ppfd, &rfd);
            if (ppfd > nfd) nfd = ppfd;
        }
        tm.tv_sec = 0;
        tm.tv_usec = 10000;
        tm.tv_usec = 001;   /* XXX */
        if (select(nfd+1, &rfd, NULL, &efd, &tm) > 0) {
            for(i = 0; stream[i].name; i++) {
                if (FD_ISSET(stream[i].lfd, &rfd)) {
                    if (stream[i].port == 0) 
                    {
                        /* Port number not set: Unix Domain Socket */
                        stream[i].fd = accept(stream[i].lfd, 0, 0);
                    }
                    else
                    {
                        /* Internet socket */
                        struct sockaddr_in sa;
                        int tmp;
                        tmp = sizeof(sa);
                        stream[i].fd = accept (stream[i].lfd, (struct sockaddr *) &sa, &tmp);
                        if (stream[i].fd >= 0)
                        {
                            /* Convert IP address to string.  */
                            printf ("Connect to %s from host %s\n", 
                                    stream[i].name, inet_ntoa (sa.sin_addr));

                            /* TCP Socket */
                            /* Enable TCP keep alive process. */
                            tmp = 1;
                            setsockopt (stream[i].lfd, SOL_SOCKET, SO_KEEPALIVE, 
                                        (char *) &tmp, sizeof (tmp));
                            
                            /* Tell TCP not to delay small packets.  This greatly speeds up
                               interactive response. */
                            tmp = 1;
                            setsockopt (stream[i].fd, IPPROTO_TCP, TCP_NODELAY,
                                        (char *) &tmp, sizeof (tmp));
                        }
                        else
                        {
                            fprintf(stderr, "Accept on %s failed\n", stream[i].name);
                            continue;
                        }
                    }

                    if ((i == DEV_DEBUG) && !(stream[i].flag & F_VALID))
                    {
                        fprintf(stderr, "BB player not ready to accept connections on %s.\n",
                                stream[i].name);
                        close_stream(i);
                        continue;
                    }

                    if (verbose > 1)
                        printf("accept %s %d\n", stream[i].name, stream[i].fd);
                    if (i == DEV_KDEBUG || i == DEV_PROFILE || i == DEV_DATA || i == DEV_DEBUG)
                        stream[i].flag |= F_WRITE;
#ifdef EPP_JTAG
                    /*XXXblythe this should go away altogether in EPP_JTAG mode */
                    if (!usb) {
                        if (i == DEV_JTAG)
                            dbp_set_protocol(dbp_JTAG);
                        else
                            dbp_set_protocol(dbp_Rmon);
                        dbp_cr(1);
                    }
#endif
                } else if (FD_ISSET(stream[i].fd, &rfd)) {
                    /* handle request */
//          printf("request %s\n", stream[i].name);
                    switch(i) {
                    case DEV_MAIN:
                        rv = empty_stream(i, 1);
                        break;
                    case DEV_DATA:
                        if (!stream[i].outgoing_ct) {
                            rv = read(stream[i].fd, &stream[i].outgoing_ct, sizeof stream[i].outgoing_ct);
//              printf("xfer %d\n", stream[i].outgoing_ct);
                            if (rv <= 0) break;
                        }
                        if ((stream[i].flag & F_WRITE) && stream[i].outgoing_ct)
                            rv = copy_in(i, RDB_TYPE_HtoG_DATA);
                        break;
                    case DEV_DEBUG:
                        if (!(stream[i].flag & F_VALID))
                        {
                            fprintf(stderr, "BB player not ready to accept debugging messages.\n");
                            fprintf(stderr, "Closing %s.\n", stream[i].name);
                            rv = 0;  /* Set rv to 0 so this stream will get closed */
                        }
                        else if (stream[i].flag & F_WRITE) {
                            rv = copy_in(i, RDB_TYPE_HtoG_DEBUG);
                        }
                        else { 
                            /* Drop messages - rely on GDB to retransmit */
                            fprintf(stderr, "BB player not ready to accept debugging messages.\n");
                            rv = empty_stream(i, 1); 
                        } 
                        break;
                    case DEV_FAULT:
                        rv = empty_stream(i, 1);
                        break;
                    case DEV_KDEBUG:
                        rv = copy_in(i, RDB_TYPE_HtoG_KDEBUG);
                        break;
                    case DEV_LOGGING:
                        rv = empty_stream(i, 1);
                        break;
                    case DEV_PRINT:
                        rv = empty_stream(i, 1);
                        break;
                    case DEV_PROFILE:
                        rv = copy_in(i, RDB_TYPE_HtoG_PROF_SIGNAL);
                        break;
                    case DEV_JTAG: {
                        unsigned char data;
                        rv = read(stream[i].fd, &data, 1);
                        if (rv == 1) {
                            if (data & 0x80) {
                                int x = data&0x7f;
                                jtag_write(x);
                            } else {
                                data = jtag_read();
                                write(stream[i].fd, &data, 1);
                            }
                        }
                        break;
                    case DEV_POKE:
                        rv = empty_stream(i, 0);
                        if (rv) dump(16, stream[i].fd);
                        break;
                    }
                    }
                    if (rv == 0) {
                        close_stream(i);
                    }
                } else if (FD_ISSET(stream[i].fd, &efd)) {
                    printf("efd %s\n", stream[i].name);
                } else if (usb) {
                    if (usbfd >= 0 && FD_ISSET(usbfd, &rfd)) {
                        usb_event();
                        if (usbfd >= 0) FD_CLR(usbfd, &rfd);
                    }
                } else if (FD_ISSET(ppfd, &rfd) || irq) {
                    //printf("pp event\n");
                    pp_event(ppfd);
                    irq = 0;
                }
            }
        }
        if (usb)
            usb_event();
        else if (__epp) pp_event(ppfd);
        if (write_avail)
            while (write_avail && send_next()) ;
    }
    for(i = 0; stream[i].name; i++) {
        close(stream[i].lfd);
        if (stream[i].fd) close(stream[i].fd);
        unlink(stream[i].name);
    }
    if (usb)
        (void) close(usbfd);
    else
        dbp_close();

    return 0;
}