2009-05-05 Michael Walle <michael@walle.cc>

	* shared/gdbstub/README, shared/gdbstub/gdb_if.h,
	shared/gdbstub/lm32-debug.S, shared/gdbstub/lm32-stub.c: New files.

5 files changed, 1519 insertions, 0 deletions

diff --git a/c/src/lib/libbsp/lm32/ChangeLog b/c/src/lib/libbsp/lm32/ChangeLog
index 3761a36b0d..10ea10cab1 100644
--- a/c/src/lib/libbsp/lm32/ChangeLog
+++ b/c/src/lib/libbsp/lm32/ChangeLog
@@ -1,3 +1,8 @@
+2009-05-05	Michael Walle <michael@walle.cc>
+
+	* shared/gdbstub/README, shared/gdbstub/gdb_if.h,
+	shared/gdbstub/lm32-debug.S, shared/gdbstub/lm32-stub.c: New files.
+
 2009-04-28	Chris Johns <chrisj@rtems.org>
 
 	* shared/start/start.S: Update for boot_card command line change.
diff --git a/c/src/lib/libbsp/lm32/shared/gdbstub/README b/c/src/lib/libbsp/lm32/shared/gdbstub/README
new file mode 100644
index 0000000000..027d354435
--- /dev/null
+++ b/c/src/lib/libbsp/lm32/shared/gdbstub/README
@@ -0,0 +1,86 @@
+#
+#  $Id$
+#
+
+This is a thread-aware gdb stub for the lm32 architecture. It has to be
+linked with the application, which should be debugged. The application has
+to call 'lm32_gdb_stub_install' to setup the stub.
+   The stub remaps _all_ h/w exceptions to an own code (lm32-debug.S), which
+saves all the registers, calls the gdb stub and restores the registers again.
+   The interrupt exceptions gets handled in a special way. Because we remapped
+this exception, we need to do
+ - the same as the original one (in cpu_asm.S),
+ - and, as we might use an ISR for breaking into a running application with
+   gdb, we need to save all registers as well. To be backward compatible
+   the missing callee saved registers gets appended to CPU_Interrupt_frame.
+   There is a mapping in 'gdb_handle_break' for that.
+
+To use this gdb stub, your bsp has to provide the following functions:
+ - void gdb_put_debug_char(char c)
+   Puts the given charater c to the debug console output. The function can
+   block until the character can be written to the output buffer.
+
+ - char gdb_get_debug_char(void)
+   Returns the character in the input buffer of the debug console. If no one
+   is availabe, the function must block.
+
+ - void gdb_console_init()
+   This function can be used to initialize the debug console. Additionally,
+   it should set up the ISR for the debug console to call the function
+   'gdb_handle_break', which is provided by the gdb stub and enable the
+   interrupt for a break symbol on the debug serial port. If no ISR is
+   provided, you won't be able to interrupt a running application.
+
+ - void gdb_ack_irq()
+   If an ISR is used, this function is used to acknowledge the interrupt.
+
+NOTE: the stub don't skip a hardcoded 'break' in the code. So you have to
+   set the PC an instruction further in the debugger (set $pc += 4).
+
+NOTE2: make sure you have the following CFLAGS set:
+     -mbarrel-shift-enabled -mmultiply-enabled -mdivide-enabled 
+     -msign-extend-enabled
+   Without the hardware support, it is done in software. Unfortunately, the
+   stub also uses some shifts and multiplies. If you step through your code,
+   there will be a chance that a breakpoint is set to one of that functions,
+   which then causes an endless loop.
+
+
+EXAMPLES
+
+  char gdb_get_debug_char(void)
+  {
+    /* Wait until there is a byte in RXTX */
+    while (!(uartread(LM32_UART_LSR) & LM32_UART_LSR_DR));
+
+    return (char) uartread(LM32_UART_RBR);
+  }
+
+  void gdb_put_debug_char(char c)
+  {
+    /* Wait until RXTX is empty. */
+    while (!(uartread(LM32_UART_LSR) & LM32_UART_LSR_THRE));
+    uartwrite(LM32_UART_RBR, c);
+  }
+
+  extern void gdb_handle_break(
+    rtems_vector_number vector,
+    CPU_Interrupt_frame *frame
+  );
+  void gdb_console_init()
+  {
+    rtems_isr_entry old;
+
+    /* enable interrupts */
+    uartwrite(LM32_UART_IER, 1);
+
+    rtems_interrupt_catch((rtems_isr_entry) gdb_handle_break, DEBUG_UART_IRQ,
+      &old);
+    lm32_interrupt_unmask(1 << DEBUG_UART_IRQ);
+  }
+
+  void gdb_ack_irq()
+  {
+    lm32_interrupt_ack(1 << DEBUG_UART_IRQ);
+  }
+
diff --git a/c/src/lib/libbsp/lm32/shared/gdbstub/gdb_if.h b/c/src/lib/libbsp/lm32/shared/gdbstub/gdb_if.h
new file mode 100644
index 0000000000..bed4ffa5e4
--- /dev/null
+++ b/c/src/lib/libbsp/lm32/shared/gdbstub/gdb_if.h
@@ -0,0 +1,112 @@
+/*
+ * gdb_if.h - definition of the interface between the stub and gdb
+ *
+ *                   THIS SOFTWARE IS NOT COPYRIGHTED
+ *
+ *  The following software is offered for use in the public domain.
+ *  There is no warranty with regard to this software or its performance
+ *  and the user must accept the software "AS IS" with all faults.
+ *
+ *  THE CONTRIBUTORS DISCLAIM ANY WARRANTIES, EXPRESS OR IMPLIED, WITH
+ *  REGARD TO THIS SOFTWARE INCLUDING BUT NOT LIMITED TO THE WARRANTIES
+ *  OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ *  $Id$
+ */
+
+#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);
+
+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);
+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
+);
+
+/* Exception IDs */
+#define LM32_EXCEPTION_RESET                  0x0
+#define LM32_EXCEPTION_INST_BREAKPOINT        0x1
+#define LM32_EXCEPTION_INST_BUS_ERROR         0x2
+#define LM32_EXCEPTION_DATA_BREAKPOINT        0x3
+#define LM32_EXCEPTION_DATA_BUS_ERROR         0x4
+#define LM32_EXCEPTION_DIVIDE_BY_ZERO         0x5
+#define LM32_EXCEPTION_INTERRUPT              0x6
+#define LM32_EXCEPTION_SYSTEM_CALL            0x7
+
+/* Breakpoint instruction */
+#define LM32_BREAK                            0xac000002UL
+
+/* This numbering must be consistant with GDBs numbering in gdb/lm32-tdep.c */
+enum lm32_regnames {
+  LM32_REG_R0, LM32_REG_R1, LM32_REG_R2, LM32_REG_R3, LM32_REG_R4, LM32_REG_R5,
+  LM32_REG_R6, LM32_REG_R7, LM32_REG_R8, LM32_REG_R9, LM32_REG_R10,
+  LM32_REG_R11, LM32_REG_R12, LM32_REG_R13, LM32_REG_R14, LM32_REG_R15,
+  LM32_REG_R16, LM32_REG_R17, LM32_REG_R18, LM32_REG_R19, LM32_REG_R20,
+  LM32_REG_R21, LM32_REG_R22, LM32_REG_R23, LM32_REG_R24, LM32_REG_R25,
+  LM32_REG_GP, LM32_REG_FP, LM32_REG_SP, LM32_REG_RA, LM32_REG_EA, LM32_REG_BA,
+  LM32_REG_PC, LM32_REG_EID, LM32_REG_EBA, LM32_REG_DEBA, LM32_REG_IE, NUM_REGS
+};
+
+/* keep this in sync with the debug isr handler in lm32-debug.S */
+enum lm32_int_regnames {
+  LM32_INT_REG_R1, LM32_INT_REG_R2, LM32_INT_REG_R3, LM32_INT_REG_R4,
+  LM32_INT_REG_R5, LM32_INT_REG_R6, LM32_INT_REG_R7, LM32_INT_REG_R8,
+  LM32_INT_REG_R9, LM32_INT_REG_R10, LM32_INT_REG_RA, LM32_INT_REG_EA,
+  LM32_INT_REG_BA, LM32_INT_REG_R11, LM32_INT_REG_R12, LM32_INT_REG_R13,
+  LM32_INT_REG_R14, LM32_INT_REG_R15, LM32_INT_REG_R16, LM32_INT_REG_R17,
+  LM32_INT_REG_R18, LM32_INT_REG_R19, LM32_INT_REG_R20, LM32_INT_REG_R21,
+  LM32_INT_REG_R22, LM32_INT_REG_R23, LM32_INT_REG_R24, LM32_INT_REG_R25,
+  LM32_INT_REG_GP, LM32_INT_REG_FP, LM32_INT_REG_SP, LM32_INT_REG_PC,
+  LM32_INT_REG_EID, LM32_INT_REG_EBA, LM32_INT_REG_DEBA, LM32_INT_REG_IE,
+};
+
+#endif /* _GDB_IF_H */
diff --git a/c/src/lib/libbsp/lm32/shared/gdbstub/lm32-debug.S b/c/src/lib/libbsp/lm32/shared/gdbstub/lm32-debug.S
new file mode 100644
index 0000000000..d4d28f68fa
--- /dev/null
+++ b/c/src/lib/libbsp/lm32/shared/gdbstub/lm32-debug.S
@@ -0,0 +1,340 @@
+/*
+ *  lm32 debug exception vectors
+ *
+ *  Michael Walle <michael@walle.cc>, 2009
+ * 
+ *  If debugging is enabled the debug exception base address (deba) gets
+ *  remapped to this file.
+ *
+ *  The license and distribution terms for this file may be
+ *  found in the file LICENSE in this distribution or at
+ *  http://www.rtems.com/license/LICENSE.
+ *
+ *  $Id$
+ *
+ */
+
+#include "bspopts.h"
+
+.section .text
+/* (D)EBA alignment */
+.align    256
+.globl  _deba
+
+_deba:
+debug_reset_handler:
+	/* Clear r0 */
+	xor	r0,r0,r0
+	/* Disable interrupts */
+	wcsr	IE, r0
+	/* Mask all interrupts */
+	wcsr	IM,r0
+	/* Jump to original crt0 */
+	.extern crt0
+	mvhi	r1, hi(crt0)
+	ori	r1, r1, lo(crt0)
+	b	r1
+	nop
+	nop
+debug_breakpoint_handler:
+	/* Clear r0 in case it was corrupted */
+	xor	r0, r0, r0
+	mvhi	r0, hi(registers)
+	ori	r0, r0, lo(registers)
+	sw	(r0+116), ra
+	sw	(r0+128), ba
+	calli	save_all
+	calli	handle_exception
+	calli	b_restore_and_return
+debug_instruction_bus_error_handler:
+	/* Clear r0 in case it was corrupted */
+	xor	r0, r0, r0
+	mvhi	r0, hi(registers)
+	ori	r0, r0, lo(registers)
+	sw	(r0+116), ra
+	sw	(r0+128), ea
+	calli	save_all
+	calli	handle_exception
+	calli	e_restore_and_return
+debug_watchpoint_handler:
+	/* Clear r0 in case it was corrupted */
+	xor	r0, r0, r0
+	mvhi	r0, hi(registers)
+	ori	r0, r0, lo(registers)
+	sw	(r0+116), ra
+	sw	(r0+128), ba
+	calli	save_all
+	calli	handle_exception
+	calli	b_restore_and_return
+debug_data_bus_error_handler:
+	/* Clear r0 in case it was corrupted */
+	xor	r0, r0, r0
+	mvhi	r0, hi(registers)
+	ori	r0, r0, lo(registers)
+	sw	(r0+116), ra
+	sw	(r0+128), ea
+	calli	save_all
+	calli	handle_exception
+	calli	e_restore_and_return
+debug_divide_by_zero_handler:
+	/* Clear r0 in case it was corrupted */
+	xor	r0, r0, r0
+	mvhi	r0, hi(registers)
+	ori	r0, r0, lo(registers)
+	sw	(r0+116), ra
+	sw	(r0+128), ea
+	calli	save_all
+	calli	handle_exception
+	calli	e_restore_and_return
+debug_interrupt_handler:
+	bi	debug_isr_handler
+	nop
+	nop
+	nop
+	nop
+	nop
+	nop
+	nop
+debug_system_call_handler:
+	/* Clear r0 in case it was corrupted */
+	xor	r0, r0, r0
+	mvhi	r0, hi(registers)
+	ori	r0, r0, lo(registers)
+	sw	(r0+116), ra
+	sw	(r0+128), ea
+	calli	save_all
+	calli	handle_exception
+	calli	e_restore_and_return
+
+debug_isr_handler:
+	addi	sp, sp, -156
+	sw	(sp+4), r1
+	sw	(sp+8), r2
+	sw	(sp+12), r3
+	sw	(sp+16), r4
+	sw	(sp+20), r5
+	sw	(sp+24), r6
+	sw	(sp+28), r7
+	sw	(sp+32), r8
+	sw	(sp+36), r9
+	sw	(sp+40), r10
+	sw	(sp+44), ra
+	sw	(sp+48), ea
+	sw	(sp+52), ba
+	sw	(sp+56), r11
+	sw	(sp+60), r12
+	sw	(sp+64), r13
+	sw	(sp+68), r14
+	sw	(sp+72), r15
+	sw	(sp+76), r16
+	sw	(sp+80), r17
+	sw	(sp+84), r18
+	sw	(sp+88), r19
+	sw	(sp+92), r20
+	sw	(sp+96), r21
+	sw	(sp+100), r22
+	sw	(sp+104), r23
+	sw	(sp+108), r24
+	sw	(sp+112), r25
+	sw	(sp+116), r26
+	sw	(sp+120), r27
+	/* 124 - SP */
+	addi	r1, sp, 156
+	sw	(sp+124), r1
+	/* 128 - PC */
+	sw	(sp+128), ea
+	/* 132 - EID */
+	mvi	r1, 6
+	sw	(sp+132), r1
+	rcsr	r1, EBA
+	sw	(sp+136), r1
+	rcsr	r1, DEBA
+	sw	(sp+140), r1
+	rcsr	r1, IE
+	sw	(sp+144), r1
+
+	/* This is the same code as in cpu_asm.S */
+	rcsr	r2, IP
+	rcsr	r3, IM
+	mv	r1, r0
+	and	r2, r2, r3
+	mvi	r3, 1
+	be	r2, r0, 3f
+1:
+	and	r4, r2, r3
+	bne	r4, r0, 2f
+	sli	r3, r3, 1
+	addi	r1, r1, 1
+	bi	1b
+2:
+	addi	r2, sp, 4
+
+	.extern __ISR_Handler
+	mvhi	r3, hi(__ISR_Handler)
+	ori	r3, r3, lo(__ISR_Handler)
+	call	r3
+3:
+	lw	r1, (sp+4)
+	lw	r2, (sp+8) 
+	lw	r3, (sp+12) 
+	lw	r4, (sp+16) 
+	lw	r5, (sp+20) 
+	lw	r6, (sp+24) 
+	lw	r7, (sp+28) 
+	lw	r8, (sp+32) 
+	lw	r9, (sp+36) 
+	lw	r10, (sp+40)
+	lw	ra, (sp+44)
+	lw	ea, (sp+48)
+	lw	ba, (sp+52)
+	lw	r11, (sp+56)
+	lw	r12, (sp+60)
+	lw	r13, (sp+64)
+	lw	r14, (sp+68)
+	lw	r15, (sp+72)
+	lw	r16, (sp+76)
+	lw	r17, (sp+80)
+	lw	r18, (sp+84)
+	lw	r19, (sp+88)
+	lw	r20, (sp+92)
+	lw	r21, (sp+96)
+	lw	r22, (sp+100)
+	lw	r23, (sp+104)
+	lw	r24, (sp+108)
+	lw	r25, (sp+112)
+	lw	r26, (sp+116)
+	lw	r27, (sp+120)
+	lw	ea, (sp+136)
+	wcsr	EBA, ea
+	lw	ea, (sp+140)
+	wcsr	DEBA, ea
+	/* Restore EA from PC */        
+	lw	ea, (sp+128)
+	/* Stack pointer must be restored last, in case it has been updated */
+	lw	sp, (sp+124)
+	eret
+
+save_all:
+	sw	(r0+4), r1
+	sw	(r0+8), r2
+	sw	(r0+12), r3
+	sw	(r0+16), r4
+	sw	(r0+20), r5
+	sw	(r0+24), r6
+	sw	(r0+28), r7
+	sw	(r0+32), r8
+	sw	(r0+36), r9
+	sw	(r0+40), r10
+	sw	(r0+44), r11
+	sw	(r0+48), r12
+	sw	(r0+52), r13
+	sw	(r0+56), r14
+	sw	(r0+60), r15
+	sw	(r0+64), r16
+	sw	(r0+68), r17
+	sw	(r0+72), r18
+	sw	(r0+76), r19
+	sw	(r0+80), r20
+	sw	(r0+84), r21
+	sw	(r0+88), r22
+	sw	(r0+92), r23
+	sw	(r0+96), r24
+	sw	(r0+100), r25
+	sw	(r0+104), r26
+	sw	(r0+108), r27
+	sw	(r0+112), sp
+	/* 116 - RA - saved in handler code above */
+	sw	(r0+120), ea
+	sw	(r0+124), ba
+	/* 128 - PC - saved in handler code above */
+	/* 132 - EID - saved below */
+	rcsr	r1, EBA
+	sw	(r0+136), r1
+	rcsr	r1, DEBA
+	sw	(r0+140), r1
+	rcsr	r1, IE
+	sw	(r0+144), r1
+	               
+	/* Work out EID from exception entry point address */
+	andi    r1, ra, 0xff
+	srui    r1, r1, 5
+	sw      (r0+132), r1
+
+	/* Save pointer to registers */
+	mv	r1, r0
+
+	/* Restore r0 to 0 */
+	xor	r0, r0, r0
+
+	/* Save r0 (hardcoded to 0) */
+	sw	(r1+0), r0
+	ret
+
+
+/* Restore gp registers */
+restore_gp:
+	lw	r1, (r0+4)
+	lw	r2, (r0+8) 
+	lw	r3, (r0+12) 
+	lw	r4, (r0+16) 
+	lw	r5, (r0+20) 
+	lw	r6, (r0+24) 
+	lw	r7, (r0+28) 
+	lw	r8, (r0+32) 
+	lw	r9, (r0+36) 
+	lw	r10, (r0+40)
+	lw	r11, (r0+44)
+	lw	r12, (r0+48)
+	lw	r13, (r0+52)
+	lw	r14, (r0+56)
+	lw	r15, (r0+60)
+	lw	r16, (r0+64)
+	lw	r17, (r0+68)
+	lw	r18, (r0+72)
+	lw	r19, (r0+76)
+	lw	r20, (r0+80)
+	lw	r21, (r0+84)
+	lw	r22, (r0+88)
+	lw	r23, (r0+92)
+	lw	r24, (r0+96)
+	lw	r25, (r0+100)
+	lw	r26, (r0+104)
+	lw	r27, (r0+108)
+	ret
+
+/* Restore registers and return from exception */
+e_restore_and_return:
+	/* first restore gp registers */
+	mvhi	r0, hi(registers)
+	ori	r0, r0, lo(registers)
+	calli	restore_gp
+	lw	sp, (r0+112)
+	lw	ra, (r0+116)
+	lw	ba, (r0+124)
+	lw	ea, (r0+136)
+	wcsr	EBA, ea
+	lw	ea, (r0+140)
+	wcsr	DEBA, ea
+	/* Restore EA from PC */        
+	lw	ea, (r0+128)
+	xor	r0, r0, r0
+	eret
+
+/* Restore registers and return from breakpoint */
+b_restore_and_return:
+	/* first restore gp registers */
+	mvhi	r0, hi(registers)
+	ori	r0, r0, lo(registers)
+	calli   restore_gp
+	lw	sp, (r0+112)
+	lw	ra, (r0+116)
+	lw	ea, (r0+120)
+	lw	ba, (r0+136)
+	wcsr	EBA, ba
+	lw	ba, (r0+140)
+	wcsr	DEBA, ba
+	/* Restore BA from PC */        
+	lw	ba, (r0+128)
+	xor	r0, r0, r0
+	bret
+
diff --git a/c/src/lib/libbsp/lm32/shared/gdbstub/lm32-stub.c b/c/src/lib/libbsp/lm32/shared/gdbstub/lm32-stub.c
new file mode 100644
index 0000000000..14c5fb0ea1
--- /dev/null
+++ b/c/src/lib/libbsp/lm32/shared/gdbstub/lm32-stub.c
@@ -0,0 +1,976 @@
+/*
+ * Low-level support for LM32 remote debuging with GDB.
+ * Contributed by Jon Beniston <jon@beniston.com>
+ * Modified for RTEMS with thread support by Michael Walle <michael@walle.cc>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+ 
+#include <bsp.h>
+#include <string.h>
+#include <signal.h>
+#include <rtems.h>
+#include <rtems/score/cpu.h>
+#include "gdb_if.h"
+
+/* Enable support for run-length encoding */
+#undef GDB_RLE_ENABLED
+/* Enable support for restart packets */
+#undef GDB_RESTART_ENABLED
+
+#define GDB_STUB_ENABLE_THREAD_SUPPORT
+
+/*
+ * The following external functions provide character input and output.
+ */
+extern char gdb_get_debug_char(void);
+extern void gdb_put_debug_char(char);
+extern void gdb_console_init(void);
+extern void gdb_ack_irq(void);
+extern void *_deba;
+
+/* Function prototypes */
+static void allow_nested_exception(void);
+static void disallow_nested_exception(void);
+static char *mem2hex(unsigned char *mem, char *buf, int count);
+static unsigned char *hex2mem(char *buf, unsigned char *mem, int count);
+static unsigned char *bin2mem(char *buf, unsigned char *mem, int count);
+static int compute_signal(int eid);
+static void flush_cache(void);
+static int hex2int(char **ptr, int *int_value);
+static char *getpacket(void);
+static void putpacket(char *buffer);
+
+unsigned int registers[NUM_REGS];
+
+/* BUFMAX defines the maximum number of characters in inbound/outbound buffers */
+#define BUFMAX 1500
+
+/* I/O packet buffers */
+static char remcomInBuffer[BUFMAX];
+static char remcomOutBuffer[BUFMAX];
+
+/*
+ * Set by debugger to indicate that when handling memory faults (bus errors), the 
+ * handler should set the mem_err flag and skip over the faulting instruction
+ */
+static volatile int may_fault;
+
+/*
+ * Set by bus error exception handler, this indicates to caller of mem2hex, 
+ * hex2mem or bin2mem that there has been an error.
+ */
+static volatile int mem_err;
+
+/* Indicates if we're single stepping */
+static unsigned char stepping;
+static char branch_step;
+
+/* Saved instructions */
+static unsigned int *seq_ptr;
+static unsigned int seq_insn;
+static unsigned int *branch_ptr;                      
+static unsigned int branch_insn;
+
+#if defined(GDB_STUB_ENABLE_THREAD_SUPPORT)
+static char do_threads;
+int current_thread_registers[NUM_REGS];
+#endif
+
+/* this mapping is used to copy the registers from a debug interrupt frame
+ * see gdb_handle_break() */
+static unsigned char reg_map[] = {
+  0, LM32_INT_REG_R1, LM32_INT_REG_R2, LM32_INT_REG_R3, LM32_INT_REG_R4,
+  LM32_INT_REG_R5, LM32_INT_REG_R6, LM32_INT_REG_R7, LM32_INT_REG_R8,
+  LM32_INT_REG_R9, LM32_INT_REG_R10, LM32_INT_REG_R11, LM32_INT_REG_R12,
+  LM32_INT_REG_R13, LM32_INT_REG_R14, LM32_INT_REG_R15, LM32_INT_REG_R16,
+  LM32_INT_REG_R17, LM32_INT_REG_R18, LM32_INT_REG_R19, LM32_INT_REG_R20,
+  LM32_INT_REG_R21, LM32_INT_REG_R22, LM32_INT_REG_R23, LM32_INT_REG_R24,
+  LM32_INT_REG_R25, LM32_INT_REG_GP, LM32_INT_REG_FP, LM32_INT_REG_SP,
+  LM32_INT_REG_RA, LM32_INT_REG_EA, LM32_INT_REG_BA, LM32_INT_REG_PC,
+  LM32_INT_REG_EID, LM32_INT_REG_EBA, LM32_INT_REG_DEBA, LM32_INT_REG_IE
+};
+
+/*
+ * Conversion helper functions
+ */
+
+/* For integer to ASCII conversion */
+#define highhex(x) gdb_hexchars [(x >> 4) & 0xf]
+#define lowhex(x) gdb_hexchars [x & 0xf]
+const char gdb_hexchars[]="0123456789abcdef";
+
+/* Convert ch from a hex digit to an int */
+static int hex(
+  unsigned char ch
+)
+{
+  if (ch >= 'a' && ch <= 'f')
+    return ch-'a'+10;
+  if (ch >= '0' && ch <= '9')
+    return ch-'0';
+  if (ch >= 'A' && ch <= 'F')
+    return ch-'A'+10;
+  return -1;
+}
+
+/*
+ * Convert the memory pointed to by mem into hex, placing result in buf.
+ * Return a pointer to the last char put in buf ('\0'), in case of mem fault,
+ * return NULL.
+ */
+static char *mem2hex(
+  unsigned char *mem,
+  char *buf, int count
+)
+{
+  unsigned char ch;
+  
+  while (count-- > 0)
+  {
+    ch = *mem++;
+    if (mem_err)
+      return NULL;
+    *buf++ = highhex(ch);
+    *buf++ = lowhex(ch);
+  }
+
+  *buf = '\0';
+  return buf;
+}
+
+/*
+ * Convert 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.
+ */
+static unsigned char *hex2mem(
+  char *buf,
+  unsigned char *mem,
+  int count
+)
+{
+  int i;
+  unsigned char ch;
+
+  for (i = 0; i < count; i++)
+  {
+    /* Convert hex data to 8-bit value */
+    ch = hex(*buf++) << 4;
+    ch |= hex(*buf++);
+    /* Attempt to write data to memory */
+    *mem++ = ch;
+    /* Return NULL if write caused an exception */      
+    if (mem_err)
+      return NULL;
+  }
+  return mem;
+}
+
+/*
+ * Copy the binary data pointed to by buf to mem and return a pointer to the
+ * character AFTER the last byte written $, # and 0x7d are escaped with 0x7d.
+ */
+static unsigned char *bin2mem(
+  char *buf,
+  unsigned char *mem,
+  int count
+)
+{
+  int i;
+  unsigned char c;
+
+  for (i = 0; i < count; i++)
+  {
+    /* Convert binary data to unsigned byte */
+    c = *buf++;
+    if (c == 0x7d)
+      c = *buf++ ^ 0x20;
+    /* Attempt to write value to memory */  
+    *mem++ = c;
+    /* Return NULL if write caused an exception */      
+    if (mem_err)
+      return NULL; 
+  }
+
+  return mem;
+}
+
+/*
+ * While we find nice hex chars, build an int.
+ * Return number of chars processed.
+ */
+static int hex2int(
+  char **ptr,
+  int *int_value
+)
+{
+  int num_chars = 0;
+  int hex_value;
+
+  *int_value = 0;
+
+  while(**ptr)
+  {
+    hex_value = hex(**ptr);
+    if (hex_value < 0)
+      break;
+
+    *int_value = (*int_value << 4) | hex_value;
+    num_chars ++;
+
+    (*ptr)++;
+  }
+
+  return (num_chars);
+}
+
+/* Convert the exception identifier to a signal number. */
+static int compute_signal(
+  int eid
+)
+{
+  switch (eid)
+  {
+    case LM32_EXCEPTION_RESET:
+      return 0;
+    case LM32_EXCEPTION_INTERRUPT:
+      return SIGINT;
+    case LM32_EXCEPTION_DATA_BREAKPOINT:
+    case LM32_EXCEPTION_INST_BREAKPOINT:
+      return SIGTRAP;
+    case LM32_EXCEPTION_INST_BUS_ERROR:
+    case LM32_EXCEPTION_DATA_BUS_ERROR:
+      return SIGSEGV;
+    case LM32_EXCEPTION_DIVIDE_BY_ZERO:
+      return SIGFPE;
+  }  
+
+  return SIGHUP;  /* default for things we don't know about */
+}
+
+/* Scan for the sequence $<data>#<checksum> */
+static char *getpacket(void)
+{
+  char *buffer = &remcomInBuffer[0];
+  unsigned char checksum;
+  unsigned char xmitcsum;
+  int count;
+  char ch;
+
+  while (1)
+  {
+    /* wait around for the start character, ignore all other characters */
+    while ((ch = gdb_get_debug_char()) != '$');
+
+retry:
+    checksum = 0;
+    xmitcsum = -1;
+    count = 0;
+
+    /* now, read until a # or end of buffer is found */
+    while (count < BUFMAX)
+    {
+      ch = gdb_get_debug_char();
+      if (ch == '$')
+        goto retry;
+      if (ch == '#')
+        break;     
+      checksum = checksum + ch;
+      buffer[count] = ch;
+      count = count + 1;
+    }
+    buffer[count] = 0;
+
+    if (ch == '#')
+    {
+      ch = gdb_get_debug_char();
+      xmitcsum = hex(ch) << 4;
+      ch = gdb_get_debug_char();
+      xmitcsum += hex(ch);
+
+      if (checksum != xmitcsum)
+      {
+	    /* failed checksum */
+        gdb_put_debug_char('-');
+      }
+      else
+      {
+	    /* successful transfer */
+        gdb_put_debug_char('+');
+
+        /* if a sequence char is present, reply the sequence ID */
+        if (buffer[2] == ':')
+        {
+          gdb_put_debug_char(buffer[0]);
+          gdb_put_debug_char(buffer[1]);
+
+          return &buffer[3];
+        }
+
+        return &buffer[0];
+      }
+    }
+  }
+}
+
+/* Send the packet in buffer.  */
+static void putpacket(
+  char *buffer
+)
+{
+  unsigned char checksum;
+  int count;
+  unsigned char ch;
+
+#ifdef GDB_RLE_ENABLED
+  int run_length;
+  int run_idx;
+  char run_length_char;
+#endif
+
+  /*  $<packet info>#<checksum>. */
+  do {
+    gdb_put_debug_char('$');
+    checksum = 0;
+    count = 0;
+
+#ifdef GDB_RLE_ENABLED
+    while (ch = buffer[count])
+    {
+      /* Transmit character */
+      gdb_put_debug_char(ch);
+      checksum += ch;
+      count += 1;
+
+      /*
+       * Determine how many consecutive characters there are that are the same
+       * as the character we just transmitted
+       */
+      run_length = 0;
+      run_idx = count;  
+      while ((buffer[run_idx++] == ch) && (run_length < 97))
+        run_length++;
+      /* Encode run length as an ASCII character */
+      run_length_char = (char)(run_length + 29);
+      if (   (run_length >= 3) 
+          && (run_length_char != '$')
+          && (run_length_char != '#')
+          && (run_length_char != '+')
+          && (run_length_char != '-')
+         )
+      {
+        /* Transmit run-length */ 
+        gdb_put_debug_char('*');
+        checksum += '*';
+        gdb_put_debug_char(run_length_char);
+        checksum += run_length_char;
+        count += run_length;
+      }
+    }
+#else
+    while ((ch = buffer[count]))
+    {
+      gdb_put_debug_char(ch);
+      checksum += ch;
+      count += 1;
+    }
+#endif
+   
+    gdb_put_debug_char('#');
+    gdb_put_debug_char(highhex(checksum));
+    gdb_put_debug_char(lowhex(checksum));
+  } while (gdb_get_debug_char() != '+');
+}
+
+static void allow_nested_exception(void)
+{
+  mem_err = 0;
+  may_fault = 1;
+}
+
+static void disallow_nested_exception(void)
+{
+  mem_err = 0;
+  may_fault = 0;
+}
+
+/* Flush the instruction cache */
+static void flush_cache(void)
+{
+  /*
+   * Executing this does no harm on CPUs without a cache. We flush data cache as
+   * well as instruction cache in case the debugger has accessed memory
+   * directly.
+   */
+  __asm__ __volatile__ ("wcsr ICC, r0\n"
+                        "nop\n"
+                        "nop\n"
+                        "nop\n" 
+                        "wcsr DCC, r0\n"
+                        "nop\n"
+                        "nop\n"
+                        "nop" 
+                       );
+}
+
+/* Set a h/w breakpoint at the given address */
+static int set_hw_breakpoint(
+  int address,
+  int length
+)
+{
+  int bp;
+  
+  /* Find a free break point register and then set it */      
+  __asm__ ("rcsr  %0, BP0" : "=r" (bp));
+  if ((bp & 0x01) == 0) 
+  {
+    __asm__ ("wcsr  BP0, %0" : : "r" (address | 1));
+    return 1;
+  }
+  __asm__ ("rcsr  %0, BP1" : "=r" (bp));
+  if ((bp & 0x01) == 0) 
+  {
+    __asm__ ("wcsr  BP1, %0" : : "r" (address | 1));
+    return 1;
+  }
+  __asm__ ("rcsr  %0, BP2" : "=r" (bp));
+  if ((bp & 0x01) == 0) 
+  {
+    __asm__ ("wcsr  BP2, %0" : : "r" (address | 1));
+    return 1;
+  }
+  __asm__ ("rcsr  %0, BP3" : "=r" (bp));
+  if ((bp & 0x01) == 0) 
+  {
+    __asm__ ("wcsr  BP3, %0" : : "r" (address | 1));
+    return 1;
+  }
+
+  /* No free breakpoint registers */
+  return -1;        
+}
+
+/* Remove a h/w breakpoint which should be set at the given address */
+static int disable_hw_breakpoint(
+  int address,
+  int length
+)
+{
+  int bp;
+  
+  /* Try to find matching breakpoint register */
+  __asm__ ("rcsr  %0, BP0" : "=r" (bp));
+  if ((bp & 0xfffffffc) == (address & 0xfffffffc)) 
+  {
+    __asm__ ("wcsr  BP0, %0" : : "r" (0));
+    return 1;
+  }
+  __asm__ ("rcsr  %0, BP1" : "=r" (bp));
+  if ((bp & 0xfffffffc) == (address & 0xfffffffc)) 
+  {
+    __asm__ ("wcsr  BP1, %0" : : "r" (0));
+    return 1;
+  }
+  __asm__ ("rcsr  %0, BP2" : "=r" (bp));
+  if ((bp & 0xfffffffc) == (address & 0xfffffffc)) 
+  {
+    __asm__ ("wcsr  BP2, %0" : : "r" (0));
+    return 1;
+  }
+  __asm__ ("rcsr  %0, BP3" : "=r" (bp));
+  if ((bp & 0xfffffffc) == (address & 0xfffffffc)) 
+  {
+    __asm__ ("wcsr  BP3, %0" : : "r" (0));
+    return 1;
+  }
+
+  /* Breakpoint not found */
+  return -1;
+}
+
+/*
+ *  This support function prepares and sends the message containing the
+ *  basic information about this exception.
+ */
+static void gdb_stub_report_exception_info(
+  int thread
+)
+{
+  char *ptr;
+  int sigval;
+
+  /* Convert exception ID to a signal number */          
+  sigval = compute_signal(registers[LM32_REG_EID]);
+
+  /* Set pointer to start of output buffer */  
+  ptr = remcomOutBuffer;
+
+  *ptr++ = 'T';
+  *ptr++ = highhex(sigval);
+  *ptr++ = lowhex(sigval);
+
+  *ptr++ = highhex(LM32_REG_PC);
+  *ptr++ = lowhex(LM32_REG_PC);
+  *ptr++ = ':';
+  ptr    = mem2hex((unsigned char *)&(registers[LM32_REG_PC]), ptr, 4);
+  *ptr++ = ';';
+
+  *ptr++ = highhex(LM32_REG_SP);
+  *ptr++ = lowhex(LM32_REG_SP);
+  *ptr++ = ':';
+  ptr    = mem2hex((unsigned char *)&(registers[LM32_REG_SP]), ptr, 4);
+  *ptr++ = ';';
+
+#if defined(GDB_STUB_ENABLE_THREAD_SUPPORT)
+  if (do_threads)
+  {
+    *ptr++ = 't';
+    *ptr++ = 'h';
+    *ptr++ = 'r';
+    *ptr++ = 'e';
+    *ptr++ = 'a';
+    *ptr++ = 'd';
+    *ptr++ = ':';
+    ptr   = thread2vhstr(ptr, thread);
+    *ptr++ = ';';
+  }
+#endif
+
+  *ptr++ = '\0';
+}
+
+/*
+ * This function does all command procesing for interfacing to gdb. The error
+ * codes we return are errno numbers.
+ */
+void handle_exception(void)
+{
+  int addr;
+  int length;
+  char *ptr;
+  int err;
+  int reg;
+  unsigned insn;
+  unsigned opcode;
+  unsigned branch_target = 0;
+  int current_thread;
+  int thread;
+  void *regptr;
+  int host_has_detached = 0;
+  int binary;
+
+  thread = 0;
+#if defined(GDB_STUB_ENABLE_THREAD_SUPPORT)
+  if (do_threads)
+    thread = rtems_gdb_stub_get_current_thread();
+#endif
+  current_thread = thread;
+  
+  /*
+   * Check for bus error caused by this code (rather than the program being
+   * debugged)
+   */
+  if (may_fault && (registers[LM32_REG_EID] == LM32_EXCEPTION_DATA_BUS_ERROR))
+  {
+    /* Indicate that a fault occured */
+    mem_err = 1;
+    /* Skip over faulting instruction */
+    registers[LM32_REG_PC] += 4;
+    /* Resume execution */
+    return;
+  }
+
+  if (stepping)
+  {
+    /* Remove breakpoints */
+    *seq_ptr = seq_insn;
+    if (branch_step)      
+      *branch_ptr = branch_insn;      
+    stepping = 0;
+  }   
+
+  /* Reply to host that an exception has occured with some basic info */
+  gdb_stub_report_exception_info(thread);
+  putpacket(remcomOutBuffer);
+
+  while (!host_has_detached)
+  {
+    remcomOutBuffer[0] = '\0';
+    ptr = getpacket();
+    binary = 0;
+
+    switch (*ptr++)
+    {
+      /* Return last signal */
+      case '?':
+        gdb_stub_report_exception_info(thread);
+        break;
+
+      /* Detach - exit from debugger */
+	  case 'D':
+	    strcpy(remcomOutBuffer, "OK");
+		host_has_detached = 1;
+		break;
+
+      /* Return the value of the CPU registers */
+      case 'g':
+        regptr = registers;
+#if defined(GDB_STUB_ENABLE_THREAD_SUPPORT)
+        if (do_threads && current_thread != thread )
+          regptr = &current_thread_registers;
+#endif
+        ptr = mem2hex((unsigned char*)regptr, remcomOutBuffer, NUM_REGS * 4); 
+        break;
+
+      /* Set the value of the CPU registers */
+      case 'G':
+        regptr = registers;
+#if defined(GDB_STUB_ENABLE_THREAD_SUPPORT)
+        if (do_threads && current_thread != thread )
+          regptr = &current_thread_registers;
+#endif
+        hex2mem(ptr, (unsigned char*)regptr, NUM_REGS * 4); 
+        strcpy(remcomOutBuffer, "OK");
+        break;
+
+      /* Return the value of the specified register */
+      case 'p':
+        if (hex2int(&ptr, &reg))
+        {
+          ptr = remcomOutBuffer;
+          ptr = mem2hex((unsigned char *)&registers[reg], ptr, 4);
+        } else
+          strcpy(remcomOutBuffer, "E22");
+        break;
+
+      /* Set the specified register to the given value */
+      case 'P':
+        if (hex2int(&ptr, &reg)
+            && *ptr++ == '=')
+        {
+          hex2mem(ptr, (unsigned char *)&registers[reg], 4);
+          strcpy(remcomOutBuffer, "OK");
+        }
+        else
+          strcpy(remcomOutBuffer, "E22");
+        break;
+
+      /* Read memory */
+      case 'm':
+        /* Try to read %x,%x.  */
+        if (hex2int(&ptr, &addr)
+            && *ptr++ == ','
+            && hex2int(&ptr, &length)
+            && length < (sizeof(remcomOutBuffer)/2))
+        {
+          allow_nested_exception();
+          if (NULL == mem2hex((unsigned char *)addr, remcomOutBuffer, length))
+            strcpy(remcomOutBuffer, "E14");
+          disallow_nested_exception();
+        }
+        else
+          strcpy(remcomOutBuffer,"E22");
+        break;
+
+      /* Write memory */
+      case 'X':
+	    binary = 1;
+      case 'M':
+        /* Try to read '%x,%x:'.  */
+        if (hex2int(&ptr, &addr)
+            && *ptr++ == ','
+            && hex2int(&ptr, &length)
+            && *ptr++ == ':')
+        {
+          allow_nested_exception();
+          if (binary)
+            err = (int)bin2mem(ptr, (unsigned char *)addr, length);
+          else
+            err = (int)hex2mem(ptr, (unsigned char *)addr, length);
+          if (err)
+            strcpy(remcomOutBuffer, "OK");
+          else
+            strcpy(remcomOutBuffer, "E14");
+          disallow_nested_exception();
+        }
+        else
+          strcpy(remcomOutBuffer, "E22");
+        break;
+
+      /* Continue */
+      case 'c':
+        /* try to read optional parameter, pc unchanged if no parm */
+        if (hex2int(&ptr, &addr))
+          registers[LM32_REG_PC] = addr;
+        flush_cache();
+        return;
+        
+      /* Step */
+      case 's':
+        /* try to read optional parameter, pc unchanged if no parm */
+        if (hex2int(&ptr, &addr))
+          registers[LM32_REG_PC] = addr;
+        stepping = 1;
+        /* Is instruction a branch? */
+        insn = *(unsigned int*)registers[LM32_REG_PC];
+        opcode = insn & 0xfc000000;
+        if (   (opcode == 0xe0000000)
+            || (opcode == 0xf8000000)
+           ) 
+        {
+          branch_step = 1;
+          branch_target = registers[LM32_REG_PC]
+              + ((signed)(insn & 0x3ffffff) << 2);
+        }   
+        else if (   (opcode == 0x44000000)
+                 || (opcode == 0x48000000) 
+                 || (opcode == 0x4c000000) 
+                 || (opcode == 0x50000000) 
+                 || (opcode == 0x54000000) 
+                 || (opcode == 0x5c000000) 
+                ) 
+        {
+          branch_step = 1;
+          branch_target = registers[LM32_REG_PC] +
+              + ((signed)(insn & 0x0000ffff) << 2);
+        }
+        else if (   (opcode == 0xd8000000)
+                 || (opcode == 0xc0000000) 
+                )
+        {
+          branch_step = 1;
+          branch_target = registers[(insn >> 21) & 0x1f];
+        }      
+        else 
+          branch_step = 0;
+
+        /* Set breakpoint after instruction we're stepping */
+        seq_ptr = (unsigned int *)registers[LM32_REG_PC];
+        seq_ptr++;
+        seq_insn = *seq_ptr;
+        *seq_ptr = LM32_BREAK;
+		
+        /* Make sure one insn doesn't get replaced twice */
+        if (seq_ptr == (unsigned int*)branch_target)
+          branch_step = 0;
+
+        if (branch_step)
+        {
+          /* Set breakpoint on branch target */
+          branch_ptr = (unsigned int*)branch_target;
+          branch_insn = *branch_ptr;
+          *branch_ptr = LM32_BREAK;
+        }
+        flush_cache();
+        return;
+
+      case 'Z':
+        switch (*ptr++)
+        {
+          /* Insert h/w breakpoint */
+          case '1':
+            if (*ptr++ == ','
+                && hex2int(&ptr, &addr)
+                && *ptr++ == ','
+                && hex2int(&ptr, &length))
+            {
+              err = set_hw_breakpoint(addr, length);
+              if (err > 0)
+                strcpy(remcomOutBuffer, "OK");
+              else if (err < 0)
+                strcpy(remcomOutBuffer, "E28");
+            }
+            else
+              strcpy(remcomOutBuffer, "E22");
+            break;
+        }
+        break;
+                  
+      case 'z':
+        switch (*ptr++)
+        {
+          /* Remove h/w breakpoint */
+          case '1':
+            if (*ptr++ == ','
+                && hex2int(&ptr, &addr)
+                && *ptr++ == ','
+                && hex2int(&ptr, &length))
+            {
+              err = disable_hw_breakpoint(addr, length);
+              if (err > 0)
+                strcpy(remcomOutBuffer, "OK");
+              else if (err < 0)
+                strcpy(remcomOutBuffer, "E28"); 
+            }              
+            else
+              strcpy(remcomOutBuffer, "E22");
+            break; 
+        }
+        break;
+
+      /* Query */
+      case 'q':
+#if defined(GDB_STUB_ENABLE_THREAD_SUPPORT)
+        rtems_gdb_process_query(
+            remcomInBuffer,
+            remcomOutBuffer,
+            do_threads,
+            thread );
+#endif
+        break;
+
+#if defined(GDB_STUB_ENABLE_THREAD_SUPPORT)
+      /* Thread alive */
+      case 'T':
+      {
+        int testThread;
+
+        if (vhstr2thread(&remcomInBuffer[1], &testThread) == NULL)
+        {
+          strcpy(remcomOutBuffer, "E01");
+          break;
+        }
+
+        if (rtems_gdb_index_to_stub_id(testThread) == NULL)
+          strcpy(remcomOutBuffer, "E02");
+        else
+          strcpy(remcomOutBuffer, "OK");
+      }
+      break;
+#endif
+ 
+      /* Set thread */
+      case 'H':
+#if defined(GDB_STUB_ENABLE_THREAD_SUPPORT)
+        if (remcomInBuffer[1] != 'g')
+          break;
+
+        if (!do_threads)
+          break;
+
+        {
+          int tmp, ret;
+
+          /* Set new generic thread */
+          if (vhstr2thread(&remcomInBuffer[2], &tmp) == NULL)
+          {
+            strcpy(remcomOutBuffer, "E01");
+            break;
+          }
+
+          /* 0 means `thread' */
+          if (tmp == 0)
+            tmp = thread;
+
+          if (tmp == current_thread)
+          {
+            /* No changes */
+            strcpy(remcomOutBuffer, "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);
+          }
+
+          /* 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(remcomOutBuffer, "E02");
+              break;
+            }
+          }
+
+          current_thread = tmp;
+          strcpy(remcomOutBuffer, "OK");
+        }
+#endif
+      break;
+
+#ifdef GDB_RESTART_ENABLED
+      /* Reset */
+      case 'r':
+      case 'R':
+        /* We reset by branching to the reset exception handler. */
+        registers[LM32_REG_PC] = 0;
+        return;
+#endif          
+      }                       
+
+    /* reply to the request */
+    putpacket(remcomOutBuffer);
+  }
+}
+
+void gdb_handle_break(rtems_vector_number vector, CPU_Interrupt_frame *frame)
+{
+  int i;
+  unsigned int *int_regs = (unsigned int*)frame;
+  
+  /* copy extended frame to registers */
+  registers[LM32_REG_R0]   = 0;
+  for (i = 1; i < NUM_REGS; i++)
+  {
+    registers[i] = int_regs[reg_map[i]];
+  }
+
+  /* now call the real handler */
+  handle_exception();
+  gdb_ack_irq();
+
+  /* copy registers back to extended frame */
+  for (i = 1; i < NUM_REGS; i++)
+  {
+    int_regs[reg_map[i]] = registers[i];
+  }
+}
+
+void lm32_gdb_stub_install(int enable_threads)
+{
+  unsigned int dc;
+  
+  /* set DEBA and remap all exception */
+  __asm__("wcsr DEBA, %0" : : "r" (&_deba));
+  __asm__("rcsr %0, DC" : "=r" (dc));
+  dc |= 0x2;
+  __asm__("wcsr DC, %0" : : "r" (dc));
+  
+#if defined(GDB_STUB_ENABLE_THREAD_SUPPORT)
+  if( enable_threads )
+    do_threads = 1;
+  else
+    do_threads = 0;
+#endif
+
+  gdb_console_init();
+}
+