From fd67814e06a734cd0d78274d61fc04c7af79b5fc Mon Sep 17 00:00:00 2001 From: Sebastian Huber Date: Mon, 23 Apr 2018 12:06:14 +0200 Subject: bsps: Move GDB stubs to bsps This patch is a part of the BSP source reorganization. Update #3285. --- bsps/lm32/shared/gdbstub/README | 82 + bsps/lm32/shared/gdbstub/gdb_if.h | 125 ++ bsps/lm32/shared/gdbstub/lm32-debug.S | 338 +++++ bsps/lm32/shared/gdbstub/lm32-stub.c | 977 ++++++++++++ bsps/m68k/shared/gdbstub/gdb_if.h | 195 +++ bsps/m68k/shared/gdbstub/m68k-stub.c | 1294 ++++++++++++++++ bsps/mips/shared/gdbstub/README | 151 ++ bsps/mips/shared/gdbstub/gdb_if.h | 194 +++ bsps/mips/shared/gdbstub/memlimits.h | 100 ++ bsps/mips/shared/gdbstub/mips-stub.c | 1589 ++++++++++++++++++++ bsps/mips/shared/gdbstub/mips_opcode.h | 336 +++++ c/src/lib/libbsp/lm32/shared/gdbstub/README | 82 - c/src/lib/libbsp/lm32/shared/gdbstub/gdb_if.h | 125 -- c/src/lib/libbsp/lm32/shared/gdbstub/lm32-debug.S | 338 ----- c/src/lib/libbsp/lm32/shared/gdbstub/lm32-stub.c | 977 ------------ c/src/lib/libbsp/m68k/shared/gdbstub/gdb_if.h | 195 --- c/src/lib/libbsp/m68k/shared/gdbstub/m68k-stub.c | 1294 ---------------- c/src/lib/libbsp/mips/shared/gdbstub/README | 151 -- c/src/lib/libbsp/mips/shared/gdbstub/gdb_if.h | 194 --- c/src/lib/libbsp/mips/shared/gdbstub/memlimits.h | 100 -- c/src/lib/libbsp/mips/shared/gdbstub/mips-stub.c | 1589 -------------------- c/src/lib/libbsp/mips/shared/gdbstub/mips_opcode.h | 336 ----- 22 files changed, 5381 insertions(+), 5381 deletions(-) create mode 100644 bsps/lm32/shared/gdbstub/README create mode 100644 bsps/lm32/shared/gdbstub/gdb_if.h create mode 100644 bsps/lm32/shared/gdbstub/lm32-debug.S create mode 100644 bsps/lm32/shared/gdbstub/lm32-stub.c create mode 100644 bsps/m68k/shared/gdbstub/gdb_if.h create mode 100644 bsps/m68k/shared/gdbstub/m68k-stub.c create mode 100644 bsps/mips/shared/gdbstub/README create mode 100644 bsps/mips/shared/gdbstub/gdb_if.h create mode 100644 bsps/mips/shared/gdbstub/memlimits.h create mode 100644 bsps/mips/shared/gdbstub/mips-stub.c create mode 100644 bsps/mips/shared/gdbstub/mips_opcode.h delete mode 100644 c/src/lib/libbsp/lm32/shared/gdbstub/README delete mode 100644 c/src/lib/libbsp/lm32/shared/gdbstub/gdb_if.h delete mode 100644 c/src/lib/libbsp/lm32/shared/gdbstub/lm32-debug.S delete mode 100644 c/src/lib/libbsp/lm32/shared/gdbstub/lm32-stub.c delete mode 100644 c/src/lib/libbsp/m68k/shared/gdbstub/gdb_if.h delete mode 100644 c/src/lib/libbsp/m68k/shared/gdbstub/m68k-stub.c delete mode 100644 c/src/lib/libbsp/mips/shared/gdbstub/README delete mode 100644 c/src/lib/libbsp/mips/shared/gdbstub/gdb_if.h delete mode 100644 c/src/lib/libbsp/mips/shared/gdbstub/memlimits.h delete mode 100644 c/src/lib/libbsp/mips/shared/gdbstub/mips-stub.c delete mode 100644 c/src/lib/libbsp/mips/shared/gdbstub/mips_opcode.h diff --git a/bsps/lm32/shared/gdbstub/README b/bsps/lm32/shared/gdbstub/README new file mode 100644 index 0000000000..bbd1f53a9d --- /dev/null +++ b/bsps/lm32/shared/gdbstub/README @@ -0,0 +1,82 @@ +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/bsps/lm32/shared/gdbstub/gdb_if.h b/bsps/lm32/shared/gdbstub/gdb_if.h new file mode 100644 index 0000000000..6df9c8e921 --- /dev/null +++ b/bsps/lm32/shared/gdbstub/gdb_if.h @@ -0,0 +1,125 @@ +/** + * @file + * @ingroup lm32_gdb + * @brief definition of the interface between the stub and gdb + */ + +/* + * 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. + */ + +/** + * @defgroup lm32_gdb LM32 GDB Interface + * @ingroup lm32_shared + * @brief Definition of the interface between the stub and gdb, + * @{ + */ + +#ifndef _GDB_IF_H +#define _GDB_IF_H + +/** @brief 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 +); + +/** @brief 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 + +/** @brief Breakpoint instruction */ +#define LM32_BREAK 0xac000002UL + +/** @brief 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/bsps/lm32/shared/gdbstub/lm32-debug.S b/bsps/lm32/shared/gdbstub/lm32-debug.S new file mode 100644 index 0000000000..7bd2c504e2 --- /dev/null +++ b/bsps/lm32/shared/gdbstub/lm32-debug.S @@ -0,0 +1,338 @@ +/* + * lm32 debug exception vectors + * + * Michael Walle , 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.org/license/LICENSE. + * + */ + +#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/bsps/lm32/shared/gdbstub/lm32-stub.c b/bsps/lm32/shared/gdbstub/lm32-stub.c new file mode 100644 index 0000000000..8ca1a91054 --- /dev/null +++ b/bsps/lm32/shared/gdbstub/lm32-stub.c @@ -0,0 +1,977 @@ +/* + * Low-level support for LM32 remote debuging with GDB. + * Contributed by Jon Beniston + * Modified for RTEMS with thread support by Michael Walle + * + * 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 +#include +#include +#include +#include +#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 $# */ +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 + + /* $#. */ + 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 = ¤t_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 = ¤t_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, ®)) + { + ptr = remcomOutBuffer; + ptr = mem2hex((unsigned char *)®isters[reg], ptr, 4); + } else + strcpy(remcomOutBuffer, "E22"); + break; + + /* Set the specified register to the given value */ + case 'P': + if (hex2int(&ptr, ®) + && *ptr++ == '=') + { + hex2mem(ptr, (unsigned char *)®isters[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 << 6) >> 4); + } + 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 << 16) >> 14); + } + 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 *) ¤t_thread_registers); + } + + /* Read new registers if necessary */ + if (tmp != thread) + { + ret = rtems_gdb_stub_get_thread_regs( + tmp, (unsigned int *) ¤t_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(); +} + diff --git a/bsps/m68k/shared/gdbstub/gdb_if.h b/bsps/m68k/shared/gdbstub/gdb_if.h new file mode 100644 index 0000000000..6c68703b76 --- /dev/null +++ b/bsps/m68k/shared/gdbstub/gdb_if.h @@ -0,0 +1,195 @@ +/** + * @file + * + * @ingroup m68k_gdbstub + * + * @brief 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. + */ + +#ifndef _GDB_IF_H +#define _GDB_IF_H + +/** + * @defgroup m68k_gdbstub GDB Stub + * + * @ingroup m68k_shared + * + * @brief GDB Stub interface support + */ + +/* 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 +); + +#if defined (__mc68000__) +/* there are 180 bytes of registers on a 68020 w/68881 */ +/* many of the fpa registers are 12 byte (96 bit) registers */ +#define NUMREGBYTES 180 +enum regnames {D0,D1,D2,D3,D4,D5,D6,D7, + A0,A1,A2,A3,A4,A5,A6,A7, + PS,PC, + FP0,FP1,FP2,FP3,FP4,FP5,FP6,FP7, + FPCONTROL,FPSTATUS,FPIADDR + }; +#elif defined (__mips__) +/* + * MIPS registers, numbered in the order in which gdb expects to see them. + */ +#define ZERO 0 +#define AT 1 +#define V0 2 +#define V1 3 +#define A0 4 +#define A1 5 +#define A2 6 +#define A3 7 + +#define T0 8 +#define T1 9 +#define T2 10 +#define T3 11 +#define T4 12 +#define T5 13 +#define T6 14 +#define T7 15 + +#define S0 16 +#define S1 17 +#define S2 18 +#define S3 19 +#define S4 20 +#define S5 21 +#define S6 22 +#define S7 23 + +#define T8 24 +#define T9 25 +#define K0 26 +#define K1 27 +#define GP 28 +#define SP 29 +#define S8 30 +#define RA 31 + +#define SR 32 +#define LO 33 +#define HI 34 +#define BAD_VA 35 +#define CAUSE 36 +#define PC 37 + +#define F0 38 +#define F1 39 +#define F2 40 +#define F3 41 +#define F4 42 +#define F5 43 +#define F6 44 +#define F7 45 + +#define F8 46 +#define F9 47 +#define F10 48 +#define F11 49 +#define F12 50 +#define F13 51 +#define F14 52 +#define F15 53 + +#define F16 54 +#define F17 55 +#define F18 56 +#define F19 57 +#define F20 58 +#define F21 59 +#define F22 60 +#define F23 61 + +#define F24 62 +#define F25 63 +#define F26 64 +#define F27 65 +#define F28 66 +#define F29 67 +#define F30 68 +#define F31 69 + +#define FCSR 70 +#define FIRR 71 + +#define NUM_REGS 72 + +void mips_gdb_stub_install(int enableThreads) ; +#endif /* defined (__mips__) */ + +#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/bsps/m68k/shared/gdbstub/m68k-stub.c b/bsps/m68k/shared/gdbstub/m68k-stub.c new file mode 100644 index 0000000000..36a3c0285f --- /dev/null +++ b/bsps/m68k/shared/gdbstub/m68k-stub.c @@ -0,0 +1,1294 @@ +#define GDB_STUB_ENABLE_THREAD_SUPPORT 1 +/**************************************************************************** + + THIS SOFTWARE IS NOT COPYRIGHTED + + HP offers the following for use in the public domain. HP makes no + warranty with regard to the software or it's performance and the + user accepts the software "AS IS" with all faults. + + HP DISCLAIMS 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. + +****************************************************************************/ + +/**************************************************************************** + * Header: remcom.c,v 1.34 91/03/09 12:29:49 glenne Exp $ + * + * Module name: remcom.c $ + * Revision: 1.34 $ + * Date: 91/03/09 12:29:49 $ + * Contributor: Lake Stevens Instrument Division$ + * + * Description: low level support for gdb debugger. $ + * + * Considerations: only works on target hardware $ + * + * Written by: Glenn Engel $ + * ModuleState: Experimental $ + * + * NOTES: See Below $ + * + * To enable debugger support, two things need to happen. One, a + * call to set_debug_traps() is necessary in order to allow any breakpoints + * or error conditions to be properly intercepted and reported to gdb. + * Two, a breakpoint needs to be generated to begin communication. This + * is most easily accomplished by a call to breakpoint(). Breakpoint() + * simulates a breakpoint by executing a trap #1. The breakpoint instruction + * is hardwired to trap #1 because not to do so is a compatibility problem-- + * there either should be a standard breakpoint instruction, or the protocol + * should be extended to provide some means to communicate which breakpoint + * instruction is in use (or have the stub insert the breakpoint). + * + * Some explanation is probably necessary to explain how exceptions are + * handled. When an exception is encountered the 68000 pushes the current + * program counter and status register onto the supervisor stack and then + * transfers execution to a location specified in it's vector table. + * The handlers for the exception vectors are hardwired to jmp to an address + * given by the relation: (exception - 256) * 6. These are decending + * addresses starting from -6, -12, -18, ... By allowing 6 bytes for + * each entry, a jsr, jmp, bsr, ... can be used to enter the exception + * handler. Using a jsr to handle an exception has an added benefit of + * allowing a single handler to service several exceptions and use the + * return address as the key differentiation. The vector number can be + * computed from the return address by [ exception = (addr + 1530) / 6 ]. + * The sole purpose of the routine _catchException is to compute the + * exception number and push it on the stack in place of the return address. + * The external function exceptionHandler() is + * used to attach a specific handler to a specific m68k exception. + * For 68020 machines, the ability to have a return address around just + * so the vector can be determined is not necessary because the '020 pushes an + * extra word onto the stack containing the vector offset + * + * Because gdb will sometimes write to the stack area to execute function + * calls, this program cannot rely on using the supervisor stack so it + * uses it's own stack area reserved in the int array remcomStack. + * + ************* + * + * The following gdb commands are supported: + * + * command function Return value + * + * g return the value of the CPU registers hex data or ENN + * G set the value of the CPU registers OK or ENN + * + * mAA..AA,LLLL Read LLLL bytes at address AA..AA hex data or ENN + * MAA..AA,LLLL: Write LLLL bytes at address AA.AA OK or ENN + * + * c Resume at current address SNN ( signal NN) + * cAA..AA Continue at address AA..AA SNN + * + * s Step one instruction SNN + * sAA..AA Step one instruction from AA..AA SNN + * + * k kill + * + * ? What was the last sigval ? SNN (signal NN) + * + * All commands and responses are sent with a packet which includes a + * checksum. A packet consists of + * + * $#. + * + * where + * :: + * :: < two hex digits computed as modulo 256 sum of > + * + * When a packet is received, it is first acknowledged with either '+' or '-'. + * '+' indicates a successful transfer. '-' indicates a failed transfer. + * + * Example: + * + * Host: Reply: + * $m0,10#2a +$00010203040506070809101112131415#42 + * + *************************************************************************** + * added 05/27/02 by IMD, Thomas Doerfler: + * XAA..AA,LLLL: Write LLLL binary bytes at address AA.AA OK or ENN + ****************************************************************************/ + +#include +#include +#include +/* #include */ +#include +#include +#include "gdb_if.h" + +#if defined(GDB_STUB_ENABLE_THREAD_SUPPORT) +static char do_threads = 1; /* != 0 means we are supporting threads */ +int current_thread_registers[NUMREGBYTES/4]; +#endif + +/************************************************************************ + * + * external low-level support routines + */ +typedef void (*ExceptionHook)(int); /* pointer to function with int parm */ +typedef void (*Function)(); /* pointer to a function */ + +extern void putDebugChar(); /* write a single character */ +extern int getDebugChar(); /* read and return a single char */ + +/************************/ +/* FORWARD DECLARATIONS */ +/************************/ +static void +initializeRemcomErrorFrame (); + +/************************************************************************/ +/* BUFMAX defines the maximum number of characters in inbound/outbound buffers*/ +/* at least NUMREGBYTES*2 are needed for register packets */ +#define BUFMAX 400 + +static bool initialized = false ; /* boolean flag. != 0 means we've been initialized */ + +int remote_debug; +/* debug > 0 prints ill-formed commands in valid packets & checksum errors */ + +const char gdb_hexchars[]="0123456789abcdef"; +#define highhex(x) gdb_hexchars [(x >> 4) & 0xf] +#define lowhex(x) gdb_hexchars [x & 0xf] + + +/* We keep a whole frame cache here. "Why?", I hear you cry, "doesn't + GDB handle that sort of thing?" Well, yes, I believe the only + reason for this cache is to save and restore floating point state + (fsave/frestore). A cleaner way to do this would be to make the + fsave data part of the registers which GDB deals with like any + other registers. This should not be a performance problem if the + ability to read individual registers is added to the protocol. */ + +typedef struct FrameStruct +{ + struct FrameStruct *previous; + int exceptionPC; /* pc value when this frame created */ + int exceptionVector; /* cpu vector causing exception */ + short frameSize; /* size of cpu frame in words */ + short sr; /* for 68000, this not always sr */ + int pc; + short format; + int fsaveHeader; + int morejunk[0]; /* exception frame, fp save... */ +} Frame; + +#define FRAMESIZE 500 +int gdbFrameStack[FRAMESIZE]; +static Frame *lastFrame; + +/* + * these should not be static cuz they can be used outside this module + */ +int registers[NUMREGBYTES/4]; +int superStack; + +#define STACKSIZE 10000 +int remcomStack[STACKSIZE/sizeof(int)]; +static int* stackPtr = &remcomStack[STACKSIZE/sizeof(int) - 1]; + +/* + * In many cases, the system will want to continue exception processing + * when a continue command is given. + * oldExceptionHook is a function to invoke in this case. + */ + +static ExceptionHook oldExceptionHook; +/* the size of the exception stack on the 68020/30/40/CPU32 varies with + * the type of exception. The following table is the number of WORDS used + * for each exception format. + * This table should be common for all 68k with VBR + * although each member only implements some of the formats + */ +const short exceptionSize[] = { 4,4,6,6,4,4,4,30,29,10,16,46,12,4,4,4 }; + +/************* jump buffer used for setjmp/longjmp **************************/ +jmp_buf remcomEnv; + +/*************************** ASSEMBLY CODE MACROS *************************/ +/* */ + +#ifdef __HAVE_68881__ +/* do an fsave, then remember the address to begin a restore from */ +#define SAVE_FP_REGS() __asm__ (" fsave -(%a0)"); \ + __asm__ (" fmovem.x %fp0-%fp7,registers+72"); \ + __asm__ (" fmovem.l %fpcr/%fpsr/%fpi,registers+168"); +#define RESTORE_FP_REGS() \ +__asm__ (" \n\ + fmovem.l registers+168,%fpcr/%fpsr/%fpi \n\ + fmovem.x registers+72,%fp0-%fp7 \n\ + cmp.l #-1,(%a0) | skip frestore flag set ? \n\ + beq skip_frestore \n\ + frestore (%a0)+ \n\ +skip_frestore: \n\ +"); + +#else +#define SAVE_FP_REGS() +#define RESTORE_FP_REGS() +#endif /* __HAVE_68881__ */ + +void return_to_super(); +void return_to_user(); +extern void _catchException (); + +void m68k_exceptionHandler +( + int vecnum, /* vector index to hook at */ + void *vector /* address of handler function */ +) +{ + void **vec_base = NULL; +#if M68K_HAS_VBR + /* + * get vector base register + */ + __asm__ (" movec.l %%vbr,%0":"=a" (vec_base)); +#endif + vec_base[vecnum] = vector; +} + +ExceptionHook exceptionHook; /* hook variable for errors/exceptions */ + +void m68k_stub_dummy_asm_wrapper() + /* + * this dummy wrapper function ensures, + * that the C compiler manages sections properly + */ +{ +__asm__ ("\n\ +.globl return_to_super \n\ +return_to_super: \n\ + move.l registers+60,%sp /* get new stack pointer */ \n\ + move.l lastFrame,%a0 /* get last frame info */ \n\ + bra return_to_any \n\ + \n\ +.globl _return_to_user \n\ +return_to_user: \n\ + move.l registers+60,%a0 /* get usp */ \n\ + move.l %a0,%usp /* set usp */ \n\ + move.l superStack,%sp /* get original stack pointer */ \n\ + \n\ +return_to_any: \n\ + move.l lastFrame,%a0 /* get last frame info */ \n\ + move.l (%a0)+,lastFrame /* link in previous frame */ \n\ + addq.l #8,%a0 /* skip over pc, vector#*/ \n\ + move.w (%a0)+,%d0 /* get # of words in cpu frame */ \n\ + add.w %d0,%a0 /* point to end of data */ \n\ + add.w %d0,%a0 /* point to end of data */ \n\ + move.l %a0,%a1 \n\ +# \n\ +# copy the stack frame \n\ + subq.l #1,%d0 \n\ +copyUserLoop: \n\ + move.w -(%a1),-(%sp) \n\ + dbf %d0,copyUserLoop \n\ +"); + RESTORE_FP_REGS() + __asm__ (" movem.l registers,%d0-%d7/%a0-%a6"); + __asm__ (" rte"); /* pop and go! */ + +#define DISABLE_INTERRUPTS() __asm__ (" oriw #0x0700,%sr"); +#define BREAKPOINT() __asm__ (" trap #2"); + +/* this function is called immediately when a level 7 interrupt occurs */ +/* if the previous interrupt level was 7 then we're already servicing */ +/* this interrupt and an rte is in order to return to the debugger. */ +/* For the 68000, the offset for sr is 6 due to the jsr return address */ +__asm__ (" \n\ +.text \n\ +.globl _debug_level7 \n\ +_debug_level7: \n\ + move.w %d0,-(%sp)"); +#if M68K_HAS_VBR +__asm__ (" move.w 2(%sp),%d0"); +#else +__asm__ (" move.w 6(%sp),%d0"); +#endif +__asm__ (" andi.w #0x700,%d0 \n\ + cmpi.w #0x700,%d0 \n\ + beq already7 \n\ + move.w (%sp)+,%d0 \n\ + bra _catchException \n\ +already7: \n\ + move.w (%sp)+,%d0"); +#if !M68K_HAS_VBR +__asm__ (" lea 4(%sp),%sp"); /* pull off 68000 return address */ +#endif +__asm__ (" rte"); + +#if M68K_HAS_VBR +/* This function is called when a 68020 exception occurs. It saves + * all the cpu and fpcp regs in the _registers array, creates a frame on a + * linked list of frames which has the cpu and fpcp stack frames needed + * to properly restore the context of these processors, and invokes + * an exception handler (remcom_handler). + * + * stack on entry: stack on exit: + * N bytes of junk exception # MSWord + * Exception Format Word exception # MSWord + * Program counter LSWord + * Program counter MSWord + * Status Register + * + * + */ +__asm__ (" \n\ +.text \n\ +.globl _catchException \n\ +_catchException:"); +DISABLE_INTERRUPTS(); +__asm__ (" \n\ + movem.l %d0-%d7/%a0-%a6,registers /* save registers */ \n\ + move.l lastFrame,%a0 /* last frame pointer */ \n\ +"); +SAVE_FP_REGS(); +__asm__ ("\n\ + lea registers,%a5 /* get address of registers */\n\ + move.w (%sp),%d1 /* get status register */\n\ + move.w %d1,66(%a5) /* save sr */ \n\ + move.l 2(%sp),%a4 /* save pc in a4 for later use */\n\ + move.w 6(%sp),%d0 /* get '020 exception format */\n\ + move.w %d0,%d2 /* make a copy of format word */\n\ +#\n\ +# compute exception number\n\ + and.l #0xfff,%d2 /* mask off vector offset */\n\ + lsr.w #2,%d2 /* divide by 4 to get vect num */\n\ +/* #if 1 */\n\ + cmp.l #33,%d2\n\ + bne nopc_adjust\n\ + subq.l #2,%a4\n\ +nopc_adjust:\n\ +/* #endif */\n\ + move.l %a4,68(%a5) /* save pc in _regisers[] */\n\ +\n\ +#\n\ +# figure out how many bytes in the stack frame\n\ + andi.w #0xf000,%d0 /* mask off format type */\n\ + rol.w #5,%d0 /* rotate into the low byte *2 */\n\ + lea exceptionSize,%a1 \n\ + add.w %d0,%a1 /* index into the table */\n\ + move.w (%a1),%d0 /* get number of words in frame */\n\ + move.w %d0,%d3 /* save it */\n\ + sub.w %d0,%a0 /* adjust save pointer */\n\ + sub.w %d0,%a0 /* adjust save pointer(bytes) */\n\ + move.l %a0,%a1 /* copy save pointer */\n\ + subq.l #1,%d0 /* predecrement loop counter */\n\ +#\n\ +# copy the frame\n\ +saveFrameLoop:\n\ + move.w (%sp)+,(%a1)+\n\ + dbf %d0,saveFrameLoop\n\ +#\n\ +# now that the stack has been clenaed,\n\ +# save the a7 in use at time of exception\n\ + move.l %sp,superStack /* save supervisor sp */\n\ + andi.w #0x2000,%d1 /* were we in supervisor mode ? */\n\ + beq userMode \n\ + move.l %a7,60(%a5) /* save a7 */\n\ + bra a7saveDone\n\ +userMode: \n\ + move.l %usp,%a1 \n\ + move.l %a1,60(%a5) /* save user stack pointer */\n\ +a7saveDone:\n\ +\n\ +#\n\ +# save size of frame\n\ + move.w %d3,-(%a0)\n\ +\n\ + move.l %d2,-(%a0) /* save vector number */\n\ +#\n\ +# save pc causing exception\n\ + move.l %a4,-(%a0)\n\ +#\n\ +# save old frame link and set the new value\n\ + move.l lastFrame,%a1 /* last frame pointer */\n\ + move.l %a1,-(%a0) /* save pointer to prev frame */\n\ + move.l %a0,lastFrame\n\ +\n\ + move.l %d2,-(%sp) /* push exception num */\n\ + move.l exceptionHook,%a0 /* get address of handler */\n\ + jbsr (%a0) /* and call it */\n\ + clr.l (%sp) /* replace exception num parm with frame ptr */\n\ + jbsr _returnFromException /* jbsr, but never returns */\n\ +"); +#else /* mc68000 */ +/* This function is called when an exception occurs. It translates the + * return address found on the stack into an exception vector # which + * is then handled by either handle_exception or a system handler. + * _catchException provides a front end for both. + * + * stack on entry: stack on exit: + * Program counter MSWord exception # MSWord + * Program counter LSWord exception # MSWord + * Status Register + * Return Address MSWord + * Return Address LSWord + */ +__asm__ ("\n\ +.text\n\ +.globl _catchException\n\ +_catchException:"); +DISABLE_INTERRUPTS(); +__asm__ ("\ + moveml %d0-%d7/%a0-%a6,registers /* save registers */ \n\ + movel lastFrame,%a0 /* last frame pointer */ \n\ +"); +SAVE_FP_REGS(); +__asm__ (" \n\ + lea registers,%a5 /* get address of registers */ \n\ + movel (%sp)+,%d2 /* pop return address */ \n\ + addl #1530,%d2 /* convert return addr to */ \n\ + divs #6,%d2 /* exception number */ \n\ + extl %d2 \n\ + \n\ + moveql #3,%d3 /* assume a three word frame */ \n\ + \n\ + cmpiw #3,%d2 /* bus error or address error ? */ \n\ + bgt normal /* if >3 then normal error */ \n\ + movel (%sp)+,-(%a0) /* copy error info to frame buff*/ \n\ + movel (%sp)+,-(%a0) /* these are never used */ \n\ + moveql #7,%d3 /* this is a 7 word frame */ \n\ + \n\ +normal: \n\ + movew (%sp)+,%d1 /* pop status register */ \n\ + movel (%sp)+,%a4 /* pop program counter */ \n\ + movew %d1,66(%a5) /* save sr */ \n\ + movel %a4,68(%a5) /* save pc in _regisers[] */ \n\ + movel %a4,-(%a0) /* copy pc to frame buffer */ \n\ + movew %d1,-(%a0) /* copy sr to frame buffer */ \n\ + \n\ + movel %sp,superStack /* save supervisor sp */ \n\ + \n\ + andiw #0x2000,%d1 /* were we in supervisor mode ? */ \n\ + beq userMode \n\ + movel %a7,60(%a5) /* save a7 */ \n\ + bra saveDone \n\ +userMode: \n\ + movel %usp,%a1 /* save user stack pointer */ \n\ + movel %a1,60(%a5) /* save user stack pointer */ \n\ +saveDone: \n\ + \n\ + movew %d3,-(%a0) /* push frame size in words */ \n\ + movel %d2,-(%a0) /* push vector number */ \n\ + movel %a4,-(%a0) /* push exception pc */ \n\ + \n\ +# \n\ +# save old frame link and set the new value \n\ + movel _lastFrame,%a1 /* last frame pointer */ \n\ + movel %a1,-(%a0) /* save pointer to prev frame */ \n\ + movel %a0,lastFrame \n\ + \n\ + movel %d2,-(%sp) /* push exception num */ \n\ + movel exceptionHook,%a0 /* get address of handler */ \n\ + jbsr (%a0) /* and call it */ \n\ + clrl (%sp) /* replace exception num parm with frame ptr */ \n\ + jbsr _returnFromException /* jbsr, but never returns */ \n\ +"); +#endif + +/* + * remcomHandler is a front end for handle_exception. It moves the + * stack pointer into an area reserved for debugger use in case the + * breakpoint happened in supervisor mode. + */ +__asm__ ("remcomHandler:"); +__asm__ (" add.l #4,%sp"); /* pop off return address */ +__asm__ (" move.l (%sp)+,%d0"); /* get the exception number */ +__asm__ (" move.l stackPtr,%sp"); /* move to remcom stack area */ +__asm__ (" move.l %d0,-(%sp)"); /* push exception onto stack */ +__asm__ (" jbsr handle_exception"); /* this never returns */ +__asm__ (" rts"); /* return */ +} /* end of stub_dummy_asm_wrapper function */ + +void _returnFromException( Frame *frame ) +{ + /* if no passed in frame, use the last one */ + if (! frame) + { + frame = lastFrame; + frame->frameSize = 4; + frame->format = 0; + frame->fsaveHeader = -1; /* restore regs, but we dont have fsave info*/ + } + +#if !M68K_HAS_VBR + /* a 68000 cannot use the internal info pushed onto a bus error + * or address error frame when doing an RTE so don't put this info + * onto the stack or the stack will creep every time this happens. + */ + frame->frameSize=3; +#endif + + /* throw away any frames in the list after this frame */ + lastFrame = frame; + + frame->sr = registers[(int) PS]; + frame->pc = registers[(int) PC]; + + if (registers[(int) PS] & 0x2000) + { + /* return to supervisor mode... */ + return_to_super(); + } + else + { /* return to user mode */ + return_to_user(); + } +} + +int hex(ch) +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); +} + +/* scan for the sequence $# */ +void getpacket(buffer) +char * buffer; +{ + unsigned char checksum; + unsigned char xmitcsum; + int i; + int count; + char ch; + + do { + /* wait around for the start character, ignore all other characters */ + while ((ch = getDebugChar()) != '$'); + checksum = 0; + xmitcsum = -1; + + count = 0; + + /* now, read until a # or end of buffer is found */ + while (count < BUFMAX) { + ch = getDebugChar(); + if (ch == '#') break; + checksum = checksum + ch; + buffer[count] = ch; + count = count + 1; + } + buffer[count] = 0; + + if (ch == '#') { + xmitcsum = hex(getDebugChar()) << 4; + xmitcsum += hex(getDebugChar()); + if ((remote_debug ) && (checksum != xmitcsum)) { + fprintf (stderr,"bad checksum. My count = 0x%x, sent=0x%x. buf=%s\n", + checksum,xmitcsum,buffer); + } + + if (checksum != xmitcsum) putDebugChar('-'); /* failed checksum */ + else { + putDebugChar('+'); /* successful transfer */ + /* if a sequence char is present, reply the sequence ID */ + if (buffer[2] == ':') { + putDebugChar( buffer[0] ); + putDebugChar( buffer[1] ); + /* remove sequence chars from buffer */ + count = strlen(buffer); + for (i=3; i <= count; i++) buffer[i-3] = buffer[i]; + } + } + } + } while (checksum != xmitcsum); + +} + +/* send the packet in buffer. The host get's one chance to read it. + This routine does not wait for a positive acknowledge. */ + +/* + * Send the packet in buffer and wait for a positive acknowledgement. + */ +static void +putpacket (char *buffer) +{ + int checksum; + + /* $# */ + do + { + char *src = buffer; + putDebugChar ('$'); + checksum = 0; + + while (*src != '\0') + { + int runlen = 0; + + /* Do run length encoding */ + while ((src[runlen] == src[0]) && (runlen < 99)) + runlen++; + if (runlen > 3) + { + int encode; + /* Got a useful amount */ + putDebugChar (*src); + checksum += *src; + putDebugChar ('*'); + checksum += '*'; + checksum += (encode = (runlen - 4) + ' '); + putDebugChar (encode); + src += runlen; + } + else + { + putDebugChar (*src); + checksum += *src; + src++; + } + } + + putDebugChar ('#'); + putDebugChar (highhex (checksum)); + putDebugChar (lowhex (checksum)); + } + while (getDebugChar () != '+'); +} + +char remcomInBuffer[BUFMAX]; +char remcomOutBuffer[BUFMAX]; +static short error; + +void debug_error( + char * format, + char * parm +) +{ + if (remote_debug) fprintf (stderr,format,parm); +} + +/* convert the memory pointed to by mem into hex, placing result in buf */ +/* return a pointer to the last char put in buf (null) */ +char* mem2hex(mem, buf, count) +char* mem; +char* buf; +int count; +{ + int i; + unsigned char ch; + for (i=0;i> 4]; + *buf++ = gdb_hexchars[ch % 16]; + } + *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 */ +char* hex2mem(buf, mem, count) +char* buf; +char* mem; +int count; +{ + int i; + unsigned char ch; + for (i=0;i=0) + { + *intValue = (*intValue <<4) | hexValue; + numChars ++; + } + else + break; + + (*ptr)++; + } + + return (numChars); +} + +/* + * This support function prepares and sends the message containing the + * basic information about this exception. + */ + +void gdb_stub_report_exception_info( + int vector, + int *regs, + int thread +) +{ + char *optr; + int sigval; + + optr = remcomOutBuffer; + *optr++ = 'T'; + sigval = computeSignal (vector); + *optr++ = highhex (sigval); + *optr++ = lowhex (sigval); + + *optr++ = highhex(A7); + *optr++ = lowhex(A7); + *optr++ = ':'; + optr = mem2hstr(optr, + (unsigned char *)&(regs[A7]), + sizeof(regs[A7])); + *optr++ = ';'; + + *optr++ = highhex(PC); + *optr++ = lowhex(PC); + *optr++ = ':'; + optr = mem2hstr(optr, + (unsigned char *)&(regs[PC]), + sizeof(regs[PC]) ); + *optr++ = ';'; + +#if defined(GDB_STUB_ENABLE_THREAD_SUPPORT) + if (do_threads) + { + *optr++ = 't'; + *optr++ = 'h'; + *optr++ = 'r'; + *optr++ = 'e'; + *optr++ = 'a'; + *optr++ = 'd'; + *optr++ = ':'; + optr = thread2vhstr(optr, thread); + *optr++ = ';'; + } +#endif + *optr++ = '\0'; +} + +/* + * This function does all command procesing for interfacing to gdb. + */ +void handle_exception(int exceptionVector) +{ + int host_has_detached = 0; + int addr, length; + char * ptr; + int newPC; + Frame *frame; + int current_thread; /* current generic thread */ + int thread; /* stopped thread: context exception happened in */ + void *regptr; + int binary; + + if (remote_debug) printf("vector=%d, sr=0x%x, pc=0x%x\n", + exceptionVector, + registers[ PS ], + registers[ PC ]); + + thread = 0; +#if defined(GDB_STUB_ENABLE_THREAD_SUPPORT) + if (do_threads) { + thread = rtems_gdb_stub_get_current_thread(); + } +#endif + current_thread = thread; + +#if 0 + /* reply to host that an exception has occurred */ + sigval = computeSignal( exceptionVector ); + remcomOutBuffer[0] = 'S'; + remcomOutBuffer[1] = gdb_hexchars[sigval >> 4]; + remcomOutBuffer[2] = gdb_hexchars[sigval % 16]; + remcomOutBuffer[3] = 0; +#else + /* reply to host that an exception has occurred with some basic info */ + gdb_stub_report_exception_info(exceptionVector, registers, thread); +#endif + + putpacket(remcomOutBuffer); + + while (!(host_has_detached)) { + binary = 0; + error = 0; + remcomOutBuffer[0] = 0; + getpacket(remcomInBuffer); + switch (remcomInBuffer[0]) { +#if 0 + case '?' : remcomOutBuffer[0] = 'S'; + remcomOutBuffer[1] = gdb_hexchars[sigval >> 4]; + remcomOutBuffer[2] = gdb_hexchars[sigval % 16]; + remcomOutBuffer[3] = 0; + break; +#else + case '?' : gdb_stub_report_exception_info(exceptionVector, + registers, + thread); + break; +#endif + case 'd' : remote_debug = !(remote_debug); /* toggle debug flag */ + break; + case 'D' : /* host has detached */ + strcpy(remcomOutBuffer,"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 = ¤t_thread_registers; +#endif + mem2hex (regptr, remcomOutBuffer, sizeof registers); + break; + + case 'G': /* set the values of the CPU registers - return OK */ + regptr = registers; +#if defined(GDB_STUB_ENABLE_THREAD_SUPPORT) + if (do_threads && current_thread != thread ) + regptr = ¤t_thread_registers; +#endif + if (hex2mem (&remcomInBuffer[1], + regptr, + sizeof registers)) { + strcpy (remcomOutBuffer, "OK"); + } + else { + strcpy (remcomOutBuffer, "E00"); /* E00 = bad "set register" command */ + } + break; + + /* mAA..AA,LLLL Read LLLL bytes at address AA..AA */ + case 'm' : + if (setjmp(remcomEnv) == 0) + { + m68k_exceptionHandler(2,handle_buserror); + + /* TRY TO READ %x,%x. IF SUCCEED, SET PTR = 0 */ + ptr = &remcomInBuffer[1]; + if (hexToInt(&ptr,&addr)) + if (*(ptr++) == ',') + if (hexToInt(&ptr,&length)) + { + ptr = 0; + mem2hex((char*) addr, remcomOutBuffer, length); + } + + if (ptr) + { + strcpy(remcomOutBuffer,"E01"); + debug_error("malformed read memory command: %s",remcomInBuffer); + } + } + else { + m68k_exceptionHandler(2,_catchException); + strcpy(remcomOutBuffer,"E03"); + debug_error("bus error", 0); + } + + /* restore handler for bus error */ + m68k_exceptionHandler(2,_catchException); + break; + + /* XAA..AA,LLLL: Write LLLL bytes at address AA.AA return OK */ + /* Transfer is in binary, '$', '#' and 0x7d is escaped with 0x7d */ + case 'X' : + binary = 1; + /* MAA..AA,LLLL: Write LLLL bytes at address AA.AA return OK */ + case 'M' : + if (setjmp(remcomEnv) == 0) { + m68k_exceptionHandler(2,handle_buserror); + + /* TRY TO READ '%x,%x:'. IF SUCCEED, SET PTR = 0 */ + ptr = &remcomInBuffer[1]; + if (hexToInt(&ptr,&addr)) + if (*(ptr++) == ',') + if (hexToInt(&ptr,&length)) + if (*(ptr++) == ':') + { + if (binary) { + bin2mem(ptr,(char *)addr,length); + } + else { + hex2mem(ptr, (char*) addr, length); + } + ptr = 0; + strcpy(remcomOutBuffer,"OK"); + } + if (ptr) + { + strcpy(remcomOutBuffer,"E02"); + debug_error("malformed write memory command: %s",remcomInBuffer); + } + } + else { + m68k_exceptionHandler(2,_catchException); + strcpy(remcomOutBuffer,"E03"); + debug_error("bus error", 0); + } + + /* restore handler for bus error */ + m68k_exceptionHandler(2,_catchException); + break; + + /* cAA..AA Continue at address AA..AA(optional) */ + /* sAA..AA Step one instruction from AA..AA(optional) */ + case 'c' : + case 's' : + /* try to read optional parameter, pc unchanged if no parm */ + ptr = &remcomInBuffer[1]; + if (hexToInt(&ptr,&addr)) + registers[ PC ] = addr; + + newPC = registers[ PC]; + + /* clear the trace bit */ + registers[ PS ] &= 0x7fff; + + /* set the trace bit if we're stepping */ + if (remcomInBuffer[0] == 's') registers[ PS ] |= 0x8000; + + /* + * look for newPC in the linked list of exception frames. + * if it is found, use the old frame it. otherwise, + * fake up a dummy frame in returnFromException(). + */ + if (remote_debug) printf("new pc = 0x%x\n",newPC); + frame = lastFrame; + while (frame) + { + if (remote_debug) + printf("frame at 0x%x has pc=0x%x, except#=%d\n", + (uint32_t) frame, + (uint32_t) frame->exceptionPC, + (uint32_t) frame->exceptionVector); + if (frame->exceptionPC == newPC) break; /* bingo! a match */ + /* + * for a breakpoint instruction, the saved pc may + * be off by two due to re-executing the instruction + * replaced by the trap instruction. Check for this. + */ + if ((frame->exceptionVector == 33) && + (frame->exceptionPC == (newPC+2))) break; + if (frame == frame->previous) + { + frame = 0; /* no match found */ + break; + } + frame = frame->previous; + } + + /* + * If we found a match for the PC AND we are not returning + * as a result of a breakpoint (33), + * trace exception (9), nmi (31), jmp to + * the old exception handler as if this code never ran. + */ + if (frame) + { + if ((frame->exceptionVector != 9) && + (frame->exceptionVector != 31) && + (frame->exceptionVector != 33)) + { + /* + * invoke the previous handler. + */ + if (oldExceptionHook) + (*oldExceptionHook) (frame->exceptionVector); + newPC = registers[ PC ]; /* pc may have changed */ + if (newPC != frame->exceptionPC) + { + if (remote_debug) + printf("frame at 0x%x has pc=0x%x, except#=%d\n", + (uint32_t) frame, + frame->exceptionPC, + frame->exceptionVector); + /* re-use the last frame, we're skipping it (longjump?)*/ + frame = (Frame *) 0; + _returnFromException( frame ); /* this is a jump */ + } + } + } + + /* if we couldn't find a frame, create one */ + if (frame == 0) + { + frame = lastFrame -1 ; + + /* by using a bunch of print commands with breakpoints, + it's possible for the frame stack to creep down. If it creeps + too far, give up and reset it to the top. Normal use should + not see this happen. + */ + if ((unsigned int) (frame-2) < (unsigned int) &gdbFrameStack) + { + initializeRemcomErrorFrame(); + frame = lastFrame; + } + frame->previous = lastFrame; + lastFrame = frame; + frame = 0; /* null so _return... will properly initialize it */ + } + + _returnFromException( frame ); /* this is a jump */ + + break; + + case 'q': /* queries */ +#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) + 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 + + case 'H': /* set new thread */ +#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 *) ¤t_thread_registers); + } + + /* Read new registers if necessary */ + if (tmp != thread) { + ret = rtems_gdb_stub_get_thread_regs( + tmp, (unsigned int *) ¤t_thread_registers); + + if (!ret) { + /* Thread does not exist */ + strcpy(remcomOutBuffer, "E02"); + break; + } + } + + current_thread = tmp; + strcpy(remcomOutBuffer, "OK"); + } +#endif + break; + + /* kill the program */ + case 'k' : /* do nothing */ + break; + } /* switch */ + + /* reply to the request */ + putpacket(remcomOutBuffer); + } +} + +void +initializeRemcomErrorFrame() +{ + lastFrame = ((Frame *) &gdbFrameStack[FRAMESIZE-1]) - 1; + lastFrame->previous = lastFrame; +} + +/* this function is used to set up exception handlers for tracing and + breakpoints */ +void set_debug_traps() +{ + extern void _debug_level7(); + extern void remcomHandler(); + int exception; + + initializeRemcomErrorFrame(); + stackPtr = &remcomStack[STACKSIZE/sizeof(int) - 1]; + + for (exception = 2; exception <= 23; exception++) + m68k_exceptionHandler(exception,_catchException); + + /* level 7 interrupt */ + m68k_exceptionHandler(31,_debug_level7); + + /* GDB breakpoint exception (trap #1) */ + m68k_exceptionHandler(33,_catchException); + + /* coded breakpoint exception (trap #2) */ + m68k_exceptionHandler(34,_catchException); + + /* This is a trap #8 instruction. Apparently it is someone's software + convention for some sort of SIGFPE condition. Whose? How many + people are being screwed by having this code the way it is? + Is there a clean solution? */ + m68k_exceptionHandler(40,_catchException); + + /* 48 to 54 are floating point coprocessor errors */ + for (exception = 48; exception <= 54; exception++) + m68k_exceptionHandler(exception,_catchException); + + if (oldExceptionHook != remcomHandler) + { + oldExceptionHook = exceptionHook; + exceptionHook = remcomHandler; + } + + initialized = true; + +} + +/* This function will generate a breakpoint exception. It is used at the + beginning of a program to sync up with a debugger and can be used + otherwise as a quick means to stop program execution and "break" into + the debugger. */ + +void breakpoint() +{ + if (initialized) BREAKPOINT(); +} diff --git a/bsps/mips/shared/gdbstub/README b/bsps/mips/shared/gdbstub/README new file mode 100644 index 0000000000..e61d0f0aa5 --- /dev/null +++ b/bsps/mips/shared/gdbstub/README @@ -0,0 +1,151 @@ +/*****************************************************/ + +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 + + C. M. Heard + VVNET, Inc. phone: +1 408 247 9376 + 4040 Moorpark Ave. Suite 206 fax: +1 408 244 3651 + San Jose, CA 95117 USA e-mail: heard@vvnet.com + +This package was released in the September 1996 time frame for use +with gdb 4.16 and an IDT R4600 Orion. The stub was modified to support +R3000 class CPUs and to work within the mips-rtems exeception processing +framework. + +THe file memlimits.h could end up being target board dependent. If +this is the case, copy it to your BSP directory and modify as necessary. + +--joel +8 February 2002 + +Original README +=============== + +The r46kstub directory and its compressed archive (r46kstub.tar.gz) contain +the 9/29/96 source code snapshot for a ROM-resident gdb-4.16 debug agent +(aka stub) for the IDT R4600 Orion processor. It is based on the stub for +the Hitachi SH processor written by Ben Lee and Steve Chamberlain and +supplied with the gdb-4.16 distribution; that stub in turn was "originally +based on an m68k software stub written by Glenn Engel at HP, but has changed +quite a bit". The modifications for the R4600 were contributed by C. M. +Heard of VVNET, Inc. and were based in part on the Algorithmics R4000 version +of Phil Bunce's PMON program. + +The distribution consists of the following files: + +-rw-r--r-- 1 1178 Sep 29 16:34 ChangeLog +-rw-r--r-- 1 748 Jul 26 01:18 Makefile +-rw-r--r-- 1 6652 Sep 29 16:34 README +-rw-r--r-- 1 1829 May 21 02:02 gdb_if.h +-rw-r--r-- 1 3745 Sep 29 14:03 ioaddr.h +-rw-r--r-- 1 2906 Sep 29 14:39 limits.h +-rw-r--r-- 1 6552 May 23 00:17 mips_opcode.h +-rw-r--r-- 1 14017 May 21 02:04 r4600.h +-rw-r--r-- 1 23874 Jul 21 20:31 r46kstub.c +-rw-r--r-- 1 1064 Jul 3 12:35 r46kstub.ld +-rw-r--r-- 1 13299 Sep 29 16:24 stubinit.S + +With the exception of mips_opcode.h, which is a slightly modified version +of a header file contributed by Ralph Campbell to 4.4 BSD and is therefore +copyrighted by the UC Regents, all of the source files have been dedicated +by their authors to the public domain. Use them as you wish, but do so +at your own risk! The authors accept _no_ responsibility for any errors. + +The debug agent contained herein is at this writing in active use at VVNET +supporting initial hardware debug and board bring-up of an OC-12 ATM probe +board. It uses polled I/O on a 16C450 UART. We had originally intended to +add support for interrupts to allow gdb to break in on a running program, +but we have found that this is not really necessary since the reset button +will accomplish the same purpose (thanks to the MIPS feature of saving the +program counter in the ErrorEPC register when a reset exception occurs). + +Be aware that this stub handles ALL interrupts and exceptions except for +reset (or NMI) in the same way -- by passing control to the debug command +loop. It of course uses the ROM exception vectors to do so. In order to +support code that actally needs to use interrupts we use use a more elaborate +stub that is linked with the downloaded program. It hooks the RAM exception +vectors and clears the BEV status bit to gain control. The ROM-based stub +is still used in this case for initial program loading. + +In order to port this stub to a different platform you will at a minimum +need to customize the macros in limits.h (which define the limits of readable, +writeable, and steppable address space) and the I/O addresses in ioaddr.h +(which define the 16C450 MMIO addresses). If you use something other than +a 16C450 UART you will probably also need to modify the portions of stubinit.S +which deal with the serial port. I've tried to be careful to respect all the +architecturally-defined hazards as described in Appendix F of Kane and +Heinrich, MIPS RISC Architecture, in order to minimize the work in porting +to 4000-series processors other than the R4600, but no guarantees are offered. +Support is presently restricted to big-endian addressing, and I've not even +considered what changes would be needed for little-endian support. + +When this stub is built with gcc-2.7.2 and binutils-2.6 you will see a few +warning messages from the single-step support routine where a cast is used +to sign-extend a pointer (the next instruction address) into a long long +(the PC image). Those warnings are expected; I've checked the generated +code and it is doing what I had intended. But you should not see any other +warnings or errors. Here is a log of the build: + +mips64orion-idt-elf-gcc -g -Wa,-ahld -Wall -membedded-data \ + -O3 -c r46kstub.c >r46kstub.L +r46kstub.c: In function `doSStep': +r46kstub.c:537: warning: cast to pointer from integer of different size +r46kstub.c:539: warning: cast to pointer from integer of different size +r46kstub.c:547: warning: cast to pointer from integer of different size +r46kstub.c:561: warning: cast to pointer from integer of different size +r46kstub.c:563: warning: cast to pointer from integer of different size +r46kstub.c:572: warning: cast to pointer from integer of different size +r46kstub.c:574: warning: cast to pointer from integer of different size +r46kstub.c:582: warning: cast to pointer from integer of different size +r46kstub.c:589: warning: cast to pointer from integer of different size +r46kstub.c:591: warning: cast to pointer from integer of different size +r46kstub.c:597: warning: cast to pointer from integer of different size +r46kstub.c:599: warning: cast to pointer from integer of different size +r46kstub.c:605: warning: cast to pointer from integer of different size +r46kstub.c:607: warning: cast to pointer from integer of different size +r46kstub.c:613: warning: cast to pointer from integer of different size +r46kstub.c:615: warning: cast to pointer from integer of different size +r46kstub.c:624: warning: cast to pointer from integer of different size +r46kstub.c:628: warning: cast to pointer from integer of different size +r46kstub.c:635: warning: cast to pointer from integer of different size +r46kstub.c:637: warning: cast to pointer from integer of different size +mips64orion-idt-elf-gcc -g -Wa,-ahld -Wall -membedded-data \ + -O3 -c stubinit.S >stubinit.L +mips64orion-idt-elf-ld -t -s -T r46kstub.ld -Map r46kstub.map -o r46kstub.out +mips64orion-idt-elf-ld: mode elf32bmip +stubinit.o +r46kstub.o +mips64orion-idt-elf-objcopy -S -R .bss -R .data -R .reginfo \ + -O srec r46kstub.out r46kstub.hex + +Limitations: stubinit.S deliberately forces the PC (which is a 64-bit +register) to contain a legitimate sign-extended 32-bit value. This was +done to cope with a bug in gdb-4.16, which does _not_ properly sign-extend +the initial PC when it loads a program. This means that you cannot use +the "set" command to load an unmapped sixty-four bit virtual address into +the PC, as you can for all other registers. + +Please send bug reports, comments, or suggestions for improvement to: diff --git a/bsps/mips/shared/gdbstub/gdb_if.h b/bsps/mips/shared/gdbstub/gdb_if.h new file mode 100644 index 0000000000..ba4f0eb757 --- /dev/null +++ b/bsps/mips/shared/gdbstub/gdb_if.h @@ -0,0 +1,194 @@ +/** + * @file + * @ingroup mips_gdb + * @brief Definition of the interface between stub and gdb + */ + +/* + * 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. + */ + +/** + * @defgroup mips_gdb GDB Interface + * @ingroup mips_shared + * @brief GDB Interface + * @{ + */ + +#ifndef _GDB_IF_H +#define _GDB_IF_H + +/** @brief 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]; +}; + +/** + * @name 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 +); + +/** @} */ + +/** + * @name MIPS registers + * @brief Numbered in the order in which gdb expects to see them. + * @{ + */ + +#define ZERO 0 +#define AT 1 +#define V0 2 +#define V1 3 +#define A0 4 +#define A1 5 +#define A2 6 +#define A3 7 + +#define T0 8 +#define T1 9 +#define T2 10 +#define T3 11 +#define T4 12 +#define T5 13 +#define T6 14 +#define T7 15 + +#define S0 16 +#define S1 17 +#define S2 18 +#define S3 19 +#define S4 20 +#define S5 21 +#define S6 22 +#define S7 23 + +#define T8 24 +#define T9 25 +#define K0 26 +#define K1 27 +#define GP 28 +#define SP 29 +#define S8 30 +#define RA 31 + +#define SR 32 +#define LO 33 +#define HI 34 +#define BAD_VA 35 +#define CAUSE 36 +#define PC 37 + +#define F0 38 +#define F1 39 +#define F2 40 +#define F3 41 +#define F4 42 +#define F5 43 +#define F6 44 +#define F7 45 + +#define F8 46 +#define F9 47 +#define F10 48 +#define F11 49 +#define F12 50 +#define F13 51 +#define F14 52 +#define F15 53 + +#define F16 54 +#define F17 55 +#define F18 56 +#define F19 57 +#define F20 58 +#define F21 59 +#define F22 60 +#define F23 61 + +#define F24 62 +#define F25 63 +#define F26 64 +#define F27 65 +#define F28 66 +#define F29 67 +#define F30 68 +#define F31 69 + +#define FCSR 70 +#define FIRR 71 + +#define NUM_REGS 72 + +/** @} */ + +void mips_gdb_stub_install(int enableThreads) ; + +#define MEMOPT_READABLE 1 +#define MEMOPT_WRITEABLE 2 + +#ifndef NUM_MEMSEGS +#define NUM_MEMSEGS 10 +#endif + +int gdbstub_add_memsegment(unsigned,unsigned,int); + +/** @} */ + +#endif /* _GDB_IF_H */ diff --git a/bsps/mips/shared/gdbstub/memlimits.h b/bsps/mips/shared/gdbstub/memlimits.h new file mode 100644 index 0000000000..c60ca12111 --- /dev/null +++ b/bsps/mips/shared/gdbstub/memlimits.h @@ -0,0 +1,100 @@ +/** + * @file + * @ingroup mips_limits + * @brief Definition of machine and system dependent address limits. + */ + +/* + * limits.h - definition of machine & system dependent address limits + * + * 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. + */ + +#ifndef _MEMLIMITS_H_ +#define _MEMLIMITS_H_ + +/* + * The macros in this file are specific to a given implementation. + * The general rules for their construction are as follows: + * + * 1.) is_readable(addr,length) should be true if and only if the + * region starting at the given virtual address can be read + * _without_ causing an exception or other fatal error. Note + * that the stub will use the strictest alignment satisfied + * by _both_ addr and length (e.g., if both are divisible by + * 8 then the region will be read in double-word chunks). + * + * 2.) is_writeable(addr,length) should be true if and only if the + * region starting at the given virtual address can be written + * _without_ causing an exception or other fatal error. Note + * that the stub will use the strictest alignment satisfied + * by _both_ addr and length (e.g., if both are divisible by + * 8 then the region will be written in double-word chunks). + * + * 3.) is-steppable(ptr) whould be true if and only if ptr is the + * address of a writeable region of memory which may contain + * an executable instruction. At a minimum this requires that + * ptr be word-aligned (divisible by 4) and not point to EPROM + * or memory-mapped I/O. + * + * Note: in order to satisfy constraints related to cacheability + * of certain memory subsystems it may be necessary for regions + * of kseg0 and kseg1 which map to the same physical addresses + * to have different readability and/or writeability attributes. + */ + +/** + * @defgroup mips_limits Address Limits + * @ingroup mips_shared + * @brief Address Limits + */ + + +/* +#define K0_LIMIT_FOR_READ (K0BASE+0x18000000) +#define K1_LIMIT_FOR_READ (K1BASE+K1SIZE) + +#define is_readable(addr,length) \ + (((K0BASE <= addr) && ((addr + length) <= K0_LIMIT_FOR_READ)) \ + || ((K1BASE <= addr) && ((addr + length) <= K1_LIMIT_FOR_READ))) + +#define K0_LIMIT_FOR_WRITE (K0BASE+0x08000000) +#define K1_LIMIT_FOR_WRITE (K1BASE+0x1e000000) + +#define is_writeable(addr,length) \ + (((K0BASE <= addr) && ((addr + length) <= K0_LIMIT_FOR_WRITE)) \ + || ((K1BASE <= addr) && ((addr + length) <= K1_LIMIT_FOR_WRITE))) + +#define K0_LIMIT_FOR_STEP (K0BASE+0x08000000) +#define K1_LIMIT_FOR_STEP (K1BASE+0x08000000) + +#define is_steppable(ptr) \ + ((((int)ptr & 0x3) == 0) \ + && (((K0BASE <= (int)ptr) && ((int)ptr < K0_LIMIT_FOR_STEP)) \ + || ((K1BASE <= (int)ptr) && ((int)ptr < K1_LIMIT_FOR_STEP)))) + +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/bsps/mips/shared/gdbstub/mips-stub.c b/bsps/mips/shared/gdbstub/mips-stub.c new file mode 100644 index 0000000000..8320eb66a9 --- /dev/null +++ b/bsps/mips/shared/gdbstub/mips-stub.c @@ -0,0 +1,1589 @@ +#define GDB_STUB_ENABLE_THREAD_SUPPORT 1 +/******************************************************************************* + + 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. + +******************************************************************************** +* +* r46kstub.c -- target debugging stub for the IDT R4600 Orion processor +* +* This module is based on the stub for the Hitachi SH processor written by +* Ben Lee and Steve Chamberlain and supplied with gdb 4.16. The latter +* in turn "is originally based on an m68k software stub written by Glenn +* Engel at HP, but has changed quite a bit." The changes for the R4600 +* were written by C. M. Heard at VVNET. They were based in part on the +* Algorithmics R4000 version of Phil Bunce's PMON program. +* +* Remote communication protocol: +* +* A debug packet whose contents are +* is encapsulated for transmission in the form: +* +* $ # CSUM1 CSUM2 +* +* must be ASCII alphanumeric and cannot include characters +* '$' or '#'. If starts with two characters followed by +* ':', then the existing stubs interpret this as a sequence number. +* +* CSUM1 and CSUM2 are ascii hex representation of an 8-bit +* checksum of , the most significant nibble is sent first. +* the hex digits 0-9,a-f are used. +* +* Receiver responds with: +* +* + if CSUM is correct +* - if CSUM is incorrect +* +* is as follows. All values are encoded in ascii hex digits. +* +* Request Packet +* +* read registers g +* reply XX....X Each byte of register data +* is described by two hex digits. +* Registers are in the internal order +* for GDB, and the bytes in a register +* are in the same order the machine uses. +* or ENN for an error. +* +* write regs GXX..XX Each byte of register data +* is described by two hex digits. +* reply OK for success +* ENN for an error +* +* write reg Pn...=r... Write register n... with value r.... +* reply OK for success +* ENN for an error +* +* read mem mAA..AA,LLLL AA..AA is address, LLLL is length. +* reply XX..XX XX..XX is mem contents +* Can be fewer bytes than requested +* if able to read only part of the data. +* or ENN NN is errno +* +* write mem MAA..AA,LLLL:XX..XX +* AA..AA is address, +* LLLL is number of bytes, +* XX..XX is data +* reply OK for success +* ENN for an error (this includes the case +* where only part of the data was +* written). +* +* cont cAA..AA AA..AA is address to resume +* If AA..AA is omitted, +* resume at same address. +* +* step sAA..AA AA..AA is address to resume +* If AA..AA is omitted, +* resume at same address. +* +* There is no immediate reply to step or cont. +* The reply comes when the machine stops. +* It is SAA AA is the "signal number" +* +* last signal ? Reply with the reason for stopping. +* This is the same reply as is generated +* for step or cont: SAA where AA is the +* signal number. +* +* detach D Host is detaching. Reply OK and +* end remote debugging session. +* +* reserved On other requests, the stub should +* ignore the request and send an empty +* response ($#). This way +* we can extend the protocol and GDB +* can tell whether the stub it is +* talking to uses the old or the new. +* +* Responses can be run-length encoded to save space. A '*' means that +* the next character is an ASCII encoding giving a repeat count which +* stands for that many repetitions of the character preceding the '*'. +* The encoding is n+29, yielding a printable character when n >=3 +* (which is where rle starts to win). Don't use n > 99 since gdb +* masks each character is receives with 0x7f in order to strip off +* the parity bit. +* +* As an example, "0* " means the same thing as "0000". +* +*******************************************************************************/ + + +#include +#include +#include "mips_opcode.h" +/* #include "memlimits.h" */ +#include +#include +#include "gdb_if.h" + +/* Change it to something meaningful when debugging */ +#undef ASSERT +#define ASSERT(x) if(!(x)) printk("ASSERT: stub: %d\n", __LINE__) + +/***************/ +/* Exception Codes */ +#define EXC_INT 0 /* External interrupt */ +#define EXC_MOD 1 /* TLB modification exception */ +#define EXC_TLBL 2 /* TLB miss (Load or Ifetch) */ +#define EXC_TLBS 3 /* TLB miss (Store) */ +#define EXC_ADEL 4 /* Address error (Load or Ifetch) */ +#define EXC_ADES 5 /* Address error (Store) */ +#define EXC_IBE 6 /* Bus error (Ifetch) */ +#define EXC_DBE 7 /* Bus error (data load or store) */ +#define EXC_SYS 8 /* System call */ +#define EXC_BP 9 /* Break point */ +#define EXC_RI 10 /* Reserved instruction */ +#define EXC_CPU 11 /* Coprocessor unusable */ +#define EXC_OVF 12 /* Arithmetic overflow */ +#define EXC_TRAP 13 /* Trap exception */ +#define EXC_FPE 15 /* Floating Point Exception */ + +/* FPU Control/Status register fields */ +#define CSR_FS 0x01000000 /* Set to flush denormals to zero */ +#define CSR_C 0x00800000 /* Condition bit (set by FP compare) */ + +#define CSR_CMASK (0x3f<<12) +#define CSR_CE 0x00020000 +#define CSR_CV 0x00010000 +#define CSR_CZ 0x00008000 +#define CSR_CO 0x00004000 +#define CSR_CU 0x00002000 +#define CSR_CI 0x00001000 + +#define CSR_EMASK (0x1f<<7) +#define CSR_EV 0x00000800 +#define CSR_EZ 0x00000400 +#define CSR_EO 0x00000200 +#define CSR_EU 0x00000100 +#define CSR_EI 0x00000080 + +#define CSR_FMASK (0x1f<<2) +#define CSR_FV 0x00000040 +#define CSR_FZ 0x00000020 +#define CSR_FO 0x00000010 +#define CSR_FU 0x00000008 +#define CSR_FI 0x00000004 + +#define CSR_RMODE_MASK (0x3<<0) +#define CSR_RM 0x00000003 +#define CSR_RP 0x00000002 +#define CSR_RZ 0x00000001 +#define CSR_RN 0x00000000 + +/***************/ + +/* + * Saved register information. Must be prepared by the exception + * preprocessor before handle_exception is invoked. + */ +#if (__mips == 3) +typedef long long mips_register_t; +#define R_SZ 8 +#elif (__mips == 1) +typedef unsigned int mips_register_t; +#define R_SZ 4 +#else +#error "unknown MIPS ISA" +#endif +static mips_register_t *registers; + +#if defined(GDB_STUB_ENABLE_THREAD_SUPPORT) +static char do_threads; /* != 0 means we are supporting threads */ +#endif + +/* + * The following external functions provide character input and output. + */ +extern char getDebugChar (void); +extern void putDebugChar (char); + +/* + * Exception handler + */ +void handle_exception (rtems_vector_number vector, CPU_Interrupt_frame *frame); + +/* + * Prototype needed by this code and to keep it self contained. + */ +void rtems_interrupt_catch( rtems_isr_entry, int, rtems_isr_entry *); + +/* + * 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 + * register packets. Memory dump packets can profitably use even more. + */ +#define BUFMAX 1500 + +static char inBuffer[BUFMAX]; +static char outBuffer[BUFMAX]; + +/* Structure to keep info on a z-breaks */ +#define BREAKNUM 32 + +struct z0break +{ + /* List support */ + struct z0break *next; + struct z0break *prev; + + /* Location, preserved data */ + + /* the address pointer, really, really must be a pointer to + ** a 32 bit quantity (likely 64 on the R4k), so the full instruction is read & + ** written. Making it a char * as on the i386 will cause + ** the zbreaks to mess up the breakpoint instructions + */ + unsigned char *address; + unsigned instr; +}; + +static struct z0break z0break_arr[BREAKNUM]; +static struct z0break *z0break_avail = NULL; +static struct z0break *z0break_list = NULL; + + +/* + * Convert an int to hex. + */ +const char gdb_hexchars[] = "0123456789abcdef"; + +#define highhex(x) gdb_hexchars [(x >> 4) & 0xf] +#define lowhex(x) gdb_hexchars [x & 0xf] + +/* + * Convert length bytes of data starting at addr into hex, placing the + * result in buf. Return a pointer to the last (null) char in buf. + */ +static char * +mem2hex (void *_addr, int length, char *buf) +{ + unsigned int addr = (unsigned int) _addr; + + if (((addr & 0x7) == 0) && ((length & 0x7) == 0)) /* dword aligned */ + { + long long *source = (long long *) (addr); + long long *limit = (long long *) (addr + length); + + while (source < limit) + { + int i; + long long k = *source++; + + for (i = 15; i >= 0; i--) + *buf++ = gdb_hexchars [(k >> (i*4)) & 0xf]; + } + } + else if (((addr & 0x3) == 0) && ((length & 0x3) == 0)) /* word aligned */ + { + int *source = (int *) (addr); + int *limit = (int *) (addr + length); + + while (source < limit) + { + int i; + int k = *source++; + + for (i = 7; i >= 0; i--) + *buf++ = gdb_hexchars [(k >> (i*4)) & 0xf]; + } + } + else if (((addr & 0x1) == 0) && ((length & 0x1) == 0)) /* halfword aligned */ + { + short *source = (short *) (addr); + short *limit = (short *) (addr + length); + + while (source < limit) + { + int i; + short k = *source++; + + for (i = 3; i >= 0; i--) + *buf++ = gdb_hexchars [(k >> (i*4)) & 0xf]; + } + } + else /* byte aligned */ + { + char *source = (char *) (addr); + char *limit = (char *) (addr + length); + + while (source < limit) + { + int i; + char k = *source++; + + for (i = 1; i >= 0; i--) + *buf++ = gdb_hexchars [(k >> (i*4)) & 0xf]; + } + } + + *buf = '\0'; + return (buf); +} + + +/* + * Convert a hex character to an int. + */ +static int +hex (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 a string from hex to int until a non-hex digit + * is found. Return the number of characters processed. + */ +static int +hexToInt (char **ptr, int *intValue) +{ + int numChars = 0; + int hexValue; + + *intValue = 0; + + while (**ptr) + { + hexValue = hex (**ptr); + if (hexValue >= 0) + { + *intValue = (*intValue << 4) | hexValue; + numChars++; + } + else + break; + + (*ptr)++; + } + + return (numChars); +} + +/* + * Convert a string from hex to long long until a non-hex + * digit is found. Return the number of characters processed. + */ +static int +hexToLongLong (char **ptr, long long *intValue) +{ + int numChars = 0; + int hexValue; + + *intValue = 0; + + while (**ptr) + { + hexValue = hex (**ptr); + if (hexValue >= 0) + { + *intValue = (*intValue << 4) | hexValue; + numChars++; + } + else + break; + + (*ptr)++; + } + + return (numChars); +} + +/* + * Convert the hex array buf into binary, placing the result at the + * specified address. If the conversion fails at any point (i.e., + * if fewer bytes are written than indicated by the size parameter) + * then return 0; otherwise return 1. + */ +static int +hex2mem (char *buf, void *_addr, int length) +{ + unsigned int addr = (unsigned int) _addr; + if (((addr & 0x7) == 0) && ((length & 0x7) == 0)) /* dword aligned */ + { + long long *target = (long long *) (addr); + long long *limit = (long long *) (addr + length); + + while (target < limit) + { + int i, j; + long long k = 0; + + for (i = 0; i < 16; i++) + if ((j = hex(*buf++)) < 0) + return 0; + else + k = (k << 4) + j; + *target++ = k; + } + } + else if (((addr & 0x3) == 0) && ((length & 0x3) == 0)) /* word aligned */ + { + int *target = (int *) (addr); + int *limit = (int *) (addr + length); + + while (target < limit) + { + int i, j; + int k = 0; + + for (i = 0; i < 8; i++) + if ((j = hex(*buf++)) < 0) + return 0; + else + k = (k << 4) + j; + *target++ = k; + } + } + else if (((addr & 0x1) == 0) && ((length & 0x1) == 0)) /* halfword aligned */ + { + short *target = (short *) (addr); + short *limit = (short *) (addr + length); + + while (target < limit) + { + int i, j; + short k = 0; + + for (i = 0; i < 4; i++) + if ((j = hex(*buf++)) < 0) + return 0; + else + k = (k << 4) + j; + *target++ = k; + } + } + else /* byte aligned */ + { + char *target = (char *) (addr); + char *limit = (char *) (addr + length); + + while (target < limit) + { + int i, j; + char k = 0; + + for (i = 0; i < 2; i++) + if ((j = hex(*buf++)) < 0) + return 0; + else + k = (k << 4) + j; + *target++ = k; + } + } + + return 1; +} + +/* Convert the binary stream in BUF to memory. + + Gdb will escape $, #, and the escape char (0x7d). + COUNT is the total number of bytes to write into + memory. */ +static unsigned char * +bin2mem ( + char *buf, + unsigned char *mem, + int count +) +{ + int i; + + for (i = 0; i < count; i++) { + /* Check for any escaped characters. Be paranoid and + only unescape chars that should be escaped. */ + if (*buf == 0x7d) { + switch (*(buf+1)) { + case 0x3: /* # */ + case 0x4: /* $ */ + case 0x5d: /* escape char */ + buf++; + *buf |= 0x20; + break; + default: + /* nothing */ + break; + } + } + + *mem++ = *buf++; + } + + return mem; +} + + +/* + * Scan the input stream for a sequence for the form $#. + */ +static void +getpacket (char *buffer) +{ + unsigned char checksum; + unsigned char xmitcsum; + int i; + int count; + char ch; + do + { + /* wait around for the start character, ignore all other characters */ + while ((ch = getDebugChar ()) != '$'); + checksum = 0; + xmitcsum = -1; + + count = 0; + + /* now, read until a # or end of buffer is found */ + while ( (count < BUFMAX-1) && ((ch = getDebugChar ()) != '#') ) + checksum += (buffer[count++] = ch); + + /* make sure that the buffer is null-terminated */ + buffer[count] = '\0'; + + if (ch == '#') + { + xmitcsum = hex (getDebugChar ()) << 4; + xmitcsum += hex (getDebugChar ()); + if (checksum != xmitcsum) + putDebugChar ('-'); /* failed checksum */ + else + { + putDebugChar ('+'); /* successful transfer */ + /* if a sequence char is present, reply the sequence ID */ + if (buffer[2] == ':') + { + putDebugChar (buffer[0]); + putDebugChar (buffer[1]); + /* remove sequence chars from buffer */ + for (i = 3; i <= count; i++) + buffer[i - 3] = buffer[i]; + } + } + } + } + while (checksum != xmitcsum); +} + +/* + * Get a positive/negative acknowledgment for a transmitted packet. + */ +static char +getAck (void) +{ + char c; + + do + { + c = getDebugChar (); + } + while ((c != '+') && (c != '-')); + + return c; +} + +/* + * Send the packet in buffer and wait for a positive acknowledgement. + */ +static void +putpacket (char *buffer) +{ + int checksum; + + /* $# */ + do + { + char *src = buffer; + putDebugChar ('$'); + checksum = 0; + + while (*src != '\0') + { + int runlen = 0; + + /* Do run length encoding */ + while ((src[runlen] == src[0]) && (runlen < 99)) + runlen++; + if (runlen > 3) + { + int encode; + /* Got a useful amount */ + putDebugChar (*src); + checksum += *src; + putDebugChar ('*'); + checksum += '*'; + checksum += (encode = (runlen - 4) + ' '); + putDebugChar (encode); + src += runlen; + } + else + { + putDebugChar (*src); + checksum += *src; + src++; + } + } + + putDebugChar ('#'); + putDebugChar (highhex (checksum)); + putDebugChar (lowhex (checksum)); + } + while (getAck () != '+'); +} + + +/* + * Saved instruction data for single step support + */ +static struct + { + unsigned *targetAddr; + unsigned savedInstr; + } +instrBuffer; + +/* + * If a step breakpoint was planted restore the saved instruction. + */ +static void +undoSStep (void) +{ + if (instrBuffer.targetAddr != NULL) + { + *instrBuffer.targetAddr = instrBuffer.savedInstr; + instrBuffer.targetAddr = NULL; + } + instrBuffer.savedInstr = NOP_INSTR; +} + +/* + * If a single step is requested put a temporary breakpoint at the instruction + * which logically follows the next one to be executed. If the next instruction + * is a branch instruction then skip the instruction in the delay slot. NOTE: + * ERET instructions are NOT handled, as it is impossible to single-step through + * the exit code in an exception handler. In addition, no attempt is made to + * do anything about BC0T and BC0F, since a condition bit for coprocessor 0 + * is not defined on the R4600. Finally, BC2T and BC2F are ignored since there + * is no coprocessor 2 on a 4600. + */ +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 ) + 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 + 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; + } + + if( is_steppable((unsigned)instrBuffer.targetAddr) && *(instrBuffer.targetAddr) != BREAK_INSTR ) + { + instrBuffer.savedInstr = *instrBuffer.targetAddr; + *instrBuffer.targetAddr = BREAK_INSTR; + } + else + { + instrBuffer.targetAddr = NULL; + instrBuffer.savedInstr = NOP_INSTR; + } + return; +} + + +/* + * Translate the R4600 exception code into a Unix-compatible signal. + */ +static int +computeSignal (void) +{ + int exceptionCode = (registers[CAUSE] & CAUSE_EXCMASK) >> CAUSE_EXCSHIFT; + + switch (exceptionCode) + { + case EXC_INT: + /* External interrupt */ + return SIGINT; + + case EXC_RI: + /* Reserved instruction */ + case EXC_CPU: + /* Coprocessor unusable */ + return SIGILL; + + case EXC_BP: + /* Break point */ + return SIGTRAP; + + case EXC_OVF: + /* Arithmetic overflow */ + case EXC_TRAP: + /* Trap exception */ + case EXC_FPE: + /* Floating Point Exception */ + return SIGFPE; + + case EXC_IBE: + /* Bus error (Ifetch) */ + case EXC_DBE: + /* Bus error (data load or store) */ + return SIGBUS; + + case EXC_MOD: + /* TLB modification exception */ + case EXC_TLBL: + /* TLB miss (Load or Ifetch) */ + case EXC_TLBS: + /* TLB miss (Store) */ + case EXC_ADEL: + /* Address error (Load or Ifetch) */ + case EXC_ADES: + /* Address error (Store) */ + return SIGSEGV; + + case EXC_SYS: + /* System call */ + return SIGSYS; + + default: + return SIGTERM; + } +} + +/* + * This support function prepares and sends the message containing the + * basic information about this exception. + */ +static void gdb_stub_report_exception_info( + rtems_vector_number vector, + CPU_Interrupt_frame *frame, + int thread +) +{ + char *optr; + int sigval; + + optr = outBuffer; + *optr++ = 'T'; + sigval = computeSignal (); + *optr++ = highhex (sigval); + *optr++ = lowhex (sigval); + + *optr++ = highhex(SP); /*gdb_hexchars[SP]; */ + *optr++ = lowhex(SP); + *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++ = ';'; + } +#endif + *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. + */ + +extern void clear_cache(void); +void handle_exception (rtems_vector_number vector, CPU_Interrupt_frame *frame) +{ + 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(); + } +#endif + 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); + 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; +#if defined(GDB_STUB_ENABLE_THREAD_SUPPORT) + if (do_threads && current_thread != thread ) + regptr = ¤t_thread_registers; +#endif + mem2hex (regptr, NUM_REGS * (sizeof registers), outBuffer); + break; + + case 'G': /* set the values of the CPU registers - return OK */ + regptr = registers; +#if defined(GDB_STUB_ENABLE_THREAD_SUPPORT) + if (do_threads && current_thread != thread ) + regptr = ¤t_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, ®no) && + *ptr++ == '=' && + hexToLongLong(&ptr, ®val)) + { + 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:#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; + + if (inBuffer[0] == 's') + doSStep (); + } + goto stubexit; + + 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; + } + } + + strcpy(outBuffer, "OK"); + } + break; + + case 'q': /* queries */ +#if defined(GDB_STUB_ENABLE_THREAD_SUPPORT) + 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 + + case 'H': /* set new thread */ +#if defined(GDB_STUB_ENABLE_THREAD_SUPPORT) + if (inBuffer[1] != 'g') { + break; + } + + if (!do_threads) { + break; + } + + { + int tmp, ret; + + /* Set new generic thread */ + if (vhstr2thread(&inBuffer[2], &tmp) == NULL) { + strcpy(outBuffer, "E01"); + break; + } + + /* 0 means `thread' */ + if (tmp == 0) { + tmp = thread; + } + + if (tmp == current_thread) { + /* No changes */ + strcpy(outBuffer, "OK"); + break; + } + + /* Save current thread registers if necessary */ + if (current_thread != thread) { + ret = rtems_gdb_stub_set_thread_regs( + current_thread, + (unsigned int *) (void *)¤t_thread_registers); + ASSERT(ret); + } + + /* Read new registers if necessary */ + if (tmp != thread) { + ret = rtems_gdb_stub_get_thread_regs( + tmp, (unsigned int *) (void *)¤t_thread_registers); + + if (!ret) { + /* Thread does not exist */ + strcpy(outBuffer, "E02"); + break; + } + } + + current_thread = tmp; + strcpy(outBuffer, "OK"); + } +#endif + break; + + case 'Z': /* Add breakpoint */ + { + int ret, type, len; + unsigned char *address; + struct z0break *z0; + + ret = parse_zbreak(inBuffer, &type, &address, &len); + if (!ret) { + strcpy(outBuffer, "E01"); + break; + } + + if (type != 0) { + /* We support only software break points so far */ + strcpy(outBuffer, "E02"); + break; + } + + 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) { + break; + } + } + + if (z0 != NULL) { + /* we already have a breakpoint for this address */ + strcpy(outBuffer, "E04"); + break; + } + + /* Let us allocate new break point */ + if (z0break_avail == NULL) { + 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, "E05"); + break; + }*/ + + /* Fill it */ + z0->address = address; + + if( z0->address == (unsigned char *) 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 */ + { + struct z0break *znxt = z0break_list; + + z0->prev = NULL; + z0->next = znxt; + + if( znxt ) znxt->prev = z0; + z0break_list = z0; + } + + strcpy(outBuffer, "OK"); + } + break; + + 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, R_SZ)) + { + ret = 0; + } + z0last = z0; + } + + * Free entries if any * + if (z0last != NULL) { + z0last->next = z0break_avail; + z0break_avail = z0break_list; + z0break_list = NULL; + } + + if (ret) { + strcpy(outBuffer, "OK"); + } else { + strcpy(outBuffer, "E04"); + } + break; + */ + } + else + { + int ret, type, len; + unsigned char *address; + struct z0break *z0; + + ret = parse_zbreak(inBuffer, &type, &address, &len); + if (!ret) { + strcpy(outBuffer, "E01"); + break; + } + + if (type != 0) { + /* We support only software break points so far */ + break; + } + + if (len != 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; + } + } + + if (z0 == NULL) { + /* Unknown breakpoint */ + strcpy(outBuffer, "E03"); + 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 */ + { + struct z0break *zprv = z0->prev, *znxt = z0->next; + + if( zprv ) zprv->next = znxt; + if( znxt ) znxt->prev = zprv; + + if( !zprv ) z0break_list = znxt; + + 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 + * doing it here before we return. + * + * 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. This + * is because modifications to code in ram will affect the D-cache, + * but not necessarily the I-cache. + */ + + clear_cache(); +} + +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; ibegin <= 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; ibegin <= 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; ibegin <= 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 + */ + int exceptionVector[]= { MIPS_EXCEPTION_MOD, \ + MIPS_EXCEPTION_TLBL, \ + MIPS_EXCEPTION_TLBS, \ + MIPS_EXCEPTION_ADEL, \ + MIPS_EXCEPTION_ADES, \ + MIPS_EXCEPTION_IBE, \ + MIPS_EXCEPTION_DBE, \ + MIPS_EXCEPTION_SYSCALL, \ + MIPS_EXCEPTION_BREAK, \ + MIPS_EXCEPTION_RI, \ + MIPS_EXCEPTION_CPU, \ + MIPS_EXCEPTION_OVERFLOW, \ + MIPS_EXCEPTION_TRAP, \ + MIPS_EXCEPTION_VCEI, \ + MIPS_EXCEPTION_FPE, \ + MIPS_EXCEPTION_C2E, \ + MIPS_EXCEPTION_WATCH, \ + MIPS_EXCEPTION_VCED, \ + -1 }; + int i; + rtems_isr_entry old; + + if (initialized) + { + ASSERT(0); + return; + } + + memset( memsegments,0,sizeof(struct memseg)*NUM_MEMSEGS ); + numsegs = 0; + +#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; inext = z0break_avail; + z0break_avail = z0; + } + } + + for(i=0; exceptionVector[i] > -1; i++) + { + rtems_interrupt_catch( (rtems_isr_entry) handle_exception, exceptionVector[i], &old ); + } + + initialized = 1; + + /* get the attention of gdb */ + /* mips_break(1); disabled so user code can choose to invoke it or not */ +} diff --git a/bsps/mips/shared/gdbstub/mips_opcode.h b/bsps/mips/shared/gdbstub/mips_opcode.h new file mode 100644 index 0000000000..883b1f174b --- /dev/null +++ b/bsps/mips/shared/gdbstub/mips_opcode.h @@ -0,0 +1,336 @@ +/** + * @file + * @ingroup + * @brief Instruction formats and opcode values for MIPS + */ + +/* + * Copyright (c) 1992 The Regents of the University of California. + * All rights reserved. + * + * This code is derived from software contributed to Berkeley by + * Ralph Campbell. + * + * 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. + * 3. All advertising materials mentioning features or use of this software + * must display the following acknowledgement: + * This product includes software developed by the University of + * California, Berkeley and its contributors. + * 4. Neither the name of the University nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE REGENTS 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 REGENTS 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. + * + * from: @(#)mips_opcode.h 7.1 (Berkeley) 3/19/92 + * via: mips_opcode.h,v 1.1 1994/03/10 16:15:10 (algorithmics) + */ + +/* + * Define the instruction formats and opcode values for the + * MIPS instruction set. + */ + +#ifndef _MIPS_OPCODE_H +#define _MIPS_OPCODE_H + +/** + * @defgroup mips_ops MIPS Opcodes + * @ingroup mips_shared + * @brief MIPS Instruction Formats and Opcode Values + * @{ + */ + +/** + * @name Instruction formats + * @{ + */ + +typedef union { + unsigned word; + +#ifdef MIPSEL + struct { + unsigned imm: 16; + unsigned rt: 5; + unsigned rs: 5; + unsigned op: 6; + } IType; + + struct { + unsigned target: 26; + unsigned op: 6; + } JType; + + struct { + unsigned func: 6; + unsigned shamt: 5; + unsigned rd: 5; + unsigned rt: 5; + unsigned rs: 5; + unsigned op: 6; + } RType; + + struct { + unsigned func: 6; + unsigned fd: 5; + unsigned fs: 5; + unsigned ft: 5; + unsigned fmt: 4; + unsigned : 1; /* always '1' */ + unsigned op: 6; /* always '0x11' */ + } FRType; +#else + struct { + unsigned op: 6; + unsigned rs: 5; + unsigned rt: 5; + unsigned imm: 16; + } IType; + + struct { + unsigned op: 6; + unsigned target: 26; + } JType; + + struct { + unsigned op: 6; + unsigned rs: 5; + unsigned rt: 5; + unsigned rd: 5; + unsigned shamt: 5; + unsigned func: 6; + } RType; + + struct { + unsigned op: 6; /* always '0x11' */ + unsigned : 1; /* always '1' */ + unsigned fmt: 4; + unsigned func: 6; + unsigned ft: 5; + unsigned fs: 5; + unsigned fd: 5; + } FRType; +#endif +} InstFmt; + +/** @} */ + +/** + * @name 'op' field values + * @{ + */ + +#define OP_SPECIAL 000 +#define OP_REGIMM 001 +#define OP_J 002 +#define OP_JAL 003 +#define OP_BEQ 004 +#define OP_BNE 005 +#define OP_BLEZ 006 +#define OP_BGTZ 007 + +#define OP_ADDI 010 +#define OP_ADDIU 011 +#define OP_SLTI 012 +#define OP_SLTIU 013 +#define OP_ANDI 014 +#define OP_ORI 015 +#define OP_XORI 016 +#define OP_LUI 017 + +#define OP_COP0 020 +#define OP_COP1 021 +#define OP_COP2 022 +#define OP_BEQL 024 +#define OP_BNEL 025 +#define OP_BLEZL 026 +#define OP_BGTZL 027 + +#define OP_DADDI 030 +#define OP_DADDIU 031 +#define OP_LDL 032 +#define OP_LDR 033 + +#define OP_LB 040 +#define OP_LH 041 +#define OP_LWL 042 +#define OP_LW 043 +#define OP_LBU 044 +#define OP_LHU 045 +#define OP_LWR 046 +#define OP_LWU 047 + +#define OP_SB 050 +#define OP_SH 051 +#define OP_SWL 052 +#define OP_SW 053 +#define OP_SDL 054 +#define OP_SDR 055 +#define OP_SWR 056 +#define OP_CACHE 057 + +#define OP_LL 060 +#define OP_LWC1 061 +#define OP_LWC2 062 +#define OP_LLD 064 +#define OP_LDC1 065 +#define OP_LDC2 066 +#define OP_LD 067 + +#define OP_SC 070 +#define OP_SWC1 071 +#define OP_SWC2 072 +#define OP_SCD 074 +#define OP_SDC1 075 +#define OP_SDC2 076 +#define OP_SD 077 + +/** + * @name 'func' field values when 'op' == OP_SPECIAL. + * @{ + */ + +#define OP_SLL 000 +#define OP_SRL 002 +#define OP_SRA 003 +#define OP_SLLV 004 +#define OP_SRLV 006 +#define OP_SRAV 007 + +#define OP_JR 010 +#define OP_JALR 011 +#define OP_SYSCALL 014 +#define OP_BREAK 015 +#define OP_SYNC 017 + +#define OP_MFHI 020 +#define OP_MTHI 021 +#define OP_MFLO 022 +#define OP_MTLO 023 +#define OP_DSLLV 024 +#define OP_DSRLV 026 +#define OP_DSRAV 027 + +#define OP_MULT 030 +#define OP_MULTU 031 +#define OP_DIV 032 +#define OP_DIVU 033 +#define OP_DMULT 034 +#define OP_DMULTU 035 +#define OP_DDIV 036 +#define OP_DDIVU 037 + +#define OP_ADD 040 +#define OP_ADDU 041 +#define OP_SUB 042 +#define OP_SUBU 043 +#define OP_AND 044 +#define OP_OR 045 +#define OP_XOR 046 +#define OP_NOR 047 + +#define OP_SLT 052 +#define OP_SLTU 053 +#define OP_DADD 054 +#define OP_DADDU 055 +#define OP_DSUB 056 +#define OP_DSUBU 057 + +#define OP_TGE 060 +#define OP_TGEU 061 +#define OP_TLT 062 +#define OP_TLTU 063 +#define OP_TEQ 064 +#define OP_TNE 066 + +#define OP_DSLL 070 +#define OP_DSRL 072 +#define OP_DSRA 073 +#define OP_DSLL32 074 +#define OP_DSRL32 076 +#define OP_DSRA32 077 + +/** @} */ + +/** + * 'func' field values when 'op' == OP_REGIMM. + * @{ + */ + +#define OP_BLTZ 000 +#define OP_BGEZ 001 +#define OP_BLTZL 002 +#define OP_BGEZL 003 + +#define OP_TGEI 010 +#define OP_TGEIU 011 +#define OP_TLTI 012 +#define OP_TLTIU 013 +#define OP_TEQI 014 +#define OP_TNEI 016 + +#define OP_BLTZAL 020 +#define OP_BGEZAL 021 +#define OP_BLTZALL 022 +#define OP_BGEZALL 023 + +/** @} */ + +/** + * @name 'rs' field values when 'op' == OP_COPz. + * @{ + */ + +#define OP_MF 000 +#define OP_DMF 001 +#define OP_CF 002 +#define OP_MT 004 +#define OP_DMT 005 +#define OP_CT 006 +#define OP_BC 010 + +/** @} */ + +/** + * @name 'rt' field values when 'op' == OP_COPz and 'rt' == OP_BC. + * @{ + */ + +#define COPz_BCF 0x00 +#define COPz_BCT 0x01 +#define COPz_BCFL 0x02 +#define COPz_BCTL 0x03 + +/** @} */ + +/** + * @name Instructions with specal significance to debuggers. + * @{ + */ + +#define BREAK_INSTR 0x0000000d ///< @brief instruction code for break +#define NOP_INSTR 0x00000000 ///< @brief instruction code for no-op + +/** @} */ + +/** @} */ + +#endif /* _MIPS_OPCODE_H */ diff --git a/c/src/lib/libbsp/lm32/shared/gdbstub/README b/c/src/lib/libbsp/lm32/shared/gdbstub/README deleted file mode 100644 index bbd1f53a9d..0000000000 --- a/c/src/lib/libbsp/lm32/shared/gdbstub/README +++ /dev/null @@ -1,82 +0,0 @@ -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 deleted file mode 100644 index 6df9c8e921..0000000000 --- a/c/src/lib/libbsp/lm32/shared/gdbstub/gdb_if.h +++ /dev/null @@ -1,125 +0,0 @@ -/** - * @file - * @ingroup lm32_gdb - * @brief definition of the interface between the stub and gdb - */ - -/* - * 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. - */ - -/** - * @defgroup lm32_gdb LM32 GDB Interface - * @ingroup lm32_shared - * @brief Definition of the interface between the stub and gdb, - * @{ - */ - -#ifndef _GDB_IF_H -#define _GDB_IF_H - -/** @brief 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 -); - -/** @brief 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 - -/** @brief Breakpoint instruction */ -#define LM32_BREAK 0xac000002UL - -/** @brief 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 deleted file mode 100644 index 7bd2c504e2..0000000000 --- a/c/src/lib/libbsp/lm32/shared/gdbstub/lm32-debug.S +++ /dev/null @@ -1,338 +0,0 @@ -/* - * lm32 debug exception vectors - * - * Michael Walle , 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.org/license/LICENSE. - * - */ - -#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 deleted file mode 100644 index 8ca1a91054..0000000000 --- a/c/src/lib/libbsp/lm32/shared/gdbstub/lm32-stub.c +++ /dev/null @@ -1,977 +0,0 @@ -/* - * Low-level support for LM32 remote debuging with GDB. - * Contributed by Jon Beniston - * Modified for RTEMS with thread support by Michael Walle - * - * 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 -#include -#include -#include -#include -#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 $# */ -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 - - /* $#. */ - 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 = ¤t_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 = ¤t_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, ®)) - { - ptr = remcomOutBuffer; - ptr = mem2hex((unsigned char *)®isters[reg], ptr, 4); - } else - strcpy(remcomOutBuffer, "E22"); - break; - - /* Set the specified register to the given value */ - case 'P': - if (hex2int(&ptr, ®) - && *ptr++ == '=') - { - hex2mem(ptr, (unsigned char *)®isters[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 << 6) >> 4); - } - 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 << 16) >> 14); - } - 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 *) ¤t_thread_registers); - } - - /* Read new registers if necessary */ - if (tmp != thread) - { - ret = rtems_gdb_stub_get_thread_regs( - tmp, (unsigned int *) ¤t_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(); -} - diff --git a/c/src/lib/libbsp/m68k/shared/gdbstub/gdb_if.h b/c/src/lib/libbsp/m68k/shared/gdbstub/gdb_if.h deleted file mode 100644 index 6c68703b76..0000000000 --- a/c/src/lib/libbsp/m68k/shared/gdbstub/gdb_if.h +++ /dev/null @@ -1,195 +0,0 @@ -/** - * @file - * - * @ingroup m68k_gdbstub - * - * @brief 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. - */ - -#ifndef _GDB_IF_H -#define _GDB_IF_H - -/** - * @defgroup m68k_gdbstub GDB Stub - * - * @ingroup m68k_shared - * - * @brief GDB Stub interface support - */ - -/* 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 -); - -#if defined (__mc68000__) -/* there are 180 bytes of registers on a 68020 w/68881 */ -/* many of the fpa registers are 12 byte (96 bit) registers */ -#define NUMREGBYTES 180 -enum regnames {D0,D1,D2,D3,D4,D5,D6,D7, - A0,A1,A2,A3,A4,A5,A6,A7, - PS,PC, - FP0,FP1,FP2,FP3,FP4,FP5,FP6,FP7, - FPCONTROL,FPSTATUS,FPIADDR - }; -#elif defined (__mips__) -/* - * MIPS registers, numbered in the order in which gdb expects to see them. - */ -#define ZERO 0 -#define AT 1 -#define V0 2 -#define V1 3 -#define A0 4 -#define A1 5 -#define A2 6 -#define A3 7 - -#define T0 8 -#define T1 9 -#define T2 10 -#define T3 11 -#define T4 12 -#define T5 13 -#define T6 14 -#define T7 15 - -#define S0 16 -#define S1 17 -#define S2 18 -#define S3 19 -#define S4 20 -#define S5 21 -#define S6 22 -#define S7 23 - -#define T8 24 -#define T9 25 -#define K0 26 -#define K1 27 -#define GP 28 -#define SP 29 -#define S8 30 -#define RA 31 - -#define SR 32 -#define LO 33 -#define HI 34 -#define BAD_VA 35 -#define CAUSE 36 -#define PC 37 - -#define F0 38 -#define F1 39 -#define F2 40 -#define F3 41 -#define F4 42 -#define F5 43 -#define F6 44 -#define F7 45 - -#define F8 46 -#define F9 47 -#define F10 48 -#define F11 49 -#define F12 50 -#define F13 51 -#define F14 52 -#define F15 53 - -#define F16 54 -#define F17 55 -#define F18 56 -#define F19 57 -#define F20 58 -#define F21 59 -#define F22 60 -#define F23 61 - -#define F24 62 -#define F25 63 -#define F26 64 -#define F27 65 -#define F28 66 -#define F29 67 -#define F30 68 -#define F31 69 - -#define FCSR 70 -#define FIRR 71 - -#define NUM_REGS 72 - -void mips_gdb_stub_install(int enableThreads) ; -#endif /* defined (__mips__) */ - -#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/m68k/shared/gdbstub/m68k-stub.c b/c/src/lib/libbsp/m68k/shared/gdbstub/m68k-stub.c deleted file mode 100644 index 36a3c0285f..0000000000 --- a/c/src/lib/libbsp/m68k/shared/gdbstub/m68k-stub.c +++ /dev/null @@ -1,1294 +0,0 @@ -#define GDB_STUB_ENABLE_THREAD_SUPPORT 1 -/**************************************************************************** - - THIS SOFTWARE IS NOT COPYRIGHTED - - HP offers the following for use in the public domain. HP makes no - warranty with regard to the software or it's performance and the - user accepts the software "AS IS" with all faults. - - HP DISCLAIMS 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. - -****************************************************************************/ - -/**************************************************************************** - * Header: remcom.c,v 1.34 91/03/09 12:29:49 glenne Exp $ - * - * Module name: remcom.c $ - * Revision: 1.34 $ - * Date: 91/03/09 12:29:49 $ - * Contributor: Lake Stevens Instrument Division$ - * - * Description: low level support for gdb debugger. $ - * - * Considerations: only works on target hardware $ - * - * Written by: Glenn Engel $ - * ModuleState: Experimental $ - * - * NOTES: See Below $ - * - * To enable debugger support, two things need to happen. One, a - * call to set_debug_traps() is necessary in order to allow any breakpoints - * or error conditions to be properly intercepted and reported to gdb. - * Two, a breakpoint needs to be generated to begin communication. This - * is most easily accomplished by a call to breakpoint(). Breakpoint() - * simulates a breakpoint by executing a trap #1. The breakpoint instruction - * is hardwired to trap #1 because not to do so is a compatibility problem-- - * there either should be a standard breakpoint instruction, or the protocol - * should be extended to provide some means to communicate which breakpoint - * instruction is in use (or have the stub insert the breakpoint). - * - * Some explanation is probably necessary to explain how exceptions are - * handled. When an exception is encountered the 68000 pushes the current - * program counter and status register onto the supervisor stack and then - * transfers execution to a location specified in it's vector table. - * The handlers for the exception vectors are hardwired to jmp to an address - * given by the relation: (exception - 256) * 6. These are decending - * addresses starting from -6, -12, -18, ... By allowing 6 bytes for - * each entry, a jsr, jmp, bsr, ... can be used to enter the exception - * handler. Using a jsr to handle an exception has an added benefit of - * allowing a single handler to service several exceptions and use the - * return address as the key differentiation. The vector number can be - * computed from the return address by [ exception = (addr + 1530) / 6 ]. - * The sole purpose of the routine _catchException is to compute the - * exception number and push it on the stack in place of the return address. - * The external function exceptionHandler() is - * used to attach a specific handler to a specific m68k exception. - * For 68020 machines, the ability to have a return address around just - * so the vector can be determined is not necessary because the '020 pushes an - * extra word onto the stack containing the vector offset - * - * Because gdb will sometimes write to the stack area to execute function - * calls, this program cannot rely on using the supervisor stack so it - * uses it's own stack area reserved in the int array remcomStack. - * - ************* - * - * The following gdb commands are supported: - * - * command function Return value - * - * g return the value of the CPU registers hex data or ENN - * G set the value of the CPU registers OK or ENN - * - * mAA..AA,LLLL Read LLLL bytes at address AA..AA hex data or ENN - * MAA..AA,LLLL: Write LLLL bytes at address AA.AA OK or ENN - * - * c Resume at current address SNN ( signal NN) - * cAA..AA Continue at address AA..AA SNN - * - * s Step one instruction SNN - * sAA..AA Step one instruction from AA..AA SNN - * - * k kill - * - * ? What was the last sigval ? SNN (signal NN) - * - * All commands and responses are sent with a packet which includes a - * checksum. A packet consists of - * - * $#. - * - * where - * :: - * :: < two hex digits computed as modulo 256 sum of > - * - * When a packet is received, it is first acknowledged with either '+' or '-'. - * '+' indicates a successful transfer. '-' indicates a failed transfer. - * - * Example: - * - * Host: Reply: - * $m0,10#2a +$00010203040506070809101112131415#42 - * - *************************************************************************** - * added 05/27/02 by IMD, Thomas Doerfler: - * XAA..AA,LLLL: Write LLLL binary bytes at address AA.AA OK or ENN - ****************************************************************************/ - -#include -#include -#include -/* #include */ -#include -#include -#include "gdb_if.h" - -#if defined(GDB_STUB_ENABLE_THREAD_SUPPORT) -static char do_threads = 1; /* != 0 means we are supporting threads */ -int current_thread_registers[NUMREGBYTES/4]; -#endif - -/************************************************************************ - * - * external low-level support routines - */ -typedef void (*ExceptionHook)(int); /* pointer to function with int parm */ -typedef void (*Function)(); /* pointer to a function */ - -extern void putDebugChar(); /* write a single character */ -extern int getDebugChar(); /* read and return a single char */ - -/************************/ -/* FORWARD DECLARATIONS */ -/************************/ -static void -initializeRemcomErrorFrame (); - -/************************************************************************/ -/* BUFMAX defines the maximum number of characters in inbound/outbound buffers*/ -/* at least NUMREGBYTES*2 are needed for register packets */ -#define BUFMAX 400 - -static bool initialized = false ; /* boolean flag. != 0 means we've been initialized */ - -int remote_debug; -/* debug > 0 prints ill-formed commands in valid packets & checksum errors */ - -const char gdb_hexchars[]="0123456789abcdef"; -#define highhex(x) gdb_hexchars [(x >> 4) & 0xf] -#define lowhex(x) gdb_hexchars [x & 0xf] - - -/* We keep a whole frame cache here. "Why?", I hear you cry, "doesn't - GDB handle that sort of thing?" Well, yes, I believe the only - reason for this cache is to save and restore floating point state - (fsave/frestore). A cleaner way to do this would be to make the - fsave data part of the registers which GDB deals with like any - other registers. This should not be a performance problem if the - ability to read individual registers is added to the protocol. */ - -typedef struct FrameStruct -{ - struct FrameStruct *previous; - int exceptionPC; /* pc value when this frame created */ - int exceptionVector; /* cpu vector causing exception */ - short frameSize; /* size of cpu frame in words */ - short sr; /* for 68000, this not always sr */ - int pc; - short format; - int fsaveHeader; - int morejunk[0]; /* exception frame, fp save... */ -} Frame; - -#define FRAMESIZE 500 -int gdbFrameStack[FRAMESIZE]; -static Frame *lastFrame; - -/* - * these should not be static cuz they can be used outside this module - */ -int registers[NUMREGBYTES/4]; -int superStack; - -#define STACKSIZE 10000 -int remcomStack[STACKSIZE/sizeof(int)]; -static int* stackPtr = &remcomStack[STACKSIZE/sizeof(int) - 1]; - -/* - * In many cases, the system will want to continue exception processing - * when a continue command is given. - * oldExceptionHook is a function to invoke in this case. - */ - -static ExceptionHook oldExceptionHook; -/* the size of the exception stack on the 68020/30/40/CPU32 varies with - * the type of exception. The following table is the number of WORDS used - * for each exception format. - * This table should be common for all 68k with VBR - * although each member only implements some of the formats - */ -const short exceptionSize[] = { 4,4,6,6,4,4,4,30,29,10,16,46,12,4,4,4 }; - -/************* jump buffer used for setjmp/longjmp **************************/ -jmp_buf remcomEnv; - -/*************************** ASSEMBLY CODE MACROS *************************/ -/* */ - -#ifdef __HAVE_68881__ -/* do an fsave, then remember the address to begin a restore from */ -#define SAVE_FP_REGS() __asm__ (" fsave -(%a0)"); \ - __asm__ (" fmovem.x %fp0-%fp7,registers+72"); \ - __asm__ (" fmovem.l %fpcr/%fpsr/%fpi,registers+168"); -#define RESTORE_FP_REGS() \ -__asm__ (" \n\ - fmovem.l registers+168,%fpcr/%fpsr/%fpi \n\ - fmovem.x registers+72,%fp0-%fp7 \n\ - cmp.l #-1,(%a0) | skip frestore flag set ? \n\ - beq skip_frestore \n\ - frestore (%a0)+ \n\ -skip_frestore: \n\ -"); - -#else -#define SAVE_FP_REGS() -#define RESTORE_FP_REGS() -#endif /* __HAVE_68881__ */ - -void return_to_super(); -void return_to_user(); -extern void _catchException (); - -void m68k_exceptionHandler -( - int vecnum, /* vector index to hook at */ - void *vector /* address of handler function */ -) -{ - void **vec_base = NULL; -#if M68K_HAS_VBR - /* - * get vector base register - */ - __asm__ (" movec.l %%vbr,%0":"=a" (vec_base)); -#endif - vec_base[vecnum] = vector; -} - -ExceptionHook exceptionHook; /* hook variable for errors/exceptions */ - -void m68k_stub_dummy_asm_wrapper() - /* - * this dummy wrapper function ensures, - * that the C compiler manages sections properly - */ -{ -__asm__ ("\n\ -.globl return_to_super \n\ -return_to_super: \n\ - move.l registers+60,%sp /* get new stack pointer */ \n\ - move.l lastFrame,%a0 /* get last frame info */ \n\ - bra return_to_any \n\ - \n\ -.globl _return_to_user \n\ -return_to_user: \n\ - move.l registers+60,%a0 /* get usp */ \n\ - move.l %a0,%usp /* set usp */ \n\ - move.l superStack,%sp /* get original stack pointer */ \n\ - \n\ -return_to_any: \n\ - move.l lastFrame,%a0 /* get last frame info */ \n\ - move.l (%a0)+,lastFrame /* link in previous frame */ \n\ - addq.l #8,%a0 /* skip over pc, vector#*/ \n\ - move.w (%a0)+,%d0 /* get # of words in cpu frame */ \n\ - add.w %d0,%a0 /* point to end of data */ \n\ - add.w %d0,%a0 /* point to end of data */ \n\ - move.l %a0,%a1 \n\ -# \n\ -# copy the stack frame \n\ - subq.l #1,%d0 \n\ -copyUserLoop: \n\ - move.w -(%a1),-(%sp) \n\ - dbf %d0,copyUserLoop \n\ -"); - RESTORE_FP_REGS() - __asm__ (" movem.l registers,%d0-%d7/%a0-%a6"); - __asm__ (" rte"); /* pop and go! */ - -#define DISABLE_INTERRUPTS() __asm__ (" oriw #0x0700,%sr"); -#define BREAKPOINT() __asm__ (" trap #2"); - -/* this function is called immediately when a level 7 interrupt occurs */ -/* if the previous interrupt level was 7 then we're already servicing */ -/* this interrupt and an rte is in order to return to the debugger. */ -/* For the 68000, the offset for sr is 6 due to the jsr return address */ -__asm__ (" \n\ -.text \n\ -.globl _debug_level7 \n\ -_debug_level7: \n\ - move.w %d0,-(%sp)"); -#if M68K_HAS_VBR -__asm__ (" move.w 2(%sp),%d0"); -#else -__asm__ (" move.w 6(%sp),%d0"); -#endif -__asm__ (" andi.w #0x700,%d0 \n\ - cmpi.w #0x700,%d0 \n\ - beq already7 \n\ - move.w (%sp)+,%d0 \n\ - bra _catchException \n\ -already7: \n\ - move.w (%sp)+,%d0"); -#if !M68K_HAS_VBR -__asm__ (" lea 4(%sp),%sp"); /* pull off 68000 return address */ -#endif -__asm__ (" rte"); - -#if M68K_HAS_VBR -/* This function is called when a 68020 exception occurs. It saves - * all the cpu and fpcp regs in the _registers array, creates a frame on a - * linked list of frames which has the cpu and fpcp stack frames needed - * to properly restore the context of these processors, and invokes - * an exception handler (remcom_handler). - * - * stack on entry: stack on exit: - * N bytes of junk exception # MSWord - * Exception Format Word exception # MSWord - * Program counter LSWord - * Program counter MSWord - * Status Register - * - * - */ -__asm__ (" \n\ -.text \n\ -.globl _catchException \n\ -_catchException:"); -DISABLE_INTERRUPTS(); -__asm__ (" \n\ - movem.l %d0-%d7/%a0-%a6,registers /* save registers */ \n\ - move.l lastFrame,%a0 /* last frame pointer */ \n\ -"); -SAVE_FP_REGS(); -__asm__ ("\n\ - lea registers,%a5 /* get address of registers */\n\ - move.w (%sp),%d1 /* get status register */\n\ - move.w %d1,66(%a5) /* save sr */ \n\ - move.l 2(%sp),%a4 /* save pc in a4 for later use */\n\ - move.w 6(%sp),%d0 /* get '020 exception format */\n\ - move.w %d0,%d2 /* make a copy of format word */\n\ -#\n\ -# compute exception number\n\ - and.l #0xfff,%d2 /* mask off vector offset */\n\ - lsr.w #2,%d2 /* divide by 4 to get vect num */\n\ -/* #if 1 */\n\ - cmp.l #33,%d2\n\ - bne nopc_adjust\n\ - subq.l #2,%a4\n\ -nopc_adjust:\n\ -/* #endif */\n\ - move.l %a4,68(%a5) /* save pc in _regisers[] */\n\ -\n\ -#\n\ -# figure out how many bytes in the stack frame\n\ - andi.w #0xf000,%d0 /* mask off format type */\n\ - rol.w #5,%d0 /* rotate into the low byte *2 */\n\ - lea exceptionSize,%a1 \n\ - add.w %d0,%a1 /* index into the table */\n\ - move.w (%a1),%d0 /* get number of words in frame */\n\ - move.w %d0,%d3 /* save it */\n\ - sub.w %d0,%a0 /* adjust save pointer */\n\ - sub.w %d0,%a0 /* adjust save pointer(bytes) */\n\ - move.l %a0,%a1 /* copy save pointer */\n\ - subq.l #1,%d0 /* predecrement loop counter */\n\ -#\n\ -# copy the frame\n\ -saveFrameLoop:\n\ - move.w (%sp)+,(%a1)+\n\ - dbf %d0,saveFrameLoop\n\ -#\n\ -# now that the stack has been clenaed,\n\ -# save the a7 in use at time of exception\n\ - move.l %sp,superStack /* save supervisor sp */\n\ - andi.w #0x2000,%d1 /* were we in supervisor mode ? */\n\ - beq userMode \n\ - move.l %a7,60(%a5) /* save a7 */\n\ - bra a7saveDone\n\ -userMode: \n\ - move.l %usp,%a1 \n\ - move.l %a1,60(%a5) /* save user stack pointer */\n\ -a7saveDone:\n\ -\n\ -#\n\ -# save size of frame\n\ - move.w %d3,-(%a0)\n\ -\n\ - move.l %d2,-(%a0) /* save vector number */\n\ -#\n\ -# save pc causing exception\n\ - move.l %a4,-(%a0)\n\ -#\n\ -# save old frame link and set the new value\n\ - move.l lastFrame,%a1 /* last frame pointer */\n\ - move.l %a1,-(%a0) /* save pointer to prev frame */\n\ - move.l %a0,lastFrame\n\ -\n\ - move.l %d2,-(%sp) /* push exception num */\n\ - move.l exceptionHook,%a0 /* get address of handler */\n\ - jbsr (%a0) /* and call it */\n\ - clr.l (%sp) /* replace exception num parm with frame ptr */\n\ - jbsr _returnFromException /* jbsr, but never returns */\n\ -"); -#else /* mc68000 */ -/* This function is called when an exception occurs. It translates the - * return address found on the stack into an exception vector # which - * is then handled by either handle_exception or a system handler. - * _catchException provides a front end for both. - * - * stack on entry: stack on exit: - * Program counter MSWord exception # MSWord - * Program counter LSWord exception # MSWord - * Status Register - * Return Address MSWord - * Return Address LSWord - */ -__asm__ ("\n\ -.text\n\ -.globl _catchException\n\ -_catchException:"); -DISABLE_INTERRUPTS(); -__asm__ ("\ - moveml %d0-%d7/%a0-%a6,registers /* save registers */ \n\ - movel lastFrame,%a0 /* last frame pointer */ \n\ -"); -SAVE_FP_REGS(); -__asm__ (" \n\ - lea registers,%a5 /* get address of registers */ \n\ - movel (%sp)+,%d2 /* pop return address */ \n\ - addl #1530,%d2 /* convert return addr to */ \n\ - divs #6,%d2 /* exception number */ \n\ - extl %d2 \n\ - \n\ - moveql #3,%d3 /* assume a three word frame */ \n\ - \n\ - cmpiw #3,%d2 /* bus error or address error ? */ \n\ - bgt normal /* if >3 then normal error */ \n\ - movel (%sp)+,-(%a0) /* copy error info to frame buff*/ \n\ - movel (%sp)+,-(%a0) /* these are never used */ \n\ - moveql #7,%d3 /* this is a 7 word frame */ \n\ - \n\ -normal: \n\ - movew (%sp)+,%d1 /* pop status register */ \n\ - movel (%sp)+,%a4 /* pop program counter */ \n\ - movew %d1,66(%a5) /* save sr */ \n\ - movel %a4,68(%a5) /* save pc in _regisers[] */ \n\ - movel %a4,-(%a0) /* copy pc to frame buffer */ \n\ - movew %d1,-(%a0) /* copy sr to frame buffer */ \n\ - \n\ - movel %sp,superStack /* save supervisor sp */ \n\ - \n\ - andiw #0x2000,%d1 /* were we in supervisor mode ? */ \n\ - beq userMode \n\ - movel %a7,60(%a5) /* save a7 */ \n\ - bra saveDone \n\ -userMode: \n\ - movel %usp,%a1 /* save user stack pointer */ \n\ - movel %a1,60(%a5) /* save user stack pointer */ \n\ -saveDone: \n\ - \n\ - movew %d3,-(%a0) /* push frame size in words */ \n\ - movel %d2,-(%a0) /* push vector number */ \n\ - movel %a4,-(%a0) /* push exception pc */ \n\ - \n\ -# \n\ -# save old frame link and set the new value \n\ - movel _lastFrame,%a1 /* last frame pointer */ \n\ - movel %a1,-(%a0) /* save pointer to prev frame */ \n\ - movel %a0,lastFrame \n\ - \n\ - movel %d2,-(%sp) /* push exception num */ \n\ - movel exceptionHook,%a0 /* get address of handler */ \n\ - jbsr (%a0) /* and call it */ \n\ - clrl (%sp) /* replace exception num parm with frame ptr */ \n\ - jbsr _returnFromException /* jbsr, but never returns */ \n\ -"); -#endif - -/* - * remcomHandler is a front end for handle_exception. It moves the - * stack pointer into an area reserved for debugger use in case the - * breakpoint happened in supervisor mode. - */ -__asm__ ("remcomHandler:"); -__asm__ (" add.l #4,%sp"); /* pop off return address */ -__asm__ (" move.l (%sp)+,%d0"); /* get the exception number */ -__asm__ (" move.l stackPtr,%sp"); /* move to remcom stack area */ -__asm__ (" move.l %d0,-(%sp)"); /* push exception onto stack */ -__asm__ (" jbsr handle_exception"); /* this never returns */ -__asm__ (" rts"); /* return */ -} /* end of stub_dummy_asm_wrapper function */ - -void _returnFromException( Frame *frame ) -{ - /* if no passed in frame, use the last one */ - if (! frame) - { - frame = lastFrame; - frame->frameSize = 4; - frame->format = 0; - frame->fsaveHeader = -1; /* restore regs, but we dont have fsave info*/ - } - -#if !M68K_HAS_VBR - /* a 68000 cannot use the internal info pushed onto a bus error - * or address error frame when doing an RTE so don't put this info - * onto the stack or the stack will creep every time this happens. - */ - frame->frameSize=3; -#endif - - /* throw away any frames in the list after this frame */ - lastFrame = frame; - - frame->sr = registers[(int) PS]; - frame->pc = registers[(int) PC]; - - if (registers[(int) PS] & 0x2000) - { - /* return to supervisor mode... */ - return_to_super(); - } - else - { /* return to user mode */ - return_to_user(); - } -} - -int hex(ch) -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); -} - -/* scan for the sequence $# */ -void getpacket(buffer) -char * buffer; -{ - unsigned char checksum; - unsigned char xmitcsum; - int i; - int count; - char ch; - - do { - /* wait around for the start character, ignore all other characters */ - while ((ch = getDebugChar()) != '$'); - checksum = 0; - xmitcsum = -1; - - count = 0; - - /* now, read until a # or end of buffer is found */ - while (count < BUFMAX) { - ch = getDebugChar(); - if (ch == '#') break; - checksum = checksum + ch; - buffer[count] = ch; - count = count + 1; - } - buffer[count] = 0; - - if (ch == '#') { - xmitcsum = hex(getDebugChar()) << 4; - xmitcsum += hex(getDebugChar()); - if ((remote_debug ) && (checksum != xmitcsum)) { - fprintf (stderr,"bad checksum. My count = 0x%x, sent=0x%x. buf=%s\n", - checksum,xmitcsum,buffer); - } - - if (checksum != xmitcsum) putDebugChar('-'); /* failed checksum */ - else { - putDebugChar('+'); /* successful transfer */ - /* if a sequence char is present, reply the sequence ID */ - if (buffer[2] == ':') { - putDebugChar( buffer[0] ); - putDebugChar( buffer[1] ); - /* remove sequence chars from buffer */ - count = strlen(buffer); - for (i=3; i <= count; i++) buffer[i-3] = buffer[i]; - } - } - } - } while (checksum != xmitcsum); - -} - -/* send the packet in buffer. The host get's one chance to read it. - This routine does not wait for a positive acknowledge. */ - -/* - * Send the packet in buffer and wait for a positive acknowledgement. - */ -static void -putpacket (char *buffer) -{ - int checksum; - - /* $# */ - do - { - char *src = buffer; - putDebugChar ('$'); - checksum = 0; - - while (*src != '\0') - { - int runlen = 0; - - /* Do run length encoding */ - while ((src[runlen] == src[0]) && (runlen < 99)) - runlen++; - if (runlen > 3) - { - int encode; - /* Got a useful amount */ - putDebugChar (*src); - checksum += *src; - putDebugChar ('*'); - checksum += '*'; - checksum += (encode = (runlen - 4) + ' '); - putDebugChar (encode); - src += runlen; - } - else - { - putDebugChar (*src); - checksum += *src; - src++; - } - } - - putDebugChar ('#'); - putDebugChar (highhex (checksum)); - putDebugChar (lowhex (checksum)); - } - while (getDebugChar () != '+'); -} - -char remcomInBuffer[BUFMAX]; -char remcomOutBuffer[BUFMAX]; -static short error; - -void debug_error( - char * format, - char * parm -) -{ - if (remote_debug) fprintf (stderr,format,parm); -} - -/* convert the memory pointed to by mem into hex, placing result in buf */ -/* return a pointer to the last char put in buf (null) */ -char* mem2hex(mem, buf, count) -char* mem; -char* buf; -int count; -{ - int i; - unsigned char ch; - for (i=0;i> 4]; - *buf++ = gdb_hexchars[ch % 16]; - } - *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 */ -char* hex2mem(buf, mem, count) -char* buf; -char* mem; -int count; -{ - int i; - unsigned char ch; - for (i=0;i=0) - { - *intValue = (*intValue <<4) | hexValue; - numChars ++; - } - else - break; - - (*ptr)++; - } - - return (numChars); -} - -/* - * This support function prepares and sends the message containing the - * basic information about this exception. - */ - -void gdb_stub_report_exception_info( - int vector, - int *regs, - int thread -) -{ - char *optr; - int sigval; - - optr = remcomOutBuffer; - *optr++ = 'T'; - sigval = computeSignal (vector); - *optr++ = highhex (sigval); - *optr++ = lowhex (sigval); - - *optr++ = highhex(A7); - *optr++ = lowhex(A7); - *optr++ = ':'; - optr = mem2hstr(optr, - (unsigned char *)&(regs[A7]), - sizeof(regs[A7])); - *optr++ = ';'; - - *optr++ = highhex(PC); - *optr++ = lowhex(PC); - *optr++ = ':'; - optr = mem2hstr(optr, - (unsigned char *)&(regs[PC]), - sizeof(regs[PC]) ); - *optr++ = ';'; - -#if defined(GDB_STUB_ENABLE_THREAD_SUPPORT) - if (do_threads) - { - *optr++ = 't'; - *optr++ = 'h'; - *optr++ = 'r'; - *optr++ = 'e'; - *optr++ = 'a'; - *optr++ = 'd'; - *optr++ = ':'; - optr = thread2vhstr(optr, thread); - *optr++ = ';'; - } -#endif - *optr++ = '\0'; -} - -/* - * This function does all command procesing for interfacing to gdb. - */ -void handle_exception(int exceptionVector) -{ - int host_has_detached = 0; - int addr, length; - char * ptr; - int newPC; - Frame *frame; - int current_thread; /* current generic thread */ - int thread; /* stopped thread: context exception happened in */ - void *regptr; - int binary; - - if (remote_debug) printf("vector=%d, sr=0x%x, pc=0x%x\n", - exceptionVector, - registers[ PS ], - registers[ PC ]); - - thread = 0; -#if defined(GDB_STUB_ENABLE_THREAD_SUPPORT) - if (do_threads) { - thread = rtems_gdb_stub_get_current_thread(); - } -#endif - current_thread = thread; - -#if 0 - /* reply to host that an exception has occurred */ - sigval = computeSignal( exceptionVector ); - remcomOutBuffer[0] = 'S'; - remcomOutBuffer[1] = gdb_hexchars[sigval >> 4]; - remcomOutBuffer[2] = gdb_hexchars[sigval % 16]; - remcomOutBuffer[3] = 0; -#else - /* reply to host that an exception has occurred with some basic info */ - gdb_stub_report_exception_info(exceptionVector, registers, thread); -#endif - - putpacket(remcomOutBuffer); - - while (!(host_has_detached)) { - binary = 0; - error = 0; - remcomOutBuffer[0] = 0; - getpacket(remcomInBuffer); - switch (remcomInBuffer[0]) { -#if 0 - case '?' : remcomOutBuffer[0] = 'S'; - remcomOutBuffer[1] = gdb_hexchars[sigval >> 4]; - remcomOutBuffer[2] = gdb_hexchars[sigval % 16]; - remcomOutBuffer[3] = 0; - break; -#else - case '?' : gdb_stub_report_exception_info(exceptionVector, - registers, - thread); - break; -#endif - case 'd' : remote_debug = !(remote_debug); /* toggle debug flag */ - break; - case 'D' : /* host has detached */ - strcpy(remcomOutBuffer,"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 = ¤t_thread_registers; -#endif - mem2hex (regptr, remcomOutBuffer, sizeof registers); - break; - - case 'G': /* set the values of the CPU registers - return OK */ - regptr = registers; -#if defined(GDB_STUB_ENABLE_THREAD_SUPPORT) - if (do_threads && current_thread != thread ) - regptr = ¤t_thread_registers; -#endif - if (hex2mem (&remcomInBuffer[1], - regptr, - sizeof registers)) { - strcpy (remcomOutBuffer, "OK"); - } - else { - strcpy (remcomOutBuffer, "E00"); /* E00 = bad "set register" command */ - } - break; - - /* mAA..AA,LLLL Read LLLL bytes at address AA..AA */ - case 'm' : - if (setjmp(remcomEnv) == 0) - { - m68k_exceptionHandler(2,handle_buserror); - - /* TRY TO READ %x,%x. IF SUCCEED, SET PTR = 0 */ - ptr = &remcomInBuffer[1]; - if (hexToInt(&ptr,&addr)) - if (*(ptr++) == ',') - if (hexToInt(&ptr,&length)) - { - ptr = 0; - mem2hex((char*) addr, remcomOutBuffer, length); - } - - if (ptr) - { - strcpy(remcomOutBuffer,"E01"); - debug_error("malformed read memory command: %s",remcomInBuffer); - } - } - else { - m68k_exceptionHandler(2,_catchException); - strcpy(remcomOutBuffer,"E03"); - debug_error("bus error", 0); - } - - /* restore handler for bus error */ - m68k_exceptionHandler(2,_catchException); - break; - - /* XAA..AA,LLLL: Write LLLL bytes at address AA.AA return OK */ - /* Transfer is in binary, '$', '#' and 0x7d is escaped with 0x7d */ - case 'X' : - binary = 1; - /* MAA..AA,LLLL: Write LLLL bytes at address AA.AA return OK */ - case 'M' : - if (setjmp(remcomEnv) == 0) { - m68k_exceptionHandler(2,handle_buserror); - - /* TRY TO READ '%x,%x:'. IF SUCCEED, SET PTR = 0 */ - ptr = &remcomInBuffer[1]; - if (hexToInt(&ptr,&addr)) - if (*(ptr++) == ',') - if (hexToInt(&ptr,&length)) - if (*(ptr++) == ':') - { - if (binary) { - bin2mem(ptr,(char *)addr,length); - } - else { - hex2mem(ptr, (char*) addr, length); - } - ptr = 0; - strcpy(remcomOutBuffer,"OK"); - } - if (ptr) - { - strcpy(remcomOutBuffer,"E02"); - debug_error("malformed write memory command: %s",remcomInBuffer); - } - } - else { - m68k_exceptionHandler(2,_catchException); - strcpy(remcomOutBuffer,"E03"); - debug_error("bus error", 0); - } - - /* restore handler for bus error */ - m68k_exceptionHandler(2,_catchException); - break; - - /* cAA..AA Continue at address AA..AA(optional) */ - /* sAA..AA Step one instruction from AA..AA(optional) */ - case 'c' : - case 's' : - /* try to read optional parameter, pc unchanged if no parm */ - ptr = &remcomInBuffer[1]; - if (hexToInt(&ptr,&addr)) - registers[ PC ] = addr; - - newPC = registers[ PC]; - - /* clear the trace bit */ - registers[ PS ] &= 0x7fff; - - /* set the trace bit if we're stepping */ - if (remcomInBuffer[0] == 's') registers[ PS ] |= 0x8000; - - /* - * look for newPC in the linked list of exception frames. - * if it is found, use the old frame it. otherwise, - * fake up a dummy frame in returnFromException(). - */ - if (remote_debug) printf("new pc = 0x%x\n",newPC); - frame = lastFrame; - while (frame) - { - if (remote_debug) - printf("frame at 0x%x has pc=0x%x, except#=%d\n", - (uint32_t) frame, - (uint32_t) frame->exceptionPC, - (uint32_t) frame->exceptionVector); - if (frame->exceptionPC == newPC) break; /* bingo! a match */ - /* - * for a breakpoint instruction, the saved pc may - * be off by two due to re-executing the instruction - * replaced by the trap instruction. Check for this. - */ - if ((frame->exceptionVector == 33) && - (frame->exceptionPC == (newPC+2))) break; - if (frame == frame->previous) - { - frame = 0; /* no match found */ - break; - } - frame = frame->previous; - } - - /* - * If we found a match for the PC AND we are not returning - * as a result of a breakpoint (33), - * trace exception (9), nmi (31), jmp to - * the old exception handler as if this code never ran. - */ - if (frame) - { - if ((frame->exceptionVector != 9) && - (frame->exceptionVector != 31) && - (frame->exceptionVector != 33)) - { - /* - * invoke the previous handler. - */ - if (oldExceptionHook) - (*oldExceptionHook) (frame->exceptionVector); - newPC = registers[ PC ]; /* pc may have changed */ - if (newPC != frame->exceptionPC) - { - if (remote_debug) - printf("frame at 0x%x has pc=0x%x, except#=%d\n", - (uint32_t) frame, - frame->exceptionPC, - frame->exceptionVector); - /* re-use the last frame, we're skipping it (longjump?)*/ - frame = (Frame *) 0; - _returnFromException( frame ); /* this is a jump */ - } - } - } - - /* if we couldn't find a frame, create one */ - if (frame == 0) - { - frame = lastFrame -1 ; - - /* by using a bunch of print commands with breakpoints, - it's possible for the frame stack to creep down. If it creeps - too far, give up and reset it to the top. Normal use should - not see this happen. - */ - if ((unsigned int) (frame-2) < (unsigned int) &gdbFrameStack) - { - initializeRemcomErrorFrame(); - frame = lastFrame; - } - frame->previous = lastFrame; - lastFrame = frame; - frame = 0; /* null so _return... will properly initialize it */ - } - - _returnFromException( frame ); /* this is a jump */ - - break; - - case 'q': /* queries */ -#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) - 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 - - case 'H': /* set new thread */ -#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 *) ¤t_thread_registers); - } - - /* Read new registers if necessary */ - if (tmp != thread) { - ret = rtems_gdb_stub_get_thread_regs( - tmp, (unsigned int *) ¤t_thread_registers); - - if (!ret) { - /* Thread does not exist */ - strcpy(remcomOutBuffer, "E02"); - break; - } - } - - current_thread = tmp; - strcpy(remcomOutBuffer, "OK"); - } -#endif - break; - - /* kill the program */ - case 'k' : /* do nothing */ - break; - } /* switch */ - - /* reply to the request */ - putpacket(remcomOutBuffer); - } -} - -void -initializeRemcomErrorFrame() -{ - lastFrame = ((Frame *) &gdbFrameStack[FRAMESIZE-1]) - 1; - lastFrame->previous = lastFrame; -} - -/* this function is used to set up exception handlers for tracing and - breakpoints */ -void set_debug_traps() -{ - extern void _debug_level7(); - extern void remcomHandler(); - int exception; - - initializeRemcomErrorFrame(); - stackPtr = &remcomStack[STACKSIZE/sizeof(int) - 1]; - - for (exception = 2; exception <= 23; exception++) - m68k_exceptionHandler(exception,_catchException); - - /* level 7 interrupt */ - m68k_exceptionHandler(31,_debug_level7); - - /* GDB breakpoint exception (trap #1) */ - m68k_exceptionHandler(33,_catchException); - - /* coded breakpoint exception (trap #2) */ - m68k_exceptionHandler(34,_catchException); - - /* This is a trap #8 instruction. Apparently it is someone's software - convention for some sort of SIGFPE condition. Whose? How many - people are being screwed by having this code the way it is? - Is there a clean solution? */ - m68k_exceptionHandler(40,_catchException); - - /* 48 to 54 are floating point coprocessor errors */ - for (exception = 48; exception <= 54; exception++) - m68k_exceptionHandler(exception,_catchException); - - if (oldExceptionHook != remcomHandler) - { - oldExceptionHook = exceptionHook; - exceptionHook = remcomHandler; - } - - initialized = true; - -} - -/* This function will generate a breakpoint exception. It is used at the - beginning of a program to sync up with a debugger and can be used - otherwise as a quick means to stop program execution and "break" into - the debugger. */ - -void breakpoint() -{ - if (initialized) BREAKPOINT(); -} diff --git a/c/src/lib/libbsp/mips/shared/gdbstub/README b/c/src/lib/libbsp/mips/shared/gdbstub/README deleted file mode 100644 index e61d0f0aa5..0000000000 --- a/c/src/lib/libbsp/mips/shared/gdbstub/README +++ /dev/null @@ -1,151 +0,0 @@ -/*****************************************************/ - -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 - - C. M. Heard - VVNET, Inc. phone: +1 408 247 9376 - 4040 Moorpark Ave. Suite 206 fax: +1 408 244 3651 - San Jose, CA 95117 USA e-mail: heard@vvnet.com - -This package was released in the September 1996 time frame for use -with gdb 4.16 and an IDT R4600 Orion. The stub was modified to support -R3000 class CPUs and to work within the mips-rtems exeception processing -framework. - -THe file memlimits.h could end up being target board dependent. If -this is the case, copy it to your BSP directory and modify as necessary. - ---joel -8 February 2002 - -Original README -=============== - -The r46kstub directory and its compressed archive (r46kstub.tar.gz) contain -the 9/29/96 source code snapshot for a ROM-resident gdb-4.16 debug agent -(aka stub) for the IDT R4600 Orion processor. It is based on the stub for -the Hitachi SH processor written by Ben Lee and Steve Chamberlain and -supplied with the gdb-4.16 distribution; that stub in turn was "originally -based on an m68k software stub written by Glenn Engel at HP, but has changed -quite a bit". The modifications for the R4600 were contributed by C. M. -Heard of VVNET, Inc. and were based in part on the Algorithmics R4000 version -of Phil Bunce's PMON program. - -The distribution consists of the following files: - --rw-r--r-- 1 1178 Sep 29 16:34 ChangeLog --rw-r--r-- 1 748 Jul 26 01:18 Makefile --rw-r--r-- 1 6652 Sep 29 16:34 README --rw-r--r-- 1 1829 May 21 02:02 gdb_if.h --rw-r--r-- 1 3745 Sep 29 14:03 ioaddr.h --rw-r--r-- 1 2906 Sep 29 14:39 limits.h --rw-r--r-- 1 6552 May 23 00:17 mips_opcode.h --rw-r--r-- 1 14017 May 21 02:04 r4600.h --rw-r--r-- 1 23874 Jul 21 20:31 r46kstub.c --rw-r--r-- 1 1064 Jul 3 12:35 r46kstub.ld --rw-r--r-- 1 13299 Sep 29 16:24 stubinit.S - -With the exception of mips_opcode.h, which is a slightly modified version -of a header file contributed by Ralph Campbell to 4.4 BSD and is therefore -copyrighted by the UC Regents, all of the source files have been dedicated -by their authors to the public domain. Use them as you wish, but do so -at your own risk! The authors accept _no_ responsibility for any errors. - -The debug agent contained herein is at this writing in active use at VVNET -supporting initial hardware debug and board bring-up of an OC-12 ATM probe -board. It uses polled I/O on a 16C450 UART. We had originally intended to -add support for interrupts to allow gdb to break in on a running program, -but we have found that this is not really necessary since the reset button -will accomplish the same purpose (thanks to the MIPS feature of saving the -program counter in the ErrorEPC register when a reset exception occurs). - -Be aware that this stub handles ALL interrupts and exceptions except for -reset (or NMI) in the same way -- by passing control to the debug command -loop. It of course uses the ROM exception vectors to do so. In order to -support code that actally needs to use interrupts we use use a more elaborate -stub that is linked with the downloaded program. It hooks the RAM exception -vectors and clears the BEV status bit to gain control. The ROM-based stub -is still used in this case for initial program loading. - -In order to port this stub to a different platform you will at a minimum -need to customize the macros in limits.h (which define the limits of readable, -writeable, and steppable address space) and the I/O addresses in ioaddr.h -(which define the 16C450 MMIO addresses). If you use something other than -a 16C450 UART you will probably also need to modify the portions of stubinit.S -which deal with the serial port. I've tried to be careful to respect all the -architecturally-defined hazards as described in Appendix F of Kane and -Heinrich, MIPS RISC Architecture, in order to minimize the work in porting -to 4000-series processors other than the R4600, but no guarantees are offered. -Support is presently restricted to big-endian addressing, and I've not even -considered what changes would be needed for little-endian support. - -When this stub is built with gcc-2.7.2 and binutils-2.6 you will see a few -warning messages from the single-step support routine where a cast is used -to sign-extend a pointer (the next instruction address) into a long long -(the PC image). Those warnings are expected; I've checked the generated -code and it is doing what I had intended. But you should not see any other -warnings or errors. Here is a log of the build: - -mips64orion-idt-elf-gcc -g -Wa,-ahld -Wall -membedded-data \ - -O3 -c r46kstub.c >r46kstub.L -r46kstub.c: In function `doSStep': -r46kstub.c:537: warning: cast to pointer from integer of different size -r46kstub.c:539: warning: cast to pointer from integer of different size -r46kstub.c:547: warning: cast to pointer from integer of different size -r46kstub.c:561: warning: cast to pointer from integer of different size -r46kstub.c:563: warning: cast to pointer from integer of different size -r46kstub.c:572: warning: cast to pointer from integer of different size -r46kstub.c:574: warning: cast to pointer from integer of different size -r46kstub.c:582: warning: cast to pointer from integer of different size -r46kstub.c:589: warning: cast to pointer from integer of different size -r46kstub.c:591: warning: cast to pointer from integer of different size -r46kstub.c:597: warning: cast to pointer from integer of different size -r46kstub.c:599: warning: cast to pointer from integer of different size -r46kstub.c:605: warning: cast to pointer from integer of different size -r46kstub.c:607: warning: cast to pointer from integer of different size -r46kstub.c:613: warning: cast to pointer from integer of different size -r46kstub.c:615: warning: cast to pointer from integer of different size -r46kstub.c:624: warning: cast to pointer from integer of different size -r46kstub.c:628: warning: cast to pointer from integer of different size -r46kstub.c:635: warning: cast to pointer from integer of different size -r46kstub.c:637: warning: cast to pointer from integer of different size -mips64orion-idt-elf-gcc -g -Wa,-ahld -Wall -membedded-data \ - -O3 -c stubinit.S >stubinit.L -mips64orion-idt-elf-ld -t -s -T r46kstub.ld -Map r46kstub.map -o r46kstub.out -mips64orion-idt-elf-ld: mode elf32bmip -stubinit.o -r46kstub.o -mips64orion-idt-elf-objcopy -S -R .bss -R .data -R .reginfo \ - -O srec r46kstub.out r46kstub.hex - -Limitations: stubinit.S deliberately forces the PC (which is a 64-bit -register) to contain a legitimate sign-extended 32-bit value. This was -done to cope with a bug in gdb-4.16, which does _not_ properly sign-extend -the initial PC when it loads a program. This means that you cannot use -the "set" command to load an unmapped sixty-four bit virtual address into -the PC, as you can for all other registers. - -Please send bug reports, comments, or suggestions for improvement to: diff --git a/c/src/lib/libbsp/mips/shared/gdbstub/gdb_if.h b/c/src/lib/libbsp/mips/shared/gdbstub/gdb_if.h deleted file mode 100644 index ba4f0eb757..0000000000 --- a/c/src/lib/libbsp/mips/shared/gdbstub/gdb_if.h +++ /dev/null @@ -1,194 +0,0 @@ -/** - * @file - * @ingroup mips_gdb - * @brief Definition of the interface between stub and gdb - */ - -/* - * 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. - */ - -/** - * @defgroup mips_gdb GDB Interface - * @ingroup mips_shared - * @brief GDB Interface - * @{ - */ - -#ifndef _GDB_IF_H -#define _GDB_IF_H - -/** @brief 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]; -}; - -/** - * @name 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 -); - -/** @} */ - -/** - * @name MIPS registers - * @brief Numbered in the order in which gdb expects to see them. - * @{ - */ - -#define ZERO 0 -#define AT 1 -#define V0 2 -#define V1 3 -#define A0 4 -#define A1 5 -#define A2 6 -#define A3 7 - -#define T0 8 -#define T1 9 -#define T2 10 -#define T3 11 -#define T4 12 -#define T5 13 -#define T6 14 -#define T7 15 - -#define S0 16 -#define S1 17 -#define S2 18 -#define S3 19 -#define S4 20 -#define S5 21 -#define S6 22 -#define S7 23 - -#define T8 24 -#define T9 25 -#define K0 26 -#define K1 27 -#define GP 28 -#define SP 29 -#define S8 30 -#define RA 31 - -#define SR 32 -#define LO 33 -#define HI 34 -#define BAD_VA 35 -#define CAUSE 36 -#define PC 37 - -#define F0 38 -#define F1 39 -#define F2 40 -#define F3 41 -#define F4 42 -#define F5 43 -#define F6 44 -#define F7 45 - -#define F8 46 -#define F9 47 -#define F10 48 -#define F11 49 -#define F12 50 -#define F13 51 -#define F14 52 -#define F15 53 - -#define F16 54 -#define F17 55 -#define F18 56 -#define F19 57 -#define F20 58 -#define F21 59 -#define F22 60 -#define F23 61 - -#define F24 62 -#define F25 63 -#define F26 64 -#define F27 65 -#define F28 66 -#define F29 67 -#define F30 68 -#define F31 69 - -#define FCSR 70 -#define FIRR 71 - -#define NUM_REGS 72 - -/** @} */ - -void mips_gdb_stub_install(int enableThreads) ; - -#define MEMOPT_READABLE 1 -#define MEMOPT_WRITEABLE 2 - -#ifndef NUM_MEMSEGS -#define NUM_MEMSEGS 10 -#endif - -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 deleted file mode 100644 index c60ca12111..0000000000 --- a/c/src/lib/libbsp/mips/shared/gdbstub/memlimits.h +++ /dev/null @@ -1,100 +0,0 @@ -/** - * @file - * @ingroup mips_limits - * @brief Definition of machine and system dependent address limits. - */ - -/* - * limits.h - definition of machine & system dependent address limits - * - * 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. - */ - -#ifndef _MEMLIMITS_H_ -#define _MEMLIMITS_H_ - -/* - * The macros in this file are specific to a given implementation. - * The general rules for their construction are as follows: - * - * 1.) is_readable(addr,length) should be true if and only if the - * region starting at the given virtual address can be read - * _without_ causing an exception or other fatal error. Note - * that the stub will use the strictest alignment satisfied - * by _both_ addr and length (e.g., if both are divisible by - * 8 then the region will be read in double-word chunks). - * - * 2.) is_writeable(addr,length) should be true if and only if the - * region starting at the given virtual address can be written - * _without_ causing an exception or other fatal error. Note - * that the stub will use the strictest alignment satisfied - * by _both_ addr and length (e.g., if both are divisible by - * 8 then the region will be written in double-word chunks). - * - * 3.) is-steppable(ptr) whould be true if and only if ptr is the - * address of a writeable region of memory which may contain - * an executable instruction. At a minimum this requires that - * ptr be word-aligned (divisible by 4) and not point to EPROM - * or memory-mapped I/O. - * - * Note: in order to satisfy constraints related to cacheability - * of certain memory subsystems it may be necessary for regions - * of kseg0 and kseg1 which map to the same physical addresses - * to have different readability and/or writeability attributes. - */ - -/** - * @defgroup mips_limits Address Limits - * @ingroup mips_shared - * @brief Address Limits - */ - - -/* -#define K0_LIMIT_FOR_READ (K0BASE+0x18000000) -#define K1_LIMIT_FOR_READ (K1BASE+K1SIZE) - -#define is_readable(addr,length) \ - (((K0BASE <= addr) && ((addr + length) <= K0_LIMIT_FOR_READ)) \ - || ((K1BASE <= addr) && ((addr + length) <= K1_LIMIT_FOR_READ))) - -#define K0_LIMIT_FOR_WRITE (K0BASE+0x08000000) -#define K1_LIMIT_FOR_WRITE (K1BASE+0x1e000000) - -#define is_writeable(addr,length) \ - (((K0BASE <= addr) && ((addr + length) <= K0_LIMIT_FOR_WRITE)) \ - || ((K1BASE <= addr) && ((addr + length) <= K1_LIMIT_FOR_WRITE))) - -#define K0_LIMIT_FOR_STEP (K0BASE+0x08000000) -#define K1_LIMIT_FOR_STEP (K1BASE+0x08000000) - -#define is_steppable(ptr) \ - ((((int)ptr & 0x3) == 0) \ - && (((K0BASE <= (int)ptr) && ((int)ptr < K0_LIMIT_FOR_STEP)) \ - || ((K1BASE <= (int)ptr) && ((int)ptr < K1_LIMIT_FOR_STEP)))) - -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 deleted file mode 100644 index 8320eb66a9..0000000000 --- a/c/src/lib/libbsp/mips/shared/gdbstub/mips-stub.c +++ /dev/null @@ -1,1589 +0,0 @@ -#define GDB_STUB_ENABLE_THREAD_SUPPORT 1 -/******************************************************************************* - - 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. - -******************************************************************************** -* -* r46kstub.c -- target debugging stub for the IDT R4600 Orion processor -* -* This module is based on the stub for the Hitachi SH processor written by -* Ben Lee and Steve Chamberlain and supplied with gdb 4.16. The latter -* in turn "is originally based on an m68k software stub written by Glenn -* Engel at HP, but has changed quite a bit." The changes for the R4600 -* were written by C. M. Heard at VVNET. They were based in part on the -* Algorithmics R4000 version of Phil Bunce's PMON program. -* -* Remote communication protocol: -* -* A debug packet whose contents are -* is encapsulated for transmission in the form: -* -* $ # CSUM1 CSUM2 -* -* must be ASCII alphanumeric and cannot include characters -* '$' or '#'. If starts with two characters followed by -* ':', then the existing stubs interpret this as a sequence number. -* -* CSUM1 and CSUM2 are ascii hex representation of an 8-bit -* checksum of , the most significant nibble is sent first. -* the hex digits 0-9,a-f are used. -* -* Receiver responds with: -* -* + if CSUM is correct -* - if CSUM is incorrect -* -* is as follows. All values are encoded in ascii hex digits. -* -* Request Packet -* -* read registers g -* reply XX....X Each byte of register data -* is described by two hex digits. -* Registers are in the internal order -* for GDB, and the bytes in a register -* are in the same order the machine uses. -* or ENN for an error. -* -* write regs GXX..XX Each byte of register data -* is described by two hex digits. -* reply OK for success -* ENN for an error -* -* write reg Pn...=r... Write register n... with value r.... -* reply OK for success -* ENN for an error -* -* read mem mAA..AA,LLLL AA..AA is address, LLLL is length. -* reply XX..XX XX..XX is mem contents -* Can be fewer bytes than requested -* if able to read only part of the data. -* or ENN NN is errno -* -* write mem MAA..AA,LLLL:XX..XX -* AA..AA is address, -* LLLL is number of bytes, -* XX..XX is data -* reply OK for success -* ENN for an error (this includes the case -* where only part of the data was -* written). -* -* cont cAA..AA AA..AA is address to resume -* If AA..AA is omitted, -* resume at same address. -* -* step sAA..AA AA..AA is address to resume -* If AA..AA is omitted, -* resume at same address. -* -* There is no immediate reply to step or cont. -* The reply comes when the machine stops. -* It is SAA AA is the "signal number" -* -* last signal ? Reply with the reason for stopping. -* This is the same reply as is generated -* for step or cont: SAA where AA is the -* signal number. -* -* detach D Host is detaching. Reply OK and -* end remote debugging session. -* -* reserved On other requests, the stub should -* ignore the request and send an empty -* response ($#). This way -* we can extend the protocol and GDB -* can tell whether the stub it is -* talking to uses the old or the new. -* -* Responses can be run-length encoded to save space. A '*' means that -* the next character is an ASCII encoding giving a repeat count which -* stands for that many repetitions of the character preceding the '*'. -* The encoding is n+29, yielding a printable character when n >=3 -* (which is where rle starts to win). Don't use n > 99 since gdb -* masks each character is receives with 0x7f in order to strip off -* the parity bit. -* -* As an example, "0* " means the same thing as "0000". -* -*******************************************************************************/ - - -#include -#include -#include "mips_opcode.h" -/* #include "memlimits.h" */ -#include -#include -#include "gdb_if.h" - -/* Change it to something meaningful when debugging */ -#undef ASSERT -#define ASSERT(x) if(!(x)) printk("ASSERT: stub: %d\n", __LINE__) - -/***************/ -/* Exception Codes */ -#define EXC_INT 0 /* External interrupt */ -#define EXC_MOD 1 /* TLB modification exception */ -#define EXC_TLBL 2 /* TLB miss (Load or Ifetch) */ -#define EXC_TLBS 3 /* TLB miss (Store) */ -#define EXC_ADEL 4 /* Address error (Load or Ifetch) */ -#define EXC_ADES 5 /* Address error (Store) */ -#define EXC_IBE 6 /* Bus error (Ifetch) */ -#define EXC_DBE 7 /* Bus error (data load or store) */ -#define EXC_SYS 8 /* System call */ -#define EXC_BP 9 /* Break point */ -#define EXC_RI 10 /* Reserved instruction */ -#define EXC_CPU 11 /* Coprocessor unusable */ -#define EXC_OVF 12 /* Arithmetic overflow */ -#define EXC_TRAP 13 /* Trap exception */ -#define EXC_FPE 15 /* Floating Point Exception */ - -/* FPU Control/Status register fields */ -#define CSR_FS 0x01000000 /* Set to flush denormals to zero */ -#define CSR_C 0x00800000 /* Condition bit (set by FP compare) */ - -#define CSR_CMASK (0x3f<<12) -#define CSR_CE 0x00020000 -#define CSR_CV 0x00010000 -#define CSR_CZ 0x00008000 -#define CSR_CO 0x00004000 -#define CSR_CU 0x00002000 -#define CSR_CI 0x00001000 - -#define CSR_EMASK (0x1f<<7) -#define CSR_EV 0x00000800 -#define CSR_EZ 0x00000400 -#define CSR_EO 0x00000200 -#define CSR_EU 0x00000100 -#define CSR_EI 0x00000080 - -#define CSR_FMASK (0x1f<<2) -#define CSR_FV 0x00000040 -#define CSR_FZ 0x00000020 -#define CSR_FO 0x00000010 -#define CSR_FU 0x00000008 -#define CSR_FI 0x00000004 - -#define CSR_RMODE_MASK (0x3<<0) -#define CSR_RM 0x00000003 -#define CSR_RP 0x00000002 -#define CSR_RZ 0x00000001 -#define CSR_RN 0x00000000 - -/***************/ - -/* - * Saved register information. Must be prepared by the exception - * preprocessor before handle_exception is invoked. - */ -#if (__mips == 3) -typedef long long mips_register_t; -#define R_SZ 8 -#elif (__mips == 1) -typedef unsigned int mips_register_t; -#define R_SZ 4 -#else -#error "unknown MIPS ISA" -#endif -static mips_register_t *registers; - -#if defined(GDB_STUB_ENABLE_THREAD_SUPPORT) -static char do_threads; /* != 0 means we are supporting threads */ -#endif - -/* - * The following external functions provide character input and output. - */ -extern char getDebugChar (void); -extern void putDebugChar (char); - -/* - * Exception handler - */ -void handle_exception (rtems_vector_number vector, CPU_Interrupt_frame *frame); - -/* - * Prototype needed by this code and to keep it self contained. - */ -void rtems_interrupt_catch( rtems_isr_entry, int, rtems_isr_entry *); - -/* - * 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 - * register packets. Memory dump packets can profitably use even more. - */ -#define BUFMAX 1500 - -static char inBuffer[BUFMAX]; -static char outBuffer[BUFMAX]; - -/* Structure to keep info on a z-breaks */ -#define BREAKNUM 32 - -struct z0break -{ - /* List support */ - struct z0break *next; - struct z0break *prev; - - /* Location, preserved data */ - - /* the address pointer, really, really must be a pointer to - ** a 32 bit quantity (likely 64 on the R4k), so the full instruction is read & - ** written. Making it a char * as on the i386 will cause - ** the zbreaks to mess up the breakpoint instructions - */ - unsigned char *address; - unsigned instr; -}; - -static struct z0break z0break_arr[BREAKNUM]; -static struct z0break *z0break_avail = NULL; -static struct z0break *z0break_list = NULL; - - -/* - * Convert an int to hex. - */ -const char gdb_hexchars[] = "0123456789abcdef"; - -#define highhex(x) gdb_hexchars [(x >> 4) & 0xf] -#define lowhex(x) gdb_hexchars [x & 0xf] - -/* - * Convert length bytes of data starting at addr into hex, placing the - * result in buf. Return a pointer to the last (null) char in buf. - */ -static char * -mem2hex (void *_addr, int length, char *buf) -{ - unsigned int addr = (unsigned int) _addr; - - if (((addr & 0x7) == 0) && ((length & 0x7) == 0)) /* dword aligned */ - { - long long *source = (long long *) (addr); - long long *limit = (long long *) (addr + length); - - while (source < limit) - { - int i; - long long k = *source++; - - for (i = 15; i >= 0; i--) - *buf++ = gdb_hexchars [(k >> (i*4)) & 0xf]; - } - } - else if (((addr & 0x3) == 0) && ((length & 0x3) == 0)) /* word aligned */ - { - int *source = (int *) (addr); - int *limit = (int *) (addr + length); - - while (source < limit) - { - int i; - int k = *source++; - - for (i = 7; i >= 0; i--) - *buf++ = gdb_hexchars [(k >> (i*4)) & 0xf]; - } - } - else if (((addr & 0x1) == 0) && ((length & 0x1) == 0)) /* halfword aligned */ - { - short *source = (short *) (addr); - short *limit = (short *) (addr + length); - - while (source < limit) - { - int i; - short k = *source++; - - for (i = 3; i >= 0; i--) - *buf++ = gdb_hexchars [(k >> (i*4)) & 0xf]; - } - } - else /* byte aligned */ - { - char *source = (char *) (addr); - char *limit = (char *) (addr + length); - - while (source < limit) - { - int i; - char k = *source++; - - for (i = 1; i >= 0; i--) - *buf++ = gdb_hexchars [(k >> (i*4)) & 0xf]; - } - } - - *buf = '\0'; - return (buf); -} - - -/* - * Convert a hex character to an int. - */ -static int -hex (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 a string from hex to int until a non-hex digit - * is found. Return the number of characters processed. - */ -static int -hexToInt (char **ptr, int *intValue) -{ - int numChars = 0; - int hexValue; - - *intValue = 0; - - while (**ptr) - { - hexValue = hex (**ptr); - if (hexValue >= 0) - { - *intValue = (*intValue << 4) | hexValue; - numChars++; - } - else - break; - - (*ptr)++; - } - - return (numChars); -} - -/* - * Convert a string from hex to long long until a non-hex - * digit is found. Return the number of characters processed. - */ -static int -hexToLongLong (char **ptr, long long *intValue) -{ - int numChars = 0; - int hexValue; - - *intValue = 0; - - while (**ptr) - { - hexValue = hex (**ptr); - if (hexValue >= 0) - { - *intValue = (*intValue << 4) | hexValue; - numChars++; - } - else - break; - - (*ptr)++; - } - - return (numChars); -} - -/* - * Convert the hex array buf into binary, placing the result at the - * specified address. If the conversion fails at any point (i.e., - * if fewer bytes are written than indicated by the size parameter) - * then return 0; otherwise return 1. - */ -static int -hex2mem (char *buf, void *_addr, int length) -{ - unsigned int addr = (unsigned int) _addr; - if (((addr & 0x7) == 0) && ((length & 0x7) == 0)) /* dword aligned */ - { - long long *target = (long long *) (addr); - long long *limit = (long long *) (addr + length); - - while (target < limit) - { - int i, j; - long long k = 0; - - for (i = 0; i < 16; i++) - if ((j = hex(*buf++)) < 0) - return 0; - else - k = (k << 4) + j; - *target++ = k; - } - } - else if (((addr & 0x3) == 0) && ((length & 0x3) == 0)) /* word aligned */ - { - int *target = (int *) (addr); - int *limit = (int *) (addr + length); - - while (target < limit) - { - int i, j; - int k = 0; - - for (i = 0; i < 8; i++) - if ((j = hex(*buf++)) < 0) - return 0; - else - k = (k << 4) + j; - *target++ = k; - } - } - else if (((addr & 0x1) == 0) && ((length & 0x1) == 0)) /* halfword aligned */ - { - short *target = (short *) (addr); - short *limit = (short *) (addr + length); - - while (target < limit) - { - int i, j; - short k = 0; - - for (i = 0; i < 4; i++) - if ((j = hex(*buf++)) < 0) - return 0; - else - k = (k << 4) + j; - *target++ = k; - } - } - else /* byte aligned */ - { - char *target = (char *) (addr); - char *limit = (char *) (addr + length); - - while (target < limit) - { - int i, j; - char k = 0; - - for (i = 0; i < 2; i++) - if ((j = hex(*buf++)) < 0) - return 0; - else - k = (k << 4) + j; - *target++ = k; - } - } - - return 1; -} - -/* Convert the binary stream in BUF to memory. - - Gdb will escape $, #, and the escape char (0x7d). - COUNT is the total number of bytes to write into - memory. */ -static unsigned char * -bin2mem ( - char *buf, - unsigned char *mem, - int count -) -{ - int i; - - for (i = 0; i < count; i++) { - /* Check for any escaped characters. Be paranoid and - only unescape chars that should be escaped. */ - if (*buf == 0x7d) { - switch (*(buf+1)) { - case 0x3: /* # */ - case 0x4: /* $ */ - case 0x5d: /* escape char */ - buf++; - *buf |= 0x20; - break; - default: - /* nothing */ - break; - } - } - - *mem++ = *buf++; - } - - return mem; -} - - -/* - * Scan the input stream for a sequence for the form $#. - */ -static void -getpacket (char *buffer) -{ - unsigned char checksum; - unsigned char xmitcsum; - int i; - int count; - char ch; - do - { - /* wait around for the start character, ignore all other characters */ - while ((ch = getDebugChar ()) != '$'); - checksum = 0; - xmitcsum = -1; - - count = 0; - - /* now, read until a # or end of buffer is found */ - while ( (count < BUFMAX-1) && ((ch = getDebugChar ()) != '#') ) - checksum += (buffer[count++] = ch); - - /* make sure that the buffer is null-terminated */ - buffer[count] = '\0'; - - if (ch == '#') - { - xmitcsum = hex (getDebugChar ()) << 4; - xmitcsum += hex (getDebugChar ()); - if (checksum != xmitcsum) - putDebugChar ('-'); /* failed checksum */ - else - { - putDebugChar ('+'); /* successful transfer */ - /* if a sequence char is present, reply the sequence ID */ - if (buffer[2] == ':') - { - putDebugChar (buffer[0]); - putDebugChar (buffer[1]); - /* remove sequence chars from buffer */ - for (i = 3; i <= count; i++) - buffer[i - 3] = buffer[i]; - } - } - } - } - while (checksum != xmitcsum); -} - -/* - * Get a positive/negative acknowledgment for a transmitted packet. - */ -static char -getAck (void) -{ - char c; - - do - { - c = getDebugChar (); - } - while ((c != '+') && (c != '-')); - - return c; -} - -/* - * Send the packet in buffer and wait for a positive acknowledgement. - */ -static void -putpacket (char *buffer) -{ - int checksum; - - /* $# */ - do - { - char *src = buffer; - putDebugChar ('$'); - checksum = 0; - - while (*src != '\0') - { - int runlen = 0; - - /* Do run length encoding */ - while ((src[runlen] == src[0]) && (runlen < 99)) - runlen++; - if (runlen > 3) - { - int encode; - /* Got a useful amount */ - putDebugChar (*src); - checksum += *src; - putDebugChar ('*'); - checksum += '*'; - checksum += (encode = (runlen - 4) + ' '); - putDebugChar (encode); - src += runlen; - } - else - { - putDebugChar (*src); - checksum += *src; - src++; - } - } - - putDebugChar ('#'); - putDebugChar (highhex (checksum)); - putDebugChar (lowhex (checksum)); - } - while (getAck () != '+'); -} - - -/* - * Saved instruction data for single step support - */ -static struct - { - unsigned *targetAddr; - unsigned savedInstr; - } -instrBuffer; - -/* - * If a step breakpoint was planted restore the saved instruction. - */ -static void -undoSStep (void) -{ - if (instrBuffer.targetAddr != NULL) - { - *instrBuffer.targetAddr = instrBuffer.savedInstr; - instrBuffer.targetAddr = NULL; - } - instrBuffer.savedInstr = NOP_INSTR; -} - -/* - * If a single step is requested put a temporary breakpoint at the instruction - * which logically follows the next one to be executed. If the next instruction - * is a branch instruction then skip the instruction in the delay slot. NOTE: - * ERET instructions are NOT handled, as it is impossible to single-step through - * the exit code in an exception handler. In addition, no attempt is made to - * do anything about BC0T and BC0F, since a condition bit for coprocessor 0 - * is not defined on the R4600. Finally, BC2T and BC2F are ignored since there - * is no coprocessor 2 on a 4600. - */ -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 ) - 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 - 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; - } - - if( is_steppable((unsigned)instrBuffer.targetAddr) && *(instrBuffer.targetAddr) != BREAK_INSTR ) - { - instrBuffer.savedInstr = *instrBuffer.targetAddr; - *instrBuffer.targetAddr = BREAK_INSTR; - } - else - { - instrBuffer.targetAddr = NULL; - instrBuffer.savedInstr = NOP_INSTR; - } - return; -} - - -/* - * Translate the R4600 exception code into a Unix-compatible signal. - */ -static int -computeSignal (void) -{ - int exceptionCode = (registers[CAUSE] & CAUSE_EXCMASK) >> CAUSE_EXCSHIFT; - - switch (exceptionCode) - { - case EXC_INT: - /* External interrupt */ - return SIGINT; - - case EXC_RI: - /* Reserved instruction */ - case EXC_CPU: - /* Coprocessor unusable */ - return SIGILL; - - case EXC_BP: - /* Break point */ - return SIGTRAP; - - case EXC_OVF: - /* Arithmetic overflow */ - case EXC_TRAP: - /* Trap exception */ - case EXC_FPE: - /* Floating Point Exception */ - return SIGFPE; - - case EXC_IBE: - /* Bus error (Ifetch) */ - case EXC_DBE: - /* Bus error (data load or store) */ - return SIGBUS; - - case EXC_MOD: - /* TLB modification exception */ - case EXC_TLBL: - /* TLB miss (Load or Ifetch) */ - case EXC_TLBS: - /* TLB miss (Store) */ - case EXC_ADEL: - /* Address error (Load or Ifetch) */ - case EXC_ADES: - /* Address error (Store) */ - return SIGSEGV; - - case EXC_SYS: - /* System call */ - return SIGSYS; - - default: - return SIGTERM; - } -} - -/* - * This support function prepares and sends the message containing the - * basic information about this exception. - */ -static void gdb_stub_report_exception_info( - rtems_vector_number vector, - CPU_Interrupt_frame *frame, - int thread -) -{ - char *optr; - int sigval; - - optr = outBuffer; - *optr++ = 'T'; - sigval = computeSignal (); - *optr++ = highhex (sigval); - *optr++ = lowhex (sigval); - - *optr++ = highhex(SP); /*gdb_hexchars[SP]; */ - *optr++ = lowhex(SP); - *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++ = ';'; - } -#endif - *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. - */ - -extern void clear_cache(void); -void handle_exception (rtems_vector_number vector, CPU_Interrupt_frame *frame) -{ - 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(); - } -#endif - 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); - 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; -#if defined(GDB_STUB_ENABLE_THREAD_SUPPORT) - if (do_threads && current_thread != thread ) - regptr = ¤t_thread_registers; -#endif - mem2hex (regptr, NUM_REGS * (sizeof registers), outBuffer); - break; - - case 'G': /* set the values of the CPU registers - return OK */ - regptr = registers; -#if defined(GDB_STUB_ENABLE_THREAD_SUPPORT) - if (do_threads && current_thread != thread ) - regptr = ¤t_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, ®no) && - *ptr++ == '=' && - hexToLongLong(&ptr, ®val)) - { - 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:#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; - - if (inBuffer[0] == 's') - doSStep (); - } - goto stubexit; - - 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; - } - } - - strcpy(outBuffer, "OK"); - } - break; - - case 'q': /* queries */ -#if defined(GDB_STUB_ENABLE_THREAD_SUPPORT) - 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 - - case 'H': /* set new thread */ -#if defined(GDB_STUB_ENABLE_THREAD_SUPPORT) - if (inBuffer[1] != 'g') { - break; - } - - if (!do_threads) { - break; - } - - { - int tmp, ret; - - /* Set new generic thread */ - if (vhstr2thread(&inBuffer[2], &tmp) == NULL) { - strcpy(outBuffer, "E01"); - break; - } - - /* 0 means `thread' */ - if (tmp == 0) { - tmp = thread; - } - - if (tmp == current_thread) { - /* No changes */ - strcpy(outBuffer, "OK"); - break; - } - - /* Save current thread registers if necessary */ - if (current_thread != thread) { - ret = rtems_gdb_stub_set_thread_regs( - current_thread, - (unsigned int *) (void *)¤t_thread_registers); - ASSERT(ret); - } - - /* Read new registers if necessary */ - if (tmp != thread) { - ret = rtems_gdb_stub_get_thread_regs( - tmp, (unsigned int *) (void *)¤t_thread_registers); - - if (!ret) { - /* Thread does not exist */ - strcpy(outBuffer, "E02"); - break; - } - } - - current_thread = tmp; - strcpy(outBuffer, "OK"); - } -#endif - break; - - case 'Z': /* Add breakpoint */ - { - int ret, type, len; - unsigned char *address; - struct z0break *z0; - - ret = parse_zbreak(inBuffer, &type, &address, &len); - if (!ret) { - strcpy(outBuffer, "E01"); - break; - } - - if (type != 0) { - /* We support only software break points so far */ - strcpy(outBuffer, "E02"); - break; - } - - 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) { - break; - } - } - - if (z0 != NULL) { - /* we already have a breakpoint for this address */ - strcpy(outBuffer, "E04"); - break; - } - - /* Let us allocate new break point */ - if (z0break_avail == NULL) { - 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, "E05"); - break; - }*/ - - /* Fill it */ - z0->address = address; - - if( z0->address == (unsigned char *) 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 */ - { - struct z0break *znxt = z0break_list; - - z0->prev = NULL; - z0->next = znxt; - - if( znxt ) znxt->prev = z0; - z0break_list = z0; - } - - strcpy(outBuffer, "OK"); - } - break; - - 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, R_SZ)) - { - ret = 0; - } - z0last = z0; - } - - * Free entries if any * - if (z0last != NULL) { - z0last->next = z0break_avail; - z0break_avail = z0break_list; - z0break_list = NULL; - } - - if (ret) { - strcpy(outBuffer, "OK"); - } else { - strcpy(outBuffer, "E04"); - } - break; - */ - } - else - { - int ret, type, len; - unsigned char *address; - struct z0break *z0; - - ret = parse_zbreak(inBuffer, &type, &address, &len); - if (!ret) { - strcpy(outBuffer, "E01"); - break; - } - - if (type != 0) { - /* We support only software break points so far */ - break; - } - - if (len != 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; - } - } - - if (z0 == NULL) { - /* Unknown breakpoint */ - strcpy(outBuffer, "E03"); - 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 */ - { - struct z0break *zprv = z0->prev, *znxt = z0->next; - - if( zprv ) zprv->next = znxt; - if( znxt ) znxt->prev = zprv; - - if( !zprv ) z0break_list = znxt; - - 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 - * doing it here before we return. - * - * 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. This - * is because modifications to code in ram will affect the D-cache, - * but not necessarily the I-cache. - */ - - clear_cache(); -} - -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; ibegin <= 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; ibegin <= 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; ibegin <= 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 - */ - int exceptionVector[]= { MIPS_EXCEPTION_MOD, \ - MIPS_EXCEPTION_TLBL, \ - MIPS_EXCEPTION_TLBS, \ - MIPS_EXCEPTION_ADEL, \ - MIPS_EXCEPTION_ADES, \ - MIPS_EXCEPTION_IBE, \ - MIPS_EXCEPTION_DBE, \ - MIPS_EXCEPTION_SYSCALL, \ - MIPS_EXCEPTION_BREAK, \ - MIPS_EXCEPTION_RI, \ - MIPS_EXCEPTION_CPU, \ - MIPS_EXCEPTION_OVERFLOW, \ - MIPS_EXCEPTION_TRAP, \ - MIPS_EXCEPTION_VCEI, \ - MIPS_EXCEPTION_FPE, \ - MIPS_EXCEPTION_C2E, \ - MIPS_EXCEPTION_WATCH, \ - MIPS_EXCEPTION_VCED, \ - -1 }; - int i; - rtems_isr_entry old; - - if (initialized) - { - ASSERT(0); - return; - } - - memset( memsegments,0,sizeof(struct memseg)*NUM_MEMSEGS ); - numsegs = 0; - -#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; inext = z0break_avail; - z0break_avail = z0; - } - } - - for(i=0; exceptionVector[i] > -1; i++) - { - rtems_interrupt_catch( (rtems_isr_entry) handle_exception, exceptionVector[i], &old ); - } - - initialized = 1; - - /* get the attention of gdb */ - /* mips_break(1); disabled so user code can choose to invoke it or not */ -} diff --git a/c/src/lib/libbsp/mips/shared/gdbstub/mips_opcode.h b/c/src/lib/libbsp/mips/shared/gdbstub/mips_opcode.h deleted file mode 100644 index 883b1f174b..0000000000 --- a/c/src/lib/libbsp/mips/shared/gdbstub/mips_opcode.h +++ /dev/null @@ -1,336 +0,0 @@ -/** - * @file - * @ingroup - * @brief Instruction formats and opcode values for MIPS - */ - -/* - * Copyright (c) 1992 The Regents of the University of California. - * All rights reserved. - * - * This code is derived from software contributed to Berkeley by - * Ralph Campbell. - * - * 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. - * 3. All advertising materials mentioning features or use of this software - * must display the following acknowledgement: - * This product includes software developed by the University of - * California, Berkeley and its contributors. - * 4. Neither the name of the University nor the names of its contributors - * may be used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE REGENTS 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 REGENTS 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. - * - * from: @(#)mips_opcode.h 7.1 (Berkeley) 3/19/92 - * via: mips_opcode.h,v 1.1 1994/03/10 16:15:10 (algorithmics) - */ - -/* - * Define the instruction formats and opcode values for the - * MIPS instruction set. - */ - -#ifndef _MIPS_OPCODE_H -#define _MIPS_OPCODE_H - -/** - * @defgroup mips_ops MIPS Opcodes - * @ingroup mips_shared - * @brief MIPS Instruction Formats and Opcode Values - * @{ - */ - -/** - * @name Instruction formats - * @{ - */ - -typedef union { - unsigned word; - -#ifdef MIPSEL - struct { - unsigned imm: 16; - unsigned rt: 5; - unsigned rs: 5; - unsigned op: 6; - } IType; - - struct { - unsigned target: 26; - unsigned op: 6; - } JType; - - struct { - unsigned func: 6; - unsigned shamt: 5; - unsigned rd: 5; - unsigned rt: 5; - unsigned rs: 5; - unsigned op: 6; - } RType; - - struct { - unsigned func: 6; - unsigned fd: 5; - unsigned fs: 5; - unsigned ft: 5; - unsigned fmt: 4; - unsigned : 1; /* always '1' */ - unsigned op: 6; /* always '0x11' */ - } FRType; -#else - struct { - unsigned op: 6; - unsigned rs: 5; - unsigned rt: 5; - unsigned imm: 16; - } IType; - - struct { - unsigned op: 6; - unsigned target: 26; - } JType; - - struct { - unsigned op: 6; - unsigned rs: 5; - unsigned rt: 5; - unsigned rd: 5; - unsigned shamt: 5; - unsigned func: 6; - } RType; - - struct { - unsigned op: 6; /* always '0x11' */ - unsigned : 1; /* always '1' */ - unsigned fmt: 4; - unsigned func: 6; - unsigned ft: 5; - unsigned fs: 5; - unsigned fd: 5; - } FRType; -#endif -} InstFmt; - -/** @} */ - -/** - * @name 'op' field values - * @{ - */ - -#define OP_SPECIAL 000 -#define OP_REGIMM 001 -#define OP_J 002 -#define OP_JAL 003 -#define OP_BEQ 004 -#define OP_BNE 005 -#define OP_BLEZ 006 -#define OP_BGTZ 007 - -#define OP_ADDI 010 -#define OP_ADDIU 011 -#define OP_SLTI 012 -#define OP_SLTIU 013 -#define OP_ANDI 014 -#define OP_ORI 015 -#define OP_XORI 016 -#define OP_LUI 017 - -#define OP_COP0 020 -#define OP_COP1 021 -#define OP_COP2 022 -#define OP_BEQL 024 -#define OP_BNEL 025 -#define OP_BLEZL 026 -#define OP_BGTZL 027 - -#define OP_DADDI 030 -#define OP_DADDIU 031 -#define OP_LDL 032 -#define OP_LDR 033 - -#define OP_LB 040 -#define OP_LH 041 -#define OP_LWL 042 -#define OP_LW 043 -#define OP_LBU 044 -#define OP_LHU 045 -#define OP_LWR 046 -#define OP_LWU 047 - -#define OP_SB 050 -#define OP_SH 051 -#define OP_SWL 052 -#define OP_SW 053 -#define OP_SDL 054 -#define OP_SDR 055 -#define OP_SWR 056 -#define OP_CACHE 057 - -#define OP_LL 060 -#define OP_LWC1 061 -#define OP_LWC2 062 -#define OP_LLD 064 -#define OP_LDC1 065 -#define OP_LDC2 066 -#define OP_LD 067 - -#define OP_SC 070 -#define OP_SWC1 071 -#define OP_SWC2 072 -#define OP_SCD 074 -#define OP_SDC1 075 -#define OP_SDC2 076 -#define OP_SD 077 - -/** - * @name 'func' field values when 'op' == OP_SPECIAL. - * @{ - */ - -#define OP_SLL 000 -#define OP_SRL 002 -#define OP_SRA 003 -#define OP_SLLV 004 -#define OP_SRLV 006 -#define OP_SRAV 007 - -#define OP_JR 010 -#define OP_JALR 011 -#define OP_SYSCALL 014 -#define OP_BREAK 015 -#define OP_SYNC 017 - -#define OP_MFHI 020 -#define OP_MTHI 021 -#define OP_MFLO 022 -#define OP_MTLO 023 -#define OP_DSLLV 024 -#define OP_DSRLV 026 -#define OP_DSRAV 027 - -#define OP_MULT 030 -#define OP_MULTU 031 -#define OP_DIV 032 -#define OP_DIVU 033 -#define OP_DMULT 034 -#define OP_DMULTU 035 -#define OP_DDIV 036 -#define OP_DDIVU 037 - -#define OP_ADD 040 -#define OP_ADDU 041 -#define OP_SUB 042 -#define OP_SUBU 043 -#define OP_AND 044 -#define OP_OR 045 -#define OP_XOR 046 -#define OP_NOR 047 - -#define OP_SLT 052 -#define OP_SLTU 053 -#define OP_DADD 054 -#define OP_DADDU 055 -#define OP_DSUB 056 -#define OP_DSUBU 057 - -#define OP_TGE 060 -#define OP_TGEU 061 -#define OP_TLT 062 -#define OP_TLTU 063 -#define OP_TEQ 064 -#define OP_TNE 066 - -#define OP_DSLL 070 -#define OP_DSRL 072 -#define OP_DSRA 073 -#define OP_DSLL32 074 -#define OP_DSRL32 076 -#define OP_DSRA32 077 - -/** @} */ - -/** - * 'func' field values when 'op' == OP_REGIMM. - * @{ - */ - -#define OP_BLTZ 000 -#define OP_BGEZ 001 -#define OP_BLTZL 002 -#define OP_BGEZL 003 - -#define OP_TGEI 010 -#define OP_TGEIU 011 -#define OP_TLTI 012 -#define OP_TLTIU 013 -#define OP_TEQI 014 -#define OP_TNEI 016 - -#define OP_BLTZAL 020 -#define OP_BGEZAL 021 -#define OP_BLTZALL 022 -#define OP_BGEZALL 023 - -/** @} */ - -/** - * @name 'rs' field values when 'op' == OP_COPz. - * @{ - */ - -#define OP_MF 000 -#define OP_DMF 001 -#define OP_CF 002 -#define OP_MT 004 -#define OP_DMT 005 -#define OP_CT 006 -#define OP_BC 010 - -/** @} */ - -/** - * @name 'rt' field values when 'op' == OP_COPz and 'rt' == OP_BC. - * @{ - */ - -#define COPz_BCF 0x00 -#define COPz_BCT 0x01 -#define COPz_BCFL 0x02 -#define COPz_BCTL 0x03 - -/** @} */ - -/** - * @name Instructions with specal significance to debuggers. - * @{ - */ - -#define BREAK_INSTR 0x0000000d ///< @brief instruction code for break -#define NOP_INSTR 0x00000000 ///< @brief instruction code for no-op - -/** @} */ - -/** @} */ - -#endif /* _MIPS_OPCODE_H */ -- cgit v1.2.3