val.bytes[0] = *hexMemSrc;
        }
        if (__mem_fault)
            return;

        for (i = 0;  i < len;  i++) {
            ch = val.bytes[i];
            *(hexMemDst++) = hexchars[(ch >> 4) & 0xf];
            if (__mem_fault)
                return;
            *(hexMemDst++) = hexchars[ch & 0xf];
            if (__mem_fault)
                return;
        }
        hexMemCount -= len;
        hexMemSrc += len;
    }
}

/* Convert the memory pointed to by MEM into HEX, placing result in BUF.
 * Return a pointer to the last char put in buf (NUL). In case of a memory
 * fault, return 0.
 * If MAY_FAULT is non-zero, then we will handle memory faults by returning
 * a 0 (and assume that MEM is a pointer into the user program), else we 
 * treat a fault like any other fault in the stub (and assume that MEM is
 * a pointer into the stub's memory).
 */

char *
__mem2hex (mem, buf, count, may_fault)
     char *mem;
     char *buf;
     int count;
     int may_fault;
{
  hexMemDst      = (unsigned char *) buf;
  hexMemSrc      = (unsigned char *) mem;
  hexMemCount    = count;
  may_fault_mode = may_fault;
#ifdef TARGET_HAS_HARVARD_MEMORY
  progMem = 0;
#endif
  
  if (may_fault)
    {
      if (__set_mem_fault_trap (__mem2hex_helper))
        return 0;
    }
  else
    __mem2hex_helper ();

  *hexMemDst = 0;

  return (char *) hexMemDst;
}

/* Convert the target memory identified by MEM into HEX, placing result in BUF.
 * Return a pointer to the last char put in buf (NUL). In case of a memory
 * fault, return 0.
 */

static char *
__mem2hex_safe (target_addr_t mem, char *buf, int count)
{
  hexMemDst      = (unsigned char *) buf;
  hexMemSrc      = (unsigned char *) TARGET_ADDR_TO_PTR(mem);
  hexMemCount    = count;
  may_fault_mode = 1;
#ifdef TARGET_HAS_HARVARD_MEMORY
  progMem = TARGET_ADDR_IS_PROGMEM(mem);
#endif
  
  if (__set_mem_fault_trap (__mem2hex_helper))
    return 0;

  *hexMemDst = 0;

  return (char *) hexMemDst;
}



static void
__hex2mem_helper (void)
{
    union {
        unsigned long  long_val;
        unsigned char  bytes[sizeof(long)];
    } val;
    int len, i;
    unsigned char ch = '\0';

    __mem_fault = 0;
    while (hexMemCount > 0 && *hexMemSrc) {
        if (may_fault_mode) {
            if ((hexMemCount >= sizeof(long)) &&
                (((target_register_t)hexMemDst & (sizeof(long)-1)) == 0)) {
                len = sizeof(long);
            } else if ((hexMemCount >= sizeof(short)) &&
                       (((target_register_t)hexMemDst & (sizeof(short)-1)) == 0)) {
                len = sizeof(short);
            } else {
                len = 1;
            }
        } else {
            len = 1;
        }

        for (i = 0;  i < len;  i++) {
            // Check for short data?
            ch = stubhex (*(hexMemSrc++)) << 4;
            if (__mem_fault)
                return;
            ch |= stubhex (*(hexMemSrc++));
            if (__mem_fault)
                return;
            val.bytes[i] = ch;
        }

        if (may_fault_mode) {
#ifdef TARGET_HAS_HARVARD_MEMORY
	    if (progMem)
		__write_progmem_safe (&val.bytes[0], hexMemDst, len);
	    else
#endif
            __write_mem_safe (&val.bytes[0], hexMemDst, len);
        } else
            *hexMemDst = ch;

        if (__mem_fault)
            return;
        hexMemCount -= len;
        hexMemDst += len;
    }
}

/* Convert COUNT bytes of the hex array pointed to by BUF into binary
   to be placed in MEM.  Return a pointer to the character AFTER the
   last byte written.

   If MAY_FAULT is set, we will return a non-zero value if a memory
   fault occurs (and we assume that MEM is a pointer into the user
   program). Otherwise, we will take a trap just like any other memory
   fault (and assume that MEM points into the stub's memory). */

char *
__hex2mem (buf, mem, count, may_fault)
     char *buf;
     char *mem;
     int count;
     int may_fault;
{
  hexMemSrc      = (unsigned char *) buf;
  hexMemDst      = (unsigned char *) mem;
  hexMemCount    = count;
  may_fault_mode = may_fault;
#ifdef TARGET_HAS_HARVARD_MEMORY
  progMem = 0;
#endif

  if (may_fault)
    {
      if (__set_mem_fault_trap (__hex2mem_helper))
        return 0;
    }
  else
    __hex2mem_helper ();

  return (char *) hexMemDst;
}

/* Convert COUNT bytes of the hex array pointed to by BUF into binary
   to be placed in target MEM.  Return a pointer to the character AFTER
   the last byte written.
*/
char *
__hex2mem_safe (char *buf, target_addr_t mem, int count)
{
  hexMemSrc      = (unsigned char *) buf;
  hexMemDst      = (unsigned char *) TARGET_ADDR_TO_PTR(mem);
  hexMemCount    = count;
  may_fault_mode = 1;
#ifdef TARGET_HAS_HARVARD_MEMORY
  progMem = TARGET_ADDR_IS_PROGMEM(mem);
#endif

  if (__set_mem_fault_trap (__hex2mem_helper))
    return 0;

  return (char *) hexMemDst;
}


void
set_debug_traps (void)
{
  __install_traps ();
  initialized = 1;    /* FIXME: Change this to dbg_stub_initialized */
}

/*
 * While we find nice hex chars, build an int.
 * Return number of chars processed.
 */

unsigned int
__hexToInt (char **ptr, target_register_t *intValue)
{
  int numChars = 0;
  int hexValue;

  *intValue = 0;

  while (**ptr)
    {
      hexValue = stubhex (**ptr);
      if (hexValue < 0)
        break;

      *intValue = (*intValue << 4) | hexValue;
      numChars ++;

      (*ptr)++;
    }

  return (numChars);
}

/*
 * While we find nice hex chars, build a target memory address.
 * Return number of chars processed.
 */

unsigned int
__hexToAddr (char **ptr, target_addr_t *val)
{
  int numChars = 0;
  int hexValue;

  *val = 0;

  while (**ptr)
    {
      hexValue = stubhex (**ptr);
      if (hexValue < 0)
        break;

      *val = (*val << 4) | hexValue;
      numChars ++;

      (*ptr)++;
    }

  return (numChars);
}


/* 
 * Complement of __hexToInt: take an int of size "numBits", 
 * convert it to a hex string.  Return length of (unterminated) output.
 */

unsigned int
__intToHex (char *ptr, target_register_t intValue, int numBits)
{
  int numChars = 0;

  if (intValue == 0)
    {
      *(ptr++) = '0';
      *(ptr++) = '0';
      return 2;
    }

  numBits = (numBits + 7) / 8;
  while (numBits)
    {
      int v = (intValue >> ((numBits - 1) * 8));
      if (v || (numBits == 1))
        {
          v = v & 255;
          *(ptr++) = __tohex ((v / 16) & 15);
          *(ptr++) = __tohex (v & 15);
          numChars += 2;
        }
      numBits--;
    }

  return (numChars);
}

#if DEBUG_THREADS 
/*
 * Kernel Thread Control
 *
 * If the current thread is set to other than zero (or minus one),
 * then ask the kernel to lock it's scheduler so that only that thread
 * can run.
 */

static unsigned char did_lock_scheduler = 0;
static unsigned char did_disable_interrupts = 0;

/* Pointer to "kernel call" for scheduler control */
static int (*schedlock_fn) (int, int, long) = stub_lock_scheduler;

/* Pointer to target stub call for disabling interrupts.
   Target stub will initialize this if it can.  */
int (*__disable_interrupts_hook) (int); /* don't initialize here! */
#endif

static void
lock_thread_scheduler (int kind)        /* "step" or "continue" */
{
#if DEBUG_THREADS 
  int ret = 0;

  /* GDB will signal its desire to run a single thread
     by setting _gdb_cont_thread to non-zero / non-negative.  */
  if (_gdb_cont_thread <= 0)
    return;

  if (schedlock_fn)                     /* kernel call */
    ret = (*schedlock_fn) (1, kind, _gdb_cont_thread);

  if (ret == 1)
    {
      did_lock_scheduler = 1;
      return;
    }

  if (schedlock_fn == 0 ||              /* no kernel scheduler call */
      ret == -1)                        /* kernel asks stub to handle it */
    if (__disable_interrupts_hook)      /* target stub has capability */
      if ((*__disable_interrupts_hook) (1))
        {
          did_disable_interrupts = 1;
          return;
        }
#endif /* DEBUG_THREADS */
}

static void
unlock_thread_scheduler ()
{
#if DEBUG_THREADS
  if (did_lock_scheduler)
    if (schedlock_fn)                   /* kernel call */
      {
        (*schedlock_fn) (0, 0, _gdb_cont_thread);
        /* I could check the return value, but 
           what would I do if it failed???  */
        did_lock_scheduler = 0;
      }
  if (did_disable_interrupts)
    if (__disable_interrupts_hook)      /* target stub call */
      {
        (*__disable_interrupts_hook) (0);
        /* Again, I could check the return value, but 
           what would I do if it failed???  */
        did_disable_interrupts = 0;
      }
#endif /* DEBUG_THREADS */
}

#ifdef CYGPKG_CYGMON
int processing_breakpoint_function = 0;
#endif

void
__handle_exception (void)
{
  int sigval = 0;

#ifdef TARGET_HAS_NEXT_STEP
  if (! __next_step_done ())
    {
      __clear_breakpoints ();
      __install_breakpoints ();
      __single_step ();
      return;
    }
#endif

#ifdef __ECOS__
  // We need to unpack the registers before they are accessed.
  if (__cleanup_vec != NULL)
    __cleanup_vec ();

#if defined(CYGSEM_REDBOOT_BSP_SYSCALLS)
  // Temporary support for gnupro bsp SWIs
  if (__is_bsp_syscall())
  {
      sigval = hal_syscall_handler();
      if (sigval <= 0)
      {
	  if (sigval < 0)
	      __process_exit_vec ();

	  if (__init_vec != NULL)
              __init_vec ();
	  return;
      }
  }
#endif

#ifdef CYGDBG_HAL_DEBUG_GDB_BREAK_SUPPORT
  // Special case for GDB BREAKs. This flag is set by cyg_stub_cleanup.
  if (cyg_hal_gdb_break) {
      cyg_hal_gdb_break = 0;
      sigval = SIGINT;
  }
#endif
   
  // Only compute sigval if it wasn't already computed (in
  // hal_syscall_handler or as a result of a GDB async break)
  if (0 == sigval)
      sigval = __computeSignal (__get_trap_number ());

#else  // __ECOS__
  /* reply to host that an exception has occurred */
  sigval = __computeSignal (__get_trap_number ());
#endif // __ECOS__

  if (__is_breakpoint_function ())
  {
#ifdef CYGPKG_CYGMON
    processing_breakpoint_function = 1;
#endif
    __skipinst ();
  } else {
#ifdef CYGPKG_CYGMON
    processing_breakpoint_function = 0;
#endif
  }

#ifndef __ECOS__
  if (__cleanup_vec != NULL)
    __cleanup_vec ();
#endif // !__ECOS__

  __clear_breakpoints (); 

  /* Undo effect of previous single step.  */
  unlock_thread_scheduler ();
  __clear_single_step ();

#ifdef __ECOS__
      /* Need to flush the data and instruction cache here, as we may have
         removed a breakpoint in __single_step - and we may be sharing
         some code with the application! */

        __data_cache (CACHE_FLUSH) ;
        __instruction_cache (CACHE_FLUSH) ;
#endif

#ifdef SIGSYSCALL
  if (sigval == SIGSYSCALL)
    {
      int val;
      /* Do the skipinst FIRST. */
#ifndef SYSCALL_PC_AFTER_INST
      __skipinst ();
#endif
      val =  __process_syscall_vec (__get_syscall_num ());
      if (val < 0)
        sigval = -val;
      else
        sigval = 0;
    }

#endif

  /* Indirect function call to stub, cygmon monitor or other */
  if (sigval != 0)
    {
      while (__process_exception_vec (sigval))
        {
          /* Empty! */
        }
    }

  __install_breakpoints ();

  if (__init_vec != NULL)
    __init_vec ();
}

/*
 * _get_trace_register_hook:
 * This function pointer will be non-zero if the trace component
 * wants to intercept requests for register values.
 * 
 * FIXME: evidently I need a new hook for large registers...
 */

int   (*_get_trace_register_hook) (regnames_t, target_register_t *);

void
stub_format_registers(char *packet, char *ptr)
{
    int regnum;
    int sr = 0, er = NUMREGS_GDB;

    if (packet[0] == 'p')
      {
	 target_register_t regno;
         char *p = &packet[1];
	 if (__hexToInt (&p, &regno))
	   {
	     sr = regno;
	     er = regno + 1;
	   }
	 else
	   {