2001-03-05 Greg Menke <gregory.menke@gsfc.nasa.gov>

	* mips-stub.c: Debugged & tweaked the gdb command processing,
	zbreak stuff, breakpoint and step code.  Implemented 'T' command
	support and debugged remote gdb support w/ the Mongoose bsp.
	Added the memory segment support.
	* memlimits.h: Disabled all contents in favor of memory sement
	support.  This file could probably go away.
	* rtems-stub-glue.c (rtems_gdb_index_to_stub_id()): New routine.
	rtems_gdb_stub_get_register_from_context(): Implemented MIPS version.
	rtems_gdb_stub_get_offsets(): Implemented MIPS version.
	* README: Updated.

7 files changed, 1341 insertions, 753 deletions

diff --git a/c/src/lib/libbsp/mips/shared/gdbstub/ChangeLog b/c/src/lib/libbsp/mips/shared/gdbstub/ChangeLog
index 59e85b65de..1a2c87ce74 100644
--- a/c/src/lib/libbsp/mips/shared/gdbstub/ChangeLog
+++ b/c/src/lib/libbsp/mips/shared/gdbstub/ChangeLog
@@ -1,3 +1,16 @@
+2001-03-05	Greg Menke <gregory.menke@gsfc.nasa.gov>
+
+	* mips-stub.c: Debugged & tweaked the gdb command processing,
+	zbreak stuff, breakpoint and step code.  Implemented 'T' command
+	support and debugged remote gdb support w/ the Mongoose bsp.
+	Added the memory segment support.
+	* memlimits.h: Disabled all contents in favor of memory sement
+	support.  This file could probably go away.
+	* rtems-stub-glue.c (rtems_gdb_index_to_stub_id()): New routine.
+	rtems_gdb_stub_get_register_from_context(): Implemented MIPS version.
+	rtems_gdb_stub_get_offsets(): Implemented MIPS version.
+	* README: Updated.
+
 2001-03-01	Joel Sherrill <joel@OARcorp.com>
 
 	* ChangeLog: Corrected previous entry. 
diff --git a/c/src/lib/libbsp/mips/shared/gdbstub/README b/c/src/lib/libbsp/mips/shared/gdbstub/README
index f1ce5db96d..4565377f84 100644
--- a/c/src/lib/libbsp/mips/shared/gdbstub/README
+++ b/c/src/lib/libbsp/mips/shared/gdbstub/README
@@ -2,6 +2,29 @@
 #  $Id$
 #
 
+/*****************************************************/
+
+Debugged this stub against the MongooseV bsp.  Relies on putting break
+instructions on breakpoints and step targets- normal stuff, and does not
+employ hardware breakpoint support at this time.  As written, a single
+breakpoint in a loop will not be reasserted unless the user steps or has
+a 2nd one, since breakpoints are only reset when the gdb stub is
+re-entered.  A useful enhancement would be to fix the break instruction
+management so the stub could invisibly put a 2nd break after the 1st
+"official" one so it can silently reset breakpoints.  Shouldn't be too
+hard, mostly a matter of working it out.
+
+This was tested only against an R3000 MIPS.  It should work OK on a
+R4000.  Needs to be tested at some point.
+
+This stub supports threads as implemented by gdb 5 and doesn't have any
+bugs I'm aware of.
+
+Greg Menke
+3/5/2002
+
+/*****************************************************/
+
 
 The contents of this directory are based upon the "r46kstub.tar.gz" package
 released to the net by
diff --git a/c/src/lib/libbsp/mips/shared/gdbstub/gdb_if.h b/c/src/lib/libbsp/mips/shared/gdbstub/gdb_if.h
index 5f8e81601b..464bff8bef 100644
--- a/c/src/lib/libbsp/mips/shared/gdbstub/gdb_if.h
+++ b/c/src/lib/libbsp/mips/shared/gdbstub/gdb_if.h
@@ -49,6 +49,7 @@ const char* vhstr2int(const char *buf, int *ival);
 int   hstr2byte(const char *buf, int *bval);
 int   hstr2nibble(const char *buf, int *nibble);
 
+Thread_Control *rtems_gdb_index_to_stub_id(int);
 int rtems_gdb_stub_thread_support_ok(void);
 int rtems_gdb_stub_get_current_thread(void);
 int rtems_gdb_stub_get_next_thread(int);
@@ -159,4 +160,21 @@ void rtems_gdb_process_query(
 
 #define	NUM_REGS	72
 
+
+
+
+
+
+void mips_gdb_stub_install(int enableThreads) ;
+
+
+#define MEMOPT_READABLE   1
+#define MEMOPT_WRITEABLE  2
+
+#define NUM_MEMSEGS     10
+
+int gdbstub_add_memsegment(unsigned,unsigned,int);
+
+
+
 #endif /* _GDB_IF_H */
diff --git a/c/src/lib/libbsp/mips/shared/gdbstub/memlimits.h b/c/src/lib/libbsp/mips/shared/gdbstub/memlimits.h
index 05f82123e8..e9fd03c5f2 100644
--- a/c/src/lib/libbsp/mips/shared/gdbstub/memlimits.h
+++ b/c/src/lib/libbsp/mips/shared/gdbstub/memlimits.h
@@ -12,8 +12,8 @@
  *  OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
  */
 
-#ifndef _LIMITS_H_
-#define _LIMITS_H_
+#ifndef _MEMLIMITS_H_
+#define _MEMLIMITS_H_
 
 /*
  * The macros in this file are specific to a given implementation.
@@ -45,6 +45,7 @@
  * to have different readability and/or writeability attributes.
  */
 
+/*
 #define K0_LIMIT_FOR_READ  (K0BASE+0x18000000)
 #define K1_LIMIT_FOR_READ  (K1BASE+K1SIZE)
 
@@ -67,4 +68,21 @@
   && (((K0BASE <= (int)ptr) && ((int)ptr < K0_LIMIT_FOR_STEP)) \
       || ((K1BASE <= (int)ptr) && ((int)ptr < K1_LIMIT_FOR_STEP))))
 
-#endif  /* _LIMITS_H_ */
+struct memseg
+{
+      unsigned begin, end, opts;
+};
+
+#define MEMOPT_READABLE   1
+#define MEMOPT_WRITEABLE  2
+
+#define NUM_MEMSEGS     10
+
+
+int add_memsegment(unsigned,unsigned,int);
+int is_readable(unsigned,unsigned);
+int is_writeable(unsigned,unsigned);
+int is_steppable(unsigned);
+*/
+
+#endif  /* _MEMLIMITS_H_ */
diff --git a/c/src/lib/libbsp/mips/shared/gdbstub/mips-stub.c b/c/src/lib/libbsp/mips/shared/gdbstub/mips-stub.c
index 16ff82f70b..184de7c345 100644
--- a/c/src/lib/libbsp/mips/shared/gdbstub/mips-stub.c
+++ b/c/src/lib/libbsp/mips/shared/gdbstub/mips-stub.c
@@ -123,7 +123,7 @@
 #include <string.h>
 #include <signal.h>
 #include "mips_opcode.h"
-#include "memlimits.h"
+/* #include "memlimits.h" */
 #include <rtems.h>
 #include "gdb_if.h"
 
@@ -208,10 +208,27 @@ static char do_threads;      /* != 0 means we are supporting threads */
  * The following external functions provide character input and output.
  */
 extern char getDebugChar (void);
-
 extern void putDebugChar (char);
 
 
+
+/*
+ * The following definitions are used for the gdb stub memory map
+ */
+struct memseg
+{
+      unsigned begin, end, opts;
+};
+
+static int is_readable(unsigned,unsigned);
+static int is_writeable(unsigned,unsigned);
+static int is_steppable(unsigned);
+
+
+
+
+
+
 /*
  * BUFMAX defines the maximum number of characters in the inbound & outbound
  * packet buffers.  At least 4+(sizeof registers)*2 bytes will be needed for
@@ -224,6 +241,7 @@ static char outBuffer[BUFMAX];
 
 /* Structure to keep info on a z-breaks */
 #define BREAKNUM 32
+
 struct z0break
 {
   /* List support */
@@ -231,8 +249,8 @@ struct z0break
   struct z0break *prev;
 
   /* Location, preserved data */
-  unsigned char *address;
-  char     buf[2];
+  unsigned *address;
+  unsigned instr;
 };
 
 static struct z0break z0break_arr[BREAKNUM];
@@ -623,6 +641,11 @@ putpacket (char *buffer)
   while  (getDebugChar () != '+');
 }
 
+
+
+
+
+
 
 /*
  * Saved instruction data for single step support
@@ -663,127 +686,133 @@ undoSStep (void)
 static void
 doSStep (void)
 {
-  InstFmt inst;
-
-  instrBuffer.targetAddr = (unsigned *)(registers[PC]+4);    /* set default */
-
-  inst.word = *(unsigned *)registers[PC];     /* read the next instruction  */
-
-  switch (inst.RType.op) {                    /* override default if branch */
-    case OP_SPECIAL:
-      switch (inst.RType.func) {
-        case OP_JR:
-        case OP_JALR:
-          instrBuffer.targetAddr =
-            (unsigned *)registers[inst.RType.rs];
-          break;
-      };
-      break;
-
-    case OP_REGIMM:
-      switch (inst.IType.rt) {
-        case OP_BLTZ:
-        case OP_BLTZL:
-        case OP_BLTZAL:
-        case OP_BLTZALL:
-          if (registers[inst.IType.rs] < 0 )
+   struct z0break *z0;
+   InstFmt inst;
+
+   instrBuffer.targetAddr = (unsigned *)(registers[PC]+4);    /* set default */
+
+   inst.word = *(unsigned *)registers[PC];     /* read the next instruction  */
+
+   switch (inst.RType.op) {                    /* override default if branch */
+      case OP_SPECIAL:
+         switch (inst.RType.func) {
+            case OP_JR:
+            case OP_JALR:
+               instrBuffer.targetAddr =
+                  (unsigned *)registers[inst.RType.rs];
+               break;
+         };
+         break;
+
+      case OP_REGIMM:
+         switch (inst.IType.rt) {
+            case OP_BLTZ:
+            case OP_BLTZL:
+            case OP_BLTZAL:
+            case OP_BLTZALL:
+               if (registers[inst.IType.rs] < 0 )
+                  instrBuffer.targetAddr =
+                     (unsigned *)(((signed short)inst.IType.imm<<2)
+                                  + (registers[PC]+4));
+               else
+                  instrBuffer.targetAddr = (unsigned*)(registers[PC]+8);
+               break;
+            case OP_BGEZ:
+            case OP_BGEZL:
+            case OP_BGEZAL:
+            case OP_BGEZALL:
+               if (registers[inst.IType.rs] >= 0 )
+                  instrBuffer.targetAddr =
+                     (unsigned *)(((signed short)inst.IType.imm<<2)
+                                  + (registers[PC]+4));
+               else
+                  instrBuffer.targetAddr = (unsigned*)(registers[PC]+8);
+               break;
+         };
+         break;
+
+      case OP_J:
+      case OP_JAL:
+         instrBuffer.targetAddr =
+            (unsigned *)((inst.JType.target<<2) + ((registers[PC]+4)&0xf0000000));
+         break;
+
+      case OP_BEQ:
+      case OP_BEQL:
+         if (registers[inst.IType.rs] == registers[inst.IType.rt])
             instrBuffer.targetAddr =
-              (unsigned *)(((signed short)inst.IType.imm<<2)
-                                        + (registers[PC]+4));
-          else
+               (unsigned *)(((signed short)inst.IType.imm<<2) + (registers[PC]+4));
+         else
             instrBuffer.targetAddr = (unsigned*)(registers[PC]+8);
-          break;
-        case OP_BGEZ:
-        case OP_BGEZL:
-        case OP_BGEZAL:
-        case OP_BGEZALL:
-          if (registers[inst.IType.rs] >= 0 )
+         break;
+      case OP_BNE:
+      case OP_BNEL:
+         if (registers[inst.IType.rs] != registers[inst.IType.rt])
             instrBuffer.targetAddr =
-              (unsigned *)(((signed short)inst.IType.imm<<2)
-                                        + (registers[PC]+4));
-          else
+               (unsigned *)(((signed short)inst.IType.imm<<2) + (registers[PC]+4));
+         else
             instrBuffer.targetAddr = (unsigned*)(registers[PC]+8);
-          break;
-      };
-      break;
-
-    case OP_J:
-    case OP_JAL:
-      instrBuffer.targetAddr =
-        (unsigned *)((inst.JType.target<<2) + ((registers[PC]+4)&0xf0000000));
-      break;
-
-    case OP_BEQ:
-    case OP_BEQL:
-      if (registers[inst.IType.rs] == registers[inst.IType.rt])
-        instrBuffer.targetAddr =
-          (unsigned *)(((signed short)inst.IType.imm<<2) + (registers[PC]+4));
-      else
-        instrBuffer.targetAddr = (unsigned*)(registers[PC]+8);
-      break;
-    case OP_BNE:
-    case OP_BNEL:
-      if (registers[inst.IType.rs] != registers[inst.IType.rt])
-        instrBuffer.targetAddr =
-          (unsigned *)(((signed short)inst.IType.imm<<2) + (registers[PC]+4));
-      else
-        instrBuffer.targetAddr = (unsigned*)(registers[PC]+8);
-      break;
-    case OP_BLEZ:
-    case OP_BLEZL:
-      if (registers[inst.IType.rs] <= 0)
-        instrBuffer.targetAddr =
-          (unsigned *)(((signed short)inst.IType.imm<<2) + (registers[PC]+4));
-      else
-        instrBuffer.targetAddr = (unsigned*)(registers[PC]+8);
-      break;
-    case OP_BGTZ:
-    case OP_BGTZL:
-      if (registers[inst.IType.rs] > 0)
-        instrBuffer.targetAddr =
-          (unsigned *)(((signed short)inst.IType.imm<<2) + (registers[PC]+4));
-      else
-        instrBuffer.targetAddr = (unsigned*)(registers[PC]+8);
-      break;
-
-    case OP_COP1:
-      if (inst.RType.rs == OP_BC)
-          switch (inst.RType.rt) {
-            case COPz_BCF:
-            case COPz_BCFL:
-              if (registers[FCSR] & CSR_C)
-                instrBuffer.targetAddr = (unsigned*)(registers[PC]+8);
-              else
-                instrBuffer.targetAddr =
-                  (unsigned *)(((signed short)inst.IType.imm<<2)
-                                            + (registers[PC]+4));
-              break;
-            case COPz_BCT:
-            case COPz_BCTL:
-              if (registers[FCSR] & CSR_C)
-                instrBuffer.targetAddr =
-                  (unsigned *)(((signed short)inst.IType.imm<<2)
-                                            + (registers[PC]+4));
-              else
-                instrBuffer.targetAddr = (unsigned*)(registers[PC]+8);
-              break;
-          };
-      break;
-  }
+         break;
+      case OP_BLEZ:
+      case OP_BLEZL:
+         if (registers[inst.IType.rs] <= 0)
+            instrBuffer.targetAddr =
+               (unsigned *)(((signed short)inst.IType.imm<<2) + (registers[PC]+4));
+         else
+            instrBuffer.targetAddr = (unsigned*)(registers[PC]+8);
+         break;
+      case OP_BGTZ:
+      case OP_BGTZL:
+         if (registers[inst.IType.rs] > 0)
+            instrBuffer.targetAddr =
+               (unsigned *)(((signed short)inst.IType.imm<<2) + (registers[PC]+4));
+         else
+            instrBuffer.targetAddr = (unsigned*)(registers[PC]+8);
+         break;
+
+      case OP_COP1:
+         if (inst.RType.rs == OP_BC)
+            switch (inst.RType.rt) {
+               case COPz_BCF:
+               case COPz_BCFL:
+                  if (registers[FCSR] & CSR_C)
+                     instrBuffer.targetAddr = (unsigned*)(registers[PC]+8);
+                  else
+                     instrBuffer.targetAddr =
+                        (unsigned *)(((signed short)inst.IType.imm<<2)
+                                     + (registers[PC]+4));
+                  break;
+               case COPz_BCT:
+               case COPz_BCTL:
+                  if (registers[FCSR] & CSR_C)
+                     instrBuffer.targetAddr =
+                        (unsigned *)(((signed short)inst.IType.imm<<2)
+                                     + (registers[PC]+4));
+                  else
+                     instrBuffer.targetAddr = (unsigned*)(registers[PC]+8);
+                  break;
+            };
+         break;
+   }
 
-  if is_steppable (instrBuffer.targetAddr)
-    {
+
+   if( is_steppable((unsigned)instrBuffer.targetAddr) && *(instrBuffer.targetAddr) != BREAK_INSTR )
+   {
       instrBuffer.savedInstr = *instrBuffer.targetAddr;
       *instrBuffer.targetAddr = BREAK_INSTR;
-    }
-  else
-    {
+   }
+   else
+   {
       instrBuffer.targetAddr = NULL;
       instrBuffer.savedInstr = NOP_INSTR;
-    }
-  return;
+   }
+   return;
 }
 
+
+
+
+
 
 /*
  * Translate the R4600 exception code into a Unix-compatible signal.
@@ -855,442 +884,587 @@ void gdb_stub_report_exception_info(
   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++ = ';';
+   char *optr;
+   int sigval;
+
+   optr = outBuffer;
+   *optr++ = 'T';
+   sigval = computeSignal ();
+   *optr++ = highhex (sigval);
+   *optr++ = lowhex (sigval);
+
+   *optr++ = highhex(SP); /*gdb_hexchars[SP]; */
+   *optr++ = lowhex(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++ = ';';
+   *optr++ = highhex(PC); /*gdb_hexchars[PC]; */
+   *optr++ = lowhex(PC);
+   *optr++ = ':';
+   optr    = mem2hstr(optr, (unsigned char *)&frame->epc, 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++ = ';';
-  }
+   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';
+   *optr++ = '\0';
 }
 
 
 
+
+/*
+ * Scratch frame used to retrieve contexts for different threads, so as
+ * not to disrupt our current context on the stack
+ */
+CPU_Interrupt_frame current_thread_registers;
+
+
+
 /*
  * This function handles all exceptions.  It only does two things:
  * it figures out why it was activated and tells gdb, and then it
  * 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 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;
-
-  thread = 0;
+   int          host_has_detached = 0;
+   int          regno, addr, length;
+   char         *ptr;
+   int          current_thread;  /* current generic thread */
+   int          thread;          /* stopped thread: context exception happened in */
+
+   long long    regval;
+   void         *regptr;
+   int          binary;
+   
+
+   registers = (mips_register_t *)frame;
+
+   thread = 0;
 #if defined(GDB_STUB_ENABLE_THREAD_SUPPORT)
-  if (do_threads) {
-    thread = rtems_gdb_stub_get_current_thread();
-  }
+   if (do_threads) {
+      thread = rtems_gdb_stub_get_current_thread();
+   }
 #endif
-  current_thread = thread;
+   current_thread = thread;
+
+
+   {
+      /* reapply all breakpoints regardless of how we came in */
+      struct z0break *z0, *zother;
+
+      for (zother=z0break_list; zother!=NULL; zother=zother->next) 
+      {
+         if( zother->instr == 0xffffffff )
+         {
+            /* grab the instruction */
+            zother->instr = *(zother->address);
+            /* and insert the breakpoint */
+            *(zother->address) = BREAK_INSTR;
+         }
+      }
+
+
+
+      /* see if we're coming from a breakpoint */
+      if( *((unsigned *)frame->epc) == BREAK_INSTR )
+      {
+         /* see if its one of our zbreaks */
+         for (z0=z0break_list; z0!=NULL; z0=z0->next) 
+         {
+            if( (unsigned)z0->address == frame->epc) 
+               break;
+         }
+         if( z0 )
+         {
+            /* restore the original instruction */
+            *(z0->address) = z0->instr;
+            /* flag the breakpoint */
+            z0->instr = 0xffffffff;
+
+            /* 
+               now when we return, we'll execute the original code in
+               the original state.  This leaves our breakpoint inactive
+               since the break instruction isn't there, but we'll reapply
+               it the next time we come in via step or breakpoint
+            */
+         }
+         else
+         {
+            /* not a zbreak, see if its our trusty stepping code */
+
+            /*
+             * Restore the saved instruction at
+             * the single-step target address.
+             */
+            undoSStep();
+         }
+      }
+   }
+
 
-  /* reply to host that an exception has occurred with some basic info */
-  gdb_stub_report_exception_info(vector, frame, thread);
 
 
-  /*
-   * Restore the saved instruction at
-   * the single-step target address.
-   */
-  undoSStep ();
 
-  while (!(host_has_detached)) {
+   /* reply to host that an exception has occurred with some basic info */
+   gdb_stub_report_exception_info(vector, frame, thread);
+   putpacket (outBuffer);
+
+
+
+   while (!(host_has_detached)) {
       outBuffer[0] = '\0';
       getpacket (inBuffer);
       binary = 0;
 
       switch (inBuffer[0]) {
-        case '?':
-          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;
+         case '?':
+            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;
+            if (do_threads && current_thread != thread )
+               regptr = &current_thread_registers;
 #endif
-          mem2hex (regptr, NUM_REGS * (sizeof registers), outBuffer);
+            mem2hex (regptr, NUM_REGS * (sizeof registers), outBuffer);
+            break;
 
-          break;
 
-        case 'G':       /* set the values of the CPU registers - return OK */
-          regptr = registers;
+         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;
+            if (do_threads && current_thread != thread )
+               regptr = &current_thread_registers;
 #endif
-          if (hex2mem (&inBuffer[1], regptr, NUM_REGS * (sizeof registers)))
-              strcpy (outBuffer, "OK");
-          else
-              strcpy (outBuffer, "E00"); /* E00 = bad "set register" command */
-          break;
-
-        case 'P':
-          /* Pn...=r...  Write register n... with value r... - return OK */
-          ptr = &inBuffer[1];
-          if (hexToInt(&ptr, &regno) &&
-              *ptr++ == '=' &&
-              hexToLongLong(&ptr, &regval))
+            if (hex2mem (&inBuffer[1], regptr, NUM_REGS * (sizeof registers)))
+               strcpy (outBuffer, "OK");
+            else
+               strcpy (outBuffer, "E00"); /* E00 = bad "set register" command */
+            break;
+
+
+         case 'P':
+            /* Pn...=r...  Write register n... with value r... - return OK */
+            ptr = &inBuffer[1];
+            if (hexToInt(&ptr, &regno) &&
+                *ptr++ == '=' &&
+                hexToLongLong(&ptr, &regval))
             {
-              registers[regno] = regval;
-              strcpy (outBuffer, "OK");
+               registers[regno] = regval;
+               strcpy (outBuffer, "OK");
             }
-          else
-              strcpy (outBuffer, "E00"); /* E00 = bad "set register" command */
-          break;
-
-        case 'm':
-          /* mAA..AA,LLLL  Read LLLL bytes at address AA..AA */
-          ptr = &inBuffer[1];
-          if (hexToInt (&ptr, &addr)
-              && *ptr++ == ','
-              && hexToInt (&ptr, &length)
-              && is_readable (addr, length)
-              && (length < (BUFMAX - 4)/2))
-            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 */
-          ptr = &inBuffer[1];
-          if (hexToInt (&ptr, &addr)
-              && *ptr++ == ','
-              && hexToInt (&ptr, &length)
-              && *ptr++ == ':'
-              && 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 */
-          break;
-
-        case 'c':
-          /* cAA..AA    Continue at address AA..AA(optional) */
-        case 's':
-          /* sAA..AA    Step one instruction from AA..AA(optional) */
-          {
+            else
+               strcpy (outBuffer, "E00"); /* E00 = bad "set register" command */
+            break;
+
+
+         case 'm':
+            /* mAA..AA,LLLL  Read LLLL bytes at address AA..AA */
+            ptr = &inBuffer[1];
+            if (hexToInt (&ptr, &addr)
+                && *ptr++ == ','
+                && hexToInt (&ptr, &length)
+                && is_readable (addr, length)
+                && (length < (BUFMAX - 4)/2))
+               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 */
+            ptr = &inBuffer[1];
+            if (hexToInt (&ptr, &addr)
+                && *ptr++ == ','
+                && hexToInt (&ptr, &length)
+                && *ptr++ == ':'
+                && 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 */
+            break;
+
+
+
+         case 'c':
+            /* cAA..AA    Continue at address AA..AA(optional) */
+         case 's':
+            /* sAA..AA    Step one instruction from AA..AA(optional) */
+         {
             /* try to read optional parameter, pc unchanged if no parm */
             ptr = &inBuffer[1];
             if (hexToInt (&ptr, &addr))
-              registers[PC] = addr;
+               registers[PC] = addr;
 
             if (inBuffer[0] == 's')
-              doSStep ();
-          }
-          return;
+               doSStep ();
+         }
+         goto stubexit;
 
-        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;
+         case 'k':  /* remove all zbreaks if any */
+        dumpzbreaks:
+         {
+            {
+               /* Unlink the entire list */
+               struct z0break *z0, *znxt;
+
+               while( (z0= z0break_list) )
+               {
+
+                  /* put back the instruction */
+                  if( z0->instr != 0xffffffff )
+                     *(z0->address) = z0->instr;
+
+                  /* pop off the top entry */
+                  znxt = z0->next;
+                  if( znxt ) znxt->prev = NULL;
+                  z0break_list = znxt;
+
+                  /* and put it on the free list */
+                  z0->prev = NULL;
+                  z0->next = z0break_avail;
+                  z0break_avail = z0;
+               }
             }
-          }
 
-          break;
+            strcpy(outBuffer, "OK");
+         }
+         break;
 
-        case 'q':   /* queries */
+
+
+
+
+         case 'q':   /* queries */
 #if defined(GDB_STUB_ENABLE_THREAD_SUPPORT)
-          rtems_gdb_process_query( inBuffer, outBuffer, do_threads, thread );
+            rtems_gdb_process_query( inBuffer, outBuffer, do_threads, thread );
+#endif
+            break;
+
+#if defined(GDB_STUB_ENABLE_THREAD_SUPPORT)
+         case 'T':
+         {
+            int testThread;
+
+            if( vhstr2thread(&inBuffer[1], &testThread) == NULL ) 
+            {
+               strcpy(outBuffer, "E01");
+               break;
+            }
+
+            if( rtems_gdb_index_to_stub_id(testThread) == NULL )
+            {
+               strcpy(outBuffer, "E02");
+            }
+            else
+            {
+               strcpy(outBuffer, "OK");
+            }
+         }
+         break;
 #endif
-          break;
         
-        case 'H':  /* set new thread */
+         case 'H':  /* set new thread */
 #if defined(GDB_STUB_ENABLE_THREAD_SUPPORT)
-	  if (inBuffer[1] != 'g') {
-	    break;
-	  }
+            if (inBuffer[1] != 'g') {
+               break;
+            }
 	  
-	  if (!do_threads) {
-	    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;
-		}
-	    }
+            {
+               int tmp, ret;
 	    
-	    current_thread = tmp;
-	    strcpy(outBuffer, "OK");
-	  }
+               /* 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;
+            break;
 
-        case 'Z':  /* Add breakpoint */
-          { 
+
+
+
+         case 'Z':  /* Add breakpoint */
+         { 
             int ret, type, len;
-            unsigned char *address;
+            unsigned *address;
             struct z0break *z0;
 
             ret = parse_zbreak(inBuffer, &type, &address, &len);
             if (!ret) {
-              strcpy(outBuffer, "E01");
-              break;
+               strcpy(outBuffer, "E01");
+               break;
             }
 
             if (type != 0) {
-              /* We support only software break points so far */
-              break;
+               /* We support only software break points so far */
+               strcpy(outBuffer, "E02");
+               break;
             }
 
-            if (len != 1) {
-               strcpy(outBuffer, "E02");
+            if (len != R_SZ) {     /* was 1 */
+               strcpy(outBuffer, "E03");
                break;
             }
 
             /* Let us check whether this break point already set */
             for (z0=z0break_list; z0!=NULL; z0=z0->next) {
-                if (z0->address == address) {
+               if (z0->address == address) {
                   break;
-                }
+               }
             }
 
             if (z0 != NULL) {
-              /* Repeated packet */
-              strcpy(outBuffer, "OK");
-              break;
+               /* we already have a breakpoint for this address */
+               strcpy(outBuffer, "E04");
+               break;
             }
 
             /* Let us allocate new break point */
             if (z0break_avail == NULL) {
-              strcpy(outBuffer, "E03");
-              break;
+               strcpy(outBuffer, "E05");
+               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;
-            }
+            /*
+            *if (mem2hstr(z0->buf, address, 1) == NULL ||
+              !hstr2mem(address, "cc" , 1)) { 
+
+               * Memory error *
+               z0->next = z0break_avail;
+               z0break_avail = z0;
+               strcpy(outBuffer, "E05");
+               break;
+            }*/
 
             /* Fill it */
             z0->address = address;
 
+            if( z0->address == frame->epc )
+            {
+               /* re-asserting the breakpoint that put us in here, so
+               we'll add the breakpoint but leave the code in place
+               since we'll be returning to it when the user continues */
+               z0->instr = 0xffffffff;
+            }
+            else
+            {
+               /* grab the instruction */
+               z0->instr = *(z0->address);
+               /* and insert the break */
+               *(z0->address) = BREAK_INSTR;
+            }
+
             /* Add to the list */
-            z0->next = z0break_list;
-            z0->prev = NULL;
+            {
+               struct z0break *znxt = z0break_list;
 
-            if (z0break_list != NULL) {
-              z0break_list->prev = z0;
+               z0->prev = NULL;
+               z0->next = znxt;
+               
+               if( znxt ) znxt->prev = z0;
+               z0break_list = z0;
             }
 
-            z0break_list = z0;
-
             strcpy(outBuffer, "OK");
-          }
+         }
+         break;
 
-        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;
+         case 'z': /* remove breakpoint */
+            if (inBuffer[1] == 'z') 
+            {
+               goto dumpzbreaks;
+               
+
+               /*
+                * zz packet - remove all breaks *
                 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 */
+
+                for (z0=z0break_list; z0!=NULL; z0=z0->next) 
+                {
+                if(!hstr2mem(z0->address, z0->buf, R_SZ)) 
+                {
+                ret = 0;
+                }
+                z0last = z0;
+                }
+
+                * Free entries if any *
                 if (z0last != NULL) {
-                    z0last->next  = z0break_avail;
-                    z0break_avail = z0break_list;
-                    z0break_list  = NULL;
-                  }
+                z0last->next  = z0break_avail;
+                z0break_avail = z0break_list;
+                z0break_list  = NULL;
+                }
 
                 if (ret) {
-	          strcpy(outBuffer, "OK");
+                strcpy(outBuffer, "OK");
                 } else {
-	          strcpy(outBuffer, "E04");
-	        }
-
+                strcpy(outBuffer, "E04");
+                }
                 break;
-              }
+               */
+            }
+            else
+            {
+               int ret, type, len;
+               unsigned   *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;
-	    }
+               ret = parse_zbreak(inBuffer, &type, &address, &len);
+               if (!ret) {
+                  strcpy(outBuffer, "E01");
+                  break;
+               }
 
-            if (len != 1) {
-              strcpy(outBuffer, "E02");
-              break;
-            }
+               if (type != 0) {
+                  /* We support only software break points so far */
+                  break;
+               }
+
+               if (len != R_SZ) {
+                  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;
-	      }
-	    }
+               /* 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 (z0 == NULL) {
+                  /* Unknown breakpoint */
+                  strcpy(outBuffer, "E03");
+                  break;
+               }
 	    
-	    if (!hstr2mem(z0->address, z0->buf, 1)) {
-	      strcpy(outBuffer, "E04");
-	      break;
-	    }
+               /*
+               if (!hstr2mem(z0->address, z0->buf, R_SZ)) {
+                  strcpy(outBuffer, "E04");
+                  break;
+                  }*/
+
+               if( z0->instr != 0xffffffff )
+               {
+                  /* put the old instruction back  */
+                  *(z0->address) = z0->instr;
+               }
    
-	    /* 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");
-	 }
+               /* Unlink entry */
+               {
+                  struct z0break *zprv = z0->prev, *znxt = z0->next;
+
+                  if( zprv ) zprv->next = znxt;
+                  if( znxt ) znxt->prev = zprv;
+
+                  if( !zprv ) z0break_list = znxt;
 
-          break;
-        default: /* do nothing */
-          break;
+                  znxt = z0break_avail;
+
+                  z0break_avail = z0;
+                  z0->prev = NULL;
+                  z0->next = znxt;
+               }
+    
+               strcpy(outBuffer, "OK");
+            }
+            break;
+
+
+         default: /* do nothing */
+            break;
       }
 
       /* reply to the request */
       putpacket (outBuffer);
-    }
+   }
+
+  stubexit:
 
    /*
     *  The original code did this in the assembly wrapper.  We should consider
@@ -1299,15 +1473,115 @@ void handle_exception (rtems_vector_number vector, CPU_Interrupt_frame *frame)
     *  On exit from the exception handler invalidate each line in the I-cache
     *  and write back each dirty line in the D-cache.  This needs to be done
     *  before the target program is resumed in order to ensure that software
-    *  breakpoints and downloaded code will actually take effect.
+    *  breakpoints and downloaded code will actually take effect.  This
+    *  is because modifications to code in ram will affect the D-cache,
+    *  but not necessarily the I-cache.
     */
 
-  return;
+   {
+      extern void clear_cache();
+      clear_cache();
+   }
+
+   return;
 }
 
-static char initialized;   /* 0 means we are not initialized */
 
-void mips_gdb_stub_install(void) 
+
+
+
+
+
+
+
+
+static int numsegs;
+static struct memseg   memsegments[NUM_MEMSEGS];
+
+int gdbstub_add_memsegment( unsigned base, unsigned end, int opts )
+{
+   if( numsegs == NUM_MEMSEGS ) return -1;
+
+   memsegments[numsegs].begin = base;
+   memsegments[numsegs].end   = end;
+   memsegments[numsegs].opts   = opts;
+
+   ++numsegs;
+   return RTEMS_SUCCESSFUL;
+}
+
+
+
+
+static int is_readable(unsigned ptr, unsigned len)
+{
+   struct memseg *ms;
+   int i;
+
+   if( (ptr & 0x3) ) return -1;
+
+   for(i=0; i<numsegs; i++)
+   {
+      ms= &memsegments[i];
+
+      if( ms->begin <= ptr && ptr+len <= ms->end && (ms->opts & MEMOPT_READABLE) )
+         return -1;
+   }
+   return 0;
+}
+
+
+static int is_writeable(unsigned ptr, unsigned len)
+{
+   struct memseg *ms;
+   int i;
+
+   if( (ptr & 0x3) ) return -1;
+
+   for(i=0; i<numsegs; i++)
+   {
+      ms= &memsegments[i];
+
+      if( ms->begin <= ptr && ptr+len <= ms->end && (ms->opts & MEMOPT_WRITEABLE) )
+         return -1;
+   }
+   return 0;
+}
+
+
+static int is_steppable(unsigned ptr)
+{
+   struct memseg *ms;
+   int i;
+
+   if( (ptr & 0x3) ) return -1;
+
+   for(i=0; i<numsegs; i++)
+   {
+      ms= &memsegments[i];
+
+      if( ms->begin <= ptr && ptr <= ms->end && (ms->opts & MEMOPT_WRITEABLE) )
+         return -1;
+   }
+   return 0;
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+static char initialized = 0;   /* 0 means we are not initialized */
+
+void mips_gdb_stub_install(int enableThreads) 
 {
    /*
      These are the RTEMS-defined vectors for all the MIPS exceptions
@@ -1334,20 +1608,37 @@ void mips_gdb_stub_install(void)
    int  i;
    rtems_isr_entry old;
 
-   if (initialized) {
+   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];
-   }
+   memset( memsegments,0,sizeof(struct memseg)*NUM_MEMSEGS );
+   numsegs = 0;
 
-   z0break_arr[i].next = NULL;
-   z0break_avail       = &z0break_arr[0];
-   z0break_list        = NULL;
+#if defined(GDB_STUB_ENABLE_THREAD_SUPPORT)
+   if( enableThreads )
+      do_threads = 1;
+   else
+      do_threads = 0;
+#endif
+
+   {
+      struct z0break *z0;
 
+      z0break_avail = NULL;
+      z0break_list  = NULL;
+   
+      /* z0breaks list init, now we'll do it so it makes sense... */
+      for (i=0; i<BREAKNUM; i++) 
+      {
+         memset( (z0= &z0break_arr[i]), 0, sizeof(struct z0break));
+
+         z0->next = z0break_avail;
+         z0break_avail = z0;
+      }
+   }     
 
    for(i=0; exceptionVector[i] > -1; i++)
    {
@@ -1355,8 +1646,9 @@ void mips_gdb_stub_install(void)
    }
 
    initialized = 1;
+
    /* get the attention of gdb */
-   mips_break(1);
+   /* mips_break(1);  disabled so user code can choose to invoke it or not */
 }
 
 
diff --git a/c/src/lib/libbsp/mips/shared/gdbstub/rtems-stub-glue.c b/c/src/lib/libbsp/mips/shared/gdbstub/rtems-stub-glue.c
index 6c525bb2a0..8d17911abf 100644
--- a/c/src/lib/libbsp/mips/shared/gdbstub/rtems-stub-glue.c
+++ b/c/src/lib/libbsp/mips/shared/gdbstub/rtems-stub-glue.c
@@ -29,7 +29,6 @@
  */
 
 #include <rtems.h>
-
 #include <string.h>
 
 #include "gdb_if.h"
@@ -40,6 +39,7 @@
 
 extern const char gdb_hexchars[];
 
+
 /*
  *  Prototypes for CPU dependent routines that are conditional
  *  at the bottom of this file.
@@ -50,6 +50,12 @@ void rtems_gdb_stub_get_registers_from_context(
   Thread_Control *th
 );
 
+
+
+
+
+
+
 /* Check whether it is OK to enable thread support */
 int rtems_gdb_stub_thread_support_ok(void)
 {
@@ -59,6 +65,10 @@ int rtems_gdb_stub_thread_support_ok(void)
   return 0;
 }
 
+
+
+
+
 /*
  *  rtems_gdb_stub_id_to_index
  *
@@ -101,12 +111,84 @@ int rtems_gdb_stub_id_to_index(
   return first_posix_id + (thread_obj_id - min_id);
 } 
 
+
+
+/* Return the RTEMS thread id from a gdb thread id */
+Thread_Control *rtems_gdb_index_to_stub_id(
+  int thread
+)
+{
+   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 NULL;
+   }
+
+   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 NULL;
+   }
+
+   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 NULL;
+   }
+
+  found:
+   return th;
+}
+
+
+
+
+
+
 /* 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)
@@ -176,6 +258,10 @@ int rtems_gdb_stub_get_next_thread(int athread)
 }
 
 
+
+
+
+
 /* Get thread registers, return 0 if thread does not 
    exist, and 1 otherwise */
 int rtems_gdb_stub_get_thread_regs(
@@ -183,70 +269,27 @@ int rtems_gdb_stub_get_thread_regs(
   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);
+   Thread_Control *th;
 
-  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;
+   th= rtems_gdb_index_to_stub_id(thread);
 
-  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;
+   if( th )
+   {
+      rtems_gdb_stub_get_registers_from_context( registers, th );
+      return 1;
+   }
+   return 0;
+}
 
-  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
@@ -260,6 +303,10 @@ int rtems_gdb_stub_set_thread_regs(
 }
 
 
+
+
+
+
 /* Get thread information, return 0 if thread does not 
    exist and 1 otherwise */
 int rtems_gdb_stub_get_thread_info(
@@ -267,122 +314,126 @@ int rtems_gdb_stub_get_thread_info(
   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];
+   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);
+   ASSERT(info != NULL);
 
-  if (thread <= 0) {
-    return 0;
-  }
+   if (thread <= 0) {
+      return 0;
+   }
 
-  if (_System_state_Get() != SYSTEM_STATE_UP || thread == 1) {
-    /* We have one thread let us use value
+   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 */
+      strcpy(info->display, "idle thread");
+      strcpy(info->name, "IDLE");
+      info->more_display[0] = 0; /* Nothing */
 
-    return 1;
-  }
+      return 1;
+   }
 
-  /* Let us get object associtated with current thread */
-  thread_obj_id = _Thread_Executing->Object.id;
+   /* 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;
+   /* Let us figure out thread_id for gdb */
+   first_rtems_id = 2;
   
-  obj_info = _Objects_Information_table[OBJECTS_RTEMS_TASKS];
+   obj_info = _Objects_Information_table[OBJECTS_RTEMS_TASKS];
 
-  min_id = obj_info->minimum_id;
-  max_id = obj_info->maximum_id;
+   min_id = obj_info->minimum_id;
+   max_id = obj_info->maximum_id;
 
-  if (thread <= (first_rtems_id + (max_id - min_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;
-  }
+      if (th == NULL) {
+         /* Thread does not exist */
+         return 0;
+      }
 
-  strcpy(info->display, "rtems task:   control at 0x");
+      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;
+      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);
+      strcat(info->display, tmp_buf);
   
-  name = *(unsigned32 *)(obj_info->local_table[thread]->name);
+      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->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 */
+      info->more_display[0] = 0; /* Nothing */
 
-  return 1;
-}
+      return 1;
+   }
 
-  first_posix_id = first_rtems_id + (max_id - min_id) + 1;
+   first_posix_id = first_rtems_id + (max_id - min_id) + 1;
 
-  obj_info = _Objects_Information_table[OBJECTS_POSIX_THREADS];
+   obj_info = _Objects_Information_table[OBJECTS_POSIX_THREADS];
 
-  min_id = obj_info->minimum_id;
-  max_id = obj_info->maximum_id;
+   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)
-    {
+   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");
+   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;
+   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);
+   strcat(info->display, tmp_buf);
 
-  name = *(unsigned32 *)(obj_info->local_table[thread - 
-                                               first_posix_id + 1]->name);
+   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->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 */
+   info->more_display[0] = 0; /* Nothing */
   
-  return 1;
+   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)
 {
@@ -597,27 +648,36 @@ pack_qm_thread(char *out, int thread)
 static void
 pack_qm_header(char *out, int count, int done, int athread)
 {
-  ASSERT(out != 0);
-  ASSERT(count >= 0 && count < 256);
+   ASSERT(out != 0);
+   ASSERT(count >= 0 && count < 256);
 
-  *out++ = 'q';
-  *out++ = 'M';
+   *out++ = 'q';
+   *out++ = 'M';
 
-  *out++ = gdb_hexchars[(count >> 4) & 0x0f];
-  *out++ = gdb_hexchars[count & 0x0f];
-
-  if (done) {
-    *out++ = '1';
-  } else {
-    *out++ = '0';
-  }
+   *out++ = gdb_hexchars[(count >> 4) & 0x0f];
+   *out++ = gdb_hexchars[count & 0x0f];
 
-  thread2fhstr(out, athread);
+   if (done) {
+      *out++ = '1';
+   } else {
+      *out++ = '0';
+   }
 
-  return;
+   thread2fhstr(out, athread);
+   return;
 }
 
 
+
+
+
+
+
+
+
+
+
+
 void rtems_gdb_process_query( 
   char *inbuffer,
   char *outbuffer,
@@ -642,6 +702,9 @@ void rtems_gdb_process_query(
       *optr   = 0;
       break;
 
+
+
+
     case 'P':
       /* Thread info query */
       if (!do_threads) {
@@ -669,10 +732,14 @@ void rtems_gdb_process_query(
         /* Build response */
         pack_qq(outbuffer, mask, rthread, &info);
       }
-
       break;
+
+
+
+
+
     case 'L':
-      /* Thread info query */
+      /* Thread list query */
       if (!do_threads) {
         break;
       }
@@ -724,9 +791,14 @@ void rtems_gdb_process_query(
 
         /* Fill header */
         pack_qm_header(outbuffer, i, done, athread);
-
       }
       break;
+
+
+
+
+
+
     default:
       if (memcmp(inbuffer, "qOffsets", 8) == 0) {
         unsigned char *t, *d, *b;
@@ -772,9 +844,13 @@ void rtems_gdb_process_query(
       /*  qCRC, qRcmd, qu and qz will be added here */
       break;
     }
-
 }
 
+
+
+
+
+
 /* Present thread in the variable length string format */
 char*
 thread2vhstr(char *buf, int thread)
@@ -1196,6 +1272,7 @@ set_mem_err (void)
    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)
 {
@@ -1210,6 +1287,12 @@ set_byte (unsigned char *addr, int val)
 
 
 
+
+
+
+
+
+
 /*
  *  From here down, the code is CPU model specific and generally maps
  *  the RTEMS thread context format to gdb's.
@@ -1273,24 +1356,53 @@ int rtems_gdb_stub_get_offsets(
   return 1;
 }
 
+
+
+
+
 #elif defined(__mips__)
+
+
 void rtems_gdb_stub_get_registers_from_context( 
   int            *registers,
   Thread_Control *th
 )
 {
+   registers[S0] = (unsigned)th->Registers.s0;
+   registers[S1] = (unsigned)th->Registers.s1;
+   registers[S2] = (unsigned)th->Registers.s2;
+   registers[S3] = (unsigned)th->Registers.s3;
+   registers[S4] = (unsigned)th->Registers.s4;
+   registers[S5] = (unsigned)th->Registers.s5;
+   registers[S6] = (unsigned)th->Registers.s6;
+   registers[S7] = (unsigned)th->Registers.s7;
+
+   registers[SP] = (unsigned)th->Registers.sp;
+   registers[RA] = (unsigned)th->Registers.ra;
+
+   registers[SR] = (unsigned)th->Registers.c0_sr;
+   registers[PC] = (unsigned)th->Registers.c0_epc;
 }
 
+
 int rtems_gdb_stub_get_offsets(
   unsigned char **text_addr,
   unsigned char **data_addr, 
   unsigned char **bss_addr
 )
-{
+{ 
+/*
+  extern unsigned32 _ftext;
+  extern unsigned32 _fdata;
+  extern unsigned32 _bss_start;
+
+  *text_addr = &_ftext;
+  *data_addr = &_fdata;
+  *bss_addr  = &_bss_start;
+*/
   *text_addr = 0;
   *data_addr = 0;
   *bss_addr  = 0;
-
   return 1;
 }
 
diff --git a/c/src/lib/libbsp/shared/gdbstub/rtems-stub-glue.c b/c/src/lib/libbsp/shared/gdbstub/rtems-stub-glue.c
index 6c525bb2a0..8d17911abf 100644
--- a/c/src/lib/libbsp/shared/gdbstub/rtems-stub-glue.c
+++ b/c/src/lib/libbsp/shared/gdbstub/rtems-stub-glue.c
@@ -29,7 +29,6 @@
  */
 
 #include <rtems.h>
-
 #include <string.h>
 
 #include "gdb_if.h"
@@ -40,6 +39,7 @@
 
 extern const char gdb_hexchars[];
 
+
 /*
  *  Prototypes for CPU dependent routines that are conditional
  *  at the bottom of this file.
@@ -50,6 +50,12 @@ void rtems_gdb_stub_get_registers_from_context(
   Thread_Control *th
 );
 
+
+
+
+
+
+
 /* Check whether it is OK to enable thread support */
 int rtems_gdb_stub_thread_support_ok(void)
 {
@@ -59,6 +65,10 @@ int rtems_gdb_stub_thread_support_ok(void)
   return 0;
 }
 
+
+
+
+
 /*
  *  rtems_gdb_stub_id_to_index
  *
@@ -101,12 +111,84 @@ int rtems_gdb_stub_id_to_index(
   return first_posix_id + (thread_obj_id - min_id);
 } 
 
+
+
+/* Return the RTEMS thread id from a gdb thread id */
+Thread_Control *rtems_gdb_index_to_stub_id(
+  int thread
+)
+{
+   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 NULL;
+   }
+
+   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 NULL;
+   }
+
+   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 NULL;
+   }
+
+  found:
+   return th;
+}
+
+
+
+
+
+
 /* 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)
@@ -176,6 +258,10 @@ int rtems_gdb_stub_get_next_thread(int athread)
 }
 
 
+
+
+
+
 /* Get thread registers, return 0 if thread does not 
    exist, and 1 otherwise */
 int rtems_gdb_stub_get_thread_regs(
@@ -183,70 +269,27 @@ int rtems_gdb_stub_get_thread_regs(
   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);
+   Thread_Control *th;
 
-  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;
+   th= rtems_gdb_index_to_stub_id(thread);
 
-  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;
+   if( th )
+   {
+      rtems_gdb_stub_get_registers_from_context( registers, th );
+      return 1;
+   }
+   return 0;
+}
 
-  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
@@ -260,6 +303,10 @@ int rtems_gdb_stub_set_thread_regs(
 }
 
 
+
+
+
+
 /* Get thread information, return 0 if thread does not 
    exist and 1 otherwise */
 int rtems_gdb_stub_get_thread_info(
@@ -267,122 +314,126 @@ int rtems_gdb_stub_get_thread_info(
   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];
+   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);
+   ASSERT(info != NULL);
 
-  if (thread <= 0) {
-    return 0;
-  }
+   if (thread <= 0) {
+      return 0;
+   }
 
-  if (_System_state_Get() != SYSTEM_STATE_UP || thread == 1) {
-    /* We have one thread let us use value
+   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 */
+      strcpy(info->display, "idle thread");
+      strcpy(info->name, "IDLE");
+      info->more_display[0] = 0; /* Nothing */
 
-    return 1;
-  }
+      return 1;
+   }
 
-  /* Let us get object associtated with current thread */
-  thread_obj_id = _Thread_Executing->Object.id;
+   /* 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;
+   /* Let us figure out thread_id for gdb */
+   first_rtems_id = 2;
   
-  obj_info = _Objects_Information_table[OBJECTS_RTEMS_TASKS];
+   obj_info = _Objects_Information_table[OBJECTS_RTEMS_TASKS];
 
-  min_id = obj_info->minimum_id;
-  max_id = obj_info->maximum_id;
+   min_id = obj_info->minimum_id;
+   max_id = obj_info->maximum_id;
 
-  if (thread <= (first_rtems_id + (max_id - min_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;
-  }
+      if (th == NULL) {
+         /* Thread does not exist */
+         return 0;
+      }
 
-  strcpy(info->display, "rtems task:   control at 0x");
+      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;
+      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);
+      strcat(info->display, tmp_buf);
   
-  name = *(unsigned32 *)(obj_info->local_table[thread]->name);
+      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->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 */
+      info->more_display[0] = 0; /* Nothing */
 
-  return 1;
-}
+      return 1;
+   }
 
-  first_posix_id = first_rtems_id + (max_id - min_id) + 1;
+   first_posix_id = first_rtems_id + (max_id - min_id) + 1;
 
-  obj_info = _Objects_Information_table[OBJECTS_POSIX_THREADS];
+   obj_info = _Objects_Information_table[OBJECTS_POSIX_THREADS];
 
-  min_id = obj_info->minimum_id;
-  max_id = obj_info->maximum_id;
+   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)
-    {
+   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");
+   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;
+   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);
+   strcat(info->display, tmp_buf);
 
-  name = *(unsigned32 *)(obj_info->local_table[thread - 
-                                               first_posix_id + 1]->name);
+   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->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 */
+   info->more_display[0] = 0; /* Nothing */
   
-  return 1;
+   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)
 {
@@ -597,27 +648,36 @@ pack_qm_thread(char *out, int thread)
 static void
 pack_qm_header(char *out, int count, int done, int athread)
 {
-  ASSERT(out != 0);
-  ASSERT(count >= 0 && count < 256);
+   ASSERT(out != 0);
+   ASSERT(count >= 0 && count < 256);
 
-  *out++ = 'q';
-  *out++ = 'M';
+   *out++ = 'q';
+   *out++ = 'M';
 
-  *out++ = gdb_hexchars[(count >> 4) & 0x0f];
-  *out++ = gdb_hexchars[count & 0x0f];
-
-  if (done) {
-    *out++ = '1';
-  } else {
-    *out++ = '0';
-  }
+   *out++ = gdb_hexchars[(count >> 4) & 0x0f];
+   *out++ = gdb_hexchars[count & 0x0f];
 
-  thread2fhstr(out, athread);
+   if (done) {
+      *out++ = '1';
+   } else {
+      *out++ = '0';
+   }
 
-  return;
+   thread2fhstr(out, athread);
+   return;
 }
 
 
+
+
+
+
+
+
+
+
+
+
 void rtems_gdb_process_query( 
   char *inbuffer,
   char *outbuffer,
@@ -642,6 +702,9 @@ void rtems_gdb_process_query(
       *optr   = 0;
       break;
 
+
+
+
     case 'P':
       /* Thread info query */
       if (!do_threads) {
@@ -669,10 +732,14 @@ void rtems_gdb_process_query(
         /* Build response */
         pack_qq(outbuffer, mask, rthread, &info);
       }
-
       break;
+
+
+
+
+
     case 'L':
-      /* Thread info query */
+      /* Thread list query */
       if (!do_threads) {
         break;
       }
@@ -724,9 +791,14 @@ void rtems_gdb_process_query(
 
         /* Fill header */
         pack_qm_header(outbuffer, i, done, athread);
-
       }
       break;
+
+
+
+
+
+
     default:
       if (memcmp(inbuffer, "qOffsets", 8) == 0) {
         unsigned char *t, *d, *b;
@@ -772,9 +844,13 @@ void rtems_gdb_process_query(
       /*  qCRC, qRcmd, qu and qz will be added here */
       break;
     }
-
 }
 
+
+
+
+
+
 /* Present thread in the variable length string format */
 char*
 thread2vhstr(char *buf, int thread)
@@ -1196,6 +1272,7 @@ set_mem_err (void)
    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)
 {
@@ -1210,6 +1287,12 @@ set_byte (unsigned char *addr, int val)
 
 
 
+
+
+
+
+
+
 /*
  *  From here down, the code is CPU model specific and generally maps
  *  the RTEMS thread context format to gdb's.
@@ -1273,24 +1356,53 @@ int rtems_gdb_stub_get_offsets(
   return 1;
 }
 
+
+
+
+
 #elif defined(__mips__)
+
+
 void rtems_gdb_stub_get_registers_from_context( 
   int            *registers,
   Thread_Control *th
 )
 {
+   registers[S0] = (unsigned)th->Registers.s0;
+   registers[S1] = (unsigned)th->Registers.s1;
+   registers[S2] = (unsigned)th->Registers.s2;
+   registers[S3] = (unsigned)th->Registers.s3;
+   registers[S4] = (unsigned)th->Registers.s4;
+   registers[S5] = (unsigned)th->Registers.s5;
+   registers[S6] = (unsigned)th->Registers.s6;
+   registers[S7] = (unsigned)th->Registers.s7;
+
+   registers[SP] = (unsigned)th->Registers.sp;
+   registers[RA] = (unsigned)th->Registers.ra;
+
+   registers[SR] = (unsigned)th->Registers.c0_sr;
+   registers[PC] = (unsigned)th->Registers.c0_epc;
 }
 
+
 int rtems_gdb_stub_get_offsets(
   unsigned char **text_addr,
   unsigned char **data_addr, 
   unsigned char **bss_addr
 )
-{
+{ 
+/*
+  extern unsigned32 _ftext;
+  extern unsigned32 _fdata;
+  extern unsigned32 _bss_start;
+
+  *text_addr = &_ftext;
+  *data_addr = &_fdata;
+  *bss_addr  = &_bss_start;
+*/
   *text_addr = 0;
   *data_addr = 0;
   *bss_addr  = 0;
-
   return 1;
 }