//=================================================================
//
//        ser_filter.cxx
//
//        Serial test filter
//
//=================================================================
//####COPYRIGHTBEGIN####
//
// -------------------------------------------
// The contents of this file are subject to the Cygnus eCos Public License
// Version 1.0 (the "License"); you may not use this file except in
// compliance with the License.  You may obtain a copy of the License at
// http://sourceware.cygnus.com/ecos
// 
// 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.
// 
// The Original Code is eCos - Embedded Cygnus Operating System, released
// September 30, 1998.
// 
// The Initial Developer of the Original Code is Cygnus.  Portions created
// by Cygnus are Copyright (C) 1998, 1999 Cygnus Solutions.
// All Rights Reserved.
// -------------------------------------------
//
//####COPYRIGHTEND####
//=================================================================
//#####DESCRIPTIONBEGIN####
//
// Author(s):     jskov
// Contributors:  jskov
// Date:          1999-03-01
// Description:   This program acts as a filter between GDB and the test
//                running on the target, allowing testing of the serial
//                driver without confusing GDB.
// Usage:         Run the program with one argument, the serial port
//                on with the target is connected.
//                Run serial test in GDB, connecting to the target with
//                'target remote localhost:5678'.
//
// To Do:
//  o Add timeout setup and handling for recovery, can rely on testing
//    agent to control global timeout.
//  o Saving chunks that caused transfer failure?
//     - In SEND with echo, do CRC on 32-byte sub-packets
//  o Additional To Do items under each sub-protocol function.
//  o Option to get all serial IO written (in hex, > and < prepends 
//    input/output lines) to a file.
//  o Clean up the mess in this file....
//####DESCRIPTIONEND####

#include "stdafx.h"
#include "eCosTest.h"
#include "eCosTestSocket.h"
#include "eCosTestSerial.h"
#include <setjmp.h>

#ifdef _WIN32
#define CYG_UNUSED_PARAM( _type_, _name_ ) _name_
#else
#define CYG_UNUSED_PARAM( _type_, _name_ ) {    \
  _type_ __tmp1 = (_name_);                     \
  _type_ __tmp2 = __tmp1;                       \
  __tmp1 = __tmp2;                              \
}
#endif

// This gets uglier and uglier...
CeCosTestSocket* gdb_socket;

int opt_ser_debug = 0;
int opt_console_output = 0;

jmp_buf fallback_buf;

enum {MAX_CMD_LEN=128};
struct ser_filter_state_t {
    bool null_filter;
    int cmd_i;
    int cmd_flag;
    char cmd[MAX_CMD_LEN];

    // We need this to avoid outputting serial tracing while GDB is trying to
    // connect, or it will get confused.
    bool first_command_seen;

    ser_filter_state_t() :
        null_filter(0),
        cmd_i(0),
        cmd_flag(0),
        first_command_seen(0)
        {}
};

typedef struct ser_cfg {
    int baud_rate;
    int data_bits;
    CeCosTestSerial::StopBitsType stop_bits;
    bool parity;
    // etc...
} ser_cfg_t;

typedef enum {
    MODE_NO_ECHO = 0,
    MODE_EOP_ECHO,
    MODE_DUPLEX_ECHO
} cyg_mode_t;

char msg_ok[] = "OK";
char msg_er[] = "ER";

//----------------------------------------------------------------------------
// Macros to help extract values from the argument string.
// Note: This is probably not an ideal solution, but it was easy to make :)

#define INIT_VALUE(__args)                      \
    unsigned int v;                             \
    char *__ptr1, *__ptr2 = (__args)

#define SET_VALUE(__type, __slot)               \
{                                               \
    __ptr1 = strchr(__ptr2, (int) ':');         \
    if (__ptr1)                                 \
        *__ptr1 = 0;                            \
    v = atoi(__ptr2);                           \
    __ptr2 = __ptr1+1;                          \
    (__slot) = (__type) v;                      \
}

//------------------------
// Output helpers.

// Encode string in an O-packet and send it to GDB.
void 
gdb_write(const char* pszStr)
{
    if (gdb_socket) {
        static const char hexchars[] = "0123456789abcdef";
        char* packet = new char[strlen(pszStr)*2+6];
        char* p = packet;

        *p++ = '$';
        *p++ = 'O';
        unsigned char crc = 'O';
        char c;
        for (;;) {
            c = *pszStr++;
            if (0 == c)
                break;

            char h = hexchars[(c >> 4) & 0x0f];
            char l = hexchars[c & 0x0f];
            *p++ = h;
            *p++ = l;
            crc = (unsigned char) (crc + h + l);
        };

        *p++ = '#';
        *p++ = hexchars[(crc >> 4) & 0x0f];
        *p++ = hexchars[crc & 0x0f];
        
        // Only try to send once. If it fails, it's probably because
        // GDB has disconnected.
        gdb_socket->send(packet, p - packet);
        gdb_socket->recv(&c, 1);

        delete [] packet;
    }
}

void
console_write(const char* pszStr)
{
    fputs(pszStr, stderr);
    fflush(stderr);
}

void
trace(const char* pszFormat, ...)
{
    va_list marker;
    va_start (marker, pszFormat);
    CeCosTestUtils::String str;
    str.vFormat(pszFormat,marker);
    va_end (marker);

    // Prefix with [f]
    CeCosTestUtils::String str2("[f] ");
    str2 += str;

    if (opt_console_output)
        console_write((const char*) str2);
    else
        gdb_write((const char*) str2);
}


void
print_hex(unsigned char* d1, int len)
{
    int offset = 0;
    int i;
    char buf[128];
    int width = 8;

    while (len) {
        int count = min(width, len);
        char* p = buf;
        p += sprintf(p, "%04x ", offset);
        // Print hex values.
        for (i = 0; i < count; i++)
            p += sprintf(p, "%02x ", d1[i]);
        for (     ; i < width   ; i++)
            p += sprintf(p, ".. ");

        // Print ASCII string
        p += sprintf(p, "'");
        for (i = 0; i < count; i++) {
            int c = d1[i];
            if (' ' >= c || 'z' <= c)
                c = '.';
            p += sprintf(p, "%c", c);
        }
        sprintf(p, "'\n");

        trace("%s", buf);
        
        len -= count;
        offset += count;
        d1 += count;
    }
}

void
target_write(CeCosTestSerial &pSer, const unsigned char* buffer, int len)
{
    unsigned int __written;

    do {
        if (!(pSer.Write((void*) buffer, len, __written))) {
            fprintf(stderr, "Writing %d bytes to serial failed\n", len);
            fprintf(stderr, strerror(errno));
            throw "serial write failed";
        }
        buffer += __written;
        len -= __written;
    } while (len);

}

// Yuck! There is probably a better way to do this...
void* recover_data;
int   recover_data_len;

bool
target_read(CeCosTestSerial &pSer, unsigned char* buffer, int len)
{
    unsigned int __read;
    int __total_read = 0;
    unsigned char* __buffer_base = buffer;
    int __timeouts = 0;
    int __timeout_failure = 0;

    do {
        if (!(pSer.Read((void*) buffer, len, __read))) {
            fprintf(stderr,"Reading %d bytes from serial failed (read %d).\n",
                    len, __read);
            fprintf(stderr, strerror(errno));
            throw "serial read failed";
        }

        __total_read += __read;

        unsigned int i;
        for (i = 0; i < __read; i++) {
            if ('$' == buffer[i]) {

                trace("**** Detected $ -- resuming as null filter ****\n");

                trace("Data received %d bytes (of %d) from target:\n", 
                      __total_read, len);
                print_hex(__buffer_base, __total_read);
                trace("<end>\n");

                recover_data = (void*) &buffer[i];
                recover_data_len = __read - i;

                longjmp(fallback_buf, 1);
            }
        }

        if (0 == __read) {
            CeCosTestUtils::Sleep(20);
            __timeouts++;
            if (25 == __timeouts) {
                __timeouts = 0;
                if (5 == __timeout_failure++) {
                    trace("**** Timed out while reading -- resuming as null filter\n");

                    trace("Data received %d bytes (of %d) from target:\n", 
                          __total_read, len);
                    print_hex(__buffer_base, __total_read);
                    trace("<end>\n");

                    static const char kill_msg[] = "$X00#b8";
                    recover_data_len = strlen(kill_msg);
                    recover_data = (void*) kill_msg;

                    longjmp(fallback_buf, 2);
                }
            }
        } else {
            __timeouts = 0;
            __timeout_failure = 0;
        }

        buffer += __read;
        len -= __read;

    } while (len);

    return true;
}

// Send C ASCII string to target.
void 
target_ASCII_write(CeCosTestSerial &pSer, const char* s) 
{ 
    target_write(pSer, (const unsigned char*) s, strlen(s)); 
};


//------------------------
// Configuration Command.
void parse_config(char* cfg_str, ser_cfg_t* new_cfg);
void change_config(char* cfg_str);

// Return false if the serial configuration is not valid for the host.
int 
verify_config(ser_cfg_t* cfg)
{
    CYG_UNUSED_PARAM(ser_cfg_t*, cfg);

    // FIXME: Assume OK for now.
    return 1;
}

// Set serial configuration.
void
set_config(CeCosTestSerial &pSer, const ser_cfg_t* cfg)
{
    pSer.SetBaud(cfg->baud_rate, false);
    pSer.SetParity(cfg->parity, false);
    pSer.SetDataBits(cfg->data_bits, false);
    pSer.SetStopBits(cfg->stop_bits, true); // apply settings
}

// Set default serial configuration.
void default_config(CeCosTestSerial &pSer);


//-----------------------------------------------------------------------------
// Change serial configuration.
// To Do:
//   Proper (full) implementation.
void
change_config(CeCosTestSerial &pSer, char* cfg_str)
{
    ser_cfg_t new_cfg;

    parse_config(cfg_str, &new_cfg);


    if (verify_config(&new_cfg)) {
        target_ASCII_write(pSer, "OK");
        set_config(pSer, &new_cfg);
    } else {
        target_ASCII_write(pSer, "ER");
    }
}

// Set default configuration.
void
default_config(CeCosTestSerial &pSer)
{
    static const ser_cfg_t default_ser_cfg = { 9600, 
                                               8, 
                                               CeCosTestSerial::ONE, 
                                               false };

    target_ASCII_write(pSer, "OK");
    set_config(pSer, &default_ser_cfg);
}

// Parse config string from target and set new_cfg accordingly.
// String from target is:
//  <baud rate>:<data bits>:<stop bits>:<parity>:....
void
parse_config(char* args, ser_cfg_t* new_cfg)
{
    int ecos_parity, ecos_stop_bits, ecos_baud_rate;
    CeCosTestSerial::StopBitsType t2h_stop_bits[3] = {
        CeCosTestSerial::ONE, 
        CeCosTestSerial::ONE_POINT_FIVE,
        CeCosTestSerial::TWO};
    INIT_VALUE(args);

    SET_VALUE(int, ecos_baud_rate);
    SET_VALUE(int, new_cfg->data_bits);
    SET_VALUE(int, ecos_stop_bits);
    SET_VALUE(int, ecos_parity);

    new_cfg->parity = (ecos_parity) ? true : false;
    new_cfg->stop_bits = t2h_stop_bits[ecos_stop_bits - 1];

    // eCos->human translation of serial baud rate. This table must
    // match the one in io/serial/current/include/serialio.h
    static const int tt_baud_rate[] = {
        -1,                                 // 0 invalid
        50,                                 // 1 50
        75,                                 // 2 75
        110,                                // 3
        135,                                // 4 134_5
        150,                                // 5
        200,                                // 6 200
        300,                                // 7
        600,                                // 8
        1200,                               // 9
        1800,                               // 10 1800
        2400,                               // 11
        3600,                               // 12 3600
        4800,                               // 13
        7200,                               // 14 7200
        9600,                               // 15
        14400,                              // 16 14400
        19200,                              // 17
        38400,                              // 18
        57600,                              // 19
        115200,                             // 20
        234000                              // 21 234000
    };

    if (ecos_baud_rate > 0 && ecos_baud_rate < (int) sizeof(tt_baud_rate))
        ecos_baud_rate = tt_baud_rate[ecos_baud_rate];
    else
        ecos_baud_rate = -2;

    new_cfg->baud_rate = ecos_baud_rate;
}

// Always make sure CRC fits in 31 bits. Bit of a hack, but we want
// to send CRC as ASCII without too much hassle.
int
do_crc(unsigned char* data, int size)
{
    int i;
    unsigned long crc;

    for (i = 0, crc = 0; i < size; i++) {
        crc = (crc << 1) ^ data[i];     // FIXME: standard definition?
    }

    i = (int) crc;
    if (i < 0)
        i = -i;

    return i;
}

void
send_checksum(CeCosTestSerial &pSer, int crc)
{
    char buffer[128];
    int len;

    len = sprintf(buffer, "%d!", crc);

    target_write(pSer, (const unsigned char*)buffer, len);
}

void
send_status(CeCosTestSerial &pSer, int state)
{
    if (state)
        target_write(pSer, (unsigned char*) &msg_ok, 2);
    else
        target_write(pSer, (unsigned char*) &msg_er, 2);
}


// Receive test DONE message from target.
void
receive_done(CeCosTestSerial &pSer, unsigned char* data_in, int size)
{
    static const char msg_done[] = "DONE";
    unsigned char data_reply[4];
    int first = 1;

    target_read(pSer, data_reply, 4);
    while (0 != strncmp((char*) data_reply, msg_done, 4)) {
        if (first) {
            if (data_in && size) {
                trace("Data received from target:\n");
                print_hex(data_in, size);
                trace("<end>\n");
            }
            trace("Receiving junk instead of DONE:\n");
            first = 0;
        }
        print_hex(data_reply, 4);

        data_reply[0] = data_reply[1];
        data_reply[1] = data_reply[2];
        data_reply[2] = data_reply[3];

        // The target_read call will handle recovery in case of timeout...
        target_read(pSer, &data_reply[3], 1);
    }
}

//-----------------------------------------------------------------------------
// Test binary data transmission.
// Format in:
//  <byte size>:<mode>
// Format out:
//  <4 bytes binary checksum><#size bytes data>
// If echo mode, also:
//    Format in:
//     <#size bytes data>
//    Format out:
//     OK/ER - according to CRC match on incomin data
// Format in:
//  DONE
//
// To Do:
//  o Add mode/flag specifying 5-8 bit transfer.
//     Test that 0xff gets masked off accordingly when transfered.
//     (This should be an INFO result if failing)
//  o Clean up the DUPLEX_ECHO implementation. Currently it's an ugly hack
//    that doesn't match the arguments / behavior of the two other modes.
void
test_binary(CeCosTestSerial &pSer, char* args)
{
    int size;
    cyg_mode_t mode;
    unsigned char *data_out, *data_in;
    int i;
    int crc;

    int loop_count = 0;

    INIT_VALUE(args);

    SET_VALUE(int, size);
    SET_VALUE(cyg_mode_t, mode);

    // Change behavior for DUPLEX mode.
    if (MODE_DUPLEX_ECHO == mode) {
        loop_count = size;
        size = 1024;                    // must be at least 4*block_size
    }

    // Generate data.
    data_out = (unsigned char*) malloc(size);
    if (!data_out) {
        fprintf(stderr, "Could not allocate %d byte buffer for data!\n", size);
        throw "data_out malloc failed";
    }
    data_in = (unsigned char*) malloc(size);
    if (!data_in) {
        fprintf(stderr, "Could not allocate %d byte buffer for data!\n", size);
        throw "data_in malloc failed";
    }
    for (i = 0; i < size; i++) {
        static int count = 0;
        unsigned char c = (unsigned char) (count++ & 0xff);
        // don't allow $s and @s in the data, nor 0x03 (GDB C-c)
        if ('$' == c || '@' == c || 0x03 == c)
            c = (unsigned char) '*';
        data_out[i] = c;
    }

    // Do checksum.
    crc = do_crc(data_out, size);

    // Send checksum to target.
    send_checksum(pSer, crc);

    switch (mode) {
    case MODE_NO_ECHO:
    {
        // Simple transmit. Don't expect target to echo data back.
        target_write(pSer, data_out, size);
        receive_done(pSer, NULL, 0);
    }
    break;
    case MODE_EOP_ECHO:
    {
        int in_crc;

        target_write(pSer, data_out, size);

        // Expect target to echo the data
        target_read(pSer, data_in, size);

        // Check echoed data, and reply OK/ER accordingly.
        in_crc = do_crc(data_in, size);
        send_status(pSer, (in_crc == crc));


        // Dump seen/expected on console.
        if (in_crc != crc) {
            trace("Data seen:\n");
            print_hex(data_in, size);
            trace("<end>\n");
            trace("Data expected:\n");
            print_hex(data_out, size);
            trace("<end>\n");
        }

        receive_done(pSer, data_in, size);

    }
    break;
    case MODE_DUPLEX_ECHO:
    {
        int block_size = 64;
        int fail, j;

        // This is a simple implementation (maybe too simple).
        // Host sends 4 packets with the same size (64 bytes atm).
        // Target echoes in this way:
        //  packet1 -> packet1
        //  packet2 -> packet2, packet2
        //  packet3 -> packet3
        //  packet4 -> /dev/null
        //
        // The reads/writes are interleaved in a way that should ensure
        // the target out buffer to be full before the target starts to read
        // packet3. That is, the target should be both receiving (packet3)
        // and sending (packet2) at the same time.

        // This code needs restructuring. It's not very obvious what's
        // happening: The same block of data is output several times,
        // the target echoes the data back (one of the blocks is
        // echoed twice). Then the echoed data is compared agains the
        // outgoing data block.

        fail = 0;
        while (loop_count--) {
            int i;
            for (i = 0; i < block_size*4; i++)
                data_in[i] = 0;

            // out1: block_size -> block_size
            target_write(pSer, data_out, block_size);

            // out2: block_size -> 2 x block_size
            target_write(pSer, data_out, block_size);

            // in1:
            target_read(pSer, data_in, block_size);

            // out3: block_size -> block_size
            target_write(pSer, data_out, block_size);
        
            // in2:
            target_read(pSer, &data_in[block_size], 2*block_size);

            // out4: block_size -> 0
            target_write(pSer, data_out, block_size);
        
            // in3:
            target_read(pSer, &data_in[block_size*3], block_size);

            if (0 == loop_count % 10)
                trace("%d loops to go\n", loop_count);

            // Verify data.
            if (!fail) {
                for (j = 0; j < 4 && !fail; j++) {
                    for (i = 0; i < block_size && !fail; i++) {
                        if (data_out[i] != data_in[j*block_size + i]) {
                            fail = 1;
                            trace("Failed at byte %d\n", j*block_size + i);
                            
                            trace("Data seen:\n");
                            print_hex(&data_in[j*block_size], 
                                           block_size);
                            trace("<end>\n");
                            trace("Data expected:\n");
                            print_hex(data_out, block_size);
                            trace("<end>\n");
                        }
                    }
                }
            }
        }
        // Check echoed data, and reply OK/ER accordingly.
        send_status(pSer, (!fail));
        receive_done(pSer, data_in, block_size*4);
    }
    break;
    default:
        trace("Unknown mode. Ignoring.\n");
    }

    // Free buffer.
    free(data_in);
    free(data_out);
}

//-----------------------------------------------------------------------------
// Test transformations on text transmissions
//
// This test transmits null-terminated C strings back and forth. Since
// the translation is under test and may fail, the length of the data is
// (potentially) unknown. Sending with a null-terminator allows proper
// recovery even if the translations do not work as intended.
//
// Format in:
//  <flags>!<4 bytes binary checksum><C string>
// Format out:
//  <C string>
//  OK/ER
//
// Mode:
//   MODE_EOP_ECHO:
//       Receive data, verify CRC, resend data.
//       Send OK/ER reply when done.
//   MODE_DUPLEX_ECHO:
//       Receive data, echo data, verify CRC.
//       Send OK/ER reply when done.
//
// To Do:
//  Implement.
void
test_text(CeCosTestSerial &pSer, char* args)
{
    CYG_UNUSED_PARAM(char*, args);

    send_status(pSer, 1);
}

//-----------------------------------------------------------------------------
// Reply to PING packet from target.
// Format in:
//  "!"
// Format out:
//  OK
void
test_ping(CeCosTestSerial &pSer, char* args)
{ 
    CYG_UNUSED_PARAM(char*, args);

    send_status(pSer, 1);
}

//-----------------------------------------------------------------------------
// Dispatch test command. 
void
dispatch_command(CeCosTestSerial &pSer, char* cmd)
{
    char* args;

    args = strchr(cmd, (int) ':');
    if (!args) {
        trace("Bogus command (%s) Ignoring.\n", cmd);
        return;
    }
        
    *args++ = 0;

    trace("Dispatching command %s.\n", cmd);

    if (0 == strcmp("CONFIG", cmd)) {
        change_config(pSer, args);
    } 
    else if (0 == strcmp("DEFCONFIG", cmd)) {
        // Note: Currently the arguments are ignored. 9600 8N1 is default.
        default_config(pSer);
    }
    else if (0 == strcmp("BINARY", cmd)) {
        test_binary(pSer, args);
    }
    else if (0 == strcmp("TEXT", cmd)) {
        test_text(pSer, args);
    }
    else if (0 == strcmp("PING", cmd)) {
        test_ping(pSer, args);
    }
    else
        trace("Unknown command '%s'.\n", cmd);

    trace("Command %s completed.\n", cmd);
}

bool CALLBACK
serial_filter(void*& pBuf,
              unsigned int& nRead,
              CeCosTestSerial& serial,
              CeCosTestSocket& socket,
              void* pParem)
{
    ser_filter_state_t* state = (ser_filter_state_t*) pParem;
    char* buffer = (char*) pBuf;

    // Don't do anything in the null_filter mode.
    if (state->null_filter)
        return true;

    // Allows trace to be called without a reference to the socket...
    gdb_socket = &socket;

    // This allows the filter to unwind, wherever in the protocol it
    // may be, when a $ is detected from the target side.
    // When this happens, we may have a trap/exception on the target
    // and we want the user to access the target via GDB without
    // intervention.
    recover_data_len = 0;
    state->null_filter = setjmp(fallback_buf) ? true : false;
    if (state->null_filter) {

        if (recover_data_len) {
            unsigned char *s, *d;
            d = (unsigned char*) pBuf;
            s = (unsigned char*) recover_data;
            for (int i = 0; i < recover_data_len; i++)
                *d++ = *s++;
        }

        nRead = recover_data_len;
        return true;
    }

    if (opt_ser_debug && state->first_command_seen)
        print_hex((unsigned char*) buffer, nRead);

    // Command handling.
    // If we are not presently reading a command, look for the
    // start marker.
    unsigned int i = 0;
    if (!state->cmd_flag)
        for (; i < nRead; i++) {
            if ('@' == buffer[i]) {
                state->cmd_flag = 1;
                // Send the data before the marker.
                if (i)
                    socket.send(buffer, i);
                break;
            }
        }

    // If reading a command, look for the end marker.
    if (state->cmd_flag) {
        char c = 0;
        while (i < nRead && state->cmd_i < MAX_CMD_LEN) {
            c = buffer[i++];
            state->cmd[state->cmd_i++] = c;
            if ('!' == c) {
                if (i != nRead) {
                    // Hopefully, we'll never have to cope 
                    // with this.
                    fprintf(stderr, 
                            "Yikes! We need to unread %d bytes!\n",
                            nRead - i);
                    throw "Yikes! We need to unread some bytes!";
                }
                break;
            }
        }

        if (MAX_CMD_LEN == state->cmd_i) {
            trace("Received too long command. Ignoring it!\n");
            state->cmd_i = 0;
            state->cmd_flag = 0;
        } else if ('!' == c) {
            // Was the command completed?
            state->cmd[state->cmd_i - 1] = 0; // terminate cmd
            state->cmd_i = 0;
            state->cmd_flag = 0;
            state->first_command_seen = true;

            // skip @ when passing ptr
            dispatch_command(serial, &state->cmd[1]);
        }

        nRead = 0;                      // Never leave anything for caller
                                        // This is a violation of the intended
                                        // filter function behavior.
    }
    return true;
}

#ifdef DEFINE_MAIN

void
no_gdb(const char* pszPort, int nBaud, FilterFunc *pSerialToSocketFilterFunc,
       void *pParam, bool *pbStop)
{
    trace("no_gdb, listening on %s\n",pszPort);

    CeCosTestSocket dummy_socket;
    CeCosTestSerial serial;
    serial.SetBlockingReads(false);
    bool rc=false;

    // Open serial device.
    if (!serial.Open(pszPort,nBaud)){
        ERROR("Couldn't open port %s\n",pszPort);
    } else {
        // Flush the serial buffer.
        serial.Flush();

        serial.ClearError();
        enum {BUFSIZE=8192};
        void *pBuf=malloc(BUFSIZE);
        rc=true;
        while(rc && (0==pbStop || !(*pbStop))){
            unsigned int nRead=0;

            while (1) {
                if(serial.Read(pBuf,BUFSIZE,nRead)){
                    if(nRead>0){
                        break;
                    }
                    CeCosTestUtils::Sleep(1);
                } else {
                    trace("Serial read failed (%d)\n", errno);
                }
            }

            trace("Serial:%d\n",nRead);
            if(pSerialToSocketFilterFunc){
                rc=pSerialToSocketFilterFunc(pBuf,nRead,serial,dummy_socket,
                                             pParam);
            }
        }
        free(pBuf);
    }
}

int
main(int argc, char** argv)
{
    int nSock = 0;
    int baud_rate, nTcpPort;

    int opt_no_gdb = 0;
    char* ser_port;

    int i=1;
    while(i<argc){
        if(argv[i][0]=='-'){
            switch(argv[i][1]){
                case 't':
                    CeCosTestUtils::EnableTracing(true);
                    break;
                case 'S':
                    opt_ser_debug = 1;
                    break;
                case 'n':
                    opt_no_gdb = 1;
                    break;
                case 'c':
                    opt_console_output = 1;
                    break;
                default:
                    fprintf(stderr,"Unrecognized switch %s\n",argv[i]);
                    goto Usage;
                    break;
            }
            for(int j=i;j<argc;j++){
                argv[j]=argv[j+1];
            }
            argc--;
            argv[argc]=0;
        } else {
            i++;
        }
    }

    if(!((3==argc && opt_no_gdb) || (4==argc && !opt_no_gdb)))
    {
        goto Usage;
    }

    if (opt_no_gdb) {
        ser_port = argv[1];
        baud_rate=atoi(argv[2]);
    } else {
        nTcpPort=atoi(argv[1]);
        if(0==nTcpPort){
            fprintf(stderr,"Invalid port %s\n",argv[1]);
            return main(0,argv); // Provoke usage message
        }

        ser_port = argv[2];
        baud_rate=atoi(argv[3]);

        nSock = CeCosTestSocket::Listen(nTcpPort);
        if (-1 == nSock) {
            fprintf(stderr, "Couldn't access socket.\n");
            throw "listen failed";
        }
    }

    while (1) {
        ser_filter_state_t state;

        gdb_socket = NULL;
    
        try {
            if (opt_no_gdb)
                no_gdb(ser_port, baud_rate, &serial_filter, &state, NULL);
            else
                ConnectSocketToSerial(nSock, ser_port, baud_rate, 
                                      &serial_filter, NULL, &state, NULL);
        } 
        catch (const char* p) {
            trace("Caught filter crash: %s\n", p);
        }
    }

    return 0;

Usage:
    const char *pszMe=CeCosTestUtils::Tail(argv[0]);
    fprintf(stderr,"Usage: %s [-t -c -S] TcpIPport SerialPort BaudRate\n",
    //                                   1         2          3
            pszMe);
    fprintf(stderr," or:   %s -n [-t -c -S] SerialPort BaudRate\n",
    //                                      1          2
            pszMe);
    fprintf(stderr, " -t: Enable tracing.\n");
    fprintf(stderr, " -S: Output data read from serial line.\n");
    fprintf(stderr, " -c: Output data on console instead of via GDB.\n");
    fprintf(stderr, " -n: No GDB.\n");
    return 1;
}
#endif

