Index: c/src/lib/libbsp/mips/shared/gdbstub/ChangeLog
===================================================================
--- c/src/lib/libbsp/mips/shared/gdbstub/ChangeLog	(revision 3f1c12435fd6ce9f4f5d6bb6589250212f8c511c)
+++ c/src/lib/libbsp/mips/shared/gdbstub/ChangeLog	(revision 1bbe2e1dbb59eaa89d637ffd95a04ba2919d7010)
@@ -1,2 +1,13 @@
+2001-02-27	Joel Sherrill <joel@OARcorp.com>
+
+	* Significant modifications including adding thread support, the 'X'
+	command, and reorganizing so that target CPU independent routines
+	could be reused.
+	* gdb_if.h: Added numerous prototypes.
+	* mips-stub.c: Added thread support as well as 'X' command.
+	Also noticed that the 'P' command was from the mips protocol.
+	* rtems-stub-glue.c: New file.  This file contains all generic
+	support which should be able to be reused on another target CPU.
+
 2002-02-08	Joel Sherrill <joel@OARcorp.com>
 
Index: c/src/lib/libbsp/mips/shared/gdbstub/gdb_if.h
===================================================================
--- c/src/lib/libbsp/mips/shared/gdbstub/gdb_if.h	(revision 3f1c12435fd6ce9f4f5d6bb6589250212f8c511c)
+++ c/src/lib/libbsp/mips/shared/gdbstub/gdb_if.h	(revision 1bbe2e1dbb59eaa89d637ffd95a04ba2919d7010)
@@ -17,4 +17,59 @@
 #ifndef _GDB_IF_H
 #define _GDB_IF_H
+
+/* Max number of threads in qM response */
+#define QM_MAX_THREADS (20)
+
+struct rtems_gdb_stub_thread_info {
+  char display[256];
+  char name[256];
+  char more_display[256];
+};
+
+/*
+ *  Prototypes
+ */
+
+int parse_zbreak(const char *in, int *type, unsigned char **addr, int *len);
+
+
+char* mem2hstr(char *buf, const unsigned char *mem, int count);
+int   hstr2mem(unsigned char *mem, const char *buf, int count);
+void  set_mem_err(void);
+unsigned char get_byte(const unsigned char *ptr);
+void  set_byte(unsigned char *ptr, int val);
+char* thread2vhstr(char *buf, int thread);
+char* thread2fhstr(char *buf, int thread);
+const char* fhstr2thread(const char *buf, int *thread);
+const char* vhstr2thread(const char *buf, int *thread);
+char* int2fhstr(char *buf, int val);
+char* int2vhstr(char *buf, int vali);
+const char* fhstr2int(const char *buf, int *ival);
+const char* vhstr2int(const char *buf, int *ival);
+int   hstr2byte(const char *buf, int *bval);
+int   hstr2nibble(const char *buf, int *nibble);
+
+int rtems_gdb_stub_thread_support_ok(void);
+int rtems_gdb_stub_get_current_thread(void);
+int rtems_gdb_stub_get_next_thread(int);
+int rtems_gdb_stub_get_offsets(
+  unsigned char **text_addr,
+  unsigned char **data_addr, 
+  unsigned char **bss_addr
+);
+int rtems_gdb_stub_get_thread_regs(
+  int thread,
+  unsigned int *registers
+);
+int rtems_gdb_stub_set_thread_regs(
+  int thread,
+  unsigned int *registers
+);
+void rtems_gdb_process_query( 
+  char *inbuffer,
+  char *outbuffer,
+  int   do_threads,
+  int   thread
+);
 
 /*
Index: c/src/lib/libbsp/mips/shared/gdbstub/mips-stub.c
===================================================================
--- c/src/lib/libbsp/mips/shared/gdbstub/mips-stub.c	(revision 3f1c12435fd6ce9f4f5d6bb6589250212f8c511c)
+++ c/src/lib/libbsp/mips/shared/gdbstub/mips-stub.c	(revision 1bbe2e1dbb59eaa89d637ffd95a04ba2919d7010)
@@ -1,2 +1,3 @@
+#define  GDB_STUB_ENABLE_THREAD_SUPPORT 1
 /*******************************************************************************
 
@@ -127,4 +128,10 @@
 #include <rtems.h>
 #include "gdb_if.h"
+
+extern int printk(const char *fmt, ...);
+
+/* Change it to something meaningful when debugging */
+#undef ASSERT
+#define ASSERT(x) if(!(x)) printk("ASSERT: stub: %d\n", __LINE__)
 
 /***************/
@@ -186,6 +193,8 @@
 #if (__mips == 3)
 typedef long long mips_register_t;
+#define R_SZ 8
 #elif (__mips == 1)
 typedef unsigned int mips_register_t;
+#define R_SZ 4
 #else
 #error "unknown MIPS ISA"
@@ -193,4 +202,7 @@
 static mips_register_t *registers;
 
+#if defined(GDB_STUB_ENABLE_THREAD_SUPPORT)
+static char do_threads;      /* != 0 means we are supporting threads */
+#endif
 
 /*
@@ -212,4 +224,21 @@
 static char outBuffer[BUFMAX];
 
+/* Structure to keep info on a z-breaks */
+#define BREAKNUM 32
+struct z0break
+{
+  /* List support */
+  struct z0break *next;
+  struct z0break *prev;
+
+  /* Location, preserved data */
+  unsigned char *address;
+  char     buf[2];
+};
+
+static struct z0break z0break_arr[BREAKNUM];
+static struct z0break *z0break_avail = NULL;
+static struct z0break *z0break_list  = NULL;
+
 
 
@@ -217,8 +246,8 @@
  * Convert an int to hex.
  */
-static const char hexchars[] = "0123456789abcdef";
-
-#define highhex(x) hexchars [(x >> 4) & 0xf]
-#define lowhex(x) hexchars [x & 0xf]
+const char gdb_hexchars[] = "0123456789abcdef";
+
+#define highhex(x) gdb_hexchars [(x >> 4) & 0xf]
+#define lowhex(x) gdb_hexchars [x & 0xf]
 
 
@@ -228,6 +257,8 @@
  */
 static char *
-mem2hex (int addr, int length, char *buf)
-{
+mem2hex (void *_addr, int length, char *buf)
+{
+  unsigned int addr = (unsigned int) _addr;
+
   if (((addr & 0x7) == 0) && ((length & 0x7) == 0))      /* dword aligned */
     {
@@ -241,5 +272,5 @@
 
           for (i = 15; i >= 0; i--)
-            *buf++ = hexchars [(k >> (i*4)) & 0xf];
+            *buf++ = gdb_hexchars [(k >> (i*4)) & 0xf];
         }
     }
@@ -255,5 +286,5 @@
 
           for (i = 7; i >= 0; i--)
-            *buf++ = hexchars [(k >> (i*4)) & 0xf];
+            *buf++ = gdb_hexchars [(k >> (i*4)) & 0xf];
         }
     }
@@ -269,5 +300,5 @@
 
           for (i = 3; i >= 0; i--)
-            *buf++ = hexchars [(k >> (i*4)) & 0xf];
+            *buf++ = gdb_hexchars [(k >> (i*4)) & 0xf];
         }
     }
@@ -283,5 +314,5 @@
 
           for (i = 1; i >= 0; i--)
-            *buf++ = hexchars [(k >> (i*4)) & 0xf];
+            *buf++ = gdb_hexchars [(k >> (i*4)) & 0xf];
         }
     }
@@ -376,6 +407,7 @@
  */
 static int
-hex2mem (char *buf, int addr, int length)
-{
+hex2mem (char *buf, void *_addr, int length)
+{
+  unsigned int addr = (unsigned int) _addr;
   if (((addr & 0x7) == 0) && ((length & 0x7) == 0))      /* dword aligned */
     {
@@ -453,4 +485,43 @@
   return 1;
 }
+
+
+/* Convert the binary stream in BUF to memory.
+
+   Gdb will escape $, #, and the escape char (0x7d).
+   COUNT is the total number of bytes to write into
+   memory. */
+static unsigned char *
+bin2mem (
+  unsigned char *buf,
+  unsigned char *mem,
+  int   count
+)
+{
+  int i;
+
+  for (i = 0; i < count; i++) {
+      /* Check for any escaped characters. Be paranoid and
+         only unescape chars that should be escaped. */
+      if (*buf == 0x7d) {
+          switch (*(buf+1)) {
+            case 0x3:  /* # */
+            case 0x4:  /* $ */
+            case 0x5d: /* escape char */
+              buf++;
+              *buf |= 0x20;
+              break;
+            default:
+              /* nothing */
+              break;
+            }
+        }
+
+      *mem++ = *buf++;
+    }
+
+  return mem;
+}
+
 
 
@@ -780,4 +851,53 @@
 }
 
+/*
+ *  This support function prepares and sends the message containing the
+ *  basic information about this exception.
+ */
+
+void gdb_stub_report_exception_info(
+  rtems_vector_number vector,
+  CPU_Interrupt_frame *frame,
+  int                  thread
+)
+{
+  char *optr;
+  int sigval;
+
+  optr = outBuffer;
+  *optr++ = 'T';
+  sigval = computeSignal ();
+  *optr++ = highhex (sigval);
+  *optr++ = lowhex (sigval);
+
+  *optr++ = gdb_hexchars[SP];
+  *optr++ = ':';
+  optr    = mem2hstr(optr, (unsigned char *)&frame->sp, R_SZ );
+  *optr++ = ';';
+  
+  *optr++ = gdb_hexchars[PC];
+  *optr++ = ':';
+  optr    = mem2hstr(optr, (unsigned char *)&frame->sp, R_SZ );
+  *optr++ = ';';
+
+#if defined(GDB_STUB_ENABLE_THREAD_SUPPORT)
+  if (do_threads) {
+    *optr++ = 't';
+    *optr++ = 'h';
+    *optr++ = 'r';
+    *optr++ = 'e';
+    *optr++ = 'a';
+    *optr++ = 'd';
+    *optr++ = ':';
+    optr   = thread2vhstr(optr, thread);
+    *optr++ = ';';
+  }
+#endif
+  putpacket (outBuffer);
+  
+  *optr++ = '\0';
+}
+
+
 
 
@@ -787,22 +907,31 @@
  * reacts to gdb's requests.
  */
+
+CPU_Interrupt_frame current_thread_registers;
+
 void handle_exception (rtems_vector_number vector, CPU_Interrupt_frame *frame)
 {
   int host_has_detached = 0;
-  int sigval;
   int regno, addr, length;
   long long regval;
   char *ptr;
+  int  current_thread;  /* current generic thread */
+  int  thread;          /* stopped thread: context exception happened in */
+  void *regptr;
+  int  binary;
 
   registers = (mips_register_t *)frame;
 
-  /* reply to host that an exception has occurred */
-  sigval = computeSignal ();
-  outBuffer[0] = 'S';
-  outBuffer[1] = highhex (sigval);
-  outBuffer[2] = lowhex (sigval);
-  outBuffer[3] = '\0';
-
-  putpacket (outBuffer);
+  thread = 0;
+#if defined(GDB_STUB_ENABLE_THREAD_SUPPORT)
+  if (do_threads) {
+    thread = rtems_gdb_stub_get_current_thread();
+  }
+#endif
+  current_thread = thread;
+
+  /* reply to host that an exception has occurred with some basic info */
+  gdb_stub_report_exception_info(vector, frame, thread);
+
 
   /*
@@ -812,30 +941,41 @@
   undoSStep ();
 
-  while (!(host_has_detached))
-    {
+  while (!(host_has_detached)) {
       outBuffer[0] = '\0';
       getpacket (inBuffer);
-
-      switch (inBuffer[0])
-        {
+      binary = 0;
+
+      switch (inBuffer[0]) {
         case '?':
-          outBuffer[0] = 'S';
-          outBuffer[1] = highhex (sigval);
-          outBuffer[2] = lowhex (sigval);
-          outBuffer[3] = '\0';
-          break;
-
-        case 'd':
-          /* toggle debug flag */
-          break;
-
-        case 'g':
-          /* return the values of the CPU registers */
-          mem2hex ((int) registers, sizeof registers, outBuffer);
-          break;
-
-        case 'G':
-          /* set the values of the CPU registers - return OK */
-          if (hex2mem (&inBuffer[1], (int) registers, sizeof registers))
+          gdb_stub_report_exception_info(vector, frame, thread);
+          break;
+
+        case 'd': /* toggle debug flag */
+          /* can print ill-formed commands in valid packets & checksum errors */
+          break;
+
+        case 'D':
+          /* remote system is detaching - return OK and exit from debugger */
+          strcpy (outBuffer, "OK");
+          host_has_detached = 1;
+          break;
+
+        case 'g':               /* return the values of the CPU registers */
+          regptr = registers;
+#if defined(GDB_STUB_ENABLE_THREAD_SUPPORT)
+          if (do_threads && current_thread != thread )
+             regptr = &current_thread_registers;
+#endif
+          mem2hex (regptr, NUM_REGS * (sizeof registers), outBuffer);
+
+          break;
+
+        case 'G':       /* set the values of the CPU registers - return OK */
+          regptr = registers;
+#if defined(GDB_STUB_ENABLE_THREAD_SUPPORT)
+          if (do_threads && current_thread != thread )
+             regptr = &current_thread_registers;
+#endif
+          if (hex2mem (&inBuffer[1], regptr, NUM_REGS * (sizeof registers)))
               strcpy (outBuffer, "OK");
           else
@@ -865,9 +1005,11 @@
               && is_readable (addr, length)
               && (length < (BUFMAX - 4)/2))
-            mem2hex (addr, length, outBuffer);
+            mem2hex ((void *)addr, length, outBuffer);
           else
             strcpy (outBuffer, "E01"); /* E01 = bad 'm' command */
           break;
 
+        case 'X':  /* XAA..AA,LLLL:<binary data>#cs */
+          binary = 1;
         case 'M':
           /* MAA..AA,LLLL:  Write LLLL bytes at address AA..AA - return OK */
@@ -877,7 +1019,11 @@
               && hexToInt (&ptr, &length)
               && *ptr++ == ':'
-              && is_writeable (addr, length)
-              && hex2mem (ptr, addr, length))
-            strcpy (outBuffer, "OK");
+              && is_writeable (addr, length) ) {
+              if ( binary )
+		hex2mem (ptr, (void *)addr, length);
+	      else
+		bin2mem (ptr, (void *)addr, length);
+              strcpy (outBuffer, "OK");
+          }
           else
             strcpy (outBuffer, "E02"); /* E02 = bad 'M' command */
@@ -899,14 +1045,254 @@
           return;
 
-        case 'D':
-          /* remote system is detaching - return OK and exit from debugger */
-          strcpy (outBuffer, "OK");
-          host_has_detached = 1;
-          break;
-
-        default:
-          /* do nothing */
-          break;
-        }                       /* switch */
+        case 'k':  /* remove all zbreaks if any */
+          {
+            int ret;
+            struct z0break *z0, *z0last;
+
+            ret = 1;
+            z0last = NULL;
+
+            for (z0=z0break_list; z0!=NULL; z0=z0->next) {
+                if (!hstr2mem(z0->address, z0->buf, 1)) {
+                   ret = 0;
+                }
+
+                z0last = z0;
+            }
+
+            /* Free entries if any */
+            if (z0last != NULL) {
+              z0last->next  = z0break_avail;
+              z0break_avail = z0break_list;
+              z0break_list  = NULL;
+            }
+          }
+
+          break;
+
+        case 'q':   /* queries */
+#if defined(GDB_STUB_ENABLE_THREAD_SUPPORT)
+          rtems_gdb_process_query( inBuffer, outBuffer, do_threads, thread );
+#endif
+          break;
+        
+        case 'H':  /* set new thread */
+#if defined(GDB_STUB_ENABLE_THREAD_SUPPORT)
+	  if (inBuffer[1] != 'g') {
+	    break;
+	  }
+	  
+	  if (!do_threads) {
+	    break;
+	  }
+	  
+	  {
+	    int tmp, ret;
+	    
+	    /* Set new generic thread */
+	    if (vhstr2thread(&inBuffer[2], &tmp) == NULL) {
+	      strcpy(outBuffer, "E01");
+	      break;
+	    }
+
+	    /* 0 means `thread' */
+	    if (tmp == 0) {
+	      tmp = thread;
+	    }
+
+            if (tmp == current_thread) {
+              /* No changes */
+	      strcpy(outBuffer, "OK");
+	      break;
+	    }
+
+            /* Save current thread registers if necessary */ 
+            if (current_thread != thread) {
+              ret = rtems_gdb_stub_set_thread_regs(
+                   current_thread, (unsigned int *) &current_thread_registers);
+	      ASSERT(ret);
+	    }
+
+	    /* Read new registers if necessary */
+	    if (tmp != thread) {
+		ret = rtems_gdb_stub_get_thread_regs(
+                    tmp, (unsigned int *) &current_thread_registers);
+
+		if (!ret) {
+		  /* Thread does not exist */
+		  strcpy(outBuffer, "E02");
+		  break;
+		}
+	    }
+	    
+	    current_thread = tmp;
+	    strcpy(outBuffer, "OK");
+	  }
+
+#endif
+          break;
+
+        case 'Z':  /* Add breakpoint */
+          { 
+            int ret, type, len;
+            unsigned char *address;
+            struct z0break *z0;
+
+            ret = parse_zbreak(inBuffer, &type, &address, &len);
+            if (!ret) {
+              strcpy(outBuffer, "E01");
+              break;
+            }
+
+            if (type != 0) {
+              /* We support only software break points so far */
+              break;
+            }
+
+            if (len != 1) {
+               strcpy(outBuffer, "E02");
+               break;
+            }
+
+            /* Let us check whether this break point already set */
+            for (z0=z0break_list; z0!=NULL; z0=z0->next) {
+                if (z0->address == address) {
+                  break;
+                }
+            }
+
+            if (z0 != NULL) {
+              /* Repeated packet */
+              strcpy(outBuffer, "OK");
+              break;
+            }
+
+            /* Let us allocate new break point */
+            if (z0break_avail == NULL) {
+              strcpy(outBuffer, "E03");
+              break;
+            }
+
+            /* Get entry */
+            z0 = z0break_avail;
+            z0break_avail = z0break_avail->next;
+
+            /* Let us copy memory from address add stuff the break point in */
+            if (mem2hstr(z0->buf, address, 1) == NULL ||
+                !hstr2mem(address, "cc" , 1)) {
+              /* Memory error */
+              z0->next = z0break_avail;
+              z0break_avail = z0;
+              strcpy(outBuffer, "E03");
+              break;
+            }
+
+            /* Fill it */
+            z0->address = address;
+
+            /* Add to the list */
+            z0->next = z0break_list;
+            z0->prev = NULL;
+
+            if (z0break_list != NULL) {
+              z0break_list->prev = z0;
+            }
+
+            z0break_list = z0;
+
+            strcpy(outBuffer, "OK");
+          }
+
+        case 'z': /* remove breakpoint */
+	  {
+	    int ret, type, len;
+	    unsigned char  *address;
+	    struct z0break *z0, *z0last;
+
+            if (inBuffer[1] == 'z') {
+                /* zz packet - remove all breaks */
+                ret = 1;
+                z0last = NULL;
+	        for (z0=z0break_list; z0!=NULL; z0=z0->next) {
+	            if(!hstr2mem(z0->address, z0->buf, 1)) {
+                        ret = 0;
+                      }
+                     
+                     z0last = z0;
+                   }
+
+                /* Free entries if any */
+                if (z0last != NULL) {
+                    z0last->next  = z0break_avail;
+                    z0break_avail = z0break_list;
+                    z0break_list  = NULL;
+                  }
+
+                if (ret) {
+	          strcpy(outBuffer, "OK");
+                } else {
+	          strcpy(outBuffer, "E04");
+	        }
+
+                break;
+              }
+              
+	    ret = parse_zbreak(inBuffer, &type, &address, &len);
+	    if (!ret) {
+		strcpy(outBuffer, "E01");
+		break;
+	      }
+	    
+	    if (type != 0) {
+	      /* We support only software break points so far */
+	      break;
+	    }
+
+            if (len != 1) {
+              strcpy(outBuffer, "E02");
+              break;
+            }
+	   
+	    /* Let us check whether this break point set */
+	    for (z0=z0break_list; z0!=NULL; z0=z0->next) {
+	      if (z0->address == address) {
+		break;
+	      }
+	    }
+              
+	    if (z0 == NULL) {
+	      /* Unknown breakpoint */
+	      strcpy(outBuffer, "E03");
+	      break;
+	    }
+	    
+	    if (!hstr2mem(z0->address, z0->buf, 1)) {
+	      strcpy(outBuffer, "E04");
+	      break;
+	    }
+   
+	    /* Unlink entry */
+	    if (z0->prev == NULL) {
+	      z0break_list = z0->next;
+	      if (z0break_list != NULL) {
+	        z0break_list->prev = NULL;
+	      }
+	    } else if (z0->next == NULL) {
+	      z0->prev->next = NULL;
+            } else {
+	      z0->prev->next = z0->next;
+	      z0->next->prev = z0->prev;
+	   }
+
+	   z0->next = z0break_avail;
+	   z0break_avail = z0;
+	    
+	   strcpy(outBuffer, "OK");
+	 }
+
+          break;
+        default: /* do nothing */
+          break;
+      }
 
       /* reply to the request */
@@ -927,12 +1313,32 @@
 }
 
+static char initialized;   /* 0 means we are not initialized */
+
 void mips_gdb_stub_install(void) 
 {
   rtems_isr_entry old;
+  int i;
+
+  if (initialized) {
+    ASSERT(0);
+    return;
+  }
+
+  /* z0breaks */
+  for (i=0; i<(sizeof(z0break_arr)/sizeof(z0break_arr[0]))-1; i++) {
+    z0break_arr[i].next = &z0break_arr[i+1];
+  }
+
+  z0break_arr[i].next = NULL;
+  z0break_avail       = &z0break_arr[0];
+  z0break_list        = NULL;
+
 
   rtems_interrupt_catch( (rtems_isr_entry) handle_exception, MIPS_EXCEPTION_SYSCALL, &old );
   /* rtems_interrupt_catch( handle_exception, MIPS_EXCEPTION_BREAK, &old ); */
 
+  initialized = 1;
   /* get the attention of gdb */
-  mips_break();
+  mips_break(1);
 }
+
Index: c/src/lib/libbsp/mips/shared/gdbstub/rtems-stub-glue.c
===================================================================
--- c/src/lib/libbsp/mips/shared/gdbstub/rtems-stub-glue.c	(revision 1bbe2e1dbb59eaa89d637ffd95a04ba2919d7010)
+++ c/src/lib/libbsp/mips/shared/gdbstub/rtems-stub-glue.c	(revision 1bbe2e1dbb59eaa89d637ffd95a04ba2919d7010)
@@ -0,0 +1,1299 @@
+/*
+ *  This file contains the RTEMS thread awareness support for GDB stubs.
+ *
+ *  This file is derived from an RTEMS thread aware i386-stub.c that
+ *  had the following copyright announcements:
+ *
+ *    This software is Copyright (C) 1998 by T.sqware - all rights limited
+ *    It is provided in to the public domain "as is", can be freely modified
+ *    as far as this copyight notice is kept unchanged, but does not imply
+ *    an endorsement by T.sqware of the product in which it is included.
+ *
+ *  
+ *     Modifications for RTEMS threads and more
+ *
+ *     Copyright (C) 2000 Quality Quorum, Inc. 
+ * 
+ *     All Rights Reserved
+ * 
+ *     Permission to use, copy, modify, and distribute this software and its 
+ *     documentation for any purpose and without fee is hereby granted. 
+ * 
+ *     QQI DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS SOFTWARE, INCLUDING
+ *     ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS, IN NO EVENT SHALL
+ *     QQI BE LIABLE FOR ANY SPECIAL, INDIRECT OR CONSEQUENTIAL DAMAGES OR
+ *     ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS,
+ *     WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION,
+ *     ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS
+ *     SOFTWARE.
+ */
+
+#include <rtems.h>
+
+#include <string.h>
+
+#include "gdb_if.h"
+
+/* Change it to something meaningful when debugging */
+#undef ASSERT
+#define ASSERT(x) 
+
+extern const char gdb_hexchars[];
+
+/*
+ *  Prototypes for CPU dependent routines that are conditional
+ *  at the bottom of this file.
+ */
+
+void rtems_gdb_stub_get_registers_from_context( 
+  int            *registers,
+  Thread_Control *th
+);
+
+/* Check whether it is OK to enable thread support */
+int rtems_gdb_stub_thread_support_ok(void)
+{
+  if (_System_state_Get() == SYSTEM_STATE_UP) {
+    return 1;
+  }
+  return 0;
+}
+
+/*
+ *  rtems_gdb_stub_id_to_index
+ *
+ *  Return the gdb thread id for the specified RTEMS thread id
+ */
+
+int rtems_gdb_stub_id_to_index(
+  Objects_Id thread_obj_id
+)
+{
+  Objects_Id min_id, max_id;
+  int first_posix_id, first_rtems_id;
+  Objects_Information *obj_info;
+
+  if (_System_state_Get() != SYSTEM_STATE_UP) {
+    /* We have one thread let us use value reserved for idle thread */
+    return 1; 
+  }
+
+  if (_Thread_Executing == _Thread_Idle) {
+    return 1;
+  }
+
+  /* Let us figure out thread_id for gdb */
+  first_rtems_id = 2;
+
+  obj_info = _Objects_Information_table[OBJECTS_RTEMS_TASKS];
+
+  min_id = obj_info->minimum_id;
+  max_id = obj_info->maximum_id;
+
+  if (thread_obj_id >= min_id && thread_obj_id < max_id) {
+    return first_rtems_id + (thread_obj_id - min_id);
+  }
+
+  first_posix_id = first_rtems_id + (max_id - min_id) + 1;
+
+  min_id = _Objects_Information_table[OBJECTS_POSIX_THREADS]->minimum_id;
+
+  return first_posix_id + (thread_obj_id - min_id);
+} 
+
+/* Get id of the thread stopped by exception */
+int rtems_gdb_stub_get_current_thread(void)
+{
+  return rtems_gdb_stub_id_to_index( _Thread_Executing->Object.id );
+}
+
+/* Get id of the next thread after athread, if argument <= 0 find the
+   first available thread, return thread if found or 0 if not */
+int rtems_gdb_stub_get_next_thread(int athread)
+{
+  Objects_Id id, min_id, max_id;
+  int lim, first_posix_id, first_rtems_id;
+  Objects_Information *obj_info;
+  int start;
+
+  if (_System_state_Get() != SYSTEM_STATE_UP) {
+    /* We have one thread let us use value of idle thread */
+    return (athread < 1) ? 1 : 0;
+  }
+
+  if (athread < 1) {
+    return 1;
+  }
+
+  first_rtems_id = 2;
+ 
+  obj_info = _Objects_Information_table[OBJECTS_RTEMS_TASKS];
+
+  min_id = obj_info->minimum_id;
+  max_id = obj_info->maximum_id;
+
+  lim = first_rtems_id + max_id - min_id;
+
+  if (athread < lim) {
+    if (athread < first_rtems_id) {
+      start = first_rtems_id;
+    } else {
+      start = 1 + athread;
+    }
+
+    for (id=start; id<=lim; id++) {
+      if (obj_info->local_table[id - first_rtems_id + 1] != NULL) {
+        return id;
+      }
+    }
+  }
+      
+  first_posix_id = first_rtems_id + (max_id - min_id) + 1;
+
+  obj_info = _Objects_Information_table[OBJECTS_POSIX_THREADS];
+
+  min_id = obj_info->minimum_id;
+  max_id = obj_info->maximum_id;
+
+  lim = first_posix_id + (max_id - min_id);
+
+  if (athread < lim) {
+    if (athread < first_posix_id) {
+      start = first_posix_id;
+    } else {
+      start = 1 + athread;
+    }
+      
+    for (id=start; id<=lim; id++) {
+      if (obj_info->local_table[id - first_posix_id + 1] != NULL) {
+        return id;
+      }
+    }
+  }
+
+  /* Not found */
+  return 0;
+}
+
+
+/* Get thread registers, return 0 if thread does not 
+   exist, and 1 otherwise */
+int rtems_gdb_stub_get_thread_regs(
+  int thread,
+  unsigned int *registers
+)
+{
+  Objects_Id thread_obj_id;
+  Objects_Id min_id, max_id;
+  int first_posix_id, first_rtems_id;
+  Objects_Information *obj_info;
+  Thread_Control *th;
+
+  ASSERT(registers != NULL);
+
+  if (_System_state_Get() != SYSTEM_STATE_UP || thread <= 0) {
+    /* Should not happen */
+    return 0;
+  }
+
+  if (thread == 1) {
+    th = _Thread_Idle;
+    goto found;
+  }
+
+  /* Let us get object associtated with current thread */
+  first_rtems_id = 2;
+
+  thread_obj_id = _Thread_Executing->Object.id;
+
+  /* Let us figure out thread_id for gdb */
+  obj_info = _Objects_Information_table[OBJECTS_RTEMS_TASKS];
+
+  min_id = obj_info->minimum_id;
+  max_id = obj_info->maximum_id;
+
+  if (thread <= (first_rtems_id + (max_id - min_id))) {
+    th = (Thread_Control *)(obj_info->local_table[thread - first_rtems_id + 1]);
+
+    if (th != NULL) {
+      goto found;
+    }
+
+    /* Thread does not exist */
+    return 0;
+  }
+
+  first_posix_id = first_rtems_id + (max_id - min_id) + 1;
+
+  obj_info = _Objects_Information_table[OBJECTS_POSIX_THREADS];
+
+  min_id = obj_info->minimum_id;
+  max_id = obj_info->maximum_id;
+
+  th = (Thread_Control *)(obj_info->local_table[thread - first_posix_id + 1]);
+  if (th == NULL) {
+    /* Thread does not exist */
+    return 0;
+  }
+
+found:
+
+  rtems_gdb_stub_get_registers_from_context( registers, th );
+
+  return 1;
+}
+
+
+/* Set thread registers, return 0 if thread does not 
+   exist or register values will screw up the threads, 
+   and 1 otherwise */
+int rtems_gdb_stub_set_thread_regs(
+  int thread,
+  unsigned int *registers
+)
+{
+  /* In current situation there is no point in changing any registers here
+     thread status is displayed as being deep inside thread switching 
+     and we better do not screw up anything there - it may be fixed eventually
+     though */
+  return 1;
+}
+
+
+/* Get thread information, return 0 if thread does not 
+   exist and 1 otherwise */
+int rtems_gdb_stub_get_thread_info(
+  int thread,
+  struct rtems_gdb_stub_thread_info *info
+)
+{
+  Objects_Id thread_obj_id;
+  Objects_Id min_id, max_id;
+  int first_posix_id, first_rtems_id;
+  Objects_Information *obj_info;
+  Thread_Control *th;
+  unsigned32 name;
+  char tmp_buf[20];
+
+  ASSERT(info != NULL);
+
+  if (thread <= 0) {
+    return 0;
+  }
+
+  if (_System_state_Get() != SYSTEM_STATE_UP || thread == 1) {
+    /* We have one thread let us use value
+         which will never happen for real thread */
+    strcpy(info->display, "idle thread");
+    strcpy(info->name, "IDLE");
+    info->more_display[0] = 0; /* Nothing */
+
+    return 1;
+  }
+
+  /* Let us get object associtated with current thread */
+  thread_obj_id = _Thread_Executing->Object.id;
+
+  /* Let us figure out thread_id for gdb */
+  first_rtems_id = 2;
+  
+  obj_info = _Objects_Information_table[OBJECTS_RTEMS_TASKS];
+
+  min_id = obj_info->minimum_id;
+  max_id = obj_info->maximum_id;
+
+  if (thread <= (first_rtems_id + (max_id - min_id))) {
+      th = (Thread_Control *)(obj_info->local_table[thread - 
+                                                    first_rtems_id + 1]);
+
+  if (th == NULL) {
+    /* Thread does not exist */
+    return 0;
+  }
+
+  strcpy(info->display, "rtems task:   control at 0x");
+
+  tmp_buf[0] = gdb_hexchars[(((int)th) >> 28) & 0xf];
+  tmp_buf[1] = gdb_hexchars[(((int)th) >> 24) & 0xf];
+  tmp_buf[2] = gdb_hexchars[(((int)th) >> 20) & 0xf];
+  tmp_buf[3] = gdb_hexchars[(((int)th) >> 16) & 0xf];
+  tmp_buf[4] = gdb_hexchars[(((int)th) >> 12) & 0xf];
+  tmp_buf[5] = gdb_hexchars[(((int)th) >> 8) & 0xf];
+  tmp_buf[6] = gdb_hexchars[(((int)th) >> 4) & 0xf];
+  tmp_buf[7] = gdb_hexchars[((int)th) & 0xf];
+  tmp_buf[8] = 0;
+
+  strcat(info->display, tmp_buf);
+  
+  name = *(unsigned32 *)(obj_info->local_table[thread]->name);
+      
+  info->name[0] = (name >> 24) & 0xff;
+  info->name[1] = (name >> 16) & 0xff;
+  info->name[2] = (name >> 8) & 0xff;
+  info->name[3] = name & 0xff;
+  info->name[4] = 0;
+
+  info->more_display[0] = 0; /* Nothing */
+
+  return 1;
+}
+
+  first_posix_id = first_rtems_id + (max_id - min_id) + 1;
+
+  obj_info = _Objects_Information_table[OBJECTS_POSIX_THREADS];
+
+  min_id = obj_info->minimum_id;
+  max_id = obj_info->maximum_id;
+
+  th = (Thread_Control *)(obj_info->local_table[thread - first_posix_id + 1]);
+  if (th == NULL)
+    {
+      /* Thread does not exist */
+      return 0;
+    }
+
+  strcpy(info->display, "posix thread: control at 0x");
+
+  tmp_buf[0] = gdb_hexchars[(((int)th) >> 28) & 0xf];
+  tmp_buf[1] = gdb_hexchars[(((int)th) >> 24) & 0xf];
+  tmp_buf[2] = gdb_hexchars[(((int)th) >> 20) & 0xf];
+  tmp_buf[3] = gdb_hexchars[(((int)th) >> 16) & 0xf];
+  tmp_buf[4] = gdb_hexchars[(((int)th) >> 12) & 0xf];
+  tmp_buf[5] = gdb_hexchars[(((int)th) >> 8) & 0xf];
+  tmp_buf[6] = gdb_hexchars[(((int)th) >> 4) & 0xf];
+  tmp_buf[7] = gdb_hexchars[((int)th) & 0xf];
+  tmp_buf[8] = 0;
+
+  strcat(info->display, tmp_buf);
+
+  name = *(unsigned32 *)(obj_info->local_table[thread - 
+                                               first_posix_id + 1]->name);
+
+  info->name[0] = (name >> 24) & 0xff;
+  info->name[1] = (name >> 16) & 0xff;
+  info->name[2] = (name >> 8) & 0xff;
+  info->name[3] = name & 0xff;
+  info->name[4] = 0;
+
+  info->more_display[0] = 0; /* Nothing */
+  
+  return 1;
+}
+
+/*******************************************************/
+
+
+/* Format: x<type-1x>,<address-x>,<length-x>, where x is 'z' or 'Z' */
+int parse_zbreak(const char *in, int *type, unsigned char **addr, int *len)
+{
+  int ttmp, atmp, ltmp;
+  
+  ASSERT(in != NULL);
+  ASSERT(type != NULL);
+  ASSERT(addr != NULL);
+  ASSERT(len != NULL);
+
+  ASSERT(*in == 'z' || *in == 'Z');
+
+  in++;
+
+  if (!hstr2nibble(in, &ttmp) || *(in+1) != ',')
+    {
+      return 0;
+    }
+  in += 2;
+
+  in = vhstr2int(in, &atmp);
+  if (in == NULL || *in != ',')
+    {
+      return 0;
+    }
+  in++;
+
+  in = vhstr2int(in, &ltmp);
+  if (in == NULL || ltmp < 1)
+    {
+      return 0;
+    }
+
+  *type = ttmp;
+  *addr = (unsigned char *)atmp;
+  *len  = ltmp;
+
+  return 1;
+}
+
+/* Format: qP<mask-08x><thread_id-ft> */
+static int
+parse_qp(const char *in, int *mask, int *thread)
+{
+  const char *ptr;
+
+  ASSERT(in != NULL);
+  ASSERT(*in == 'q');
+  ASSERT(*(in+1) == 'P');
+
+  ptr = fhstr2int(in+2, mask);
+  if (ptr == NULL)
+    {
+      return 0;
+    }
+
+  ptr = fhstr2thread(ptr, thread);
+  if (ptr == NULL)
+    {
+      return 0;
+    }
+
+  return 1;
+}
+
+/* Format: qQ<mask-08x><thread_id-ft><tag-08x><length-02x><value>...] */
+static void
+pack_qq(char *out, int mask, int thread, struct rtems_gdb_stub_thread_info *info)
+{
+  int len;
+
+  ASSERT(out != NULL);
+  ASSERT(info != NULL);
+
+  *out++ = 'q';
+  *out++ = 'Q';
+  out = int2fhstr(out, mask);
+  out = thread2fhstr(out, thread);
+
+  if (mask & 0x1) {
+    /* Thread id once again */
+    memcpy(out, "00000001", 8);
+    out   += 8;
+    *out++ = '1';
+    *out++ = '0';
+    out = thread2fhstr(out, thread);
+  }
+
+  if (mask & 0x2) {
+    /* Exists */
+    memcpy(out, "00000002", 8);
+    out   += 8;
+    *out++ = '0';
+    *out++ = '1';
+    *out++ = '1';
+  }
+
+  if (mask & 0x4) {
+    /* Display */
+    memcpy(out, "00000004", 8);
+    out += 8;
+
+    info->display[sizeof(info->display)-1] = 0; /* Fot God sake */
+
+    len = strlen(info->display);
+
+    *out++ = gdb_hexchars[len >> 4];
+    *out++ = gdb_hexchars[len & 0x0f];
+
+    memcpy(out, info->display, len);
+
+    out += len;
+  }
+
+  if (mask & 0x8) {
+    /* Name */
+    memcpy(out, "00000008", 8);
+    out += 8;
+
+    info->name[sizeof(info->name)-1] = 0; /* Fot God sake */
+
+    len = strlen(info->name);
+
+    *out++ = gdb_hexchars[len >> 4];
+    *out++ = gdb_hexchars[len & 0x0f];
+
+    memcpy(out, info->name, len);
+
+    out += len;
+  }
+
+  if (mask & 0x10) {
+    /* More display */
+    memcpy(out, "00000010", 8);
+    out += 8;
+
+    info->more_display[sizeof(info->more_display)-1] = 0; /* Fot God sake */
+
+    len = strlen(info->more_display);
+
+    *out++ = gdb_hexchars[len >> 4];
+    *out++ = gdb_hexchars[len & 0x0f];
+
+    memcpy(out, info->more_display, len);
+
+    out += len;
+  }
+
+  *out = 0;
+
+  return;
+}
+
+/* Format qL<first-01x><max_count-02x><arg_thread_id-ft> */
+static int
+parse_ql(const char *in, int *first, int *max_count, int *athread)
+{
+  const char *ptr;
+
+  ASSERT(in != NULL);
+  ASSERT(*in == 'q');
+  ASSERT(*(in+1) == 'L');
+  ASSERT(first != NULL);
+  ASSERT(max_count != NULL);
+  ASSERT(athread != NULL);
+
+  ptr = in + 2;
+
+  /*  First */
+  if (!hstr2nibble(ptr, first))
+    {
+      return 0;
+    }
+  ptr++;
+
+  /* Max count */
+  if (!hstr2byte(ptr, max_count))
+    {
+      return 0;
+    }
+  ptr += 2;
+
+  /* A thread */
+  ptr = fhstr2thread(ptr, athread);
+  if (ptr == NULL)
+    {
+      return 0;
+    }
+
+  return 1;
+}
+
+/* Format: qM<count-02x><done-01x><arg_thread_id>[<found_thread_id-ft>...] */
+static char *
+reserve_qm_header(char *out)
+{
+  ASSERT(out != NULL);
+
+  return out + 21;
+}
+
+/* Format: qM<count-02x><done-01x><arg_thread_id>[<found_thread_id-ft>...] */
+static char*
+pack_qm_thread(char *out, int thread)
+{
+  ASSERT(out != 0);
+
+  return thread2fhstr(out, thread);
+}
+
+/* Format: qM<count-02x><done-01x><arg_thread_id>[<found_thread_id-ft>...] */
+static void
+pack_qm_header(char *out, int count, int done, int athread)
+{
+  ASSERT(out != 0);
+  ASSERT(count >= 0 && count < 256);
+
+  *out++ = 'q';
+  *out++ = 'M';
+
+  *out++ = gdb_hexchars[(count >> 4) & 0x0f];
+  *out++ = gdb_hexchars[count & 0x0f];
+
+  if (done) {
+    *out++ = '1';
+  } else {
+    *out++ = '0';
+  }
+
+  thread2fhstr(out, athread);
+
+  return;
+}
+
+
+void rtems_gdb_process_query( 
+  char *inbuffer,
+  char *outbuffer,
+  int   do_threads,
+  int   thread
+)
+{
+  char *optr;
+    
+  switch(inbuffer[1]) {
+    case 'C':
+      /* Current thread query query - return stopped thread */
+      if (!do_threads) {
+        break;
+      }
+
+      optr = outbuffer;
+
+      *optr++ = 'Q';
+      *optr++ = 'C';
+      optr    = thread2vhstr(optr, thread);
+      *optr   = 0;
+      break;
+
+    case 'P':
+      /* Thread info query */
+      if (!do_threads) {
+        break;
+      }
+
+      {
+        int ret, rthread, mask;
+        struct rtems_gdb_stub_thread_info info;
+
+        ret = parse_qp(inbuffer, &mask, &rthread);
+        if (!ret|| mask & ~0x1f) {
+          strcpy(outbuffer, "E01");
+          break;
+        }
+
+        ret = rtems_gdb_stub_get_thread_info(rthread, &info);
+        if (!ret) {
+          /* Good implementation would never ask for non-existing thread,
+             should we care about bad ones - it does not seem so */
+          strcpy(outbuffer, "E02");
+          break;
+        }
+
+        /* Build response */
+        pack_qq(outbuffer, mask, rthread, &info);
+      }
+
+      break;
+    case 'L':
+      /* Thread info query */
+      if (!do_threads) {
+        break;
+      }
+
+      {
+        int ret, athread, first, max_cnt, i, done, rthread;
+
+        ret = parse_ql(inbuffer, &first, &max_cnt, &athread);
+        if (!ret) {
+          strcpy(outbuffer, "E02");
+          break;
+        }
+
+        if (max_cnt == 0) {
+          strcpy(outbuffer, "E02");
+          break;
+        }
+
+        if (max_cnt > QM_MAX_THREADS) {
+          /* Limit max count by buffer size */
+          max_cnt = QM_MAX_THREADS;
+        }
+
+        /* Reserve place for output header */
+        optr = reserve_qm_header(outbuffer);
+
+        if (first) {
+          rthread = 0;
+        } else {
+          rthread = athread;
+        }
+
+        done = 0;
+
+        for (i=0; i<max_cnt; i++) {
+          rthread = rtems_gdb_stub_get_next_thread(rthread);
+
+          if (rthread <= 0) {
+            done = 1; /* Set done flag */
+            break;
+          }
+
+          optr = pack_qm_thread(optr, rthread);
+        }
+
+        *optr = 0;
+
+        ASSERT((optr - outbuffer) < BUFMAX);
+
+        /* Fill header */
+        pack_qm_header(outbuffer, i, done, athread);
+
+      }
+      break;
+    default:
+      if (memcmp(inbuffer, "qOffsets", 8) == 0) {
+        unsigned char *t, *d, *b;
+        char *out;
+
+        if (!rtems_gdb_stub_get_offsets(&t, &d, &b)) {
+          break;
+        }
+
+        out = outbuffer;
+
+        *out++ = 'T';
+        *out++ = 'e';
+        *out++ = 'x';
+        *out++ = 't';
+        *out++ = '=';
+
+        out = int2vhstr(out, (int)t);
+
+        *out++ = ';';
+        *out++ = 'D';
+        *out++ = 'a';
+        *out++ = 't';
+        *out++ = 'a';
+        *out++ = '=';
+
+        out = int2vhstr(out, (int)d);
+
+        *out++ = ';';
+        *out++ = 'B';
+        *out++ = 's';
+        *out++ = 's';
+        *out++ = '=';
+
+        out = int2vhstr(out, (int)b);
+
+        *out++ = ';';
+        *out++ = 0;
+
+        break;
+      }
+
+      /*  qCRC, qRcmd, qu and qz will be added here */
+      break;
+    }
+
+}
+
+/* Present thread in the variable length string format */
+char*
+thread2vhstr(char *buf, int thread)
+{
+  int i, nibble, shift;
+
+  ASSERT(buf != NULL);
+
+  for(i=0, shift=28; i<8; i++, shift-=4)
+    {
+      nibble = (thread >> shift) & 0x0f;
+      
+      if (nibble != 0)
+	{
+	  break;
+	}
+    }
+
+  if (i == 8)
+    {
+      *buf++ = '0';
+      return buf;
+    }
+
+  *buf++ = gdb_hexchars[nibble];
+
+  for(i++, shift-=4; i<8; i++, shift-=4, buf++)
+    {
+      nibble = (thread >> shift) & 0x0f;
+      *buf   = gdb_hexchars[nibble];
+    }
+
+  return buf;
+}
+
+/* Present thread in fixed length string format */
+char*
+thread2fhstr(char *buf, int thread)
+{
+  int i, nibble, shift;
+
+  ASSERT(buf != NULL);
+
+  for(i=0; i<8; i++, buf++)
+    {
+      *buf = '0';
+    }
+
+  for(i=0, shift=28; i<8; i++, shift-=4, buf++)
+    {
+      nibble = (thread >> shift) & 0x0f;
+      *buf   = gdb_hexchars[nibble];
+    }
+
+  return buf;
+}
+
+/* Parse thread presented in fixed length format */
+const char*
+fhstr2thread(const char *buf, int *thread)
+{
+  int i, val, nibble;
+
+  ASSERT(buf != NULL);
+  ASSERT(thread != NULL);
+
+  for(i=0; i<8; i++, buf++)
+    {
+      if (*buf != '0')
+	{
+	  return NULL;
+	}
+    }
+
+  val = 0;
+
+  for(i=0; i<8; i++, buf++)
+    {
+      if (!hstr2nibble(buf, &nibble))
+	{
+	  return NULL;
+	}
+
+      ASSERT(nibble >=0 && nibble < 16);
+
+      val = (val << 4) | nibble;
+    }
+
+  *thread = val;
+
+  return buf;
+}
+
+/* Parse thread presented in variable length format */
+const char*
+vhstr2thread(const char *buf, int *thread)
+{
+  int i, val, nibble;
+  int found_zero, lim;
+
+  ASSERT(buf != NULL);
+  ASSERT(thread != NULL);
+
+
+  /* If we have leading zeros, skip them */
+  found_zero = 0;
+
+  for(i=0; i<16; i++, buf++)
+    {
+      if (*buf != '0')
+	{
+	  break;
+	}
+
+      found_zero = 1;
+    }
+
+  /* Process non-zeros */
+  lim = 16 - i;
+  val = 0;
+
+  for(i=0; i<lim; i++, buf++)
+    {
+      if (!hstr2nibble(buf, &nibble))
+	{
+	  if (i == 0 && !found_zero)
+	    {
+	      /* Empty value */
+	      return NULL;
+	    }
+
+	  *thread = val;
+	  return buf;
+	}
+	  
+      ASSERT(nibble >= 0 && nibble < 16);
+
+      val = (val << 4) | nibble;
+    }
+
+  if (hstr2nibble(buf, &nibble))
+    {
+      /* Value is too long */
+      return NULL;
+    }
+  
+  *thread = val;
+  return buf;
+}
+
+
+/* Present integer in the variable length string format */
+char*
+int2vhstr(char *buf, int val)
+{
+  int i, nibble, shift;
+
+  ASSERT(buf != NULL);
+
+  for(i=0, shift=28; i<8; i++, shift-=4)
+    {
+      nibble = (val >> shift) & 0x0f;
+      
+      if (nibble != 0)
+	{
+	  break;
+	}
+    }
+
+  if (i == 8)
+    {
+      *buf++ = '0';
+      return buf;
+    }
+
+  *buf++ = gdb_hexchars[nibble];
+
+  for(i++, shift-=4; i<8; i++, shift-=4, buf++)
+    {
+      nibble = (val >> shift) & 0x0f;
+      *buf   = gdb_hexchars[nibble];
+    }
+
+  return buf;
+}
+
+/* Present int in fixed length string format */
+char*
+int2fhstr(char *buf, int val)
+{
+  int i, nibble, shift;
+
+  ASSERT(buf != NULL);
+
+  for(i=0, shift=28; i<8; i++, shift-=4, buf++)
+    {
+      nibble = (val >> shift) & 0x0f;
+      *buf   = gdb_hexchars[nibble];
+    }
+
+  return buf;
+}
+
+/* Parse int presented in fixed length format */
+const char*
+fhstr2int(const char *buf, int *ival)
+{
+  int i, val, nibble;
+
+  ASSERT(buf != NULL);
+  ASSERT(ival != NULL);
+
+  val = 0;
+
+  for(i=0; i<8; i++, buf++)
+    {
+      if (!hstr2nibble(buf, &nibble))
+	{
+	  return NULL;
+	}
+
+      ASSERT(nibble >=0 && nibble < 16);
+
+      val = (val << 4) | nibble;
+    }
+
+  *ival = val;
+
+  return buf;
+}
+
+/* Parse int presented in variable length format */
+const char*
+vhstr2int(const char *buf, int *ival)
+{
+  int i, val, nibble;
+  int found_zero, lim;
+
+  ASSERT(buf != NULL);
+  ASSERT(ival != NULL);
+
+
+  /* If we have leading zeros, skip them */
+  found_zero = 0;
+
+  for(i=0; i<8; i++, buf++)
+    {
+      if (*buf != '0')
+	{
+	  break;
+	}
+
+      found_zero = 1;
+    }
+
+  /* Process non-zeros */
+  lim = 8 - i;
+  val = 0;
+
+  for(i=0; i<lim; i++, buf++)
+    {
+      if (!hstr2nibble(buf, &nibble))
+	{
+	  if (i == 0 && !found_zero)
+	    {
+	      /* Empty value */
+	      return NULL;
+	    }
+
+	  *ival = val;
+	  return buf;
+	}
+	  
+      ASSERT(nibble >= 0 && nibble < 16);
+
+      val = (val << 4) | nibble;
+    }
+
+  if (hstr2nibble(buf, &nibble))
+    {
+      /* Value is too long */
+      return NULL;
+    }
+
+  *ival = val;
+  return buf;
+}
+
+int
+hstr2byte(const char *buf, int *bval)
+{
+  int hnib, lnib;
+
+  ASSERT(buf != NULL);
+  ASSERT(bval != NULL);
+
+  if (!hstr2nibble(buf, &hnib) || !hstr2nibble(buf+1, &lnib))
+    {
+      return 0;
+    }
+
+  *bval = (hnib << 4) | lnib;
+  return 1;
+}
+
+int 
+hstr2nibble(const char *buf, int *nibble)
+{
+  int ch;
+
+  ASSERT(buf != NULL);
+  ASSERT(nibble != NULL);
+
+  ch = *buf;
+
+  if (ch >= '0' && ch <= '9')
+    {
+      *nibble = ch  - '0';
+      return 1;
+    }
+
+  if (ch >= 'a' && ch <= 'f')
+    {
+      *nibble = ch - 'a' + 10;
+      return 1;
+    }
+
+  if (ch >= 'A' && ch <= 'F')
+    {
+      *nibble = ch - 'A' + 10;
+      return 1;
+    }
+
+  return 0;
+}
+
+static volatile char mem_err = 0;
+void  set_mem_err(void);
+static void (*volatile mem_fault_routine) (void) = NULL;
+
+
+
+/* convert count bytes of the memory pointed to by mem into hex string, 
+   placing result in buf, return pointer to next location in hex strng
+   in case of success or NULL otherwise */
+char*
+mem2hstr(char *buf, const unsigned char *mem, int count)
+{
+  int i;
+  unsigned char ch;
+
+  mem_err = 0;
+
+  mem_fault_routine = set_mem_err;
+ 
+  for (i = 0; i<count; i++, mem++)
+    {
+      ch = get_byte (mem);
+      if (mem_err)
+	{
+	  mem_fault_routine = NULL;
+	  return NULL;
+	}
+     
+      *buf++ = gdb_hexchars[ch >> 4];
+      *buf++ = gdb_hexchars[ch & 0x0f];
+    }
+  
+  *buf = 0;
+
+  mem_fault_routine = NULL;
+
+  return buf;
+}
+
+/* convert the hex string to into count bytes of binary to be placed in mem
+   return 1 in case of success and  0 otherwise */
+int
+hstr2mem (unsigned char *mem, const char *buf, int count)
+{
+  int i;
+  int bval;
+
+  mem_err = 0;
+
+  mem_fault_routine = set_mem_err;
+
+  for (i = 0; i < count; i++, mem++, buf+=2)
+    {
+      if (!hstr2byte(buf, &bval))
+	{
+	  mem_fault_routine = NULL;
+	  return 0;
+	}
+	  
+      ASSERT(bval >=0 && bval < 256);
+
+      set_byte (mem, bval);
+
+      if (mem_err)
+	{
+	  mem_fault_routine = NULL;
+	  return 0;
+	}
+    }
+
+  mem_fault_routine = NULL;
+  return 1;
+}
+
+void
+set_mem_err (void)
+{
+  mem_err = 1;
+}
+
+
+/* These are separate functions so that they are so short and sweet
+   that the compiler won't save any registers (if there is a fault
+   to mem_fault, they won't get restored, so there better not be any
+   saved).  */
+unsigned char 
+get_byte (const unsigned char *addr)
+{
+  return *addr;
+}
+
+void
+set_byte (unsigned char *addr, int val)
+{
+  *addr = val;
+}
+
+
+
+/*
+ *  From here down, the code is CPU model specific and generally maps
+ *  the RTEMS thread context format to gdb's.
+ */
+
+#if defined(__i386__)
+
+#include "i386-stub.h"
+
+/* Packing order of registers */
+enum i386_stub_regnames {
+  I386_STUB_REG_EAX, I386_STUB_REG_ECX, I386_STUB_REG_EDX, I386_STUB_REG_EBX,
+  I386_STUB_REG_ESP, I386_STUB_REG_EBP, I386_STUB_REG_ESI, I386_STUB_REG_EDI,
+  I386_STUB_REG_PC /* also known as eip */ ,
+  I386_STUB_REG_PS /* also known as eflags */ ,
+  I386_STUB_REG_CS, I386_STUB_REG_SS, I386_STUB_REG_DS, I386_STUB_REG_ES,
+  I386_STUB_REG_FS, I386_STUB_REG_GS
+};
+
+void rtems_gdb_stub_get_registers_from_context( 
+  int            *registers,
+  Thread_Control *th
+)
+{
+  registers[I386_STUB_REG_EAX] = 0;
+  registers[I386_STUB_REG_ECX] = 0;
+  registers[I386_STUB_REG_EDX] = 0;
+  registers[I386_STUB_REG_EBX] = (int)th->Registers.ebx;
+  registers[I386_STUB_REG_ESP] = (int)th->Registers.esp;
+  registers[I386_STUB_REG_EBP] = (int)th->Registers.ebp;
+  registers[I386_STUB_REG_ESI] = (int)th->Registers.esi;
+  registers[I386_STUB_REG_EDI] = (int)th->Registers.edi;
+  registers[I386_STUB_REG_PC]  = *(int *)th->Registers.esp;
+  registers[I386_STUB_REG_PS]  = (int)th->Registers.eflags;
+
+  /* RTEMS never changes base registers (especially once 
+     threads are running) */
+
+  registers[I386_STUB_REG_CS]  = 0x8; /* We just know these values */
+  registers[I386_STUB_REG_SS]  = 0x10;
+  registers[I386_STUB_REG_DS]  = 0x10;
+  registers[I386_STUB_REG_ES]  = 0x10;
+  registers[I386_STUB_REG_FS]  = 0x10;
+  registers[I386_STUB_REG_GS]  = 0x10;
+}
+
+int rtems_gdb_stub_get_offsets(
+  unsigned char **text_addr,
+  unsigned char **data_addr, 
+  unsigned char **bss_addr
+)
+{
+  extern unsigned char _text_start;
+  extern unsigned char _data_start;
+  extern unsigned char _bss_start;
+
+  *text_addr = &_text_start;
+  *data_addr = &_data_start;
+  *bss_addr  = &_bss_start;
+
+  return 1;
+}
+
+#elif defined(__mips__)
+void rtems_gdb_stub_get_registers_from_context( 
+  int            *registers,
+  Thread_Control *th
+)
+{
+}
+
+int rtems_gdb_stub_get_offsets(
+  unsigned char **text_addr,
+  unsigned char **data_addr, 
+  unsigned char **bss_addr
+)
+{
+  *text_addr = 0;
+  *data_addr = 0;
+  *bss_addr  = 0;
+
+  return 1;
+}
+
+#else
+#error "rtems-gdb-stub.c: Unsupported CPU!"
+#endif
Index: c/src/lib/libbsp/shared/gdbstub/rtems-stub-glue.c
===================================================================
--- c/src/lib/libbsp/shared/gdbstub/rtems-stub-glue.c	(revision 1bbe2e1dbb59eaa89d637ffd95a04ba2919d7010)
+++ c/src/lib/libbsp/shared/gdbstub/rtems-stub-glue.c	(revision 1bbe2e1dbb59eaa89d637ffd95a04ba2919d7010)
@@ -0,0 +1,1299 @@
+/*
+ *  This file contains the RTEMS thread awareness support for GDB stubs.
+ *
+ *  This file is derived from an RTEMS thread aware i386-stub.c that
+ *  had the following copyright announcements:
+ *
+ *    This software is Copyright (C) 1998 by T.sqware - all rights limited
+ *    It is provided in to the public domain "as is", can be freely modified
+ *    as far as this copyight notice is kept unchanged, but does not imply
+ *    an endorsement by T.sqware of the product in which it is included.
+ *
+ *  
+ *     Modifications for RTEMS threads and more
+ *
+ *     Copyright (C) 2000 Quality Quorum, Inc. 
+ * 
+ *     All Rights Reserved
+ * 
+ *     Permission to use, copy, modify, and distribute this software and its 
+ *     documentation for any purpose and without fee is hereby granted. 
+ * 
+ *     QQI DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS SOFTWARE, INCLUDING
+ *     ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS, IN NO EVENT SHALL
+ *     QQI BE LIABLE FOR ANY SPECIAL, INDIRECT OR CONSEQUENTIAL DAMAGES OR
+ *     ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS,
+ *     WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION,
+ *     ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS
+ *     SOFTWARE.
+ */
+
+#include <rtems.h>
+
+#include <string.h>
+
+#include "gdb_if.h"
+
+/* Change it to something meaningful when debugging */
+#undef ASSERT
+#define ASSERT(x) 
+
+extern const char gdb_hexchars[];
+
+/*
+ *  Prototypes for CPU dependent routines that are conditional
+ *  at the bottom of this file.
+ */
+
+void rtems_gdb_stub_get_registers_from_context( 
+  int            *registers,
+  Thread_Control *th
+);
+
+/* Check whether it is OK to enable thread support */
+int rtems_gdb_stub_thread_support_ok(void)
+{
+  if (_System_state_Get() == SYSTEM_STATE_UP) {
+    return 1;
+  }
+  return 0;
+}
+
+/*
+ *  rtems_gdb_stub_id_to_index
+ *
+ *  Return the gdb thread id for the specified RTEMS thread id
+ */
+
+int rtems_gdb_stub_id_to_index(
+  Objects_Id thread_obj_id
+)
+{
+  Objects_Id min_id, max_id;
+  int first_posix_id, first_rtems_id;
+  Objects_Information *obj_info;
+
+  if (_System_state_Get() != SYSTEM_STATE_UP) {
+    /* We have one thread let us use value reserved for idle thread */
+    return 1; 
+  }
+
+  if (_Thread_Executing == _Thread_Idle) {
+    return 1;
+  }
+
+  /* Let us figure out thread_id for gdb */
+  first_rtems_id = 2;
+
+  obj_info = _Objects_Information_table[OBJECTS_RTEMS_TASKS];
+
+  min_id = obj_info->minimum_id;
+  max_id = obj_info->maximum_id;
+
+  if (thread_obj_id >= min_id && thread_obj_id < max_id) {
+    return first_rtems_id + (thread_obj_id - min_id);
+  }
+
+  first_posix_id = first_rtems_id + (max_id - min_id) + 1;
+
+  min_id = _Objects_Information_table[OBJECTS_POSIX_THREADS]->minimum_id;
+
+  return first_posix_id + (thread_obj_id - min_id);
+} 
+
+/* Get id of the thread stopped by exception */
+int rtems_gdb_stub_get_current_thread(void)
+{
+  return rtems_gdb_stub_id_to_index( _Thread_Executing->Object.id );
+}
+
+/* Get id of the next thread after athread, if argument <= 0 find the
+   first available thread, return thread if found or 0 if not */
+int rtems_gdb_stub_get_next_thread(int athread)
+{
+  Objects_Id id, min_id, max_id;
+  int lim, first_posix_id, first_rtems_id;
+  Objects_Information *obj_info;
+  int start;
+
+  if (_System_state_Get() != SYSTEM_STATE_UP) {
+    /* We have one thread let us use value of idle thread */
+    return (athread < 1) ? 1 : 0;
+  }
+
+  if (athread < 1) {
+    return 1;
+  }
+
+  first_rtems_id = 2;
+ 
+  obj_info = _Objects_Information_table[OBJECTS_RTEMS_TASKS];
+
+  min_id = obj_info->minimum_id;
+  max_id = obj_info->maximum_id;
+
+  lim = first_rtems_id + max_id - min_id;
+
+  if (athread < lim) {
+    if (athread < first_rtems_id) {
+      start = first_rtems_id;
+    } else {
+      start = 1 + athread;
+    }
+
+    for (id=start; id<=lim; id++) {
+      if (obj_info->local_table[id - first_rtems_id + 1] != NULL) {
+        return id;
+      }
+    }
+  }
+      
+  first_posix_id = first_rtems_id + (max_id - min_id) + 1;
+
+  obj_info = _Objects_Information_table[OBJECTS_POSIX_THREADS];
+
+  min_id = obj_info->minimum_id;
+  max_id = obj_info->maximum_id;
+
+  lim = first_posix_id + (max_id - min_id);
+
+  if (athread < lim) {
+    if (athread < first_posix_id) {
+      start = first_posix_id;
+    } else {
+      start = 1 + athread;
+    }
+      
+    for (id=start; id<=lim; id++) {
+      if (obj_info->local_table[id - first_posix_id + 1] != NULL) {
+        return id;
+      }
+    }
+  }
+
+  /* Not found */
+  return 0;
+}
+
+
+/* Get thread registers, return 0 if thread does not 
+   exist, and 1 otherwise */
+int rtems_gdb_stub_get_thread_regs(
+  int thread,
+  unsigned int *registers
+)
+{
+  Objects_Id thread_obj_id;
+  Objects_Id min_id, max_id;
+  int first_posix_id, first_rtems_id;
+  Objects_Information *obj_info;
+  Thread_Control *th;
+
+  ASSERT(registers != NULL);
+
+  if (_System_state_Get() != SYSTEM_STATE_UP || thread <= 0) {
+    /* Should not happen */
+    return 0;
+  }
+
+  if (thread == 1) {
+    th = _Thread_Idle;
+    goto found;
+  }
+
+  /* Let us get object associtated with current thread */
+  first_rtems_id = 2;
+
+  thread_obj_id = _Thread_Executing->Object.id;
+
+  /* Let us figure out thread_id for gdb */
+  obj_info = _Objects_Information_table[OBJECTS_RTEMS_TASKS];
+
+  min_id = obj_info->minimum_id;
+  max_id = obj_info->maximum_id;
+
+  if (thread <= (first_rtems_id + (max_id - min_id))) {
+    th = (Thread_Control *)(obj_info->local_table[thread - first_rtems_id + 1]);
+
+    if (th != NULL) {
+      goto found;
+    }
+
+    /* Thread does not exist */
+    return 0;
+  }
+
+  first_posix_id = first_rtems_id + (max_id - min_id) + 1;
+
+  obj_info = _Objects_Information_table[OBJECTS_POSIX_THREADS];
+
+  min_id = obj_info->minimum_id;
+  max_id = obj_info->maximum_id;
+
+  th = (Thread_Control *)(obj_info->local_table[thread - first_posix_id + 1]);
+  if (th == NULL) {
+    /* Thread does not exist */
+    return 0;
+  }
+
+found:
+
+  rtems_gdb_stub_get_registers_from_context( registers, th );
+
+  return 1;
+}
+
+
+/* Set thread registers, return 0 if thread does not 
+   exist or register values will screw up the threads, 
+   and 1 otherwise */
+int rtems_gdb_stub_set_thread_regs(
+  int thread,
+  unsigned int *registers
+)
+{
+  /* In current situation there is no point in changing any registers here
+     thread status is displayed as being deep inside thread switching 
+     and we better do not screw up anything there - it may be fixed eventually
+     though */
+  return 1;
+}
+
+
+/* Get thread information, return 0 if thread does not 
+   exist and 1 otherwise */
+int rtems_gdb_stub_get_thread_info(
+  int thread,
+  struct rtems_gdb_stub_thread_info *info
+)
+{
+  Objects_Id thread_obj_id;
+  Objects_Id min_id, max_id;
+  int first_posix_id, first_rtems_id;
+  Objects_Information *obj_info;
+  Thread_Control *th;
+  unsigned32 name;
+  char tmp_buf[20];
+
+  ASSERT(info != NULL);
+
+  if (thread <= 0) {
+    return 0;
+  }
+
+  if (_System_state_Get() != SYSTEM_STATE_UP || thread == 1) {
+    /* We have one thread let us use value
+         which will never happen for real thread */
+    strcpy(info->display, "idle thread");
+    strcpy(info->name, "IDLE");
+    info->more_display[0] = 0; /* Nothing */
+
+    return 1;
+  }
+
+  /* Let us get object associtated with current thread */
+  thread_obj_id = _Thread_Executing->Object.id;
+
+  /* Let us figure out thread_id for gdb */
+  first_rtems_id = 2;
+  
+  obj_info = _Objects_Information_table[OBJECTS_RTEMS_TASKS];
+
+  min_id = obj_info->minimum_id;
+  max_id = obj_info->maximum_id;
+
+  if (thread <= (first_rtems_id + (max_id - min_id))) {
+      th = (Thread_Control *)(obj_info->local_table[thread - 
+                                                    first_rtems_id + 1]);
+
+  if (th == NULL) {
+    /* Thread does not exist */
+    return 0;
+  }
+
+  strcpy(info->display, "rtems task:   control at 0x");
+
+  tmp_buf[0] = gdb_hexchars[(((int)th) >> 28) & 0xf];
+  tmp_buf[1] = gdb_hexchars[(((int)th) >> 24) & 0xf];
+  tmp_buf[2] = gdb_hexchars[(((int)th) >> 20) & 0xf];
+  tmp_buf[3] = gdb_hexchars[(((int)th) >> 16) & 0xf];
+  tmp_buf[4] = gdb_hexchars[(((int)th) >> 12) & 0xf];
+  tmp_buf[5] = gdb_hexchars[(((int)th) >> 8) & 0xf];
+  tmp_buf[6] = gdb_hexchars[(((int)th) >> 4) & 0xf];
+  tmp_buf[7] = gdb_hexchars[((int)th) & 0xf];
+  tmp_buf[8] = 0;
+
+  strcat(info->display, tmp_buf);
+  
+  name = *(unsigned32 *)(obj_info->local_table[thread]->name);
+      
+  info->name[0] = (name >> 24) & 0xff;
+  info->name[1] = (name >> 16) & 0xff;
+  info->name[2] = (name >> 8) & 0xff;
+  info->name[3] = name & 0xff;
+  info->name[4] = 0;
+
+  info->more_display[0] = 0; /* Nothing */
+
+  return 1;
+}
+
+  first_posix_id = first_rtems_id + (max_id - min_id) + 1;
+
+  obj_info = _Objects_Information_table[OBJECTS_POSIX_THREADS];
+
+  min_id = obj_info->minimum_id;
+  max_id = obj_info->maximum_id;
+
+  th = (Thread_Control *)(obj_info->local_table[thread - first_posix_id + 1]);
+  if (th == NULL)
+    {
+      /* Thread does not exist */
+      return 0;
+    }
+
+  strcpy(info->display, "posix thread: control at 0x");
+
+  tmp_buf[0] = gdb_hexchars[(((int)th) >> 28) & 0xf];
+  tmp_buf[1] = gdb_hexchars[(((int)th) >> 24) & 0xf];
+  tmp_buf[2] = gdb_hexchars[(((int)th) >> 20) & 0xf];
+  tmp_buf[3] = gdb_hexchars[(((int)th) >> 16) & 0xf];
+  tmp_buf[4] = gdb_hexchars[(((int)th) >> 12) & 0xf];
+  tmp_buf[5] = gdb_hexchars[(((int)th) >> 8) & 0xf];
+  tmp_buf[6] = gdb_hexchars[(((int)th) >> 4) & 0xf];
+  tmp_buf[7] = gdb_hexchars[((int)th) & 0xf];
+  tmp_buf[8] = 0;
+
+  strcat(info->display, tmp_buf);
+
+  name = *(unsigned32 *)(obj_info->local_table[thread - 
+                                               first_posix_id + 1]->name);
+
+  info->name[0] = (name >> 24) & 0xff;
+  info->name[1] = (name >> 16) & 0xff;
+  info->name[2] = (name >> 8) & 0xff;
+  info->name[3] = name & 0xff;
+  info->name[4] = 0;
+
+  info->more_display[0] = 0; /* Nothing */
+  
+  return 1;
+}
+
+/*******************************************************/
+
+
+/* Format: x<type-1x>,<address-x>,<length-x>, where x is 'z' or 'Z' */
+int parse_zbreak(const char *in, int *type, unsigned char **addr, int *len)
+{
+  int ttmp, atmp, ltmp;
+  
+  ASSERT(in != NULL);
+  ASSERT(type != NULL);
+  ASSERT(addr != NULL);
+  ASSERT(len != NULL);
+
+  ASSERT(*in == 'z' || *in == 'Z');
+
+  in++;
+
+  if (!hstr2nibble(in, &ttmp) || *(in+1) != ',')
+    {
+      return 0;
+    }
+  in += 2;
+
+  in = vhstr2int(in, &atmp);
+  if (in == NULL || *in != ',')
+    {
+      return 0;
+    }
+  in++;
+
+  in = vhstr2int(in, &ltmp);
+  if (in == NULL || ltmp < 1)
+    {
+      return 0;
+    }
+
+  *type = ttmp;
+  *addr = (unsigned char *)atmp;
+  *len  = ltmp;
+
+  return 1;
+}
+
+/* Format: qP<mask-08x><thread_id-ft> */
+static int
+parse_qp(const char *in, int *mask, int *thread)
+{
+  const char *ptr;
+
+  ASSERT(in != NULL);
+  ASSERT(*in == 'q');
+  ASSERT(*(in+1) == 'P');
+
+  ptr = fhstr2int(in+2, mask);
+  if (ptr == NULL)
+    {
+      return 0;
+    }
+
+  ptr = fhstr2thread(ptr, thread);
+  if (ptr == NULL)
+    {
+      return 0;
+    }
+
+  return 1;
+}
+
+/* Format: qQ<mask-08x><thread_id-ft><tag-08x><length-02x><value>...] */
+static void
+pack_qq(char *out, int mask, int thread, struct rtems_gdb_stub_thread_info *info)
+{
+  int len;
+
+  ASSERT(out != NULL);
+  ASSERT(info != NULL);
+
+  *out++ = 'q';
+  *out++ = 'Q';
+  out = int2fhstr(out, mask);
+  out = thread2fhstr(out, thread);
+
+  if (mask & 0x1) {
+    /* Thread id once again */
+    memcpy(out, "00000001", 8);
+    out   += 8;
+    *out++ = '1';
+    *out++ = '0';
+    out = thread2fhstr(out, thread);
+  }
+
+  if (mask & 0x2) {
+    /* Exists */
+    memcpy(out, "00000002", 8);
+    out   += 8;
+    *out++ = '0';
+    *out++ = '1';
+    *out++ = '1';
+  }
+
+  if (mask & 0x4) {
+    /* Display */
+    memcpy(out, "00000004", 8);
+    out += 8;
+
+    info->display[sizeof(info->display)-1] = 0; /* Fot God sake */
+
+    len = strlen(info->display);
+
+    *out++ = gdb_hexchars[len >> 4];
+    *out++ = gdb_hexchars[len & 0x0f];
+
+    memcpy(out, info->display, len);
+
+    out += len;
+  }
+
+  if (mask & 0x8) {
+    /* Name */
+    memcpy(out, "00000008", 8);
+    out += 8;
+
+    info->name[sizeof(info->name)-1] = 0; /* Fot God sake */
+
+    len = strlen(info->name);
+
+    *out++ = gdb_hexchars[len >> 4];
+    *out++ = gdb_hexchars[len & 0x0f];
+
+    memcpy(out, info->name, len);
+
+    out += len;
+  }
+
+  if (mask & 0x10) {
+    /* More display */
+    memcpy(out, "00000010", 8);
+    out += 8;
+
+    info->more_display[sizeof(info->more_display)-1] = 0; /* Fot God sake */
+
+    len = strlen(info->more_display);
+
+    *out++ = gdb_hexchars[len >> 4];
+    *out++ = gdb_hexchars[len & 0x0f];
+
+    memcpy(out, info->more_display, len);
+
+    out += len;
+  }
+
+  *out = 0;
+
+  return;
+}
+
+/* Format qL<first-01x><max_count-02x><arg_thread_id-ft> */
+static int
+parse_ql(const char *in, int *first, int *max_count, int *athread)
+{
+  const char *ptr;
+
+  ASSERT(in != NULL);
+  ASSERT(*in == 'q');
+  ASSERT(*(in+1) == 'L');
+  ASSERT(first != NULL);
+  ASSERT(max_count != NULL);
+  ASSERT(athread != NULL);
+
+  ptr = in + 2;
+
+  /*  First */
+  if (!hstr2nibble(ptr, first))
+    {
+      return 0;
+    }
+  ptr++;
+
+  /* Max count */
+  if (!hstr2byte(ptr, max_count))
+    {
+      return 0;
+    }
+  ptr += 2;
+
+  /* A thread */
+  ptr = fhstr2thread(ptr, athread);
+  if (ptr == NULL)
+    {
+      return 0;
+    }
+
+  return 1;
+}
+
+/* Format: qM<count-02x><done-01x><arg_thread_id>[<found_thread_id-ft>...] */
+static char *
+reserve_qm_header(char *out)
+{
+  ASSERT(out != NULL);
+
+  return out + 21;
+}
+
+/* Format: qM<count-02x><done-01x><arg_thread_id>[<found_thread_id-ft>...] */
+static char*
+pack_qm_thread(char *out, int thread)
+{
+  ASSERT(out != 0);
+
+  return thread2fhstr(out, thread);
+}
+
+/* Format: qM<count-02x><done-01x><arg_thread_id>[<found_thread_id-ft>...] */
+static void
+pack_qm_header(char *out, int count, int done, int athread)
+{
+  ASSERT(out != 0);
+  ASSERT(count >= 0 && count < 256);
+
+  *out++ = 'q';
+  *out++ = 'M';
+
+  *out++ = gdb_hexchars[(count >> 4) & 0x0f];
+  *out++ = gdb_hexchars[count & 0x0f];
+
+  if (done) {
+    *out++ = '1';
+  } else {
+    *out++ = '0';
+  }
+
+  thread2fhstr(out, athread);
+
+  return;
+}
+
+
+void rtems_gdb_process_query( 
+  char *inbuffer,
+  char *outbuffer,
+  int   do_threads,
+  int   thread
+)
+{
+  char *optr;
+    
+  switch(inbuffer[1]) {
+    case 'C':
+      /* Current thread query query - return stopped thread */
+      if (!do_threads) {
+        break;
+      }
+
+      optr = outbuffer;
+
+      *optr++ = 'Q';
+      *optr++ = 'C';
+      optr    = thread2vhstr(optr, thread);
+      *optr   = 0;
+      break;
+
+    case 'P':
+      /* Thread info query */
+      if (!do_threads) {
+        break;
+      }
+
+      {
+        int ret, rthread, mask;
+        struct rtems_gdb_stub_thread_info info;
+
+        ret = parse_qp(inbuffer, &mask, &rthread);
+        if (!ret|| mask & ~0x1f) {
+          strcpy(outbuffer, "E01");
+          break;
+        }
+
+        ret = rtems_gdb_stub_get_thread_info(rthread, &info);
+        if (!ret) {
+          /* Good implementation would never ask for non-existing thread,
+             should we care about bad ones - it does not seem so */
+          strcpy(outbuffer, "E02");
+          break;
+        }
+
+        /* Build response */
+        pack_qq(outbuffer, mask, rthread, &info);
+      }
+
+      break;
+    case 'L':
+      /* Thread info query */
+      if (!do_threads) {
+        break;
+      }
+
+      {
+        int ret, athread, first, max_cnt, i, done, rthread;
+
+        ret = parse_ql(inbuffer, &first, &max_cnt, &athread);
+        if (!ret) {
+          strcpy(outbuffer, "E02");
+          break;
+        }
+
+        if (max_cnt == 0) {
+          strcpy(outbuffer, "E02");
+          break;
+        }
+
+        if (max_cnt > QM_MAX_THREADS) {
+          /* Limit max count by buffer size */
+          max_cnt = QM_MAX_THREADS;
+        }
+
+        /* Reserve place for output header */
+        optr = reserve_qm_header(outbuffer);
+
+        if (first) {
+          rthread = 0;
+        } else {
+          rthread = athread;
+        }
+
+        done = 0;
+
+        for (i=0; i<max_cnt; i++) {
+          rthread = rtems_gdb_stub_get_next_thread(rthread);
+
+          if (rthread <= 0) {
+            done = 1; /* Set done flag */
+            break;
+          }
+
+          optr = pack_qm_thread(optr, rthread);
+        }
+
+        *optr = 0;
+
+        ASSERT((optr - outbuffer) < BUFMAX);
+
+        /* Fill header */
+        pack_qm_header(outbuffer, i, done, athread);
+
+      }
+      break;
+    default:
+      if (memcmp(inbuffer, "qOffsets", 8) == 0) {
+        unsigned char *t, *d, *b;
+        char *out;
+
+        if (!rtems_gdb_stub_get_offsets(&t, &d, &b)) {
+          break;
+        }
+
+        out = outbuffer;
+
+        *out++ = 'T';
+        *out++ = 'e';
+        *out++ = 'x';
+        *out++ = 't';
+        *out++ = '=';
+
+        out = int2vhstr(out, (int)t);
+
+        *out++ = ';';
+        *out++ = 'D';
+        *out++ = 'a';
+        *out++ = 't';
+        *out++ = 'a';
+        *out++ = '=';
+
+        out = int2vhstr(out, (int)d);
+
+        *out++ = ';';
+        *out++ = 'B';
+        *out++ = 's';
+        *out++ = 's';
+        *out++ = '=';
+
+        out = int2vhstr(out, (int)b);
+
+        *out++ = ';';
+        *out++ = 0;
+
+        break;
+      }
+
+      /*  qCRC, qRcmd, qu and qz will be added here */
+      break;
+    }
+
+}
+
+/* Present thread in the variable length string format */
+char*
+thread2vhstr(char *buf, int thread)
+{
+  int i, nibble, shift;
+
+  ASSERT(buf != NULL);
+
+  for(i=0, shift=28; i<8; i++, shift-=4)
+    {
+      nibble = (thread >> shift) & 0x0f;
+      
+      if (nibble != 0)
+	{
+	  break;
+	}
+    }
+
+  if (i == 8)
+    {
+      *buf++ = '0';
+      return buf;
+    }
+
+  *buf++ = gdb_hexchars[nibble];
+
+  for(i++, shift-=4; i<8; i++, shift-=4, buf++)
+    {
+      nibble = (thread >> shift) & 0x0f;
+      *buf   = gdb_hexchars[nibble];
+    }
+
+  return buf;
+}
+
+/* Present thread in fixed length string format */
+char*
+thread2fhstr(char *buf, int thread)
+{
+  int i, nibble, shift;
+
+  ASSERT(buf != NULL);
+
+  for(i=0; i<8; i++, buf++)
+    {
+      *buf = '0';
+    }
+
+  for(i=0, shift=28; i<8; i++, shift-=4, buf++)
+    {
+      nibble = (thread >> shift) & 0x0f;
+      *buf   = gdb_hexchars[nibble];
+    }
+
+  return buf;
+}
+
+/* Parse thread presented in fixed length format */
+const char*
+fhstr2thread(const char *buf, int *thread)
+{
+  int i, val, nibble;
+
+  ASSERT(buf != NULL);
+  ASSERT(thread != NULL);
+
+  for(i=0; i<8; i++, buf++)
+    {
+      if (*buf != '0')
+	{
+	  return NULL;
+	}
+    }
+
+  val = 0;
+
+  for(i=0; i<8; i++, buf++)
+    {
+      if (!hstr2nibble(buf, &nibble))
+	{
+	  return NULL;
+	}
+
+      ASSERT(nibble >=0 && nibble < 16);
+
+      val = (val << 4) | nibble;
+    }
+
+  *thread = val;
+
+  return buf;
+}
+
+/* Parse thread presented in variable length format */
+const char*
+vhstr2thread(const char *buf, int *thread)
+{
+  int i, val, nibble;
+  int found_zero, lim;
+
+  ASSERT(buf != NULL);
+  ASSERT(thread != NULL);
+
+
+  /* If we have leading zeros, skip them */
+  found_zero = 0;
+
+  for(i=0; i<16; i++, buf++)
+    {
+      if (*buf != '0')
+	{
+	  break;
+	}
+
+      found_zero = 1;
+    }
+
+  /* Process non-zeros */
+  lim = 16 - i;
+  val = 0;
+
+  for(i=0; i<lim; i++, buf++)
+    {
+      if (!hstr2nibble(buf, &nibble))
+	{
+	  if (i == 0 && !found_zero)
+	    {
+	      /* Empty value */
+	      return NULL;
+	    }
+
+	  *thread = val;
+	  return buf;
+	}
+	  
+      ASSERT(nibble >= 0 && nibble < 16);
+
+      val = (val << 4) | nibble;
+    }
+
+  if (hstr2nibble(buf, &nibble))
+    {
+      /* Value is too long */
+      return NULL;
+    }
+  
+  *thread = val;
+  return buf;
+}
+
+
+/* Present integer in the variable length string format */
+char*
+int2vhstr(char *buf, int val)
+{
+  int i, nibble, shift;
+
+  ASSERT(buf != NULL);
+
+  for(i=0, shift=28; i<8; i++, shift-=4)
+    {
+      nibble = (val >> shift) & 0x0f;
+      
+      if (nibble != 0)
+	{
+	  break;
+	}
+    }
+
+  if (i == 8)
+    {
+      *buf++ = '0';
+      return buf;
+    }
+
+  *buf++ = gdb_hexchars[nibble];
+
+  for(i++, shift-=4; i<8; i++, shift-=4, buf++)
+    {
+      nibble = (val >> shift) & 0x0f;
+      *buf   = gdb_hexchars[nibble];
+    }
+
+  return buf;
+}
+
+/* Present int in fixed length string format */
+char*
+int2fhstr(char *buf, int val)
+{
+  int i, nibble, shift;
+
+  ASSERT(buf != NULL);
+
+  for(i=0, shift=28; i<8; i++, shift-=4, buf++)
+    {
+      nibble = (val >> shift) & 0x0f;
+      *buf   = gdb_hexchars[nibble];
+    }
+
+  return buf;
+}
+
+/* Parse int presented in fixed length format */
+const char*
+fhstr2int(const char *buf, int *ival)
+{
+  int i, val, nibble;
+
+  ASSERT(buf != NULL);
+  ASSERT(ival != NULL);
+
+  val = 0;
+
+  for(i=0; i<8; i++, buf++)
+    {
+      if (!hstr2nibble(buf, &nibble))
+	{
+	  return NULL;
+	}
+
+      ASSERT(nibble >=0 && nibble < 16);
+
+      val = (val << 4) | nibble;
+    }
+
+  *ival = val;
+
+  return buf;
+}
+
+/* Parse int presented in variable length format */
+const char*
+vhstr2int(const char *buf, int *ival)
+{
+  int i, val, nibble;
+  int found_zero, lim;
+
+  ASSERT(buf != NULL);
+  ASSERT(ival != NULL);
+
+
+  /* If we have leading zeros, skip them */
+  found_zero = 0;
+
+  for(i=0; i<8; i++, buf++)
+    {
+      if (*buf != '0')
+	{
+	  break;
+	}
+
+      found_zero = 1;
+    }
+
+  /* Process non-zeros */
+  lim = 8 - i;
+  val = 0;
+
+  for(i=0; i<lim; i++, buf++)
+    {
+      if (!hstr2nibble(buf, &nibble))
+	{
+	  if (i == 0 && !found_zero)
+	    {
+	      /* Empty value */
+	      return NULL;
+	    }
+
+	  *ival = val;
+	  return buf;
+	}
+	  
+      ASSERT(nibble >= 0 && nibble < 16);
+
+      val = (val << 4) | nibble;
+    }
+
+  if (hstr2nibble(buf, &nibble))
+    {
+      /* Value is too long */
+      return NULL;
+    }
+
+  *ival = val;
+  return buf;
+}
+
+int
+hstr2byte(const char *buf, int *bval)
+{
+  int hnib, lnib;
+
+  ASSERT(buf != NULL);
+  ASSERT(bval != NULL);
+
+  if (!hstr2nibble(buf, &hnib) || !hstr2nibble(buf+1, &lnib))
+    {
+      return 0;
+    }
+
+  *bval = (hnib << 4) | lnib;
+  return 1;
+}
+
+int 
+hstr2nibble(const char *buf, int *nibble)
+{
+  int ch;
+
+  ASSERT(buf != NULL);
+  ASSERT(nibble != NULL);
+
+  ch = *buf;
+
+  if (ch >= '0' && ch <= '9')
+    {
+      *nibble = ch  - '0';
+      return 1;
+    }
+
+  if (ch >= 'a' && ch <= 'f')
+    {
+      *nibble = ch - 'a' + 10;
+      return 1;
+    }
+
+  if (ch >= 'A' && ch <= 'F')
+    {
+      *nibble = ch - 'A' + 10;
+      return 1;
+    }
+
+  return 0;
+}
+
+static volatile char mem_err = 0;
+void  set_mem_err(void);
+static void (*volatile mem_fault_routine) (void) = NULL;
+
+
+
+/* convert count bytes of the memory pointed to by mem into hex string, 
+   placing result in buf, return pointer to next location in hex strng
+   in case of success or NULL otherwise */
+char*
+mem2hstr(char *buf, const unsigned char *mem, int count)
+{
+  int i;
+  unsigned char ch;
+
+  mem_err = 0;
+
+  mem_fault_routine = set_mem_err;
+ 
+  for (i = 0; i<count; i++, mem++)
+    {
+      ch = get_byte (mem);
+      if (mem_err)
+	{
+	  mem_fault_routine = NULL;
+	  return NULL;
+	}
+     
+      *buf++ = gdb_hexchars[ch >> 4];
+      *buf++ = gdb_hexchars[ch & 0x0f];
+    }
+  
+  *buf = 0;
+
+  mem_fault_routine = NULL;
+
+  return buf;
+}
+
+/* convert the hex string to into count bytes of binary to be placed in mem
+   return 1 in case of success and  0 otherwise */
+int
+hstr2mem (unsigned char *mem, const char *buf, int count)
+{
+  int i;
+  int bval;
+
+  mem_err = 0;
+
+  mem_fault_routine = set_mem_err;
+
+  for (i = 0; i < count; i++, mem++, buf+=2)
+    {
+      if (!hstr2byte(buf, &bval))
+	{
+	  mem_fault_routine = NULL;
+	  return 0;
+	}
+	  
+      ASSERT(bval >=0 && bval < 256);
+
+      set_byte (mem, bval);
+
+      if (mem_err)
+	{
+	  mem_fault_routine = NULL;
+	  return 0;
+	}
+    }
+
+  mem_fault_routine = NULL;
+  return 1;
+}
+
+void
+set_mem_err (void)
+{
+  mem_err = 1;
+}
+
+
+/* These are separate functions so that they are so short and sweet
+   that the compiler won't save any registers (if there is a fault
+   to mem_fault, they won't get restored, so there better not be any
+   saved).  */
+unsigned char 
+get_byte (const unsigned char *addr)
+{
+  return *addr;
+}
+
+void
+set_byte (unsigned char *addr, int val)
+{
+  *addr = val;
+}
+
+
+
+/*
+ *  From here down, the code is CPU model specific and generally maps
+ *  the RTEMS thread context format to gdb's.
+ */
+
+#if defined(__i386__)
+
+#include "i386-stub.h"
+
+/* Packing order of registers */
+enum i386_stub_regnames {
+  I386_STUB_REG_EAX, I386_STUB_REG_ECX, I386_STUB_REG_EDX, I386_STUB_REG_EBX,
+  I386_STUB_REG_ESP, I386_STUB_REG_EBP, I386_STUB_REG_ESI, I386_STUB_REG_EDI,
+  I386_STUB_REG_PC /* also known as eip */ ,
+  I386_STUB_REG_PS /* also known as eflags */ ,
+  I386_STUB_REG_CS, I386_STUB_REG_SS, I386_STUB_REG_DS, I386_STUB_REG_ES,
+  I386_STUB_REG_FS, I386_STUB_REG_GS
+};
+
+void rtems_gdb_stub_get_registers_from_context( 
+  int            *registers,
+  Thread_Control *th
+)
+{
+  registers[I386_STUB_REG_EAX] = 0;
+  registers[I386_STUB_REG_ECX] = 0;
+  registers[I386_STUB_REG_EDX] = 0;
+  registers[I386_STUB_REG_EBX] = (int)th->Registers.ebx;
+  registers[I386_STUB_REG_ESP] = (int)th->Registers.esp;
+  registers[I386_STUB_REG_EBP] = (int)th->Registers.ebp;
+  registers[I386_STUB_REG_ESI] = (int)th->Registers.esi;
+  registers[I386_STUB_REG_EDI] = (int)th->Registers.edi;
+  registers[I386_STUB_REG_PC]  = *(int *)th->Registers.esp;
+  registers[I386_STUB_REG_PS]  = (int)th->Registers.eflags;
+
+  /* RTEMS never changes base registers (especially once 
+     threads are running) */
+
+  registers[I386_STUB_REG_CS]  = 0x8; /* We just know these values */
+  registers[I386_STUB_REG_SS]  = 0x10;
+  registers[I386_STUB_REG_DS]  = 0x10;
+  registers[I386_STUB_REG_ES]  = 0x10;
+  registers[I386_STUB_REG_FS]  = 0x10;
+  registers[I386_STUB_REG_GS]  = 0x10;
+}
+
+int rtems_gdb_stub_get_offsets(
+  unsigned char **text_addr,
+  unsigned char **data_addr, 
+  unsigned char **bss_addr
+)
+{
+  extern unsigned char _text_start;
+  extern unsigned char _data_start;
+  extern unsigned char _bss_start;
+
+  *text_addr = &_text_start;
+  *data_addr = &_data_start;
+  *bss_addr  = &_bss_start;
+
+  return 1;
+}
+
+#elif defined(__mips__)
+void rtems_gdb_stub_get_registers_from_context( 
+  int            *registers,
+  Thread_Control *th
+)
+{
+}
+
+int rtems_gdb_stub_get_offsets(
+  unsigned char **text_addr,
+  unsigned char **data_addr, 
+  unsigned char **bss_addr
+)
+{
+  *text_addr = 0;
+  *data_addr = 0;
+  *bss_addr  = 0;
+
+  return 1;
+}
+
+#else
+#error "rtems-gdb-stub.c: Unsupported CPU!"
+#endif
