new file mode 100644
--- /dev/null
+++ b/host/tools/ecostest/common/eCosTestSerialFilter.cpp
@@ -0,0 +1,1069 @@
+//####COPYRIGHTBEGIN####
+//                                                                          
+// ----------------------------------------------------------------------------
+// Copyright (C) 1998, 1999, 2000 Red Hat, Inc.
+//
+// This program is part of the eCos host tools.
+//
+// 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 2 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, write to the Free Software Foundation, Inc., 
+// 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
+//
+// ----------------------------------------------------------------------------
+//                                                                          
+//####COPYRIGHTEND####
+//=================================================================
+//
+//        eCosTestSerialFilter.cpp
+//
+//        Serial test filter class
+//
+//=================================================================
+//=================================================================
+//#####DESCRIPTIONBEGIN####
+//
+// Author(s):     jskov
+// Contributors:  jskov
+// Date:          1999-03-01
+// Description:   This filter sits between GDB and the test running on
+//                the target, allowing testing of the serial driver 
+//                without confusing GDB.
+// 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 "eCosStd.h"
+
+#define SER_FILTER_VER "$Id: eCosTestSerialFilter.cpp,v 1.1 2000/03/28 14:10:57 jlarmour Exp $"
+#include "eCosTestSerialFilter.h"
+
+char msg_ok[] = "OK";
+char msg_er[] = "ER";
+
+
+
+CeCosTestSerialFilter::CeCosTestSerialFilter():
+  m_bOptConsoleOutput(false), 
+  m_bOptSerDebug(false), 
+  m_bOptFilterTrace(false),
+  m_xUnreadBuffer(NULL),
+  m_nUnreadBufferIndex(0), 
+  m_nUnreadBufferSize(0), 
+  m_bNullFilter(false), 
+  m_nCmdIndex(0), 
+  m_bCmdFlag(false), 
+  m_bFirstCommandSeen(false),
+  m_cGDBSocket(NULL)
+{
+}
+
+CeCosTestSerialFilter::~CeCosTestSerialFilter()
+{
+}
+
+//------------------------
+// Output helpers.
+
+// Encode string in an O-packet and send it to GDB.
+void 
+CeCosTestSerialFilter::GDBWrite(const char* pszStr)
+{
+    if (m_cGDBSocket) {
+        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.
+        m_cGDBSocket->send(packet, p - packet);
+        m_cGDBSocket->recv(&c, 1);
+
+        delete [] packet;
+    }
+}
+
+void
+CeCosTestSerialFilter::ConsoleWrite(const char* pszStr)
+{
+    fputs(pszStr, stderr);
+    fflush(stderr);
+}
+
+void
+CeCosTestSerialFilter::Trace(const char* pszFormat, ...)
+{
+  
+  va_list marker;
+  va_start (marker, pszFormat);
+  
+  for(int nLength=100;nLength;) {
+    char *buf=new char[1+nLength];
+    int n=vsnprintf(buf+4, nLength-4, pszFormat, marker ); 
+    if(-1==n){
+      nLength*=2;  // NT behavior
+    } else if (n<nLength){
+      memcpy(buf,"[f] ",4);
+      if (m_bOptConsoleOutput) {
+        ConsoleWrite(buf);
+      } else {
+        GDBWrite(buf);
+      }
+      nLength=0;   // trigger exit from loop
+    } else {
+      nLength=n+1; // UNIX behavior generally, or NT behavior when buffer size exactly matches required length
+    }
+    delete [] buf;
+  }
+  
+  va_end (marker);
+  
+}
+
+void
+CeCosTestSerialFilter::Log(const char* pszFormat, ...)
+{
+  va_list marker;
+  va_start (marker, pszFormat);
+  
+  for(int nLength=100;nLength;) {
+    char *buf=new char[1+nLength];
+    int n=vsnprintf(buf, nLength, pszFormat, marker ); 
+    if(-1==n){
+      nLength*=2;  // NT behavior
+    } else if (n<nLength){
+      if (m_bOptConsoleOutput) {
+        ConsoleWrite(buf);
+      } else {
+        GDBWrite(buf);
+      }
+      nLength=0;   // trigger exit from loop
+    } else {
+      nLength=n+1; // UNIX behavior generally, or NT behavior when buffer size exactly matches required length
+    }
+    delete [] buf;
+  }
+  
+  va_end (marker);
+}
+
+
+void
+CeCosTestSerialFilter::PrintHex(const unsigned char* d1, int len, data_origin_t origin/*=SF_TARGET*/)
+{
+    int offset = 0;
+    int i;
+    char buf[128];
+    int width = 8;
+
+    while (len) {
+        int count = MIN(width, len);
+        char* p = buf;
+        switch (origin) {
+        case SF_TARGET:
+            p += sprintf(p, "T");
+            break;
+        case SF_FILTER:
+            p += sprintf(p, "F");
+            break;
+        }
+        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
+CeCosTestSerialFilter::TargetWrite(CeCosTestSerial &pSer, 
+                                    const unsigned char* buffer, int len)
+{
+    unsigned int __written;
+
+    if (m_bOptFilterTrace)
+        PrintHex(buffer, len, SF_FILTER);
+
+    do {
+        if (!(pSer.Write((void*) buffer, len, __written))) {
+            fprintf(stderr, "Writing %d bytes to serial failed\n", len);
+            fprintf(stderr, "%s", (LPCTSTR)pSer.ErrString());
+            throw "serial write failed";
+        }
+        buffer += __written;
+        len -= __written;
+    } while (len);
+
+}
+
+
+bool
+CeCosTestSerialFilter::TargetRead(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;
+    int __orig_len = len;
+
+    do {
+        // First check for unread data.
+        if (m_nUnreadBufferSize) {
+            int i = 0;
+            __read = 0;
+            while (i < len && m_nUnreadBufferIndex < m_nUnreadBufferSize) {
+                buffer[i++] = m_xUnreadBuffer[m_nUnreadBufferIndex++];
+                __read++;
+            }
+            
+            if (m_nUnreadBufferIndex == m_nUnreadBufferSize) {
+                free(m_xUnreadBuffer);
+                m_nUnreadBufferSize = 0;
+                m_nUnreadBufferIndex = 0;
+            }
+        } else { 
+            // Then read directly from serial.
+            if (!(pSer.Read((void*) buffer, len, __read))) {
+                fprintf(stderr,"Reading %d bytes from serial failed (read %d).\n",
+                        len, __read);
+                char *pszErr=pSer.ErrString().GetCString();
+                fprintf(stderr, "%s", pszErr);
+                delete [] pszErr;
+                throw "serial read failed";
+            }
+        }
+
+        __total_read += __read;
+
+        unsigned int i;
+        for (i = 0; i < __read; i++) {
+            if ('$' == buffer[i]) {
+
+                Log("FAIL:<target crashed>\n"); 
+
+                Trace("**** Detected $ -- resuming as null filter ****\n");
+
+                Trace("Data received %d bytes (of %d) from target:\n", 
+                      __total_read, __orig_len);
+                PrintHex(__buffer_base, __total_read);
+                Trace("<end>\n");
+
+                filter_abort_t* msg = new filter_abort_t();
+                msg->data_ptr = &buffer[i];
+                msg->data_len = __read - i;
+
+                throw msg;
+            }
+        }
+
+        if (0 == __read) {
+            Sleep(20);
+            __timeouts++;
+            if (25 == __timeouts) {
+                __timeouts = 0;
+                if (5 == __timeout_failure++) {
+                    Log("FAIL:<target timed out>\n"); 
+
+                    Trace("**** Timed out while reading -- resuming as null filter\n");
+
+                    Trace("Data received %d bytes (of %d) from target:\n", 
+                          __total_read, __orig_len);
+                    PrintHex(__buffer_base, __total_read);
+                    Trace("<end>\n");
+
+                    static const char kill_msg[] = "$X00#b8";
+
+                    filter_abort_t* msg = new filter_abort_t();
+                    msg->data_len = strlen(kill_msg);
+                    msg->data_ptr = (const unsigned char *)kill_msg;
+
+                    throw msg;
+                }
+            }
+        } else {
+            __timeouts = 0;
+            __timeout_failure = 0;
+        }
+
+        buffer += __read;
+        len -= __read;
+
+    } while (len);
+
+    return true;
+}
+
+// Send C ASCII string to target.
+void 
+CeCosTestSerialFilter::TargetASCIIWrite(CeCosTestSerial &pSer, const char* s) 
+{ 
+    TargetWrite(pSer, (const unsigned char*) s, strlen(s)); 
+}
+
+//------------------------
+// Configuration Command.
+// Set serial configuration.
+bool
+CeCosTestSerialFilter::SetConfig(CeCosTestSerial &pSer, 
+                                 const ser_cfg_t* new_cfg, 
+                                 ser_cfg_t* old_cfg)
+{
+    if (old_cfg) {
+        old_cfg->baud_rate = pSer.GetBaud();
+        old_cfg->parity = (0 != pSer.GetParity()) ? true : false;
+        old_cfg->data_bits = pSer.GetDataBits();
+        old_cfg->stop_bits = pSer.GetStopBits();
+    }
+
+    pSer.SetBaud(new_cfg->baud_rate, false);
+    pSer.SetParity(new_cfg->parity, false);
+    pSer.SetDataBits(new_cfg->data_bits, false);
+    return pSer.SetStopBits(new_cfg->stop_bits, true); // apply settings
+}
+
+// Return false if the serial configuration is not valid for the host.
+bool
+CeCosTestSerialFilter::VerifyConfig(CeCosTestSerial &pSer, ser_cfg_t* new_cfg)
+{
+    ser_cfg_t old_cfg;
+    bool rc;
+
+    // Try changing to the new config, recording the result. Then restore
+    // the original config.
+    rc = SetConfig(pSer, new_cfg, &old_cfg);
+    SetConfig(pSer, &old_cfg, NULL);
+
+    return rc;
+}
+
+//-----------------------------------------------------------------------------
+// Configuration changing function.
+//
+// First change to the new config and back again to determine if the driver
+// can handle the config.
+// If not, return error.
+//
+// Then query the host for its capability to use the config:
+// Format out:
+//  "@CONFIG:<baud rate code>:<#data bits>:<#stop bits>:<parity on/off>!"
+// Format in:
+//  OK/ER
+//
+// On ER, return error.
+//
+// On OK, change to the new configuration. Resynchronize with the host:
+//  Target waits for host to send S(ync) 
+//     [host will delay at least .1 secs after changing baud rate so the 
+//      line has time to settle.]
+//
+//  When receiving S(ync), target replies OK to the host which then
+//  acknowledges with D(one).
+//
+//  Host can also send R(esync) which means it didn't receieve the OK. If
+//  so the target resends its S(ync) message.
+//
+// If the synchronization has not succeeded within 1 second
+// (configurable in the protocol), both host and target will revert to
+// the previous configuration and attempt to synchronize again. If
+// this fails, this call will hang and the host will consider the test
+// a failure.
+//
+// To Do:
+//  Host&protocol currently only supports:
+//   - no/even parity
+void
+CeCosTestSerialFilter::CMD_ChangeConfig(CeCosTestSerial &pSer, char* cfg_str)
+{
+    ser_cfg_t new_cfg, old_cfg;
+
+    ParseConfig(cfg_str, &new_cfg);
+
+    // Return without changing the config if it's not valid.
+    if (!VerifyConfig(pSer, &new_cfg)) {
+        TargetASCIIWrite(pSer, "ER");
+        return;
+    }
+
+    // Tell target we're ready to go, wait 1/10 sec, and then change
+    // the config.
+    TargetASCIIWrite(pSer, "OK");
+    Sleep(100);
+    SetConfig(pSer, &new_cfg, &old_cfg);
+
+    int loops;
+    for (loops = 0; loops < 3; loops++) {
+        unsigned int len, read;
+        unsigned char buffer[2];
+        int delay_mticks = 0; // millisecond-ticks. 10 of these per target tick
+
+        // Start by sending a Sync.
+        TargetASCIIWrite(pSer, "S");
+        for(;;) {
+            // Did target reply?
+            len = 2;
+            read = 0;
+            buffer[0] = 0;
+            buffer[1] = 0;
+            if (!pSer.Read((void*) buffer, len, read)) {
+                throw "CMD_ChangeConfig: serial read failure";
+            }
+            
+            if (read) {
+                // If only one char read, try to get the next one.
+                if (1 == read) {
+                    unsigned int read2 = 0;
+                    len = 1;
+                    if (!pSer.Read((void*) &buffer[1], len, read2)) {
+                        throw "CMD_ChangeConfig: serial read failure";
+                    }
+                    read += read2;
+                }
+
+                if (m_bOptSerDebug)
+                    PrintHex(buffer, read);
+
+                if ('O' == buffer[0] && 'K' == buffer[1]) {
+                    // success!
+                    TargetASCIIWrite(pSer, "D");
+                    Trace("Config change succeeded.\n");
+                    return;
+                } else {
+                    // Garbage, ask target to resend its OK message.
+                    TargetASCIIWrite(pSer, "R");
+                }
+            } else {
+                // Resend Sync message.
+                TargetASCIIWrite(pSer, "S");
+            }
+
+            Sleep(1);
+            delay_mticks++;
+            // Timeout.
+            if (100 == delay_mticks/10)
+                break;
+        }
+
+        SetConfig(pSer, &old_cfg, NULL);
+    }
+
+    // Abort the test.
+    Log("FAIL:<target timed out>\n"); 
+    Trace("**** Timed out while changing config\n");
+
+    static const char kill_msg[] = "$X00#b8";
+
+    filter_abort_t* msg = new filter_abort_t();
+    msg->data_len = strlen(kill_msg);
+    msg->data_ptr = (const unsigned char *)kill_msg;
+
+    throw msg;
+}
+
+// Set default configuration.
+void
+CeCosTestSerialFilter::CMD_DefaultConfig(CeCosTestSerial &pSer)
+{
+    static const ser_cfg_t default_ser_cfg = { 9600, 
+                                               8, 
+                                               CeCosTestSerial::ONE_STOP_BIT,
+                                               false };
+
+    TargetASCIIWrite(pSer, "OK");
+    SetConfig(pSer, &default_ser_cfg, NULL);
+}
+
+// Parse config string from target and set new_cfg accordingly.
+// String from target is:
+//  <baud rate>:<data bits>:<stop bits>:<parity>:....
+void
+CeCosTestSerialFilter::ParseConfig(char* args, ser_cfg_t* new_cfg)
+{
+    int ecos_parity, ecos_stop_bits, ecos_baud_rate;
+    CeCosTestSerial::StopBitsType t2h_stop_bits[3] = {
+        CeCosTestSerial::ONE_STOP_BIT, 
+        CeCosTestSerial::ONE_POINT_FIVE_STOP_BITS,
+        CeCosTestSerial::TWO_STOP_BITS};
+    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 != 0) ? 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;
+
+    Trace("Parsed Config baud=%d, bParity=%d, stopbits=%d, databits=%d\n",
+          new_cfg->baud_rate, (int) new_cfg->parity, new_cfg->stop_bits,
+          new_cfg->data_bits);
+}
+
+// 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
+CeCosTestSerialFilter::DoCRC(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
+CeCosTestSerialFilter::SendChecksum(CeCosTestSerial &pSer, int crc)
+{
+    char buffer[128];
+    int len;
+
+    len = sprintf(buffer, "%d!", crc);
+
+    TargetWrite(pSer, (const unsigned char*)buffer, len);
+}
+
+void
+CeCosTestSerialFilter::SendStatus(CeCosTestSerial &pSer, int state)
+{
+    if (state)
+        TargetWrite(pSer, (unsigned char*) &msg_ok, 2);
+    else
+        TargetWrite(pSer, (unsigned char*) &msg_er, 2);
+}
+
+
+// Receive test DONE message from target.
+void
+CeCosTestSerialFilter::ReceiveDone(CeCosTestSerial &pSer, 
+                                   unsigned char* data_in, int size)
+{
+    static const char msg_done[] = "DONE";
+    unsigned char data_reply[4];
+    int first = 1;
+
+    TargetRead(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");
+                PrintHex(data_in, size);
+                Trace("<end>\n");
+            }
+            Trace("Receiving junk instead of DONE:\n");
+            first = 0;
+        }
+        PrintHex(data_reply, 4);
+
+        data_reply[0] = data_reply[1];
+        data_reply[1] = data_reply[2];
+        data_reply[2] = data_reply[3];
+
+        // The TargetRead call will handle recovery in case of timeout...
+        TargetRead(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
+CeCosTestSerialFilter::CMD_TestBinary(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 = DoCRC(data_out, size);
+
+    // Send checksum to target.
+    SendChecksum(pSer, crc);
+
+    switch (mode) {
+    case MODE_NO_ECHO:
+    {
+        // Simple transmit. Don't expect target to echo data back.
+        TargetWrite(pSer, data_out, size);
+        ReceiveDone(pSer, NULL, 0);
+    }
+    break;
+    case MODE_EOP_ECHO:
+    {
+        int in_crc;
+
+        TargetWrite(pSer, data_out, size);
+
+        // Expect target to echo the data
+        TargetRead(pSer, data_in, size);
+
+        // Check echoed data, and reply OK/ER accordingly.
+        in_crc = DoCRC(data_in, size);
+        SendStatus(pSer, (in_crc == crc));
+
+
+        // Dump seen/expected on console.
+        if (in_crc != crc) {
+            Trace("Data seen:\n");
+            PrintHex(data_in, size);
+            Trace("<end>\n");
+            Trace("Data expected:\n");
+            PrintHex(data_out, size);
+            Trace("<end>\n");
+        }
+
+        ReceiveDone(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
+            TargetWrite(pSer, data_out, block_size);
+
+            // out2: block_size -> 2 x block_size
+            TargetWrite(pSer, data_out, block_size);
+
+            // in1:
+            TargetRead(pSer, data_in, block_size);
+
+            // out3: block_size -> block_size
+            TargetWrite(pSer, data_out, block_size);
+        
+            // in2:
+            TargetRead(pSer, &data_in[block_size], 2*block_size);
+
+            // out4: block_size -> 0
+            TargetWrite(pSer, data_out, block_size);
+        
+            // in3:
+            TargetRead(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");
+                            PrintHex(&data_in[j*block_size], 
+                                           block_size);
+                            Trace("<end>\n");
+                            Trace("Data expected:\n");
+                            PrintHex(data_out, block_size);
+                            Trace("<end>\n");
+                        }
+                    }
+                }
+            }
+        }
+        // Check echoed data, and reply OK/ER accordingly.
+        SendStatus(pSer, (!fail));
+        ReceiveDone(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
+CeCosTestSerialFilter::CMD_TestText(CeCosTestSerial &pSer, char* args)
+{
+    CYG_UNUSED_PARAM(args);
+
+    SendStatus(pSer, 1);
+}
+
+//-----------------------------------------------------------------------------
+// Reply to PING packet from target.
+// Format in:
+//  "!"
+// Format out:
+//  OK
+void
+CeCosTestSerialFilter::CMD_TestPing(CeCosTestSerial &pSer, char* args)
+{ 
+    CYG_UNUSED_PARAM(args);
+
+    SendStatus(pSer, 1);
+}
+
+//-----------------------------------------------------------------------------
+// Dispatch test command. 
+void
+CeCosTestSerialFilter::DispatchCommand(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)) {
+        CMD_ChangeConfig(pSer, args);
+    } 
+    else if (0 == strcmp("DEFCONFIG", cmd)) {
+        // Note: Currently the arguments are ignored. 9600 8N1 is default.
+        CMD_DefaultConfig(pSer);
+    }
+    else if (0 == strcmp("BINARY", cmd)) {
+        CMD_TestBinary(pSer, args);
+    }
+    else if (0 == strcmp("TEXT", cmd)) {
+        CMD_TestText(pSer, args);
+    }
+    else if (0 == strcmp("PING", cmd)) {
+        CMD_TestPing(pSer, args);
+    }
+    else
+        Trace("Unknown command '%s'.\n", cmd);
+
+    Trace("Command %s completed.\n", cmd);
+}
+
+bool CALLBACK
+SerialFilterFunction(void*& pBuf,
+                     unsigned int& nRead,
+                     CeCosTestSerial& serial,
+                     CeCosTestSocket& socket,
+                     void* pParem)
+{
+    CeCosTestSerialFilter* p = (CeCosTestSerialFilter*) pParem;
+    return p->FilterFunctionProper(pBuf, nRead, serial, socket);
+}
+
+bool
+CeCosTestSerialFilter::FilterFunctionProper(void*& pBuf,
+                                            unsigned int& nRead,
+                                            CeCosTestSerial& serial,
+                                            CeCosTestSocket& socket)
+{
+    char* buffer = (char*) pBuf;
+
+    // Don't do anything in the null filter mode.
+    if (m_bNullFilter)
+        return true;
+
+    // Allows trace to be called without a reference to the socket...
+    m_cGDBSocket = &socket;
+
+    // Output the serial data if option enabled - but only if dumping
+    // state to the console or after the first command has been seen
+    // from the filter. GDB gets confused by O-packets if they appear
+    // when it's trying to connect.
+    if (m_bOptSerDebug && (m_bOptConsoleOutput || m_bFirstCommandSeen))
+        PrintHex((unsigned char*) buffer, nRead);
+
+    // Command handling.
+    // If we are not presently reading a command, look for the
+    // start marker.
+    unsigned int i = 0;
+    if (!m_bCmdFlag)
+        for (; i < nRead; i++) {
+            if ('@' == buffer[i]) {
+                m_bCmdFlag = true;
+                // Send the data before the marker.
+                if (i)
+                    socket.send(buffer, i);
+                break;
+            }
+        }
+
+    // If reading a command, look for the end marker.
+    if (m_bCmdFlag) {
+        char c = 0;
+        while (i < nRead && m_nCmdIndex < MAX_CMD_LEN) {
+            c = buffer[i++];
+            m_aCmd[m_nCmdIndex++] = c;
+            if ('!' == c) {
+                if (i != nRead) {
+                    m_nUnreadBufferIndex = 0;
+                    m_nUnreadBufferSize = nRead - i;
+                    m_xUnreadBuffer = 
+                        (unsigned char*) malloc(m_nUnreadBufferSize);
+                    if (!m_xUnreadBuffer) {
+                        m_nUnreadBufferSize = 0;
+                        throw "Could not allocate unread buffer!";
+                    }
+                    
+                    int ix = 0;
+                    while (i < nRead)
+                        m_xUnreadBuffer[ix++] = buffer[i++];
+                }
+                break;
+            }
+        }
+
+        if (MAX_CMD_LEN == m_nCmdIndex) {
+            Trace("Received too long command. Ignoring it!\n");
+            m_nCmdIndex = 0;
+            m_bCmdFlag = false;
+        } else if ('!' == c) {
+            // Was the command completed?
+            m_aCmd[m_nCmdIndex - 1] = 0;// terminate cmd
+            m_nCmdIndex = 0;
+            m_bCmdFlag = false;
+
+            // First command dispatched. Initialize serial to nonblocking.
+            if (!m_bFirstCommandSeen) {
+                m_bFirstCommandSeen = true;
+                serial.SetBlockingReads(false);
+            }
+  
+            try {
+                // skip @ when passing ptr
+                DispatchCommand(serial, &m_aCmd[1]);
+            } 
+            catch (filter_abort_t* msg) {
+                // 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.
+
+                // Do nothing from next call.
+                m_bNullFilter = true;
+
+                // Copy the start of the $-packet to the inbuffer.
+                unsigned char *d = (unsigned char*) pBuf;
+                const unsigned char *s = msg->data_ptr;
+                unsigned int len = msg->data_len;
+                
+                // It should be possible to re-allocate buffer. Didn't seem
+                // to work properly though. Probably won't be a problem
+                // since we would normally only see 1-2 bytes of the
+                // $-packet anyway.
+                if (len > nRead)
+                    throw "Not enough room for $-message";
+
+                while (len--)
+                    *d++ = *s++;
+
+                nRead = msg->data_len;
+
+                delete msg;
+
+                return true;
+            }
+        }
+
+        nRead = 0;                      // Never leave anything for caller
+                                        // This is a violation of the intended
+                                        // filter function behavior.
+    }
+    return true;
+}