aboutsummaryrefslogtreecommitdiff
path: root/AT91SAM7S256
diff options
context:
space:
mode:
Diffstat (limited to 'AT91SAM7S256')
-rw-r--r--AT91SAM7S256/SAM7S256/Include/Cstartup.S13
-rw-r--r--AT91SAM7S256/SAM7S256/Include/sam7s256.c6
-rw-r--r--AT91SAM7S256/SAM7S256/gcc/Makefile12
-rw-r--r--AT91SAM7S256/SAM7S256/gcc/nxt.ld26
-rw-r--r--AT91SAM7S256/Source/c_comm.c14
-rw-r--r--AT91SAM7S256/Source/c_comm.h3
-rw-r--r--AT91SAM7S256/armdebug/.gitignore41
-rw-r--r--AT91SAM7S256/armdebug/.project11
-rw-r--r--AT91SAM7S256/armdebug/AUTHORS6
-rw-r--r--AT91SAM7S256/armdebug/COPYING9
-rw-r--r--AT91SAM7S256/armdebug/Debugger/_c_arm_macros.h88
-rw-r--r--AT91SAM7S256/armdebug/Debugger/debug_comm.S538
-rw-r--r--AT91SAM7S256/armdebug/Debugger/debug_hexutils.S457
-rw-r--r--AT91SAM7S256/armdebug/Debugger/debug_internals.h303
-rw-r--r--AT91SAM7S256/armdebug/Debugger/debug_macros.h386
-rw-r--r--AT91SAM7S256/armdebug/Debugger/debug_runlooptasks.S374
-rw-r--r--AT91SAM7S256/armdebug/Debugger/debug_runlooptasks.h60
-rw-r--r--AT91SAM7S256/armdebug/Debugger/debug_stack.ld15
-rw-r--r--AT91SAM7S256/armdebug/Debugger/debug_stub.S2976
-rw-r--r--AT91SAM7S256/armdebug/Debugger/debug_stub.h151
-rw-r--r--AT91SAM7S256/armdebug/Debugger/debug_test.S53
-rw-r--r--AT91SAM7S256/armdebug/Debugger/debug_test.h35
-rw-r--r--AT91SAM7S256/armdebug/Debugger/undef_handler.S147
-rw-r--r--AT91SAM7S256/armdebug/Doxyfile243
-rw-r--r--AT91SAM7S256/armdebug/GNU-GPLv2.txt340
-rw-r--r--AT91SAM7S256/armdebug/Host/README21
-rw-r--r--AT91SAM7S256/armdebug/Host/gdb-commands.txt43
-rwxr-xr-xAT91SAM7S256/armdebug/Host/nxt-gdb-server.py208
-rwxr-xr-xAT91SAM7S256/armdebug/Host/pyenv-nxt-gdb-server3
-rw-r--r--AT91SAM7S256/armdebug/LEGO_Open_Source_License.docbin0 -> 40960 bytes
-rw-r--r--AT91SAM7S256/armdebug/README16
-rw-r--r--AT91SAM7S256/armdebug/SConscript13
-rw-r--r--AT91SAM7S256/armdebug/SConstruct182
33 files changed, 6785 insertions, 8 deletions
diff --git a/AT91SAM7S256/SAM7S256/Include/Cstartup.S b/AT91SAM7S256/SAM7S256/Include/Cstartup.S
index b60ba2c..af87159 100644
--- a/AT91SAM7S256/SAM7S256/Include/Cstartup.S
+++ b/AT91SAM7S256/SAM7S256/Include/Cstartup.S
@@ -136,13 +136,15 @@ FIQ_Handler_Entry:
/* end of fiqhandler */
Reset_Addr: .word InitReset
-Undef_Addr: .word Undef_Handler
+Undef_Addr: .word undef_handler /* BKPT instruction trap */
SWI_Addr: .word SWI_Handler
/*SWI_Addr: .word SoftwareInterruptASM*/ /*in swi_handler.S */
PAbt_Addr: .word PAbt_Handler
DAbt_Addr: .word DAbt_Handler
IRQ_Addr: .word IRQ_Handler_Entry
-
+
+ .global default_undef_handler
+default_undef_handler:
Undef_Handler: B Undef_Handler
SWI_Handler: B SWI_Handler
PAbt_Handler: B PAbt_Handler
@@ -255,6 +257,7 @@ already_remapped:
.EQU ARM_MODE_FIQ, 0x11
.EQU ARM_MODE_IRQ, 0x12
.EQU ARM_MODE_SVC, 0x13
+ .EQU ARM_MODE_ABT, 0x17
.EQU I_BIT, 0x80
.EQU F_BIT, 0x40
@@ -264,6 +267,10 @@ already_remapped:
//*-------------------------------*/
mov r0, sp /* see (**) */
+/*- Set up Abort Mode Stack for Debugger*/
+ msr CPSR_c, #ARM_MODE_ABT | I_BIT | F_BIT
+ ldr sp, =__abort_stack_top__
+
/*- Set up Fast Interrupt Mode and set FIQ Mode Stack*/
msr CPSR_c, #ARM_MODE_FIQ | I_BIT | F_BIT
mov sp, r0
@@ -286,6 +293,8 @@ already_remapped:
mov sp, r0 /* Init stack Sup */
+
+
/*- Enable interrupt & Set up Supervisor Mode and set Supervisor Mode Stack*/
/* Relocate .data section (Copy from ROM to RAM)
diff --git a/AT91SAM7S256/SAM7S256/Include/sam7s256.c b/AT91SAM7S256/SAM7S256/Include/sam7s256.c
index cc22ca3..8ff0ab4 100644
--- a/AT91SAM7S256/SAM7S256/Include/sam7s256.c
+++ b/AT91SAM7S256/SAM7S256/Include/sam7s256.c
@@ -11,6 +11,9 @@
//
// Platform C
//
+#ifdef __ARMDEBUG__
+#include "debug_stub.h"
+#endif
void main(void)
{
@@ -18,6 +21,9 @@ void main(void)
{
HARDWAREInit;
mSchedInit();
+#ifdef __ARMDEBUG__
+ dbg__bkpt_init();
+#endif
while(TRUE == mSchedCtrl())
{
OSWatchdogWrite;
diff --git a/AT91SAM7S256/SAM7S256/gcc/Makefile b/AT91SAM7S256/SAM7S256/gcc/Makefile
index 304cbe2..734e0af 100644
--- a/AT91SAM7S256/SAM7S256/gcc/Makefile
+++ b/AT91SAM7S256/SAM7S256/gcc/Makefile
@@ -1,4 +1,5 @@
BASE = ../..
+DBGDIR = $(BASE)/armdebug/Debugger
SRCDIR = $(BASE)/Source
CPUINCDIR = $(BASE)/SAM7S256/Include
@@ -7,7 +8,7 @@ CUSTOM_FIRMWAREVERSION = $(GIT_VERSION)
TARGET = nxt_firmware
-ARM_SOURCES =
+ARM_SOURCES =
THUMB_SOURCES = c_button.c c_cmd.c c_comm.c c_display.c c_input.c c_ioctrl.c \
c_loader.c c_lowspeed.c c_output.c c_sound.c c_ui.c \
d_bt.c d_button.c d_display.c d_hispeed.c d_input.c \
@@ -17,21 +18,22 @@ THUMB_SOURCES = c_button.c c_cmd.c c_comm.c c_display.c c_input.c c_ioctrl.c \
errno.c sbrk.c strtod.c sscanf.c \
Cstartup_SAM7.c
-ASM_ARM_SOURCE = Cstartup.S
+ASM_ARM_SOURCE = Cstartup.S undef_handler.S debug_hexutils.S debug_stub.S debug_comm.S debug_runlooptasks.S
ASM_THUMB_SOURCE =
vpath %.c $(SRCDIR)
vpath %.c $(CPUINCDIR)
vpath %.c lib
-vpath %.S $(CPUINCDIR)
+vpath %.S $(CPUINCDIR) $(DBGDIR)
-INCLUDES =
+INCLUDES = -I../../armdebug/Debugger
MCU = arm7tdmi
+DEBUG_DEFINES = -D__ARMDEBUG__
STARTOFUSERFLASH_DEFINES = -DSTARTOFUSERFLASH_FROM_LINKER=1
VERSION_DEFINES = -DCUSTOM_FIRMWAREVERSION=\"$(CUSTOM_FIRMWAREVERSION)\"
DEFINES = -DPROTOTYPE_PCB_4 -DNEW_MENU -DROM_RUN -DVECTORS_IN_RAM \
- $(STARTOFUSERFLASH_DEFINES) $(VERSION_DEFINES)
+ $(STARTOFUSERFLASH_DEFINES) $(VERSION_DEFINES) $(DEBUG_DEFINES)
OPTIMIZE = -Os -fno-strict-aliasing \
-ffunction-sections -fdata-sections
WARNINGS = -Wall -W -Wundef -Wno-unused -Wno-format
diff --git a/AT91SAM7S256/SAM7S256/gcc/nxt.ld b/AT91SAM7S256/SAM7S256/gcc/nxt.ld
index 8e5f0cb..3b2c7a3 100644
--- a/AT91SAM7S256/SAM7S256/gcc/nxt.ld
+++ b/AT91SAM7S256/SAM7S256/gcc/nxt.ld
@@ -80,6 +80,31 @@ SECTIONS
__STARTOFUSERFLASH_FROM_LINKER =
ALIGN (LOADADDR (.data) + SIZEOF (.data), 0x100);
+ /*
+ * The various debugger stacks.
+ */
+ .stack : ALIGN(8) {
+
+ /* abort stack */
+ __abort_stack_bottom__ = . ;
+ . += 0x80; /* 128 byte abort mode stack. */
+ __abort_stack__ = .;
+ __abort_stack_top__ = . ;
+
+ /* debugger state */
+ __debugger_stack_bottom__ = . ;
+ . += 0x48; /* 16 user mode registers + SPSR + UNDEF Next Instruction Address */
+ __debugger_stack__ = .;
+ __debugger_stack_top__ = . ;
+
+ /* breakpoints */
+ __breakpoints_start__ = . ;
+ . += 0x40; /* Single Stepping Breakpoint + 7 Breakpoints */
+ __breakpoints_end__ = . ;
+ } > DATA
+
+ __breakpoints_num__ = (__breakpoints_end__ - __breakpoints_start__) / 8;
+
/* .bss section which is used for uninitialized data */
.bss (NOLOAD) :
{
@@ -96,6 +121,7 @@ SECTIONS
__bss_end__ = . ;
+
_end = .;
PROVIDE (end = .);
diff --git a/AT91SAM7S256/Source/c_comm.c b/AT91SAM7S256/Source/c_comm.c
index c760b84..b4d3be0 100644
--- a/AT91SAM7S256/Source/c_comm.c
+++ b/AT91SAM7S256/Source/c_comm.c
@@ -26,6 +26,9 @@
#include "d_bt.h"
#include <string.h>
#include <ctype.h>
+#ifdef __ARMDEBUG__
+#include "debug_stub.h"
+#endif
enum
{
@@ -451,6 +454,16 @@ UWORD cCommInterprete(UBYTE *pInBuf, UBYTE *pOutBuf, UBYTE *pLength, UBYTE C
}
break;
+#ifdef __ARMDEBUG__
+ case DEBUG_CMD:
+ {
+ ReturnStatus = cCommHandleDebug(&(pInBuf[0]), CmdBit, MsgLength); /* Pass everything (incl. message command byte) to function */
+ /* Check that Debug Command does not expect reply */
+ ReturnStatus = (0 == ((pInBuf[0]) & NO_REPLY_BIT));
+ *pLength = 0;
+ }
+ break;
+#endif
default:
{
@@ -508,7 +521,6 @@ UWORD cCommInterprete(UBYTE *pInBuf, UBYTE *pOutBuf, UBYTE *pLength, UBYTE C
return(ReturnStatus);
}
-
UWORD cCommInterpreteCmd(UBYTE Cmd, UBYTE *pInBuf, UBYTE *pOutBuf, UBYTE *pLength, UBYTE CmdBit, UWORD MsgLength)
{
ULONG FileLength;
diff --git a/AT91SAM7S256/Source/c_comm.h b/AT91SAM7S256/Source/c_comm.h
index a1e112c..9003b24 100644
--- a/AT91SAM7S256/Source/c_comm.h
+++ b/AT91SAM7S256/Source/c_comm.h
@@ -67,6 +67,9 @@ enum
DIRECT_CMD = 0x00,
SYSTEM_CMD = 0x01,
REPLY_CMD = 0x02,
+#ifdef __ARMDEBUG__
+ DEBUG_CMD = 0x0d,
+#endif
NO_REPLY_BIT = 0x80
};
diff --git a/AT91SAM7S256/armdebug/.gitignore b/AT91SAM7S256/armdebug/.gitignore
new file mode 100644
index 0000000..46f8223
--- /dev/null
+++ b/AT91SAM7S256/armdebug/.gitignore
@@ -0,0 +1,41 @@
+# Ignore tag
+MASTER-REPO_DO-NOT-DELETE
+*.lst
+*.objdump
+.DS_Store
+
+# Generally annoying things.
+*.[oa]
+*.pyc
+*.bin
+*.elf
+*.rxe
+*.map
+*.orig
+*.log
+*~
+*.swp
+\#*\#
+.\#*
+
+# Python distutils creates this when building.
+pynxt/build/
+
+# XCode build stuff
+FantomModule/build/
+*mode1v3
+*pbxuser
+
+# SCons cruft
+.sconsign.dblite
+.sconf_temp
+build_flags.py
+
+# Precommit hooks drop a commit.msg file if they fail.
+commit.msg
+
+# The option-cache
+scons.options
+
+# pyfantom related
+pyfantom.py
diff --git a/AT91SAM7S256/armdebug/.project b/AT91SAM7S256/armdebug/.project
new file mode 100644
index 0000000..15b12fc
--- /dev/null
+++ b/AT91SAM7S256/armdebug/.project
@@ -0,0 +1,11 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<projectDescription>
+ <name>armdebug</name>
+ <comment></comment>
+ <projects>
+ </projects>
+ <buildSpec>
+ </buildSpec>
+ <natures>
+ </natures>
+</projectDescription>
diff --git a/AT91SAM7S256/armdebug/AUTHORS b/AT91SAM7S256/armdebug/AUTHORS
new file mode 100644
index 0000000..169c2b6
--- /dev/null
+++ b/AT91SAM7S256/armdebug/AUTHORS
@@ -0,0 +1,6 @@
+The following people have contributed code, in various quantities, to armdebug.
+A big thanks to all of them, armdebug is what it is in part thanks to each of
+them. In alphabetical order:
+
+Nicolas Schodet <nico (at) ni.fr.eu.org>
+Tat Chee Wan <tcwan (at) cs.usm.my>
diff --git a/AT91SAM7S256/armdebug/COPYING b/AT91SAM7S256/armdebug/COPYING
new file mode 100644
index 0000000..86fae60
--- /dev/null
+++ b/AT91SAM7S256/armdebug/COPYING
@@ -0,0 +1,9 @@
+The armdebug project is dual licensed.
+
+You can either use the GNU GPLv2 license in GNU-GPLv2.txt,
+or else the "LEGO Open Source License" in LEGO_Open_Source_License.doc
+to redistribute the code.
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
diff --git a/AT91SAM7S256/armdebug/Debugger/_c_arm_macros.h b/AT91SAM7S256/armdebug/Debugger/_c_arm_macros.h
new file mode 100644
index 0000000..025542e
--- /dev/null
+++ b/AT91SAM7S256/armdebug/Debugger/_c_arm_macros.h
@@ -0,0 +1,88 @@
+/** @file _c_arm_macros.h
+ * @brief Define macros to support shared C and ASM headers
+ *
+ */
+
+/* Copyright (C) 2010 the NxOS developers
+ *
+ * Module Developed by: TC Wan <tcwan@cs.usm.my>
+ *
+ * Thanks to Bartli (forum post @ embdev.net ARM programming with GCC/GNU tools forum)
+ *
+ * See AUTHORS for a full list of the developers.
+ *
+ * See COPYING for redistribution license
+ *
+ */
+
+#ifndef __C_ARM_MACROS__
+#define __C_ARM_MACROS__
+
+
+#ifdef __ASSEMBLY__
+
+#define NULL 0x0
+#define FALSE 0
+#define TRUE ~FALSE
+
+#define TYPEDEF @
+#define FUNCDEF @
+
+ .set last_enum_value, 0
+ .macro enum_val name
+ .equiv \name, last_enum_value
+ .set last_enum_value, last_enum_value + 1
+ .endm
+
+#define ENUM_BEGIN .set last_enum_value, 0
+
+#define ENUM_VAL(name) enum_val name
+#define ENUM_VALASSIGN(name, value) \
+ .set last_enum_value, value ;\
+ enum_val name
+#define ENUM_END(enum_name)
+
+#else /* C Defines */
+/** Macro to control typedef generation
+ *
+ */
+#define TYPEDEF typedef
+
+/** Macro to control extern generation
+ *
+ */
+#ifndef FUNCDEF
+#define FUNCDEF extern
+#endif
+
+/** Macro to control typedef enum generation
+ *
+ */
+#define ENUM_BEGIN typedef enum {
+
+/** Macro to specify enum instance (auto value assignment)
+ *
+ */
+#define ENUM_VAL(name) name,
+
+/** Macro to control enum specification and value assignment
+*
+*/
+#define ENUM_VALASSIGN(name, value) name = value,
+
+/** Macro to control enum named type generation
+ *
+ */
+#define ENUM_END(enum_name) } enum_name;
+
+#endif
+
+/* Example of how to use the ENUM definition macros
+ENUM_BEGIN
+ENUM_VAL(INIT)
+ENUM_VAL(RESET)
+ENUM_VAL(CONFIGURED)
+ENUM_END(enum_label)
+*/
+
+#endif /* __C_ARM_MACROS__ */
diff --git a/AT91SAM7S256/armdebug/Debugger/debug_comm.S b/AT91SAM7S256/armdebug/Debugger/debug_comm.S
new file mode 100644
index 0000000..e237999
--- /dev/null
+++ b/AT91SAM7S256/armdebug/Debugger/debug_comm.S
@@ -0,0 +1,538 @@
+/** @file debug_comm.S
+ * @brief GDB Server communications support routines
+ *
+ */
+
+/* Copyright (C) 2007-2011 the NxOS developers
+ *
+ * Module Developed by: TC Wan <tcwan@cs.usm.my>
+ *
+ * See AUTHORS for a full list of the developers.
+ *
+ * See COPYING for redistribution license
+ *
+ */
+
+#define __ASSEMBLY__
+#include "debug_macros.h"
+#include "debug_stub.h"
+#include "debug_internals.h"
+
+ .extern dbg__sendCommMsg
+
+ /* Hexutils function references */
+ .extern hex2char
+ .extern char2hex
+ .extern byte2ascii
+ .extern halfword2ascii_be
+ .extern halfword2ascii_le
+ .extern word2ascii_be
+ .extern word2ascii_le
+ .extern ascii2hex_varlen_be
+ .extern ascii2byte
+ .extern ascii2halfword_be
+ .extern ascii2halfword_le
+ .extern ascii2word_be
+ .extern ascii2word_le
+
+
+.bss
+.align 4
+
+ .global debug_InCommBuf
+ .global debug_OutCommBuf
+debug_InCommBuf:
+ .space USB_BUFSIZE,0
+debug_OutCommBuf:
+ .space USB_BUFSIZE,0
+
+debug_msgRxBufPtr:
+ .word 0x0
+debug_msgTxBufPtr:
+ .word 0x0
+
+debug_msgRxBuf_AppendPtr:
+ .word 0x0
+debug_msgTxBuf_AppendPtr:
+ .word 0x0
+
+ .equ RXAPPENDPTR_OFFSET, (debug_msgRxBuf_AppendPtr - debug_msgRxBufPtr)
+ .equ TXAPPENDPTR_OFFSET, (debug_msgTxBuf_AppendPtr - debug_msgTxBufPtr)
+
+debug_segmentRxNum: /* Current Rx Segment Number */
+ .word 0x0
+
+/* Comm Channel and NXT Received Message Length is now common to both NxOS and NXT Firmware */
+debug_nxtMsgLength:
+ .word 0x0
+
+ .global debug_nxtCommChannel
+debug_nxtCommChannel:
+ .word 0x0
+
+ .global debug_nxtCommOverrun
+debug_nxtCommOverrun:
+ .word 0x0
+
+ .equ NXTCOMMCHANNEL_OFFSET, (debug_nxtCommChannel - debug_nxtMsgLength)
+ .equ NXTCOMMOVERRUN_OFFSET, (debug_nxtCommOverrun - debug_nxtMsgLength)
+
+.data
+.align 4
+
+nxt_commcmd_header:
+ .byte NXT_GDBMSG_TELEGRAMTYPE, 0x00, 0x00 /* padded to 3 bytes */
+
+.code 32
+.text
+.align 4
+/* Debugger Communications Routines
+ * It does not make sense to pass information from the Debugger Module to the Comm. link one character
+ * at a time, especially if we're not using a native serial interface (e.g., EIA-232). Consequently
+ * a Message interface has been defined. This can still call getChar() and putChar() subroutines
+ * if so desired, but it'll be a purely internal matter.
+ *
+ * Message Format
+ * Since we need to use EP1&2 (Bulk channels) to communicate with the PC Host, the messages should
+ * follow the NXT Direct Commands message structure (this will allow for interoperability with NXT Firmware
+ * in addition to NxOS). The maximum length of any USB communications via the Bulk channel is 64 bytes.
+ * There is a one byte Telegram Type field which identifies the type of telegram, followed by the
+ * Telegram header and actual message.
+ *
+ * The LEGO Mindstorms Communications Protocol Direct Commands GDB Message format (including all headers)
+ * is as follows:
+ *
+ * GDB Command
+ * ===========
+ * Byte 0: Telegram Type Field (0x8d Direct Command, No response required) | NXT Msg Header
+ * Byte 1: Segment No (1-255, 0: Last Segment; limit is MSG_NUMSEGMENTS) |
+ * Byte 2: Telegram Size (Len of USB Buffer - 3, max is MSG_SEGMENTSIZE) |
+ * Byte 3-N: Message data | GDB Command
+ *
+ * The GDB Command (of size M) has the following format:
+ * Offset 0: '+'/'-' Command Received Status (Optional)
+ * Offset 1/0: '$'
+ * Offset 2/1: GDB Command char
+ * Offset 3 - (M-4): Command packet info
+ * Offset M-3: '#'
+ * Offset M-2: MSB of Checksum
+ * Offset M-1: LSB of Checksum
+ *
+ * To be safe, we assume that the Command Received Status is always sent by the GDB server. Therefore,
+ * The maximum size of a GDB Command packet is MSGBUF_SIZE - 5 ('+'/'-', '$', '#', 2 byte checksum)
+ *
+ * GDB Response
+ * ============
+ * Byte 0: Telegram Type Field (0x8d Direct Command, No response required) | NXT Msg Header
+ * Byte 1: Segment No (1-255, 0: Last Segment; limit is MSG_NUMSEGMENTS) |
+ * Byte 2: Telegram Size (Len of USB Buffer - 3, max is MSG_SEGMENTSIZE) |
+ * Byte 3-N: Message data | GDB Response
+ *
+ * The GDB Retransmission Request has the following format:
+ * Offset 0: '-' Command Received Status
+ *
+ * The GDB Response (of size M) has the following format:
+ * Offset 0: '+' Command Received Status
+ * Offset 1: '$'
+ * Offset 2 - (M-4): Response packet info
+ * Offset M-3: '#'
+ * Offset M-2: MSB of Checksum
+ * Offset M-1: LSB of Checksum
+ *
+ * The maximum size of a GDB Response packet is MSGBUF_SIZE - 5 ('+', '$', '#', 2 byte checksum)
+ *
+ * Note: The Telegram Size is the actual size of the Message Data portion
+ * (i.e., excludes the three header bytes, includes the GDB Command/Response Packet checksum bytes
+ * in the last segment)
+ */
+
+ .global dbg__comm_init
+/* dbg__comm_init
+ * Initialize communications channel.
+ * On Entry:
+ * R0: MSG Rx Buf Pointer
+ * R1: MSG Tx Buf Pointer
+ */
+
+dbg__comm_init:
+ stmfd sp!, {lr}
+ ldr r2, =debug_msgRxBufPtr
+ stmia r2!, {r0, r1} /* debug_msgRxBufPtr and debug_msgTxBufPtr */
+ stmia r2!, {r0, r1} /* debug_msgRxBuf_AppendPtr and debug_msgTxBuf_AppendPtr */
+ bl _dbg__comm_readbuf_reset
+ ldr r1, =debug_nxtMsgLength
+ mov r0, #0
+ str r0, [r1, #NXTCOMMCHANNEL_OFFSET] /* Clear NXT Channel on INIT */
+ ldmfd sp!, {pc}
+
+
+_dbg__comm_readbuf_reset:
+ ldr r1, =debug_nxtMsgLength
+ mov r0, #0
+ str r0, [r1] /* Clear Received Comm Message Length */
+ bx lr
+
+ .global dbg__copyNxtDebugMsg
+/* dbg__copyNxtDebugMsg
+ * Copy NXT Debug Message to our own Buffers, indicate Msg Received status.
+ * Note: This routine is now used by both NXT Firmware and NxOS
+ * On Entry:
+ * R0: NXT Input Buf Pointer
+ * R1: NXT Communications Channel Enum (CmdBit)
+ * R2: NXT Raw Message Length
+ * On Exit:
+ * R0-R3: Destroyed
+ */
+dbg__copyNxtDebugMsg:
+ ldr r3, =debug_nxtMsgLength
+ str r1, [r3, #NXTCOMMCHANNEL_OFFSET] /* save Communications Channel first */
+ ldr r1, [r3] /* Check if there's an unread message in the buffer */
+ cmp r1, #0
+ beq cont_dbg__copyNxtDebugMsg /* No unread message, so continue */
+exit_dbg__NxtDebugMsgOverrun:
+ ldr r1, [r3, #NXTCOMMOVERRUN_OFFSET]
+ add r1, r1, #1
+ str r1, [r3, #NXTCOMMOVERRUN_OFFSET] /* update message overrun stats */
+ b exit_dbg__copyNxtDebugMsg
+cont_dbg__copyNxtDebugMsg:
+ str r2, [r3]
+ ldr r1, =debug_InCommBuf
+ _dbg_memcpy r1, r0, r2, r3 /* r3: scratch register */
+exit_dbg__copyNxtDebugMsg:
+ bx lr
+
+/* _dbg_reset_msgTxBuf_AppendPtr
+ * Internal variable to reset pointers.
+ * On Exit:
+ * R0: debug_msgTxBuf_AppendPtr
+ * R1: destroyed
+ */
+_dbg_reset_msgTxBuf_AppendPtr:
+ ldr r1, =debug_msgTxBufPtr /* Should not be modified */
+ ldr r0, [r1]
+ str r0, [r1, #TXAPPENDPTR_OFFSET]
+ mov pc, lr
+
+/* _dbg__commHasMsg
+ * Internal Segment Reassembly Routine.
+ * On exit:
+ * r0: !0: (Availale Telegram Message Size), 0: no incoming message/segment
+ * r1: message segment number
+ */
+_dbg__commHasMsg:
+ stmfd sp!, {lr}
+ ldr r0, =debug_nxtMsgLength
+ ldr r0, [r0] /* R0 contains the Comm Buffer Size, including the NXT Direct Command Header */
+
+ ldr r2, =debug_InCommBuf
+ ldrb r1, [r2, #NXT_MSG_TELEGRAMTYPE_OFFSET]
+ cmp r1, #NXT_GDBMSG_TELEGRAMTYPE
+ bne invalid_CommMsg /* Invalid telegram type, ignore */
+
+ ldrb r1, [r2, #NXT_MSG_TELEGRAMSIZE_OFFSET]
+ sub r0, r0, r1 /* Comm Buffer Size - Telegram Size = 3 (header size) */
+ cmp r0, #NXT_GDBMSG_START /* Start offset is equal to header size */
+ bne invalid_CommMsg /* Invalid Message Length, ignore */
+
+ mov r0, r1 /* Telegram Message Size */
+ ldrb r1, [r2, #NXT_MSG_SEGNUM_OFFSET]
+ b _exit_dbg__commHasMsg
+
+invalid_CommMsg:
+ bl _dbg__comm_readbuf_reset /* Next Comm telegram transaction */
+ mov r0, #0
+_exit_dbg__commHasMsg:
+ ldmfd sp!, {pc}
+
+/* _copy_msg_from_commbuf
+ * Internal Comm buffer copy routine, handles segment reassembly.
+ * On entry:
+ * r0: number of bytes to copy
+ * r1: segment number
+ * On exit:
+ * r0: cummulative message length
+ * r1: segment number
+ * r2, r3: Destroyed
+ */
+_copy_msg_from_commbuf:
+ stmfd sp!, {r1,r4,r5,r6,lr}
+ movs r4, r0
+ beq _exit_copy_msg_from_commbuf
+
+ ldr r6, =debug_msgRxBufPtr /* Address of Pointers */
+ ldr r5, [r6] /* Rx buffer Start Address */
+ ldr r2, [r6, #RXAPPENDPTR_OFFSET] /* Append Pointer */
+
+ sub r3, r2, r5 /* r3: current length of message */
+ add r3, r3, r4 /* new cummulative length of message */
+ cmp r3, #MSGBUF_SIZE
+ movhi r4, #0 /* Buffer overflow! */
+ strhi r5, [r6, #RXAPPENDPTR_OFFSET] /* Reset AppendPtr to beginning of Rx Buffer */
+ bhi _exit_copy_msg_from_commbuf
+
+ ldr r3, =debug_InCommBuf
+ add r3, r3, #NXT_GDBMSG_START
+ _dbg_memcpy r2, r3, r4, r0 /* r2 updated to point to next empty char slot in Rx buffer */
+ sub r4, r2, r5 /* r4: cummulative length of message */
+
+ /* Update debug_msgRxBuf_AppendPtr */
+ teq r1, #0 /* Check if this is last segment (segment 0) */
+ streq r5, [r6, #RXAPPENDPTR_OFFSET] /* Reset AppendPtr to beginning of Rx Buffer if so */
+ strne r2, [r6, #RXAPPENDPTR_OFFSET] /* Otherwise, update Append Pointer to receive next segment */
+
+_exit_copy_msg_from_commbuf:
+ bl _dbg__comm_readbuf_reset /* Next Comm telegram transaction */
+ mov r0, r4 /* Return cummulative message length in R0 */
+ ldmfd sp!, {r1,r4,r5,r6,pc} /* Return segment number in R1 */
+
+
+/* _msgbuf_checksum
+ * Internal routine to calculate checksum character buffer.
+ * On entry:
+ * r0: pointer to character buffer to checksum (assume ASCIIZ terminated)
+ * On exit:
+ * r0: pointer to character buffer after ASCIIZ
+ * r1: checksum (8-bit binary)
+ * r2: message length
+ * r3: destroyed
+ */
+_msgbuf_checksum:
+ mov r1, #0 /* clear checksum */
+ mov r2, #0 /* clear length */
+1: ldrb r3, [r0], #1 /* Iterate through buffer */
+ add r1, r1, r3 /* cummulative sum of char */
+ teq r3, #0
+ addne r2, r2, #1 /* increment message length */
+ bne 1b /* until ASCIIZ found */
+ and r1, #BYTE0 /* Modulo 256 */
+ mov pc, lr
+
+ .global dbg__getDebugMsg
+/* dbg__getDebugMsg
+ * Retrieve pending Debugger Message if available (Non-Blocking).
+ * On entry:
+ * No parameters (assume pointers were initialized previously using dbg__comm_init)
+ * On exit:
+ * r0: >0 = Valid GDB Message Length (incl '$', excluding '#' and checksum),
+ * 0 = no valid message (yet), -1 = error
+ * r1: GDB Message Buffer Pointer (incl '$', excluding '#' and checksum)
+ * r2, r3: Destroyed
+ * Note: If GDB Message were returned, it is ASCIIZ terminated, does not include '#' and checksum
+ */
+dbg__getDebugMsg:
+ stmfd sp!, {r4,r5,lr}
+ bl _dbg__commHasMsg /* r0: message length, r1: segment number */
+ teq r0, #0
+ beq exit_dbg__getDebugMsg /* no new message, exit with R0 = 0 */
+
+ ldr r4, =debug_segmentRxNum
+ ldr r2, [r4] /* Get current Segment Number */
+ add r2, r2, #1 /* Expected Segment Number for comparison */
+ teq r1, #0
+ streq r1, [r4] /* Update current Segment Number with 0 since it is the last segment */
+ beq _hasMsg2Copy
+ cmp r1, #MSG_NUMSEGMENTS /* Segment Number < MSG_NUMSEGMENTS? */
+ bhs _invalid_segment
+ teq r1, r2 /* Valid Segment Number, check against Expected Segment Number */
+ beq _hasMsg2Copy /* Segment Number matches Expected Segment Number, update buffers */
+
+_invalid_segment:
+ bl _dbg__comm_readbuf_reset /* Invalid, Next Comm telegram transaction */
+ mov r0, #0 /* Reset Segment Number */
+ str r0, [r4] /* Update current Segment Number with 0 to prepare for new message */
+ b exit_dbg__getMsgError /* Exit with error */
+
+_hasMsg2Copy:
+ str r1, [r4] /* Update current Segment Number */
+ bl _copy_msg_from_commbuf /* r0: cummulative message length, r1: segment number */
+ teq r1, #0
+ movne r0, #0 /* Incomplete message, ignore for now */
+ bne exit_dbg__getDebugMsg /* Message not complete yet, exit */
+
+ /* Check for valid GDB message */
+ mov r4, r0 /* keep message length in R4, assume to be within MSGBUF_SIZE */
+ ldr r5, =debug_msgRxBufPtr
+ ldr r5, [r5] /* Rx buffer Start Address */
+
+/* Need to account for Packet Acknowledgement */
+1: ldrb r0, [r5]
+ teq r0, #MSGBUF_CTRLC /* Look for Ctrl-C */
+ moveq r0, r4 /* If found, set R0 to current message length */
+ beq exit_dbg__getDebugMsg /* and return */
+ teq r0, #MSGBUF_NAKCHAR /* Look for '-' */
+ beq exit_dbg__getMsgError /* FIXME: We can't handle retransmission, flag message error */
+ teq r0, #MSGBUF_ACKCHAR /* Look for '+' */
+ addeq r5, r5, #1 /* Adjust Buffer Start Pointer (excl '+') */
+ subeq r4, r4, #1 /* Adjust Message Length */
+ beq 1b /* Skip all Packet Acknowledgements */
+
+ /* Note: Here we assume that we won't get a single ACK '+' or NAK '-' character message.
+ * If we do, it'll be flagged as an error
+ */
+ subs r2, r4, #MSGBUF_CHKSUMOFFSET /* Look for '#': Message Length - 3 = '#' offset */
+ blt exit_dbg__getMsgError /* Message Length is too short, exit with error */
+ ldrb r0, [r5, r2]
+ teq r0, #MSGBUF_CHKSUMCHAR
+ bne exit_dbg__getMsgError /* No checksum char '#', exit with error */
+
+ mov r1, #0
+ strb r1, [r5, r2] /* Zero out '#' char for checksum calc later */
+
+#ifdef CHECK_GDBSTARTCHAR
+ /* Checked in dbg__bkpt_waitCMD */
+ ldrb r0, [r5]
+ teq r0, #MSGBUF_STARTCHAR /* Look for '$' */
+ bne exit_dbg__getMsgError /* No start char '$', exit with error */
+#endif
+
+ add r0, r5, #1 /* Checksum packet data (excl '$') */
+ bl _msgbuf_checksum /* R2: length (excl '$'), R1: calculated checksum, R0: pointer to checksum in receive buffer */
+ mov r3, r1 /* Keep calculated checksum in R3 (R1 destroyed by ascii2byte) */
+ bl ascii2byte /* R0: received checksum, R1: address of next buffer location */
+ teq r0, r3 /* Compare calculated checksum in R3 against received checksum in R0 */
+ bne exit_dbg__getMsgError /* Checksums do not match, exit with error */
+
+ subeq r0, r4, #MSGBUF_CHKSUMOFFSET /* Update message length (incl '$') as return parameter */
+ add r2, r2, #1 /* expected message length (from _msgbuf_checksum) */
+ teq r0, r2
+ beq exit_dbg__getDebugMsg /* Valid length, return */
+
+exit_dbg__getMsgError:
+ mov r0, #MSGBUF_MSGERROR
+exit_dbg__getDebugMsg:
+ mov r1, r5 /* Return GDB Message Buffer Pointer in R1 */
+ ldmfd sp!, {r4,r5,pc}
+
+
+/* _copy_msg_to_commbuf
+ * Internal Comm buffer copy routine, handles segment fragmentation.
+ * On entry:
+ * r0: number of bytes to copy
+ * r1: segment number
+ * On exit:
+ * r0: cummulative message length
+ * r1: segment number
+ * r2, r3: Destroyed
+ */
+_copy_msg_to_commbuf:
+ stmfd sp!, {r1,r4,r5,r6,lr}
+ ldr r6, =debug_msgTxBufPtr /* Address of Pointers */
+ ldr r5, [r6, #TXAPPENDPTR_OFFSET] /* Retrieve Tx Append Pointer */
+
+ movs r4, r0
+ beq _exit_copy_msg_to_commbuf
+
+#ifdef CHECK_TXLEN
+ add r0, r4, #NXT_GDBMSG_START /* offset = header size */
+ cmp r0, #USB_BUFSIZE
+ bhi _exit_copy_msg_to_commbuf /* We let calling routine detect problem (segment number will increment) */
+#endif
+
+ /* Fill in Comm Message Header */
+ ldr r3, =debug_OutCommBuf
+ mov r2, #NXT_GDBMSG_TELEGRAMTYPE
+ strb r2, [r3], #1 /* Telegram type */
+ strb r1, [r3], #1 /* Segment Number */
+ strb r0, [r3], #1 /* Message Length */
+
+ mov r2, r5 /* Copy to R2 for updating */
+ mov r1, r4 /* actual GDB message fragment length (exclude Comm header) */
+ _dbg_memcpy r3, r2, r1, r0 /* This copies over the message fragment, r3, r2 updated */
+ mov r5, r2 /* Updated Tx Append Pointer, keep in R5 for now */
+
+ add r0, r4, #NXT_GDBMSG_START /* Total Comm Buffer Size for Tx (NXT_GDBMSG_START offset = header size) */
+ bl dbg__sendCommMsg /* Common interface routine to commnuncations drivers */
+ cmp r0, #TRUE
+ ldrne r5, [r6, #TXAPPENDPTR_OFFSET] /* Tx failed, Retrieve Original Tx Append Pointer */
+ streq r5, [r6, #TXAPPENDPTR_OFFSET] /* Tx succeeded, Update Tx Append Pointer to new position */
+
+_exit_copy_msg_to_commbuf:
+ ldr r6, [r6] /* Retrieve Tx Buffer Start Address */
+ sub r0, r5, r6 /* Return calculated cummulative message length (R0) */
+ ldmfd sp!, {r1,r4,r5,r6,pc} /* Return segment number in R1 */
+
+ .global dbg__putDebugMsg
+/* dbg__putDebugMsg
+ * Sends Debugger Message from calling routine after appending checksum (Blocking) .
+ * On entry:
+ * No parameters (assume pointers were initialized previously using dbg__comm_init)
+ * On exit:
+ * r0: status (0: success, -1: error)
+ * Note: GDB Message to be sent must be ASCIIZ terminated, does not include '#' and checksum
+ * Response packets start with '+' followed by '$' (2 bytes prefix)
+ */
+dbg__putDebugMsg:
+ stmfd sp!, {r4,r5,lr}
+ /* Perform Checksum Calculation */
+ ldr r5, =debug_msgTxBufPtr /* R5: data structure base pointer */
+ ldr r4, [r5] /* Tx buffer Start Address */
+ str r4, [r5, #TXAPPENDPTR_OFFSET] /* Reset Tx buffer Append Pointer */
+ add r0, r4, #2 /* skip '+' and '$' */
+ bl _msgbuf_checksum /* R2: length (excl '+' and '$'), R1: calculated checksum, R0: pointer to checksum in tx buffer */
+
+#ifdef CHECK_TXLEN
+ add r2, r2, #2 /* r2: returned length from _msgbuf_checksum, added with prefix length */
+ sub r3, r0, r4 /* r3: calculated length from pointers (incl. prefix length) */
+ teq r2, r3
+ bne exit_dbg__putMsgError
+#endif
+
+ mov r3, #MSGBUF_CHKSUMCHAR
+ strb r3, [r0, #-1] /* Insert '#' */
+ bl byte2ascii /* On return, R0 points to location after checksum bytes, R1 is original pointer to checksum */
+ sub r4, r0, r4 /* R4 = Calculated total message length (incl '+' and '$', '#' and checksum bytes */
+ cmp r4, #MSG_SEGMENTSIZE /* If total message length > MSG_SEGMENTSIZE */
+ mov r1, #0 /* Initialize Segment Number = 0 (last segment) first */
+ mov r0, #0 /* Initial cummulative message length */
+ mov r5, #0 /* Previous cummulative message length */
+
+ /* We assume unsigned message lengths, so the arithmetic MUST NOT result in negative values */
+_cont_putMsg:
+ cmp r4, r0
+ movls r0, #0 /* R0: Exit status (success) */
+ bls exit_dbg__putDebugMsg /* If Total message length (r4) <= Cummulative message length (r0), we're done */
+ add r2, r0, #MSG_SEGMENTSIZE /* R2: calculate new Max cummulative message length */
+ cmp r4, r2 /* Check total message length (R4) against new Max cummulative message length (R2) */
+ subls r0, r4, r0 /* if total message length (R4) <= new Max cummulative message length (R2), send remainder */
+ movls r1, #0 /* Flag as last segment (Segment Number = 0) */
+ movhi r0, #MSG_SEGMENTSIZE /* else send MSG_SEGMENTSIZE bytes */
+ addhi r1, r1, #1 /* Increment Segment Number */
+ cmp r1, #MSG_NUMSEGMENTS
+ bhs exit_dbg__putMsgError /* If Segment Number >= MSG_NUMSEGMENTS, flag error */
+ bl _copy_msg_to_commbuf /* R0: cummulative message length, R1: segment number */
+ teq r5, r0 /* Check if we managed to transmit the previous message */
+ beq exit_dbg__putMsgError /* No, flag error */
+ movne r5, r0 /* Update previous cummulative message length */
+ b _cont_putMsg
+
+exit_dbg__putMsgError:
+ mov r0, #MSGBUF_MSGERROR
+exit_dbg__putDebugMsg:
+ ldmfd sp!, {r4,r5,pc}
+
+ .global dbg__sendAckOrNak
+/* dbg__sendAckOrNak
+ * Send Ack (for successful receipt of message)
+ * or Nak (for Retransmission due to received message Checksum error) (Blocking) .
+ * On entry:
+ * No parameters (assume pointers were initialized previously using dbg__comm_init)
+ * On exit:
+ * r0: status (0: success, -1: error)
+ * r1: destroyed
+ * Note: An Ack Or Nak is indicated by '+' or '-', which is prepended with the Comm header and sent (without checksum)
+ * Sending Ack is only done for Continue and Step commands, where GDB does not expect any replies.
+ */
+dbg__sendAckOrNak:
+ stmfd sp!, {lr}
+ ldr r1, =debug_msgTxBufPtr /* R2: data structure base pointer */
+ ldr r0, [r1] /* Tx buffer Start Address */
+ str r0, [r1, #TXAPPENDPTR_OFFSET] /* Reset Tx buffer Append Pointer */
+
+ mov r1, #0 /* Initialize Segment Number = 0 (last segment) */
+ mov r0, #1 /* Retransmission message length = 1 */
+ bl _copy_msg_to_commbuf /* R0: cummulative message length, R1: segment number */
+ cmp r0, #1 /* Check if we managed to transmit the previous message */
+ moveq r0, #0 /* R0: Exit status (success) */
+ movne r0, #MSGBUF_MSGERROR /* R0: Exit status (error) */
+ ldmfd sp!, {pc}
+
diff --git a/AT91SAM7S256/armdebug/Debugger/debug_hexutils.S b/AT91SAM7S256/armdebug/Debugger/debug_hexutils.S
new file mode 100644
index 0000000..d54445b
--- /dev/null
+++ b/AT91SAM7S256/armdebug/Debugger/debug_hexutils.S
@@ -0,0 +1,457 @@
+/** @file debug_hexutils.S
+ * @brief GDB hexadecimal conversion utility routines
+ *
+ */
+
+/* Copyright (C) 2007-2011 the NxOS developers
+ *
+ * Module Developed by: TC Wan <tcwan@cs.usm.my>
+ *
+ * See AUTHORS for a full list of the developers.
+ *
+ * See COPYING for redistribution license
+ *
+ */
+
+
+#define __ASSEMBLY__
+
+#include "debug_internals.h"
+
+.data
+.align 4
+
+hex2char_lut:
+ .ascii "0123456789ABCDEF"
+
+/* Macros
+ */
+
+
+/* _hex2char_lut
+ * Internal routine to intialize the LUT address pointer
+ */
+ .macro _hex2char_lut addrptr
+ ldr \addrptr, =hex2char_lut
+ .endm
+
+/* _hex2char_cont
+ * Internal routine that assumes that the LUT has been loaded.
+ * This macro accepts a byte sized hex value as a parameter register(7:0) and returns the
+ * ASCII equivalent in in the same register(7:0)
+ * The second parameter is the LUT address pointer register to use (assumed to be initialized)
+ * WARNING: Assumes that the value in register is sanity checked before invoking macro
+ */
+ .macro _hex2char_cont reg, addrptr
+ ldrb \reg, [\addrptr, \reg]
+ .endm
+
+/* _hex2char
+ * This macro accepts a byte sized hex value as a parameter register(7:0) and returns the
+ * ASCII equivalent in in the same register(7:0)
+ * The second parameter is the LUT address pointer register to use (register content is destroyed)
+ * WARNING: Assumes that the value in register is sanity checked before invoking macro
+ */
+ .macro _hex2char reg, addrptr
+ _hex2char_lut \addrptr
+ _hex2char_cont \reg, \addrptr
+ .endm
+
+/* _char2hex
+ * This macro accepts an ASCII char as a parameter register(7:0) and returns the
+ * equivalent byte sized hex value in in the same register(7:0)
+ * WARNING: Assumes that the char in register is a valid hex char before invoking macro
+ */
+ .macro _char2hex reg
+ cmp \reg, #'A' /* If Alpha */
+ bichs \reg, \reg, #ASCII_LOWER2UPPER_MASK /* Convert to Uppercase */
+ subhs \reg, \reg, #7 /* Adjustment to allow for subtraction with 0x30 */
+ sub \reg, \reg, #0x30 /* get final hex value */
+ .endm
+
+
+.code 32
+.text
+.align 4
+
+
+/* Utility Routines
+ * GDB requires command parameters to be specified as Big Endian values.
+ * However, the read/write register command expect the register contents to be specified in target byte order.
+ * Hence we need both versions of multibyte conversion routines for word sized values.
+ */
+
+/* hex2char
+ * This routine accepts a byte sized hex value in R0(7:0) and returns the
+ * ASCII equivalent in R0(7:0)
+ */
+ .global hex2char
+
+hex2char:
+ stmfd sp!, {r1,lr}
+ and r0, #NIBBLE0 /* make sure that input is sane */
+ _hex2char r0, r1
+ ldmfd sp!, {r1,pc}
+
+/* char2hex
+ * This routine accepts an ASCII character in R0(7:0) and returns the
+ * equivalent byte sized hex value in R0(7:0).
+ * It accepts lowercase and uppercase ASCII Hex char inputs.
+ * Invalid inputs return -1 as the value
+* On entry:
+ * R0: ASCII character
+ * On exit:
+ * R0: Hex value
+ */
+ .global char2hex
+
+char2hex:
+ and r0, r0, #BYTE0 /* make sure that input is sane */
+ cmp r0, #'0'
+ blo char2hex_error
+ cmp r0, #'9'
+ bls perform_char2hex
+ cmp r0, #'A'
+ blo char2hex_error
+ cmp r0, #'F'
+ bls perform_char2hex
+ cmp r0, #'a'
+ blo char2hex_error
+ cmp r0, #'f'
+ bhi char2hex_error
+ /* Validated Hex Char */
+perform_char2hex:
+ _char2hex r0 /* Return hex value in R0 */
+ bx lr
+
+char2hex_error:
+ mov r0, #-1 /* Set Return value to Error value */
+ bx lr
+
+/* byte2ascii_cont
+ * (Shared routine, does not perform sanity checks)
+ * On entry:
+ * R0: ASCII buffer pointer
+ * R1[7:0]: byte value
+ * On exit:
+ * R0: Address of next empty char slot in buffer
+ * R1: Destroyed
+ *
+ * This routine accepts an ASCII buffer pointer in R0 and a byte value in R1,
+ * and stores the ASCII equivalent byte value in the buffer pointed to by R0.
+ * Note: On return, R0 points to next empty char slot in buffer
+ */
+byte2ascii_cont:
+ stmfd sp!, {r2,r3,r4, lr}
+ lsl r2, r1, #24 /* Keep copy of input byte value R1[7:0], shifted to MSB R2[31:24] */
+ mov r4, #2 /* Loop counter */
+ _hex2char_lut r3 /* initialize LUT pointer */
+1: ror r2, r2, #28 /* Rotate MSNibble R2[31:28] into LSNibble position R2[3:0] */
+ and r1, r2, #NIBBLE0 /* Mask out everything else, store Nibble in R1 */
+ _hex2char_cont r1, r3 /* Convert nibble to ASCII char */
+ strb r1, [r0], #1
+ subs r4, r4, #1 /* decrement loop counter */
+ bne 1b
+ ldmfd sp!, {r2,r3,r4, pc}
+
+/* byte2ascii
+ * On entry:
+ * R0: ASCII buffer pointer
+ * R1[7:0]: Byte value
+ * On exit:
+ * R0: Address of next empty char slot in buffer
+ * R1: Original Address of Buffer
+ *
+ * This routine accepts an ASCII buffer pointer in R0 and a byte value in R1,
+ * and stores the ASCII equivalent byte value in the buffer pointed to by R0.
+ * Note: On return, R0 points to the next empty char slot in buffer
+ */
+ .global byte2ascii
+
+byte2ascii:
+ stmfd sp!, {r0, lr} /* Keep ASCII buffer pointer */
+ and r1, #BYTE0 /* sanitize input */
+ bl byte2ascii_cont
+ ldmfd sp!, {r1, pc} /* return original string pointer in R1 */
+
+/* halfword2ascii_be
+ * Big Endian version of halfword2ascii conversion routine
+ * On entry:
+ * R0: ASCII buffer pointer
+ * R1[15:0]: Halfword value
+ * On exit:
+ * R0: Address of next empty char slot in buffer
+ * R1: Original Address of Buffer
+ *
+ * This routine accepts an ASCII buffer pointer in R0 and a halfword value in R1,
+ * and stores the ASCII equivalent halfword value in the buffer pointed to by R0.
+ * Note: On return, R0 points to the next empty char slot in buffer
+ */
+ .global halfword2ascii_be
+halfword2ascii_be:
+ stmfd sp!, {r0,r2,r3, lr} /* Keep ASCII buffer pointer */
+ mov r3, #2 /* Loop Counter */
+ mov r2, r1, lsl #16 /* copy of input halfword value R1[15:0], shifted to MSH R2[31:16] */
+ b _conv_byte2ascii_be /* goto Byte conversion loop */
+
+/* halfword2ascii_le
+ * Little Endian version of halfword2ascii conversion routine
+ * On entry:
+ * R0: ASCII buffer pointer
+ * R1[15:0]: Halfword value
+ * On exit:
+ * R0: Address of next empty char slot in buffer
+ * R1: Original Address of Buffer
+ *
+ * This routine accepts an ASCII buffer pointer in R0 and a halfword value in R1,
+ * and stores the ASCII equivalent halfword value in the buffer pointed to by R0.
+ * Note: On return, R0 points to the next empty char slot in buffer
+ */
+ .global halfword2ascii_le
+halfword2ascii_le:
+ stmfd sp!, {r0,r2,r3, lr} /* Keep ASCII buffer pointer */
+ mov r3, #2 /* Loop Counter */
+ b _conv_byte2ascii_le /* goto Byte conversion loop */
+
+
+/* word2ascii_be
+ * Big Endian version of word2ascii conversion routine
+ * On entry:
+ * R0: ASCII buffer pointer
+ * R1[31:0]: Word value
+ * On exit:
+ * R0: Address of next empty char slot in buffer
+ * R1: Original Address of Buffer
+ *
+ * This routine accepts an ASCII buffer pointer in R0 and a word value in R1,
+ * and stores the ASCII equivalent word value in the buffer pointed to by R0.
+ * Note: On return, R0 points to the next empty char slot in buffer
+ */
+ .global word2ascii_be
+word2ascii_be:
+ stmfd sp!, {r0,r2,r3, lr} /* Keep ASCII buffer pointer */
+ mov r2, r1 /* copy of input word value R1[31:0] */
+ mov r3, #4 /* Loop Counter */
+
+ /* Fall through to byte coversion loop */
+
+
+/* Big Endian Multibyte Convert: Rotate then convert */
+_conv_byte2ascii_be:
+ ror r2, r2, #24 /* Rotate MSB R2[31:24] into LSB position R2[7:0] */
+ and r1, r2, #BYTE0 /* Copy byte value in R2[7:0] into R1 */
+ bl byte2ascii_cont /* R0: next ASCII buffer location pointer, R1: destroyed */
+ subs r3, r3, #1
+ bne _conv_byte2ascii_be
+ ldmfd sp!, {r1,r2,r3, pc}
+
+/* word2ascii_le
+ * Little Endian version of word2ascii conversion routine
+ * On entry:
+ * R0: ASCII buffer pointer
+ * R1[31:0]: Word value
+ * On exit:
+ * R0: Address of next empty char slot in buffer
+ * R1: Original Address of Buffer
+ *
+ * This routine accepts an ASCII buffer pointer in R0 and a word value in R1,
+ * and stores the ASCII equivalent word value in the buffer pointed to by R0.
+ * Note: On return, R0 points to the next empty char slot in buffer
+ */
+ .global word2ascii_le
+word2ascii_le:
+ stmfd sp!, {r0,r2,r3, lr} /* Keep ASCII buffer pointer */
+ mov r2, r1 /* copy of input word value R1[31:0] */
+ mov r3, #4 /* Loop Counter */
+
+ /* Fall through to byte coversion loop */
+
+/* Little Endian Multibyte Convert: Convert then rotate */
+_conv_byte2ascii_le:
+ and r1, r2, #BYTE0 /* Copy byte value in R2[7:0] into R1 */
+ bl byte2ascii_cont /* R0: next ASCII buffer location pointer, R1: destroyed */
+ ror r2, r2, #8 /* Rotate LSB+1 R2[15:8] into LSB position R2[7:0] */
+ subs r3, r3, #1
+ bne _conv_byte2ascii_le
+ ldmfd sp!, {r1,r2,r3, pc}
+
+
+/* ascii2hex_varlen_be
+ * Big Endian version of ascii2hex_varlen conversion routine
+ * (There is no Little Endian Version)
+ * On entry:
+ * R0: ASCII buffer pointer
+ * On exit:
+ * R0: Hex value
+ * R1: Address of next char slot in buffer
+ *
+ * This routine accepts an ASCII buffer pointer in R0,
+ * and returns the hex value in R0 for up to 8 Hex characters.
+ * Note: On return, R1 points to the ASCII buffer location after the hex value chars.
+ */
+ .global ascii2hex_varlen_be
+
+ascii2hex_varlen_be:
+ stmfd sp!, {r2,r3, lr}
+ mov r3, #CMD_REG_REGPARAMLEN /* Set max count to 8 (Max Register size) */
+ mov r1, r0 /* Use R1 as ASCII buffer pointer */
+ mov r2, #0 /* Initialize Cummulative Results */
+2: ldrb r0, [r1] /* Load ASCII char for Hex Value */
+ bl char2hex /* on return, hex value in R0, -1 for error */
+ cmp r0, #-1
+ beq _exit_ascii2hex_varlen
+ orr r2, r0, r2, lsl #4 /* combined byte value */
+ subs r3, r3, #1 /* Decrement Counter */
+ add r1, r1, #1 /* Go to next char slot */
+ bne 2b
+_exit_ascii2hex_varlen:
+ mov r0, r2 /* Return results in R0 */
+ ldmfd sp!, {r2,r3, pc}
+
+
+/* ascii2byte
+ * On entry:
+ * R0: ASCII buffer pointer
+ * On exit:
+ * R0[7:0]: Byte value
+ * R1: Address of next char slot in buffer
+ *
+ * This routine accepts an ASCII buffer pointer in R0,
+ * and returns the byte value in R0[7:0].
+ * Note: On return, R1 points to the ASCII buffer location after the current 2 chars.
+ * WARNING: This routine assumes that the input buffer was sanitized and contains valid Hex chars,
+ * otherwise it will return invalid results.
+ */
+ .global ascii2byte
+
+ascii2byte:
+ stmfd sp!, {r2, lr}
+ mov r1, r0 /* Use R1 as ASCII buffer pointer */
+ ldrb r0, [r1], #1 /* Load ASCII char for MSN */
+ bl char2hex /* on return, hex value in R0, -1 for error (ignored) */
+ mov r2, r0, lsl #4 /* Intermediate Results register */
+ ldrb r0, [r1], #1 /* Load ASCII char for LSN */
+ bl char2hex /* on return, hex value in R0, -1 for error (ignored) */
+ orr r0, r2, r0 /* combined byte value */
+ ldmfd sp!, {r2, pc}
+
+/* ascii2halfword_be
+ * Big Endian version of ascii2halfword conversion routine
+ * On entry:
+ * R0: ASCII buffer pointer
+ * On exit:
+ * R0[15:0]: Halfword value
+ * R1: Address of next char slot in buffer
+ *
+ * This routine accepts an ASCII buffer pointer in R0,
+ * and returns the Halfword value in R0[15:0].
+ * Note: On return, R1 points to the ASCII buffer location after the current 4 chars.
+ * WARNING: This routine assumes that the input buffer was sanitized and contains valid Hex chars,
+ * otherwise it will return invalid results.
+ */
+ .global ascii2halfword_be
+
+ascii2halfword_be:
+ stmfd sp!, {r2,r3, lr}
+ mov r3, #2 /* Loop counter */
+ b _conv_ascii2byte_be
+
+/* ascii2halfword_le
+ * Little Endian version of ascii2halfword conversion routine
+ * On entry:
+ * R0: ASCII buffer pointer
+ * On exit:
+ * R0[15:0]: Halfword value
+ * R1: Address of next char slot in buffer
+ *
+ * This routine accepts an ASCII buffer pointer in R0,
+ * and returns the Halfword value in R0[15:0].
+ * Note: On return, R1 points to the ASCII buffer location after the current 4 chars.
+ * WARNING: This routine assumes that the input buffer was sanitized and contains valid Hex chars,
+ * otherwise it will return invalid results.
+ */
+ .global ascii2halfword_le
+
+ascii2halfword_le:
+ stmfd sp!, {r2,r3, lr}
+ mov r3, #2 /* Loop counter */
+ b _conv_ascii2byte_le
+
+
+/* ascii2word_be
+ * Big Endian version of ascii2word conversion routine
+ * On entry:
+ * R0: ASCII buffer pointer
+ * On exit:
+ * R0[31:0]: Word value
+ * R1: Address of next char slot in buffer
+ *
+ * This routine accepts an ASCII buffer pointer in R0,
+ * and returns the word value in R0[31:0].
+ * Note: On return, R1 points to the ASCII buffer location after the current 8 chars.
+ * WARNING: This routine assumes that the input buffer was sanitized and contains valid Hex chars,
+ * otherwise it will return invalid results.
+ */
+ .global ascii2word_be
+
+ascii2word_be:
+ stmfd sp!, {r2,r3, lr}
+ mov r3, #4 /* Loop counter */
+
+ /* Fall through to byte coversion loop */
+
+_conv_ascii2byte_be:
+ teq r0, #0
+ beq _exit_conv_ascii2byte_be /* exit if NULL pointer in R0 */
+ mov r2, #0 /* Initialize Cummulative value */
+2: bl ascii2byte
+ orr r2, r0, r2, lsl #8 /* Merge current byte with cummulative value */
+ mov r0, r1 /* Copy next char pointer to R0 for next byte */
+ subs r3, r3, #1
+ bne 2b
+ mov r0, r2 /* Copy it to R0 as return value */
+
+_exit_conv_ascii2byte_be:
+ ldmfd sp!, {r2,r3, pc} /* return hex value in R0 */
+
+/* ascii2word_le
+ * Litle Endian version of ascii2word conversion routine
+ * On entry:
+ * R0: ASCII buffer pointer
+ * On exit:
+ * R0[31:0]: Word value
+ * R1: Address of next char slot in buffer
+ *
+ * This routine accepts an ASCII buffer pointer in R0,
+ * and returns the word value in R0[31:0].
+ * Note: On return, R1 points to the ASCII buffer location after the current 8 chars.
+ * WARNING: This routine assumes that the input buffer was sanitized and contains valid Hex chars,
+ * otherwise it will return invalid results.
+ */
+ .global ascii2word_le
+
+ascii2word_le:
+ stmfd sp!, {r2,r3, lr}
+ mov r3, #4 /* Loop counter */
+
+ /* Fall through to byte coversion loop */
+
+_conv_ascii2byte_le:
+ teq r0, #0
+ beq _exit_conv_ascii2byte_le /* exit if NULL pointer in R0 */
+ push {r3} /* Need to keep couter for final value adjustment */
+ mov r2, #0 /* Initialize Cummulative value */
+2: bl ascii2byte
+ orr r2, r0, r2, ror #8 /* Merge current byte with cummulative value */
+ mov r0, r1 /* Copy next char pointer to R0 for next byte */
+ subs r3, r3, #1
+ bne 2b
+ /* Cummulative value done, need to rotate it into the correct position for return value */
+ pop {r3} /* retrieve counter */
+ rsb r3, r3, #5 /* 5 - count */
+ lsl r3, r3, #3 /* [(5-count) x 8] bits to rotate */
+ mov r0, r2, ror r3 /* Copy it to R0 as return value */
+
+_exit_conv_ascii2byte_le:
+ ldmfd sp!, {r2,r3, pc} /* return hex value in R0 */
+
diff --git a/AT91SAM7S256/armdebug/Debugger/debug_internals.h b/AT91SAM7S256/armdebug/Debugger/debug_internals.h
new file mode 100644
index 0000000..c7f5d16
--- /dev/null
+++ b/AT91SAM7S256/armdebug/Debugger/debug_internals.h
@@ -0,0 +1,303 @@
+/** @file debug_internals.h
+ * @brief Shared C/ASM header file for debugger internal constants
+ *
+ */
+
+/* Copyright (C) 2007-2011 the NxOS developers
+ *
+ * Module Developed by: TC Wan <tcwan@cs.usm.my>
+ *
+ * See AUTHORS for a full list of the developers.
+ *
+ * See COPYING for redistribution license
+ *
+ */
+
+#ifndef __DEBUG_INTERNALS_H__
+#define __DEBUG_INTERNALS_H__
+
+#include "_c_arm_macros.h"
+
+
+/** @addtogroup debugger */
+/*@{*/
+
+
+/* Declarations go here. */
+/** @name Debug Message Constants.
+ *
+ * Debug Message Values
+ */
+/*@{*/
+
+/*
+ * USB Buffer Sizes: Ctrl Intr Iso Bulk
+ * Full Speed Device 64 64 1023 64
+ * High Speed Device 64 1024 1024 512
+ */
+
+#define USB_BUFSIZE 64 /* USB Buffer size for AT91SAM7S */
+
+#define NXT_MSG_TELEGRAMTYPE_OFFSET 0 /* NXT Direct Command/Response Header */
+#define NXT_MSG_SEGNUM_OFFSET 1
+#define NXT_MSG_TELEGRAMSIZE_OFFSET 2
+
+#define NXT_GDBMSG_TELEGRAMTYPE 0x8d /* GDB debugger specific, no Response required */
+
+#define NXT_GDBMSG_START 3 /* Offset into USB Telegram buffer */
+
+#define MSG_NUMSEGMENTS 3 /* For packet transfers */
+#define MSG_SEGMENTSIZE (USB_BUFSIZE - NXT_GDBMSG_START) /* 61 bytes per segment */
+#define MSGBUF_SIZE (MSG_SEGMENTSIZE*MSG_NUMSEGMENTS) /* Debug Message Buffer Size, 61 x 3 = 183 chars = ~80 bytes of actual data */
+#define MSGBUF_CHKSUMOFFSET 3 /* to be subtracted from message length */
+#define MSGBUF_IN_OVERHEADLEN 5 /* For calculating max message data length (exclude ASCIIZ char) */
+#define MSGBUF_OUT_OVERHEADLEN 5 /* For calculating max message data length (exclude ASCIIZ char) */
+
+#define MSGBUF_CTRLC 0x03 /* For Out of Band Signaling: not implemented yet */
+#define MSGBUF_STARTCHAR '$'
+#define MSGBUF_ACKCHAR '+'
+#define MSGBUF_NAKCHAR '-'
+#define MSGBUF_ERRCHAR 'E'
+#define MSGBUF_SIGCHAR 'S'
+#define MSGBUF_SETCHAR '='
+#define MSGBUF_CHKSUMCHAR '#'
+#define MSGBUF_SEPCHAR ','
+#define MSGBUF_ARGCHAR ':'
+#define MSGBUF_MSGERROR -1
+/*@}*/
+
+/** @name Debug Command Lookup Constants.
+ *
+ * Debug Command Lookup
+ */
+/*@{*/
+
+#define CMDINDEX_OUTOFRANGE -1
+/*@}*/
+
+/** @name Debug Register Command Constants.
+ *
+ * Debug Register Command
+ */
+/*@{*/
+#define CMD_REG_NUMREGS 17
+#define CMD_REG_GETONE_MINPARAMLEN 1
+#define CMD_REG_GETONE_MAXPARAMLEN 2
+#define CMD_REG_GETALL_PARAMLEN 0
+#define CMD_REG_REGPARAMLEN 8 /* 32-bit ASCII Hex Value */
+#define CMD_REG_SETONE_MINPARAMLEN (2 + CMD_REG_REGPARAMLEN)
+#define CMD_REG_SETONE_MAXPARAMLEN (3 + CMD_REG_REGPARAMLEN)
+#define CMD_REG_SETALL_PARAMLEN (CMD_REG_NUMREGS*CMD_REG_REGPARAMLEN)
+#define CMD_KILL_PARAMLEN 0
+#define CMD_DETACH_PARAMLEN 0
+
+/*@}*/
+
+/** @name Debug Memory Command Constants.
+ *
+ * Debug Memory Command
+ * FIXME: These limits are not enforced by the GDB client, it truncates addresses and lengths to remove leading '0's
+ * The PARAMLEN constants would probably be removed
+ */
+/*@{*/
+#define CMD_NUMITEMS_PARAMLEN 4 /* 16-bit ASCII Hex Value */
+#define CMD_MEM_READ_PARAMLEN (CMD_REG_REGPARAMLEN + CMD_NUMITEMS_PARAMLEN + 1) /* Address length is equivalent to reg param len */
+#define CMD_MEM_WRITE_MINPARAMLEN (CMD_REG_REGPARAMLEN + CMD_NUMITEMS_PARAMLEN + 2) /* Address length is equivalent to reg param len */
+#define CMD_MEM_SEPCHAR_OFFSET CMD_REG_REGPARAMLEN /* Address length is equivalent to reg param len */
+#define CMD_MEM_MAXOUTBUFLEN (MSGBUF_SIZE - MSGBUF_OUT_OVERHEADLEN)
+#define CMD_MEM_MAXREADBYTES (CMD_MEM_MAXOUTBUFLEN/2)
+#define CMD_MEM_MAXINBUFLEN (MSGBUF_SIZE - MSGBUF_IN_OVERHEADLEN)
+#define CMD_MEM_MAXWRITEBYTES ((CMD_MEM_MAXINBUFLEN - CMD_MEM_WRITE_MINPARAMLEN)/2)
+/*@}*/
+
+/** @name Debug Continue and Step Command Constants.
+ *
+ * Debug Continue and Step Command
+ */
+/*@{*/
+#define CMD_CONTINUE_MINPARAMLEN 0
+#define CMD_STEP_MINPARAMLEN 0
+/*@}*/
+
+
+
+/** @name Debug Breakpoint Command Constants.
+ *
+ * Debug Breakpoint Command
+ */
+/*@{*/
+
+#define CMD_BKPT_INSERT_MINPARAMLEN 5
+#define CMD_BKPT_REMOVE_MINPARAMLEN 5
+
+
+#define CMD_BKPT_TYPE_BREAK_MEMORY 0
+#define CMD_BKPT_TYPE_BREAK_HARD 1 /* Not supported */
+#define CMD_BKPT_TYPE_WATCH_WRITE 2 /* Not supported (yet) */
+#define CMD_BKPT_TYPE_WATCH_READ 3 /* Not supported (yet) */
+#define CMD_BKPT_TYPE_WATCH_ACCESS 4 /* Not supported (yet) */
+
+#define CMD_BKPT_KIND_THUMB 2
+#define CMD_BKPT_KIND_THUMB2 3 /* Not supported */
+#define CMD_BKPT_KIND_ARM 4
+
+#define CMD_BKPT_NOTFOUND -1
+
+/*@}*/
+
+/** @name Debug Stack Constants.
+ *
+ * Debug Stack Manipulation Values
+ */
+/*@{*/
+#define DBGSTACK_NEXTINSTR_INDEX 0 /* Next Instruction Address is at index 0 from bottom of Debug Stack */
+#define DBGSTACK_USERCPSR_INDEX 1 /* User CPSR (SPSR_UNDEF) is at index 1 from bottom of Debug Stack */
+#define DBGSTACK_USERREG_INDEX 2 /* R0 starts at index 2 from bottom of Debug Stack */
+#define DBGSTACK_USERSP_INDEX (DBGSTACK_USERREG_INDEX + REG_SP) /* SP is R13 */
+#define DBGSTACK_USERLR_INDEX (DBGSTACK_USERREG_INDEX + REG_LR) /* LR is R14 */
+#define DBGSTACK_USERPC_INDEX (DBGSTACK_USERREG_INDEX + REG_PC) /* PC is R15 */
+/*@}*/
+
+
+/** @name Exception Handler Vector Definitions.
+ *
+ * Exception Handler Vectors.
+ */
+/*@{*/
+
+#define RESET_VECTOR 0x00000000
+#define UNDEF_VECTOR 0x00000004
+#define SVC_VECTOR 0x00000008
+#define PABRT_VECTOR 0x0000000C
+#define DABRT_VECTOR 0x00000010
+#define RESERVED_VECTOR 0x00000014
+#define IRQ_VECTOR 0x00000018
+#define FIQ_VECTOR 0x0000001C
+
+
+/*@}*/
+
+
+/** @name Bitmask Definitions.
+ *
+ * Various Bitmasks used for data manipulation.
+ */
+/*@{*/
+#define BKPT_STATE_THUMB_FLAG 0x01 /* Flag Thumb Breakpoint */
+#define ASCII_LOWER2UPPER_MASK 0x20 /* ASCII Conversion bitmask */
+#define NIBBLE0 0x0000000F /* Nibble 0 word(3:0) */
+#define NIBBLE1 0x000000F0 /* Nibble 1 word(7:4) */
+#define NIBBLE2 0x00000F00 /* Nibble 2 word(11:8) */
+#define NIBBLE3 0x0000F000 /* Nibble 3 word(15:12) */
+#define NIBBLE4 0x000F0000 /* Nibble 4 word(19:16) */
+#define NIBBLE5 0x00F00000 /* Nibble 5 word(23:20) */
+#define NIBBLE6 0x0F000000 /* Nibble 6 word(27:24) */
+#define NIBBLE7 0xF0000000 /* Nibble 7 word(31:28) */
+#define BYTE0 0x000000FF /* Byte 0 word(7:0) */
+#define BYTE1 0x0000FF00 /* Byte 1 word(15:8) */
+#define BYTE2 0x00FF0000 /* Byte 2 word(23:16) */
+#define BYTE3 0xFF000000 /* Byte 3 word(31:24) */
+#define HLFWRD0 0x0000FFFF /* Halfword 0 word(15:0) */
+#define HLFWRD1 0xFFFF0000 /* Halfword 0 word(31:16) */
+/*@}*/
+
+/** @name CPSR Bit Definitions.
+ *
+ * Various Bit definitions for accessing the CPSR register.
+ */
+/*@{*/
+#define CPSR_THUMB 0x00000020
+#define CPSR_FIQ 0x00000040
+#define CPSR_IRQ 0x00000080
+#define CPSR_MODE 0x0000001F
+#define CPSR_COND 0xF0000000
+
+/* ARM Exception Modes */
+#define MODE_USR 0x10 /* User mode */
+#define MODE_FIQ 0x11 /* FIQ mode */
+#define MODE_IRQ 0x12 /* IRQ mode */
+#define MODE_SVC 0x13 /* Supervisor mode */
+#define MODE_ABT 0x17 /* Abort mode */
+#define MODE_UND 0x1B /* Undefined mode */
+#define MODE_SYS 0x1F /* System mode */
+
+/* Condition Flags
+ * b31 b30 b29 b28
+ * N Z C V
+ */
+#define CPSR_NFLAG 0x80000000
+#define CPSR_ZFLAG 0x40000000
+#define CPSR_CFLAG 0x20000000
+#define CPSR_VFLAG 0x10000000
+
+/*@}*/
+
+/** Debugger State Enums
+ *
+ * Debugger State.
+ * The enums must be consecutive, starting from 0
+ */
+ENUM_BEGIN
+ENUM_VALASSIGN(DBG_RESET, 0) /**< Initial State. */
+ENUM_VAL(DBG_INIT) /**< Debugger Initialized. */
+ENUM_VAL(DBG_CONFIGURED) /**< Debugger has been configured by GDB Server */
+ENUM_END(dbg_state_t)
+
+/** Breakpoint Type Enums
+ *
+ * Breakpoint Type.
+ * The enums must be consecutive, starting from 0
+ */
+ENUM_BEGIN
+ENUM_VALASSIGN(DBG_AUTO_BKPT,0) /**< Auto Breakpoint (Instruction resume after breakpoint). */
+ENUM_VAL(DBG_MANUAL_BKPT_ARM) /**< Manual ARM Breakpoint. */
+ENUM_VAL(DBG_NORMAL_BKPT_ARM) /**< Normal ARM Breakpoint (Single Step, Normal). */
+ENUM_VAL(DBG_MANUAL_BKPT_THUMB) /**< Manual Thumb Breakpoint. */
+ENUM_VAL(DBG_NORMAL_BKPT_THUMB) /**< Normal Thumb Breakpoint (Single Step, Normal). */
+ENUM_END(bkpt_type_t)
+
+/** Debugger Message Error Enums
+ *
+ * Debugger Error Message Enums.
+ * The enums must be consecutive, starting from 1
+ */
+/* FIXME: Need to validate against the ecos-generic-stub.c Error enums */
+ENUM_BEGIN
+ENUM_VALASSIGN(MSG_ERRIMPL, 0) /**< Stub (not implemented) Error. */
+ENUM_VAL(MSG_ERRINLENGTH) /**< Message Write Length Error. */
+ENUM_VAL(MSG_ERROUTLENGTH) /**< Message Read Length Error. */
+ENUM_VAL(MSG_ERRFORMAT) /**< Message Format Error. */
+ENUM_VAL(MSG_UNKNOWNCMD) /**< Unrecognized Command Error. */
+ENUM_VAL(MSG_UNKNOWNPARAM) /**< Unrecognized Parameter Error. */
+ENUM_VAL(MSG_UNKNOWNBRKPT) /**< Unrecognized Breakpoint Error. */
+ENUM_END(dbg_msg_errno)
+
+/** Register Enums
+ *
+ * Register Enums.
+ * Refer to eCOS's arm_stub.h for enum values
+ */
+ENUM_BEGIN
+ENUM_VALASSIGN(REG_R0, 0) /**< User Reg R0 */
+ENUM_VAL(REG_R1) /**< User Reg R1 */
+ENUM_VAL(REG_R2) /**< User Reg R2 */
+ENUM_VAL(REG_R3) /**< User Reg R3 */
+ENUM_VAL(REG_R4) /**< User Reg R4 */
+ENUM_VAL(REG_R5) /**< User Reg R5 */
+ENUM_VAL(REG_R6) /**< User Reg R6 */
+ENUM_VAL(REG_R7) /**< User Reg R7 */
+ENUM_VAL(REG_R8) /**< User Reg R8 */
+ENUM_VAL(REG_R9) /**< User Reg R9 */
+ENUM_VAL(REG_R10) /**< User Reg R10 */
+ENUM_VAL(REG_R11) /**< User Reg R11 */
+ENUM_VAL(REG_R12) /**< User Reg R12 */
+ENUM_VAL(REG_SP) /**< Previous Mode SP (R13) */
+ENUM_VAL(REG_LR) /**< Previous Mode LR (R14) */
+ENUM_VAL(REG_PC) /**< Program Counter (R15) */
+ENUM_VALASSIGN(REG_FPSCR, 24) /**< Previous Mode FPSCR (dummy) */
+ENUM_VAL(REG_CPSR) /**< Previous Mode CPSR */
+
+ENUM_END(register_enum_t)
+
+
+#endif /* __DEBUG_INTERNALS_H__ */
diff --git a/AT91SAM7S256/armdebug/Debugger/debug_macros.h b/AT91SAM7S256/armdebug/Debugger/debug_macros.h
new file mode 100644
index 0000000..75d6299
--- /dev/null
+++ b/AT91SAM7S256/armdebug/Debugger/debug_macros.h
@@ -0,0 +1,386 @@
+/** @file debug_macros.h
+ * @brief internal macros used by debug_stub routines
+ *
+ */
+
+/* Copyright (C) 2007-2010 the NxOS developers
+ *
+ * Module Developed by: TC Wan <tcwan@cs.usm.my>
+ *
+ * See AUTHORS for a full list of the developers.
+ *
+ * See COPYING for redistribution license
+ *
+ */
+
+#ifndef __DEBUG_MACROS_H__
+#define __DEBUG_MACROS_H__
+
+
+
+/** @addtogroup debug_macros */
+/*@{*/
+
+/* _dbg_jumpTableHandler
+ * Call Jump Table Routine based on Index
+ * On entry:
+ * jumptableaddr is the address (constant) of the Jump Table
+ * jumpreg is the register used to perform the indirect jump
+ * indexreg contains jump table index value
+ */
+ .macro _dbg_jumpTableHandler jumptableaddr, jumpreg, indexreg
+
+ ldr \jumpreg, =\jumptableaddr
+ ldr \jumpreg, [\jumpreg, \indexreg, lsl #2]
+ mov lr, pc
+ bx \jumpreg /* Call Command Handler Routine */
+ .endm
+
+
+/* _dbg_thumbDecodeEntry
+ * Load Thumb Instruction Decoder Entry
+ * On entry:
+ * instrreg is the register to load the instruction into
+ * instrmask is the register to load the instruction mask into
+ * codehandler is the register to load the code handling routine into
+ * indexreg contains decode table index value
+ * NOTE: instrreg, instrmask, codehandler must be in increasing register number order
+ */
+ .macro _dbg_thumbDecodeEntry instrreg, instrmask, codehandler, indexreg
+
+ ldr \instrmask, =debug_thumbDecodeTable /* Temporary register */
+ add \instrmask, \instrmask, \indexreg, lsl #3
+ ldm \instrmask, {\instrreg, \codehandler} /* LSHW: IID, MSHW: IBM */
+ mov \instrmask, \instrreg, lsr #16
+ mov \instrreg, \instrreg, lsl #16
+ mov \instrreg, \instrreg, lsr #16 /* Keep HLFWORD0 containing instruction */
+ .endm
+
+/* _dbg_armDecodeEntry
+ * Load ARM Instruction Decoder Entry
+ * On entry:
+ * instrreg is the register to load the instruction into
+ * instrmask is the register to load the instruction mask into
+ * codehandler is the register to load the code handling routine into
+ * indexreg contains decode table index value
+ * NOTE: instrreg, instrmask, codehandler must be in increasing register number order
+ */
+ .macro _dbg_armDecodeEntry instrreg, instrmask, codehandler, indexreg
+
+ ldr \instrmask, =debug_thumbDecodeTable /* Temporary register */
+ add \instrmask, \instrmask, \indexreg, lsl #3
+ add \instrmask, \instrmask, \indexreg, lsl #2 /* 12 byte entries */
+ ldm \instrmask, {\instrreg, \instrmask, \codehandler}
+ .endm
+
+/* _asciiz
+ * Terminate string given string buffer pointer in \strptr
+ * scratchreg is used as a scratch register (destroyed)
+ *
+ */
+ .macro _asciiz strptr, scratchreg
+ mov \scratchreg, #0 /* ASCIIZ character */
+ strb \scratchreg, [\strptr] /* Terminate ASCII string */
+ .endm
+
+
+/* _dbg_stpcpy
+ * _dbg_stpcpy macro
+ * On entry:
+ * deststrptr: Destination string
+ * sourcestrptr: Source string
+ * scratchreg: scratch register for copying
+ * On exit:
+ * deststrptr: Pointer to ASCIIZ character in destination string
+ * sourcestrptr: Pointer to next character slot in source string (after ASCIIZ)
+ * scratchreg: destroyed
+ */
+ .macro _dbg_stpcpy deststrptr, sourcestrptr, scratchreg
+1: ldrb \scratchreg, [\sourcestrptr], #1
+ strb \scratchreg, [\deststrptr], #1
+ teq \scratchreg, #0
+ bne 1b
+ sub \deststrptr, \deststrptr, #1 /* Adjust Destination string pointer to point at ASCIIZ character */
+ .endm
+
+/* _dbg_memcpy
+ * _dbg_stpcpy macro
+ * On entry:
+ * deststrptr: Destination string
+ * sourcestrptr: Source string
+ * sizereg: Number of bytes to copy
+ * scratchreg: scratch register for copying
+ * On exit:
+ * deststrptr: Pointer to next character slot in destination string
+ * sourcestrptr: Pointer to next character slot in source string
+ * sizereg, scratchreg: destroyed
+ */
+ .macro _dbg_memcpy deststrptr, sourcestrptr, sizereg, scratchreg
+1: ldrb \scratchreg, [\sourcestrptr], #1
+ strb \scratchreg, [\deststrptr], #1
+ subs \sizereg, \sizereg, #1
+ bne 1b
+ .endm
+
+/* _dbg_outputAckOnlyFlag
+ * Return Flag ('+') for Continue or Step
+ * On exit:
+ * R0: Pointer to Output Buffer ASCIIZ location
+ * R1: destroyed
+ * R2: destroyed
+ */
+ .macro _dbg_outputAckOnlyFlag
+ ldr r0, =debug_OutMsgBuf
+ ldr r1, =debug_AckOnlyFlag /* ASCIIZ terminated */
+ _dbg_stpcpy r0, r1, r2
+ .endm
+
+
+/* _dbg_outputRetransmitFlag
+ * Return Flag ('-') for Checksum Error (retransmission needed)
+ * On exit:
+ * R0: Pointer to Output Buffer ASCIIZ location
+ * R1: destroyed
+ * R2: destroyed
+ */
+ .macro _dbg_outputRetransmitFlag
+ ldr r0, =debug_OutMsgBuf
+ ldr r1, =debug_RetransmitFlag /* ASCIIZ terminated */
+ _dbg_stpcpy r0, r1, r2
+ .endm
+
+/* _dbg_outputMsgValidResponse
+ * Return Message with valid response ('+$')
+ * On exit:
+ * R0: Pointer to Output Buffer next character slot location
+ * R1: destroyed
+ * R2: destroyed
+ */
+ .macro _dbg_outputMsgValidResponse
+ ldr r0, =debug_OutMsgBuf
+ ldr r1, =debug_ValidResponsePrefix
+ _dbg_stpcpy r0, r1, r2
+ .endm
+
+
+/* _dbg_outputMsgStatusOk
+ * Return Message with Ok ('+$OK') status
+ * On exit:
+ * R0: Pointer to Output Buffer ASCIIZ location
+ * R1: destroyed
+ * R2: destroyed
+ */
+ .macro _dbg_outputMsgStatusOk
+ ldr r0, =debug_OutMsgBuf
+ ldr r1, =debug_OkResponse /* ASCIIZ terminated */
+ _dbg_stpcpy r0, r1, r2
+ .endm
+
+/* __dbg_outputErrMsg
+ * Internal Routine called to generate error messages
+ * Return Message with Error ('+$ENN') status
+ * On entry:
+ * R1: error code
+ * On exit:
+ * R0: Pointer to Output Buffer ASCIIZ location
+ * R1: destroyed
+ * R2: destroyed
+ * R3: destroyed
+ */
+ .macro __dbg_outputErrMsg
+ ldr r0, =debug_OutMsgBuf
+ ldr r2, =debug_ErrorResponsePrefix
+ _dbg_stpcpy r0, r2, r3
+ bl byte2ascii /* R0 points to buffer position after byte value */
+ _asciiz r0, r1
+ .endm
+
+/* _dbg_outputMsgStatusErr
+ * Return Message with Error ('+$ENN') status
+ * On entry:
+ * R0: error code
+ * On exit:
+ * R0: Pointer to Output Buffer ASCIIZ location
+ * R1: destroyed
+ * R2: destroyed
+ * R3: destroyed
+ */
+ .macro _dbg_outputMsgStatusErr
+ mov r1, r0
+ __dbg_outputErrMsg
+ .endm
+
+/* _dbg_outputMsgStatusErrCode
+ * Return Message with Error ('+$ENN') status
+ * On exit:
+ * R0: Pointer to Output Buffer ASCIIZ location
+ * R1: destroyed
+ * R2: destroyed
+ */
+ .macro _dbg_outputMsgStatusErrCode errcode
+ mov r1, #\errcode
+ __dbg_outputErrMsg
+ .endm
+
+/* _dbg_outputMsgStatusSig
+ * Return Message with Signal ('+$SNN') status
+ * On exit:
+ * R0: Pointer to Output Buffer ASCIIZ location
+ * R1: destroyed
+ * R2: destroyed
+ */
+ .macro _dbg_outputMsgStatusSig statuscode
+ ldr r0, =debug_OutMsgBuf
+ ldr r1, =debug_SignalResponsePrefix
+ _dbg_stpcpy r0, r1, r2
+ mov r1, #\statuscode
+ bl byte2ascii /* R0 points to buffer position after byte value */
+ _asciiz r0, r1
+ .endm
+
+/* _regenum2index
+ * Convert register enum to debugger stack index
+ *
+ * On entry:
+ * indexenum: enum representing Register to access
+ * indexreg: register to be used to store the debugger stack index value (0-max index)
+ * On exit:
+ * indexreg contains debugger stack index value (0-max index)
+ */
+ .macro _regenum2index indexenum, indexreg
+ add \indexreg, \indexenum, #DBGSTACK_USERREG_INDEX /* Convert register index to Debug Stack index */
+ .endm
+
+/* _getdbgregisterfromindex
+ * Retrieve register contents from debugger stack given index
+ *
+ * On entry:
+ * indexreg contains debugger stack index value (0-max index)
+ * On exit:
+ * indexreg: Breakpoint index (preserved)
+ * contentsreg: Register Contents for given index
+ */
+ .macro _getdbgregisterfromindex indexreg, contentsreg
+ ldr \contentsreg, =__debugger_stack_bottom__
+ ldr \contentsreg, [\contentsreg, \indexreg, lsl #2]
+ .endm
+
+/* _setdbgregisterfromindex
+ * Store register contents to debugger stack given index
+ *
+ * On entry:
+ * indexreg contains debugger stack index value (0-max index)
+ * contentsreg: Register Contents for given index
+ * addressreg: Scratch register for address pointer
+ * On exit:
+ * indexreg: Breakpoint index (preserved)
+ * contentsreg: Register Contents for given index
+ */
+ .macro _setdbgregisterfromindex indexreg, contentsreg, addressreg
+ ldr \addressreg, =__debugger_stack_bottom__
+ str \contentsreg, [\addressreg, \indexreg, lsl #2]
+ .endm
+
+
+/* _index2bkptindex_addr
+ * Convert Breakpoint index to breakpoing entry address
+ *
+ * On entry:
+ * indexreg contains breakpoint index value
+ * On exit:
+ * indexreg: Breakpoint index (preserved)
+ * addrreg: Breakpoint Entry Address
+ */
+ .macro _index2bkptindex_addr indexreg, addrreg
+ ldr \addrreg, =__breakpoints_start__
+ add \addrreg, \addrreg, \indexreg, lsl #3 /* Calculate Breakpoint Entry Address */
+ .endm
+
+/* _dbg_getstate
+ * Get Debugger State
+ * On exit:
+ * reg: Debugger State enum
+ */
+ .macro _dbg_getstate reg
+ ldr \reg, =debug_state
+ ldrb \reg, [\reg]
+ .endm
+
+/* _dbg_setstate
+ * Set Debugger State to given value
+ * On exit:
+ * r0, r1: destroyed
+ */
+ .macro _dbg_setstate state
+ mov r0, #\state
+ ldr r1, =debug_state
+ strb r0, [r1]
+ .endm
+
+/* _dbg_getmode
+ * Get Debugger Mode
+ * On exit:
+ * reg: Debugger Mode (Boolean)
+ */
+ .macro _dbg_getmode reg
+ ldr \reg, =debug_mode
+ ldrb \reg, [\reg]
+ .endm
+
+/* _dbg_setmode
+ * Set Debugger Mode to given value
+ * On exit:
+ * r0, r1: destroyed
+ */
+ .macro _dbg_setmode mode
+ mov r0, #\mode
+ ldr r1, =debug_mode
+ strb r0, [r1]
+ .endm
+
+/* _dbg_get_bkpt_type
+ * Get Breakpoint Type
+ * On exit:
+ * reg: Breakpoint Type
+ */
+ .macro _dbg_get_bkpt_type reg
+ ldr \reg, =debug_bkpt_type
+ ldrb \reg, [\reg]
+ .endm
+
+/* _dbg_set_bkpt_type
+ * Set Breakpoint Type to given value
+ * On exit:
+ * r0, r1: destroyed
+ */
+ .macro _dbg_set_bkpt_type bkpt_type
+ mov r0, #\bkpt_type
+ ldr r1, =debug_bkpt_type
+ strb r0, [r1]
+ .endm
+
+
+
+/* _dbg_getcurrbkpt_index
+ * Get current breakpoint index
+ * On exit:
+ * reg: Breakpoint index
+ */
+ .macro _dbg_getcurrbkpt_index reg
+ ldr \reg, =debug_curr_breakpoint
+ ldrb \reg, [\reg]
+ .endm
+
+/* _dbg_setcurrbkpt_index
+ * Set current breakpoint index
+ * On exit:
+ * r1: destroyed
+ */
+ .macro _dbg_setcurrbkpt_index reg
+ ldr r1, =debug_curr_breakpoint
+ strb \reg, [r1]
+ .endm
+
+ /*@}*/
+
+#endif /* __DEBUG_MACROS_H__ */
diff --git a/AT91SAM7S256/armdebug/Debugger/debug_runlooptasks.S b/AT91SAM7S256/armdebug/Debugger/debug_runlooptasks.S
new file mode 100644
index 0000000..75c477c
--- /dev/null
+++ b/AT91SAM7S256/armdebug/Debugger/debug_runlooptasks.S
@@ -0,0 +1,374 @@
+/** @file debug_runlooptasks.S
+ * @brief GDB Server platform Run Loop
+ *
+ */
+
+/* Copyright (C) 2007-2011 the NxOS developers
+ *
+ * Module Developed by: TC Wan <tcwan@cs.usm.my>
+ *
+ * See AUTHORS for a full list of the developers.
+ *
+ * See COPYING for redistribution license
+ *
+ */
+
+/*
+ * This file contains platform specific code.
+ * This include ABORT Mode Debugger Run Loop operation,
+ * as well as Debugger Interfacing code to the platform code.
+ */
+
+/*
+ * The Debugger has to implement a Run Loop in ABORT mode
+ * since the hardware is still running. Consequently,
+ * the communications subsystems such as USB (and Bluetooth?)
+ * which is used to communicate with the Host needs to be
+ * serviced in order for actual data transmission and reception
+ * to take place. Currently we're reusing the platform's
+ * communication routines to do the actual tx/rx, so it means
+ * that it is not possible to set breakpoints in those modules.
+ * In addition, since the platform communication modules may
+ * handle other tasks, it is currently possible to enter an
+ * indeterminate state where certain communication messages trigger
+ * a platform response which cannot be handled by the Debugger Run Loop.
+ * The alternative is to implement our own communications routines, but
+ * that will take even more code.
+ *
+ * FIXME: It may become necessary to hack the platform communications
+ * routines to detect that we're in the Debugger Run Loop and not the
+ * normal run loop to avoid system crashes, but the current goal is to
+ * have as minimal changes to the platform code as possible.
+ *
+ * Since there are two Run Loops for the platform, the way in which
+ * they interact is as follows:
+ *
+ * [Platform Run Loop] - DBG_INIT/ GDB Cmd/ BKPT -> [Debugger Run Loop]
+ * \ <------ GO/ STEP/ CONT ----- /
+ * ... ...
+ * ... Handle GDB Cmd/Resp
+ * ... ...
+ * {normal runloop {communications /
+ * processing} watchdog routines}
+ * ^-------v v-------^
+ *
+ * The Platform will invoke dbg__bkpt_init() after hardware and system initialization,
+ * before entering the Platform Run Loop. This configures the Debugger, but does not
+ * invoke the Debugger Run Loop unless a Manual Breakpoint is found in the platform code.
+ *
+ * Subsequently, the Debugger Run Loop will be triggered by Breakpoints, or
+ * when the communications subsystem receives a GDB Command.
+ *
+ * The Debugger Run Loop is actually dbg__bkpt_waitCMD(), this file contains
+ * the Run Loop Tasks which needs to be invoked periodically by the Run Loop,
+ * to minimize the coupling between the ARMDEBUG modules and the Platform.
+ *
+ * Note: The Debugger Run Loop does not handle Hardware Shutdown, it is
+ * assumed that we wouldn't need to do so in Debug Mode.
+ *
+ */
+#define __ASSEMBLY__
+
+#define REBOOT_POWERDOWN
+#include "debug_runlooptasks.h"
+
+#include "debug_internals.h"
+#include "debug_macros.h"
+#include "debug_stub.h"
+
+ .code 32
+ .align 4
+ .global dbg__runloopTasks
+ .global dbg__reboot
+ .global dbg__sendCommMsg
+
+#ifdef __NXOS__
+/****************************************************************************
+ *
+ * NxOS Run Loop
+ *
+ ****************************************************************************/
+dbg__runloopTasks:
+/* Currently, there's nothing that needs to be done in the NxOS Run Loop */
+ bx lr
+
+#else
+/****************************************************************************
+ *
+ * NXT Firmware Run Loop
+ *
+ ****************************************************************************/
+dbg__runloopTasks:
+ push {lr}
+ /* FIXME: Add necessary cXXXCtrl calls here */
+ bl cCommCtrl
+ /* OSWatchdogWrite is a NULL function in the NXT Firmware?! */
+ pop {pc}
+#endif
+
+#ifdef __NXOS__
+/****************************************************************************
+ *
+ * NxOS Reboot Routine
+ *
+ ****************************************************************************/
+ dbg__reboot:
+ b nx_core_reset /* Reboot Brick, won't return */
+
+#else
+/****************************************************************************
+ *
+ * NXT Firmware Reboot Routine
+ *
+ ****************************************************************************/
+dbg__reboot:
+ /* Powerdown Sequence
+ dIOCtrlSetPower((POWERDOWN>>8));
+ dIOCtrlTransfer();
+
+ Reboot Sequence
+ dIOCtrlSetPower((UBYTE)(BOOT>>8));
+ dIOCtrlSetPwm((UBYTE)BOOT);
+ dIOCtrlTransfer();
+ */
+
+ /* We implement the powerdown sequence for now */
+
+#ifdef REBOOT_POWERDOWN
+ /* Powerdown sequence */
+ ldr r0, =((POWERDOWN >> 8) & 0xFF)
+ ldr r1, =dIOCtrlSetPower
+ mov lr,pc
+ bx r1
+#else
+ /* Reboot sequence: this forces SAMBA mode??!! */
+ ldr r0, =((BOOT >> 8) & 0xFF)
+ ldr r1, =dIOCtrlSetPower
+ mov lr,pc
+ bx r1
+
+ ldr r0, =(BOOT & 0xFF)
+ ldr r1, =dIOCtrlSetPwm
+ mov lr,pc
+ bx r1
+#endif
+
+_dbg__reboot_wait:
+ ldr r1, =dIOCtrlTransfer
+ mov lr,pc
+ bx r1
+
+ b _dbg__reboot_wait /* Wait for AVR... */
+#endif
+
+
+#ifdef __NXOS__
+ .extern debug_OutCommBuf
+/****************************************************************************
+ *
+ * NxOS Communications Driver Interface Routine
+ *
+ ****************************************************************************/
+/* dbg__sendCommMsg
+ * Internal send routine (interfaces with drivers).
+ * On entry:
+ * R0: Total Message Buffer length
+ * On exit:
+ R0: Tx Status (TRUE if data sent)
+ */
+dbg__sendCommMsg:
+ stmfd sp!, {r4, lr}
+ mov r4, r0 /* Keep Comm Buffer length in R4 */
+ /* Check USB bus status, transmit message if possible */
+ bl nx_usb_is_connected /* R0 = TRUE (#1) if USB is ready */
+ teq r0, #0 /* FALSE == #0;
+ We can't check for True condition since values
+ used by C-Compiler & ARMDEBUG are different */
+ beq dbg__sendCommMsgFailed
+
+ /* Actual transmission (blocking) */
+ ldr r0, =debug_OutCommBuf /* data pointer parameter */
+ mov r1, r4 /* Comm buffer length */
+ bl nx_usb_write
+
+1: bl nx_usb_data_written /* R0 = True if data has been sent */
+ teq r0, #0 /* FALSE == #0;
+ We can't check for True condition since values
+ used by C-Compiler & ARMDEBUG are different */
+ /* FIXME: implement timeout */
+ beq 1b /* Busy Wait Loop */
+dbg__sendCommMsgFailed:
+ mov r0, #FALSE
+
+exit_dbg__sendCommMsg:
+ ldmfd sp!, {r4, pc}
+
+
+#else
+/****************************************************************************
+ *
+ * NXT Firmware Communications Driver Interface Routine
+ *
+ ****************************************************************************/
+/* dbg__sendCommMsg
+ * Internal send routine (interfaces with drivers).
+ * On entry:
+ * R0: Total Message Buffer length
+ * On exit:
+ R0: Tx Status (TRUE if data sent)
+ */
+dbg__sendCommMsg:
+ stmfd sp!, {r4, lr}
+ mov r4, r0 /* Keep Comm Buffer length in R4 */
+ ldr r0, =debug_nxtCommChannel
+ ldr r0, [r0] /* Get Channel Enum */
+ teq r0, #BT_CMD_READY
+ beq dbg__sendBTMsg
+ teq r0, #USB_CMD_READY
+ beq dbg__sendUSBMsg
+ b dbg__sendCommMsgFailed /* Channel Enum Doesn't Match, shouldn't happen? */
+
+dbg__sendBTMsg:
+ /* NXT BT routines do not have any configuration checks */
+ ldr r0, =debug_OutCommBuf /* data pointer parameter */
+ mov r1, r4 /* BT Bytes to Send */
+ mov r2, r4 /* BT Message Size */
+ bl dBtSendMsg /* Send it via Bluetooth (complete message) */
+ mov r0, #TRUE /* Always flag Success */
+ b exit_dbg__sendCommMsg
+
+dbg__sendUSBMsg:
+ /* Check USB bus status, transmit message if possible */
+ bl dUsbIsConfigured /* R0: UByte status, TRUE / FALSE */
+ teq r0, #nxt_UBYTE_TRUE
+ bne dbg__sendCommMsgFailed
+
+ /* Actual transmission (blocking) */
+ ldr r0, =debug_OutCommBuf /* data pointer parameter */
+ mov r1, r4 /* Comm buffer length */
+ bl dUsbWrite /* call NXT Firmware USB driver, return 0: done, !0: remaining chars */
+ teq r0, #0 /* Tx done if returned length is 0 */
+ moveq r0, #TRUE /* Convert NXT firmware return value to our Status (TRUE/FALSE) */
+ beq exit_dbg__sendCommMsg
+dbg__sendCommMsgFailed:
+ mov r0, #FALSE
+
+exit_dbg__sendCommMsg:
+ ldmfd sp!, {r4, pc}
+#endif
+
+
+#ifdef __NXOS__
+/****************************************************************************
+ *
+ * GDB Debugger Invocation Routine for NxOS
+ *
+ ****************************************************************************/
+ .code 32
+ .align 4
+
+ .extern dbg__install_singlestep
+ .extern dbg__activate_singlestep
+ .extern irq_stack_frame_address
+ .global nxos__handleDebug
+/* nxos__handleDebug
+ * Prepare to switch to Debug Mode
+ * int nxos__handleDebug(U8 *buffer, comm_chan_t channel, U32 length);
+ *
+ * This routine is called from NxOS Fantom library to setup
+ * Single Step Breakpoint in preparation for Debugger invocation if we're in
+ * normal execution mode.
+ *
+ * It returns to complete the IRQ handling normally, after which the single
+ * step breakpoint will be triggered, and the incoming GDB message will then
+ * be processed in the dbg__bkpt_waitCMD() loop.
+ *
+ * If we're in Debugger Mode already, then just return and let the
+ * dbg__bkpt_waitCMD() loop handle it normally.
+ *
+ * If we're operating in normal NxOS mode, return True (!0)
+ * If we're already in Debugger Mode, return False (0)
+ */
+nxos__handleDebug:
+ push {lr}
+ /* This routine is called from nx__irq_handler() via fantom_filter_packet().
+ * The operating mode should already have been configured by the IRQ interrupt handler.
+ *
+ * The IRQ Stack Frame Pointer will contains the LR and SPSR from the topmost interrupted task
+ * if it is non-zero (NxOS supports nested IRQs)
+ */
+ bl dbg__copyNxtDebugMsg /* Copy to Debugger Message Buffer, Remember Comm Channel */
+ mov r0, #FALSE /* Setup Default Return value (False) */
+ _dbg_getmode r1 /* Get Debug Mode */
+ cmp r1, #(TRUE & BYTE0) /* Confine it to Byte size */
+ /* If Debug Mode is TRUE, this means that we're already running the Debugger */
+ beq exit_nxos__handleDebug /* Yes, return False */
+
+ /* Retrieve ISR Return Address */
+ ldr r3, =irq_stack_frame_address
+ ldr r3, [r3] /* Get Interrupt Stack Pointer */
+ teq r3, #0
+ beq exit_nxos__handleDebug /* NULL Interrupt Stack Frame Pointer, exit (status: False) */
+
+nxos_switch2debug:
+ /* Since the Interrupt Stack Frame Pointer points to the top of the stack frame,
+ * we'll have to use Load Empty Ascending Stack (LDMEA == LDMDB) to access the variables
+ */
+ ldmdb r3, {r1,r2} /* R1: LR, R2: SPSR */
+ tst r2, #CPSR_THUMB /* Check for Thumb Mode */
+ orrne r1, r1, #1 /* Configure for Thumb Single Step Breakpoint */
+ bl dbg__install_singlestep /* Setup Single Step, next instruction address returned in r1 */
+ bl dbg__activate_singlestep
+ mov r0, #TRUE /* We're going to switch to Debug Mode (via Single Step Breakpoint) */
+
+exit_nxos__handleDebug:
+ pop {r1}
+ bx r1 /* In case we have Interworking from different caller mode */
+
+#else
+
+/****************************************************************************
+ *
+ * GDB Debugger Invocation Routine for NXT Firmware
+ *
+ ****************************************************************************/
+ .code 16
+ .align 2
+
+ .extern dbg__copyNxtDebugMsg
+ .global cCommHandleDebug
+ .thumb_func
+/* cCommHandleDebug
+ * Switch Mode to Debugger.
+ * Used by NXT Firmware only
+ *
+ * UWORD cCommHandleDebug(UBYTE *pInBuf, UBYTE CmdBit, UWORD MsgLength);
+ *
+ * This routine is called from cCommInterprete either in normal operation mode (SVC)
+ * or else when we're in debug mode (ABORT) which uses the cCommCtrl() routine to handle
+ * I/O with the Host.
+ *
+ * On entry, the message is copied from the NXT buffer into our own buffers.
+ *
+ * If this is accessed from normal operation mode, we need to switch mode to
+ * ABORT mode to handle the incoming message using a Manual Breakpoint instruction.
+ * When DEBUG is exited, the execution resumes from the instruction following the Breakpoint.
+ */
+cCommHandleDebug:
+/* Arg Registers are not preserved since this is invoked explicitly */
+ push {lr} /* store arg registers */
+ bl dbg__copyNxtDebugMsg /* Copy to Debugger Message Buffer, Remember Comm Channel */
+ _dbg_getmode r0 /* Get Debug Mode */
+ cmp r0, #(TRUE & BYTE0) /* Confine it to Byte size */
+
+ /* If Debug Mode is TRUE, this means that we're already running the Debugger */
+ beq _cCommHandleDebug_cont
+ /* Else, we're in normal operation mode (SVC), or other mode (??!) and need to force a switch to Debug mode */
+ dbg__bkpt_thumb
+_cCommHandleDebug_cont:
+ mov r0, #0 /* FIXME: Return Status */
+ pop {r1} /* Can't Pop LR directly */
+ bx r1 /* Safe code: actually we should be able to Pop PC since the caller is Thumb Mode */
+
+ .ltorg
+#endif
diff --git a/AT91SAM7S256/armdebug/Debugger/debug_runlooptasks.h b/AT91SAM7S256/armdebug/Debugger/debug_runlooptasks.h
new file mode 100644
index 0000000..360e0d1
--- /dev/null
+++ b/AT91SAM7S256/armdebug/Debugger/debug_runlooptasks.h
@@ -0,0 +1,60 @@
+/** @file debug_runlooptasks.h
+ * @brief Shared C/ASM header file for debugger communications
+ *
+ */
+
+/* Copyright (C) 2007-2011 the NxOS developers
+ *
+ * Module Developed by: TC Wan <tcwan@cs.usm.my>
+ *
+ * See AUTHORS for a full list of the developers.
+ *
+ * See COPYING for redistribution license
+ *
+ */
+
+#ifndef __DEBUG_RUNLOOPTASKS_H__
+#define __DEBUG_RUNLOOPTASKS_H__
+
+#include "_c_arm_macros.h"
+
+/* This is a place holder header file to allow for interfacing with C Routines in either
+ * NxOS or NXT Firmware.
+ *
+ * Since the header files from the original source trees were meant for C programs, we can't
+ * include them directly. Here we just use .extern to reference the routines.
+ */
+
+#ifdef __NXOS__
+
+ .extern nx_usb_is_connected
+ .extern nx_usb_can_write
+ .extern nx_usb_write
+ .extern nx_usb_data_written
+ .extern nx_usb_read
+ .extern nx_usb_data_read
+ .extern nx_core_reset
+
+#else /* NXT Firmware */
+
+ .extern cCommInit
+ .extern cCommCtrl
+ .extern cCommExit
+ .extern dUsbWrite
+ .extern dUsbRead
+ .extern dUsbIsConfigured
+ .extern dBtSendMsg
+ .equ nxt_UBYTE_TRUE, 1
+ .equ nxt_UBYTE_FALSE, 0
+ .equ USB_CMD_READY, 0x01 /* From c_comm.iom */
+ .equ BT_CMD_READY, 0x02 /* From c_comm.iom */
+
+ .extern dIOCtrlSetPower
+ .extern dIOCtrlSetPwm
+ .extern dIOCtrlTransfer
+ .equ BOOT, 0xA55A /* from c_ioctrl.iom */
+ .equ POWERDOWN, 0x5A00 /* from c_ioctrl.iom */
+
+#endif
+
+#endif
diff --git a/AT91SAM7S256/armdebug/Debugger/debug_stack.ld b/AT91SAM7S256/armdebug/Debugger/debug_stack.ld
new file mode 100644
index 0000000..8fc4cb7
--- /dev/null
+++ b/AT91SAM7S256/armdebug/Debugger/debug_stack.ld
@@ -0,0 +1,15 @@
+/* The following linker definitions should be placed in the stack section */
+
+ /* debugger state */
+ __debugger_stack_bottom__ = . ;
+ . += 0x48; /* 16 previous mode registers + SPSR + UNDEF Next Instruction Address */
+ __debugger_stack__ = .;
+ __debugger_stack_top__ = . ;
+
+ /* breakpoints */
+ __breakpoints_start__ = . ;
+ . += 0x40; /* Single Stepping Breakpoint + 7 Breakpoints */
+ __breakpoints_end__ = . ;
+
+/* Symbols */
+ __breakpoints_num__ = (__breakpoints_end__ - __breakpoints_start__) / 8;
diff --git a/AT91SAM7S256/armdebug/Debugger/debug_stub.S b/AT91SAM7S256/armdebug/Debugger/debug_stub.S
new file mode 100644
index 0000000..e32c2dc
--- /dev/null
+++ b/AT91SAM7S256/armdebug/Debugger/debug_stub.S
@@ -0,0 +1,2976 @@
+/** @file debug_stub.S
+ * @brief ARM Breakpoint Debugger support routines
+ *
+ */
+
+/* Copyright (C) 2007-2011 the NxOS developers
+ *
+ * Module Developed by: TC Wan <tcwan@cs.usm.my>
+ *
+ * See AUTHORS for a full list of the developers.
+ *
+ * See COPYING for redistribution license
+ *
+ */
+
+ /* GDB sparc-stub.c comments header included below to document GDB Server Remote protocol */
+ /* This header has been modified to include additional commands not documented in the header stub */
+
+ /****************************************************************************
+
+ 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 $
+ *
+ * Modified for SPARC by Stu Grossman, Cygnus Support.
+ *
+ * This code has been extensively tested on the Fujitsu SPARClite demo board.
+ *
+ * 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 following gdb commands are supported:
+ *
+ * command function Return value
+ *
+ * g return the value of the CPU registers hex data or ENN
+ * GrrrrRRRR.. set the value of the CPU registers OK or ENN
+ * where register values are given as
+ * 32-bit hex values in the sequence:
+ * User R0, R1, ..., R15, CPSR
+ * px get the value of one register (x) hex data or ENN
+ * Px=rrrr set the value of one register (x) to OK or ENN
+ * 32-bit hex value rrrr.
+ * x = ['0','F'] for R0-R15,
+ * '18' for FPSCR (dummy), '19' for User CPSR
+ *
+ * mAA..AA,LLLL Read LLLL bytes at address AA..AA hex data or ENN
+ * MAA..AA,LLLL:bb..bb
+ * Write LLLL bytes at address AA.AA OK or ENN
+ *
+ * D Detach (equivalent to continue Ack Only
+ * at current address)
+ * 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)
+ *
+ * zt,AA..AA,k Remove a Breakpoint of type t at addr OK or ENN
+ * AA..AA of kind k
+ * Zt,AA..AA,k Insert a Breakpoint of type t at addr OK or ENN
+ * AA..AA of kind k
+ * t 0: memory breakpoint
+ * 1: hardware breakpoint
+ * 2: write watchpoint
+ * 3: read watchpoint
+ * 4: access watchpoint
+ * k: 2 (16-bit Thumb), 3 (32-bit Thumb2)
+ * or 4 (32-bit ARM) for t=[0,1]
+ * Num. bytes to watch for t=[2,4]
+ *
+ * All commands and responses are sent with a packet which includes a
+ * checksum. A packet consists of
+ *
+ * $<packet info>#<checksum>.
+ *
+ * where
+ * <packet info> :: <characters representing the command or response>
+ * <checksum> :: < two hex digits computed as modulo 256 sum of <packetinfo>>
+ *
+ * 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
+ *
+ ****************************************************************************/
+ /* Modified GDB Server Remote Protocol definition from GDB's sparc-stub.c Comment Header included above
+ * Additional commands from GDB Reference Appendix D.2
+ *
+ * Note: ARMDEBUG can only implement Memory Breakpoints t=0. Hardware breakpoints requires a JTAG debugger.
+ * Currently, watchpoints are not implemented as they require hardware support as well (need verification).
+ *
+ * GDB requires command parameters to be specified as Big Endian values.
+ * However, the read/write register command expect the register contents to be specified in target byte order.
+ * The default target byte order is Little Endian for the AT91SAM7xxx processor in the NXT.
+ * If Big Endian target byte order is required, the __BIG_ENDIAN__ preprocessor label should be defined.
+ */
+
+/* FIXME: The Hex value arguments passed by GDB does not have fixed lengths! Although the standard says
+ * there should be x digits, it does not follow this requirement. e.g., register index.
+ */
+
+
+#define __ASSEMBLY__
+#include "debug_stub.h"
+#include "debug_internals.h"
+#include "debug_macros.h"
+
+ /* Hexutils function references */
+ .extern hex2char
+ .extern char2hex
+ .extern byte2ascii
+ .extern halfword2ascii_be
+ .extern halfword2ascii_le
+ .extern word2ascii_be
+ .extern word2ascii_le
+ .extern ascii2hex_varlen_be
+ .extern ascii2byte
+ .extern ascii2halfword_be
+ .extern ascii2halfword_le
+ .extern ascii2word_be
+ .extern ascii2word_le
+
+/* Macro definitions */
+
+/* _check_msgseparator
+ * Look for separator ','
+ * On entry:
+ * bufferptr: points to the parameter buffer [can't be R0]
+ * On exit:
+ * R0: destroyed
+ * bufferptr: points to the next character location in the parameter buffer
+ * Flags: Updated
+ */
+
+ .macro _check_msgseparator bufferptr
+ ldrb r0, [\bufferptr], #1 /* get separator */
+ cmp r0, #MSGBUF_SEPCHAR
+ .endm
+
+/* _check_msgargument
+ * Look for argument ':'
+ * On entry:
+ * bufferptr: points to the parameter buffer [can't be R0]
+ * On exit:
+ * R0: destroyed
+ * bufferptr: points to the next character location in the parameter buffer
+ * Flags: Updated
+ */
+
+ .macro _check_msgargument bufferptr
+ ldrb r0, [\bufferptr], #1 /* get separator */
+ cmp r0, #MSGBUF_ARGCHAR
+ .endm
+
+/* _check_msgassignment
+ * Look for assignment '='
+ * On entry:
+ * bufferptr: points to the parameter buffer [can't be R0]
+ * On exit:
+ * R0: destroyed
+ * bufferptr: points to the next character location in the parameter buffer
+ * Flags: Updated
+ */
+
+ .macro _check_msgassignment bufferptr
+ ldrb r0, [\bufferptr], #1 /* get separator */
+ cmp r0, #MSGBUF_SETCHAR
+ .endm
+
+.bss
+.align 4
+debug_InMsgBuf:
+ .space MSGBUF_SIZE,0
+debug_OutMsgBuf:
+ .space MSGBUF_SIZE,0
+
+ /* Make Debugger State accessible from other modules */
+ .global debug_state
+ .global debug_mode
+ .global debug_bkpt_type
+ .global debug_curr_breakpoint
+
+debug_state:
+ .byte 0x0 /* dbg_state_t variable */
+debug_mode:
+ .byte 0x0 /* Boolean variable */
+debug_bkpt_type:
+ .byte 0x0 /* bkpt_type_t variable */
+debug_curr_breakpoint:
+ .byte 0x0
+
+.data
+.align 4
+debug_RetransmitFlag:
+ .byte '-',0
+
+debug_AckOnlyFlag:
+ .byte '+',0
+
+debug_ValidResponsePrefix:
+ .byte '+','$',0
+
+debug_ErrorResponsePrefix:
+ .byte '+','$','E',0
+
+debug_SignalResponsePrefix:
+ .byte '+','$','S',0
+
+debug_OkResponse:
+ .byte '+','$','O','K',0
+
+/* The CmdIndexTable and CmdJumpTable must be kept in sync */
+debug_cmdIndexTable:
+ .byte 'g','G','p','P','m','M','D','c','s','k','z','Z','?','q','Q',0
+
+/* Command Handlers
+ * On entry:
+ * R0: Input Message Parameter Buffer address pointer (points to contents after '$' and '<cmdchar>')
+ */
+.align 4
+debug_cmdJumpTable:
+ .word _dbg__cmd_GetAllRegs /* 'g' */
+ .word _dbg__cmd_SetAllRegs /* 'G' */
+ .word _dbg__cmd_GetOneReg /* 'p' */
+ .word _dbg__cmd_SetOneReg /* 'P' */
+ .word _dbg__cmd_ReadMem /* 'm' */
+ .word _dbg__cmd_WriteMem /* 'M' */
+ .word _dbg__cmd_Detach /* 'D' */
+ .word _dbg__cmd_Continue /* 'c' */
+#ifdef __NXOS__
+ .word _dbg__cmd_Step /* 's' */
+#else
+ /* NXT Firmware does not support Stepping */
+ .word _dbg__nop /* 's' */
+#endif
+ .word _dbg__cmd_Kill /* 'k' */
+ .word _dbg__cmd_RemoveBreakpoint /* 'z' */
+ .word _dbg__cmd_InsertBreakpoint /* 'Z' */
+ .word _dbg__cmd_Status /* '?' */
+ .word _dbg__cmd_Query /* 'q' */
+ .word _dbg__nop /* 'Q' */
+ .word 0
+
+/* Rm Shifted Shift Type Jump Table
+ * On entry:
+ * R0: Register Rm
+ * R1: Shift/Rotate Amount
+ * On exit:
+ * R0: RmShifted result
+ *
+ */
+debug_regShiftJumpTable:
+ .word _reg_lsl /* 00 */
+ .word _reg_lsr /* 01 */
+ .word _reg_asr /* 02 */
+ .word _reg_ror /* 03 */
+ .word _reg_rrx /* 04 */
+
+/* Data Processing Instruction Jump Table
+ * On entry:
+ * R0: Register Rn (Operand 1) value
+ * R1: Operand 2 value
+ * R2: Default Next Instruction Address
+ * R5[3:0]: CPSR condition codes
+ * On exit:
+ * R0: Calculated result
+ * R1, R2, R3: Destroyed
+ *
+ */
+debug_dataInstrJumpTable:
+ .word _opcode_and /* 00 */
+ .word _opcode_eor /* 01 */
+ .word _opcode_sub /* 02 */
+ .word _opcode_rsb /* 03 */
+ .word _opcode_add /* 04 */
+ .word _opcode_adc /* 05 */
+ .word _opcode_sbc /* 06 */
+ .word _opcode_rsc /* 07 */
+ .word _opcode_tst /* 08 */
+ .word _opcode_teq /* 09 */
+ .word _opcode_cmp /* 0A */
+ .word _opcode_cmn /* 0B */
+ .word _opcode_orr /* 0C */
+ .word _opcode_mov /* 0D */
+ .word _opcode_bic /* 0E */
+ .word _opcode_mvn /* 0F */
+
+
+/*
+ * To determine the next instruction to execute, we need to check current (breakpointed) instruction
+ * and determine whether it will be executed or not. This necessitates a mini instruction decoder
+ * that can check the type of instruction, as well as if it'll affect the PC.
+ * The instruction decoder used here is table based. Each entry in the table consists of:
+ * Instruction Identifier (IID), Instruction Bitmask (IBM), Instruction Handler Address (IHA)
+ * Null entries are placed at the end of the table.
+ *
+ * This allows for a flexible approach to handling instructions that we're interested in, at the expense
+ * of memory usage.
+ *
+ * For ARM, the IID & IBM are both 4 bytes, whereas the Thumb IID & IBM are 2 bytes.
+ * The IHA is always 4 bytes.
+ */
+
+/* ARM Instruction Decode Table
+ * .word IID, IBM, IHA (12 bytes)
+ */
+
+debug_armDecodeTable:
+ .word 0x0000f000, 0x0c00f000, _arm_data_instr_handler /* Data Processing instr with Rd = R15 */
+ .word 0x012fff10, 0x0ffffff0, _arm_bx_blx_handler /* BX or BLX. Note v4t does not have BLX instr */
+ .word 0x0410f000, 0x0410f000, _arm_ldr_pc_handler /* LDR with Rd = PC */
+/* .word 0x06000010, 0x0e000010, _arm_undef_handler */ /* Undefined instr: shouldn't occur, as it would've been trapped already. See _dbg_following_instruction_addr */
+ .word 0x08108000, 0x0e108000, _arm_ldm_pc_handler /* LDM {pc} */
+ .word 0x0a000000, 0x0e000000, _arm_b_bl_blx_handler /* B, BL or BLX. Note v4t does not have BLX instr */
+ .word 0x0c000000, 0x0c000000, _arm_coproc_swi_handler /* Coprocessor instr or SWI */
+ .word 0x0,0x0,0x0 /* Null Entry */
+
+/* Thumb Instruction Decode Table
+ * .hword IID, IBM
+ * .word IHA (8 bytes)
+ */
+
+debug_thumbDecodeTable:
+ .hword 0x4700, 0xff07
+ .word _thumb_bx_blx_handler /* BX or BLX. Note: b7 (H1) is not matched in the mask */
+ .hword 0xbd00, 0xff00
+ .word _thumb_poppc_handler /* PUSH/POP, specifically POP {Rlist,PC} */
+ .hword 0xd000, 0xf000
+ .word _thumb_bcond_swi_handler /* B<cond> or SWI */
+ .hword 0xe000, 0xf800
+ .word _thumb_b_handler /* B */
+ .hword 0xf000, 0xf000
+ .word _thumb_long_bl_blx_handler /* Long BL or BLX (4 bytes) Note: b11 (H) indicates 1st or 2nd instr */
+ .hword 0x0,0x0
+ .word 0x0 /* Null Entry */
+
+/* ARM Condition Code Mapping Table
+ * Converts Instruction encoding to SPSR Flags.
+ * b31 b30 b29 b28
+ * N Z C V
+ * Indexed according to Instruction Encoding order (pg 30, Table 6, ATMEL ARM7TDMI Data Sheet)
+ * Condition Code stored in MSN(set), LSN(clr) order
+ * Note1: 0x00 = AL. NV is deprecated, treat as AL
+ * Note2: 0xFF indicates that the condition checks needs to be handled separately (complex checks)
+ *
+ * EQ: Z set
+ * NE: Z clr
+ * HS/CS: C set
+ * LO/CC: C clr
+ * MI: N set
+ * PL: N clr
+ * VS: V set
+ * VC: V clr
+ * HI: C set AND Z clr
+ * LS: C clr AND Z set
+ */
+
+
+debug_armCondCodeTable:
+ /* EQ, NE, HS/CS, LO/CC, MI, PL, VS, VC, HI, LS, GE, LT, GT, LE, AL, NV */
+ .byte 0x40, 0x04, 0x20, 0x02, 0x80, 0x08, 0x10, 0x01, 0x24, 0x42, 0xFF, 0xFF, 0xFF, 0xFF, 0x00, 0x00
+
+/* ARM Complex Condition Code Mapping Table
+ * Converts Instruction encoding to SPSR Flags.
+ * b31 b30 b29 b28
+ * N Z C V
+ * Indexed according to Instruction Encoding order (pg 30, Table 6, ATMEL ARM7TDMI Data Sheet)
+ * for GE, LT, GT and LE instructions only
+ * Condition Code stored in the following order:
+ * b7 b6 b5 b4 b3 b2 b1 b0
+ * - - - ANDOR - Z set AND N==V (bit set = 1)
+ * - - - ANDOR - Z clr OR N!=V (bit clr = 0)
+ *
+ * GE: N == V
+ * LT: N != V
+ * GT: Z clr AND (N == V)
+ * LE: Z set OR (N != V)
+ */
+
+#define COMPLEX_CONDCODE_START 0x0A
+#define COMPLEX_CONDCODE_NEQV_MASK 0x01
+#define COMPLEX_CONDCODE_AND_MASK 0x02
+#define COMPLEX_CONDCODE_ZSET_MASK 0x04
+#define COMPLEX_CONDCODE_ANDOR_MASK 0x10
+
+#define COMPLEX_CONDCODE_NFLAG 0x08
+#define COMPLEX_CONDCODE_ZFLAG 0x04
+#define COMPLEX_CONDCODE_CFLAG 0x02
+#define COMPLEX_CONDCODE_VFLAG 0x01
+
+
+debug_armComplexCCTable:
+ /* GE, LT, GT, LE */
+ .byte 0x01, 0x00, 0x15, 0x12
+
+.code 32
+.text
+.align 4
+ .extern __breakpoints_num__
+ .extern dbg__getDebugMsg /* Read a message from the communications link */
+ .extern dbg__putDebugMsg /* Write a message to the communications link */
+ .extern dbg__sendAckOrNak /* Send Ack or Nak to indicate success/failure of message receipt */
+ .extern dbg__runloopTasks /* Platform specific Run Loop processing */
+
+
+/* The Debugger Interface can handle a total of (n-1) Breakpoint States and 1 Single Stepping State,
+ * where n is a power of 2. The value of n is given by __breakpoints_num__ defined in the linker file.
+ *
+ * In addition, a Debugger Stack contains the User Mode Register Stack Frame + SPSR + Bkpt Instr Addr.
+ * These are currently stored in the .stack area in RAM, so there is no fixed address
+ * location that is used for this purpose.
+ *
+ * The Breakpoint feature assumes that the program is executed in RAM. It is not possible
+ * to set dynamic breakpoints for programs executed from Flash in the AT91SAM7S which lacks
+ * instruction breakpointing support in hardware without using JTAG. The only type of breakpoints
+ * that can be supported in Flash based programs are Static (predefined) breakpoints inserted into
+ * the code.
+ *
+ * Each Breakpoint State i is a struct comprising the Breakpoint Address + Memory Contents
+ * stored in 8 bytes as:
+ * [High Memory Address]
+ * ADDR [i*8+4]: Memory Contents (32 bits)
+ * ADDR [i*8]: Breakpoint Address (31 bits, b0 = THUMB flag [not implemented yet])
+ * [Low Memory Address]
+ *
+ * A Non-zero Breakpoint Address means that the breakpoint is active, whereas the memory contents
+ * contains the instruction which resided at that address initially (now replaced by a BKPT <index>
+ * instruction).
+ * Note: Currently it is not possible to resume execution of a program with breakpoints enabled
+ * after a RESET, since the RESET will clear all contents of the stack, destroying the instruction
+ * contained in a given breakpoint.
+ * Fortunately the NXT will also need to reload the program into RAM so this is not expected to be
+ * an issue.
+ *
+ * The Memory Map for the Debugger State is as follows:
+ *
+ * [High Memory Address] __breakpoints_end__
+ * Breakpoint 07 State
+ * Breakpoint 06 State
+ * ...
+ * Breakpoint 02 State
+ * Breakpoint 01 State
+ * Single Step State __debugger_stack__ / __breakpoints_start__
+ * Previous Mode R15
+ * Previous Mode R14
+ * Previous Mode R13
+ * ...
+ * User Mode R02
+ * User Mode R01
+ * User Mode R00
+ * Previous Mode CPSR (UNDEF SPSR)
+ * UNDEF Next Instr Addr __debugger_stack_bottom__
+ * [Low Memory Address]
+ *
+ * Each Breakpoint State will initially be zeroed.
+ *
+ */
+ /* FIXME: The Debugger Stack Frame is probably not 100% consistent with the order that
+ GDB expects in the g/G messages. CSPR is probably located above R15 */
+
+/****************************************************************************
+ *
+ * GDB Debugger Init and Breakpoint Handler Routines
+ *
+ ****************************************************************************/
+ .code 32
+ .align 4
+ .global dbg__bkpt_init
+/* dbg__bkpt_init
+ * GDB set_debug_traps() routine
+ */
+dbg__bkpt_init:
+ push {lr}
+ bl _dbg__clear_breakpoints
+ mov r2, #0
+ ldr r1, =debug_curr_breakpoint
+ strb r2, [r1]
+ ldr r0, =debug_InMsgBuf
+ strb r2, [r0]
+ ldr r1, =debug_OutMsgBuf
+ strb r2, [r1]
+ bl dbg__comm_init /* Pass R0: Rx Buffer, R1: Tx Buffer to comm submodule */
+
+/* FIXME: Initialize other stuff here */
+ _dbg_setstate DBG_INIT
+ _dbg_setmode FALSE /* Debug Mode = False */
+ pop {lr}
+ bx lr /* Must return via BX; may have been called from Thumb mode (NXT Firmware) */
+
+
+/* _dbg__flush_icache
+ * Flush the Instruction cache
+ * Defined by GDB Stub, but not needed for ARMv4T architecture
+ */
+_dbg__flush_icache:
+ /* nop */
+ bx lr
+
+
+ .global dbg__thumb_bkpt_handler
+/* dbg__thumb_bkpt_handler
+ * GDB handle_exception() routine (Thumb Mode)
+ */
+dbg__thumb_bkpt_handler:
+/* On entry, r0 contains breakpoint index value */
+ /* mov r4, #BKPT16_AUTO_BKPT Redundant?! */
+ and r4, r0, #BKPT16_AUTO_BKPT /* keep AUTO flag value in r4 */
+ bic r0, r0, #BKPT16_AUTO_BKPT /* mask out AUTO flag */
+ _dbg_setcurrbkpt_index r0 /* keep current breakpoint index in memory */
+ ldr r1, =BKPT16_MANUAL_BKPT
+ teq r0, r1
+ beq _process_manual_breakpoint_thumb
+ ldr r1, =__breakpoints_num__
+ cmp r0, r1 /* Sanity check that index is in range */
+ bhs dbg__bkpt_offset_outofrange
+/* Valid index value found */
+ teq r4, #0 /* Check if AUTO flag set */
+ bne _process_auto_breakpoint
+/* else */
+ _dbg_set_bkpt_type DBG_NORMAL_BKPT_THUMB
+ b _process_normal_breakpoint
+
+ .global dbg__arm_bkpt_handler
+/* dbg__arm_bkpt_handler
+ * GDB handle_exception() routine (ARM Mode)
+ */
+dbg__arm_bkpt_handler:
+/* On entry, r0 contains breakpoint index value */
+ /* mov r4, #BKPT32_AUTO_BKPT Redundant?! */
+ and r4, r0, #BKPT32_AUTO_BKPT /* keep AUTO flag value in r4 */
+ bic r0, r0, #BKPT32_AUTO_BKPT /* mask out AUTO flag */
+ _dbg_setcurrbkpt_index r0 /* keep current breakpoint index in memory */
+ ldr r1, =BKPT32_MANUAL_BKPT
+ teq r0, r1
+ beq _process_manual_breakpoint_arm
+ ldr r1, =__breakpoints_num__
+ cmp r0, r1 /* Sanity check that index is in range */
+ bhs dbg__bkpt_offset_outofrange
+/* Valid index value found */
+ teq r4, #0 /* Check if AUTO flag set */
+ bne _process_auto_breakpoint
+/* else */
+ _dbg_set_bkpt_type DBG_NORMAL_BKPT_ARM
+/* b _process_normal_breakpoint */
+
+_process_normal_breakpoint:
+ bl _dbg__restore_breakpoints /* includes restoring single step */
+/* bl _dbg__restore_singlestep */
+ bl _dbg__clear_singlestep
+ bl _dbg__flush_icache
+ b dbg__bkpt_waitCMD
+
+_process_auto_breakpoint:
+/* Load Auto BKPT for Breakpoint index given in r0 */
+ _index2bkptindex_addr r0, r1 /* Calculate Breakpoint Entry Address */
+ ldm r1, {r1, r2} /* r1: Breakpoint Address, r2: Breakpoint Instruction */
+ teq r1, #0 /* Check that Breakpoint is active */
+ beq dbg__bkpt_inactive
+ bl _dbg__activate_one_breakpoint
+ bl _dbg__restore_singlestep
+ bl _dbg__clear_singlestep
+ _dbg_set_bkpt_type DBG_AUTO_BKPT
+ b _dbg__switch2undefmode
+
+/* _dbg__switch2undefmode
+ * Common internal routine to return execution to user program
+ */
+_dbg__switch2undefmode:
+ bl _dbg__flush_icache
+ msr cpsr_c, #(MODE_UND | CPSR_FIQ | CPSR_IRQ) /* Configure Undef Mode */
+ _dbg_setmode FALSE /* Debug Mode = False */
+ ldr lr, =resume_execution
+ mov pc, lr /* Exit via UNDEF mode */
+
+_process_manual_breakpoint_thumb:
+ _dbg_set_bkpt_type DBG_MANUAL_BKPT_THUMB
+ b dbg__bkpt_waitCMD
+
+_process_manual_breakpoint_arm:
+ _dbg_set_bkpt_type DBG_MANUAL_BKPT_ARM
+/* b dbg__bkpt_waitCMD */
+
+dbg__bkpt_inactive:
+/* b dbg__bkpt_waitCMD */
+
+dbg__bkpt_offset_outofrange:
+/* b dbg__bkpt_waitCMD */
+
+ .global dbg__bkpt_waitCMD
+/* dbg__bkpt_waitCMD
+ * GDB Stub Remote Command Handler
+ */
+
+/****************************************************************************
+ *
+ * GDB Server Command Processing Routines
+ *
+ ****************************************************************************/
+dbg__bkpt_waitCMD:
+ /* We enter this code section when a Breakpoint Triggers */
+ _dbg_setmode TRUE /* Debug Mode = True */
+ msr cpsr_c, #(MODE_ABT) /* Re-enable Interrupts */
+
+ _dbg_getstate r0
+ cmp r0, #DBG_CONFIGURED
+ blo dbg__bkpt_waitCMD_cont /* Not configured yet, don't send Breakpoint Signal Response */
+ bl _dbg__cmd_Status /* Send signal response to the GDB server */
+
+dbg__bkpt_waitCMD_cont:
+ bl dbg__runloopTasks /* Execute housekeeping tasks while in ABRT mode */
+ bl dbg__getDebugMsg /* Read new message from Debugger, buflen in R0, 0 if none, -1 if error, msgbuf pointer in R1 */
+ cmp r0, #0
+ beq dbg__bkpt_waitCMD_cont /* No message yet, do housekeeping tasks */
+ bgt _proc_command /* valid message, process it */
+ bl __dbg__procChecksumError /* Message invalid, checksum error? */
+ b dbg__bkpt_waitCMD_cont
+
+_proc_command:
+/* Message now has Ctrl-C or $<packet info>\0 */
+ mov r4, r1 /* Use R4 as buffer pointer */
+ ldrb r0, [r4], #1 /* Look for Ctrl-C or '$' */
+ teq r0, #MSGBUF_CTRLC
+ bne _dbg_check_gdb_command
+ /* Ctrl-C detected, do nothing (wait for next command from GDB) */
+#if 0
+ /* On entering the Debugger via Ctrl-C, _dbg__cmd_Status has already sent a reply, so just keep quiet */
+ bl __dbg__procAckOnly /* send Ack */
+#endif
+ b dbg__bkpt_waitCMD_cont
+
+_dbg_check_gdb_command:
+ teq r0, #MSGBUF_STARTCHAR
+ movne r0, #MSG_ERRFORMAT /* Message Format invalid (not '$') */
+ bne _dbg__cmdError /* Shouldn't happen */
+ ldrb r0, [r4], #1 /* Look for command char */
+ bl _dbg__cmdChar2Index /* Index in R0 */
+ mov r1, #CMDINDEX_OUTOFRANGE
+ teq r0, r1
+ bne _dbg__cmdExists /* Found valid command, execute it */
+_dbg_unknown_command:
+ bl _dbg__nop /* Command character not recognized, send empty response to GDB server */
+ b dbg__bkpt_waitCMD_cont
+
+#if 0
+ moveq r0, #MSG_UNKNOWNCMD /* Out of range, Command character not recognized */
+ beq _dbg__cmdError /* Send response to GDB server */
+#endif
+
+_dbg__cmdExists:
+ mov r3, r0 /* put Command Handler Index in R3 */
+ mov r0, r4 /* R0 now contains Input Message Buffer Parameter Pointer (previously in R4) */
+ _dbg_jumpTableHandler debug_cmdJumpTable, r2, r3 /* Call Command Handler Routine, use R2 as jump address pointer */
+ b dbg__bkpt_waitCMD_cont
+
+_dbg__cmdError:
+ _dbg_outputMsgStatusErr
+ bl dbg__putDebugMsg /* Send error response to the GDB server */
+ b dbg__bkpt_waitCMD_cont
+
+
+/* __dbg__procOkOnly
+ * Send OK to GDB due to Detach
+ * On entry:
+ * None
+ * On exit:
+ * r0-r3: destroyed
+ */
+__dbg__procOkOnly:
+ stmfd sp!, {lr}
+ _dbg_outputMsgStatusOk /* Acknowledge Detach command from GDB server */
+_cont_procOkOnly:
+ bl dbg__putDebugMsg /* Send OK response to the GDB server */
+ cmp r0, #0
+ beq _cont_procOkOnly_exit /* Sending of OK succeeded */
+ bl dbg__runloopTasks /* Service run loop tasks */
+ b _cont_procOkOnly /* Retry OK transmission */
+_cont_procOkOnly_exit:
+ ldmfd sp!, {pc}
+
+
+/* __dbg__procAckOnly
+ * Send Ack to GDB due to Continue or Step
+ * On entry:
+ * None
+ * On exit:
+ * r0-r3: destroyed
+ */
+__dbg__procAckOnly:
+ stmfd sp!, {lr}
+ _dbg_outputAckOnlyFlag
+ b _cont_sendAckOrNak /* Acknowledge Continue or Step command from GDB server */
+
+/* __dbg__procChecksumError
+ * Request Retransmission from GDB due to Checksum error
+ * On entry:
+ * None
+ * On exit:
+ * r0-r3: destroyed
+ */
+__dbg__procChecksumError:
+ stmfd sp!, {lr}
+ _dbg_outputRetransmitFlag
+ /* b _cont_sendAckOrNak */ /* Acknowledge Continue or Step command from GDB server */
+
+_cont_sendAckOrNak:
+ bl dbg__sendAckOrNak /* send Ack or Nak to GDB server */
+ cmp r0, #0
+ beq _cont_sendAckOrNak_exit /* Sending of Ack or Nak succeeded */
+ bl dbg__runloopTasks /* Service run loop tasks */
+ b _cont_sendAckOrNak /* Retry Ack or Nak transmission */
+_cont_sendAckOrNak_exit:
+ ldmfd sp!, {pc}
+
+/* _dbg__cmdChar2Index
+ * Convert Command Character to Jump Table Index
+ * On entry:
+ * r0: command character
+ * On exit:
+ * r0: jump table index (-1 for command not found)
+ * R1: destroyed
+ * R2: destroyed
+ * R3: destroyed
+ */
+_dbg__cmdChar2Index:
+ mov r1, r0 /* Copy command character to r1 */
+ mov r0, #0 /* Clear return value */
+ ldr r3, =debug_cmdIndexTable /* Convert command to index using r3 as Index Lookup Address Pointer */
+1: ldrb r2, [r3, r0] /* Get table entry */
+ teq r2, #0
+ moveq r0, #CMDINDEX_OUTOFRANGE /* End of Index Table, Not found */
+ beq _exit_cmdIndexTable
+ teq r1, r2
+ addne r0, #1 /* Increment Index */
+ bne 1b /* No match, skip to next command char */
+_exit_cmdIndexTable:
+ bx lr
+
+/* __dbg__cmdParamLen
+ * Determines the length of the parameter buffer for a given command
+ * On entry:
+ * R0: parameter buffer pointer (contents after '$' and '<cmdchar>')
+ * On exit:
+ * R0: Address of parameter buffer (preserved)
+ * R1: length
+ */
+__dbg__cmdParamLen:
+ stmfd sp!, {r0,r2,lr} /* R2: scratch register */
+ mov r1, #0
+1: ldrb r2, [r0], #1
+ teq r2, #0
+ addne r1, r1, #1
+ bne 1b
+ ldmfd sp!, {r0,r2,pc}
+
+/* __dbg__procCmdOk
+ * Common subroutine exit stub to return Command Ok Status for Command Handlers
+ * DO NOT CALL THIS STUB DIRECTLY! It Assumes that the return address is in the stack.
+ *
+ */
+__dbg__procCmdOk:
+ _dbg_outputMsgStatusOk
+ b __dbg__sendDebugMsgExit
+
+/* __dbg__procUnimplementedError
+ * Common subroutine exit stub to handle Unimplemented GDB Command Error
+ * DO NOT CALL THIS STUB DIRECTLY! It Assumes that the return address is in the stack.
+ * Note: GDB Remote Protocol specifies returning blank message ('+$')
+ *
+ */
+__dbg__procUnimplementedError:
+ _dbg_outputMsgValidResponse
+ b __dbg__sendDebugMsgExit
+
+/* __dbg__procCmdParamError
+ * Common subroutine exit stub to handle Command Parameter Error for Command Handlers
+ * DO NOT CALL THIS STUB DIRECTLY! It Assumes that the return address is in the stack.
+ *
+ */
+__dbg__procCmdParamError:
+ mov r1, #MSG_UNKNOWNPARAM
+ b __dbg__procErrorMsg
+
+/* __dbg__procCmdReturnInputLengthError
+ * Common subroutine exit stub to handle Command Input Length Error for Command Handlers
+ * DO NOT CALL THIS STUB DIRECTLY! It Assumes that the return address is in the stack.
+ *
+ */
+__dbg__procCmdReturnInputLengthError:
+ mov r1, #MSG_ERRINLENGTH
+ b __dbg__procErrorMsg
+
+/* __dbg__procCmdReturnOutputLengthError
+ * Common subroutine exit stub to handle Command Return Output Error for Command Handlers
+ * DO NOT CALL THIS STUB DIRECTLY! It Assumes that the return address is in the stack.
+ *
+ */
+__dbg__procCmdReturnOutputLengthError:
+ mov r1, #MSG_ERROUTLENGTH
+ b __dbg__procErrorMsg
+
+/* __dbg__procBreakpointAddrError
+ * Common subroutine exit stub to handle Breakpoint Address Error for Breakpoint Insert/Remove Handlers
+ * DO NOT CALL THIS STUB DIRECTLY! It Assumes that the return address is in the stack.
+ *
+ */
+__dbg__procBreakpointAddrError:
+ mov r1, #MSG_UNKNOWNBRKPT
+ b __dbg__procErrorMsg
+
+
+__dbg__procErrorMsg:
+ __dbg_outputErrMsg
+ /* b __dbg__sendDebugMsgExit */
+
+__dbg__sendDebugMsgExit:
+ bl dbg__putDebugMsg /* Send error response to the GDB server */
+ ldmfd sp!, {pc}
+
+/* _dbg__cmd_Status
+ * Status Command Handler
+ * On entry:
+ * r0: parameter buffer (contents after '$' and '<cmdchar>')
+ * On exit:
+ * r0, r1, r2, r3: destroyed
+ */
+_dbg__cmd_Status:
+ stmfd sp!, {lr}
+ _dbg_outputMsgStatusSig 0x0 /* FIXME: Dummy Signal number */
+ b __dbg__sendDebugMsgExit
+
+/* _dbg__cmd_Query
+ * Query Command Handler
+ * On entry:
+ * r0: parameter buffer (contents after '$' and '<cmdchar>')
+ * [varied, see GDB General Packets query docs]
+ * http://sourceware.org/gdb/current/onlinedocs/gdb/General-Query-Packets.html
+ * On exit:
+ * r0, r1, r2, r3: destroyed
+ */
+_dbg__cmd_Query:
+ stmfd sp!, {lr}
+ _dbg_setstate DBG_CONFIGURED /* We have exchanged query messages with the GDB server */
+ b __dbg__procUnimplementedError /* FIXME: return an empty message to GDB (no modifiable settings) */
+
+
+/* _dbg__cmd_GetOneReg
+ * Get One Register Value Command Handler
+ * Valid command parameter x is from '0' to 'F' for User Mode Registers R0-R15,
+ * '18' for FPSCR (dummy), and '19' for CPSR
+ * On entry:
+ * r0: parameter buffer pointer (contents after '$' and '<cmdchar>')
+ * x
+ * On exit:
+ * r0, r1, r2, r3: destroyed
+ *
+ */
+_dbg__cmd_GetOneReg:
+ stmfd sp!, {lr}
+ bl __dbg__cmdParamLen
+ cmp r1, #CMD_REG_GETONE_MINPARAMLEN /* Check for correct length */
+ blo __dbg__procCmdParamError /* Unexpected input, report error */
+ cmp r1, #CMD_REG_GETONE_MAXPARAMLEN /* Check for correct length */
+ bhi __dbg__procCmdParamError /* Unexpected input, report error */
+ bl ascii2hex_varlen_be /* convert ASCII reg enum to Hex (in R0), R1 has address of next buffer char */
+
+_dbg__proc_getRegister:
+ mov r3, r0 /* Keep register index safe */
+ _dbg_outputMsgValidResponse /* R0: address of output message buffer data pointer (after response prefix) */
+ mov r1, r3 /* Move register index value to R1 */
+ bl _dbg_outputOneRegValue /* update output buffer */
+ _asciiz r0, r1
+ bl dbg__putDebugMsg /* Send response to the GDB server */
+ ldmfd sp!, {pc}
+
+/* _dbg_outputOneRegValue
+ * Given Register Enum (0-F: R0-R15, 0x19: CPSR, OtherVal: dummy), output hex char to buffer
+ * On entry:
+ * r0: output message buffer pointer
+ * r1: register enum (0-F, 0x19)
+ * On exit:
+ * r0: updated (points to next character slot at end of Output Buffer)
+ * r1: original output message buffer pointer
+ * r2: destroyed
+ */
+_dbg_outputOneRegValue:
+ stmfd sp!, {lr}
+ cmp r1, #REG_PC
+ addls r2, r1, #DBGSTACK_USERREG_INDEX /* Convert register enum to Debug Stack index */
+ bls _retrieve_RegVal
+
+ cmp r1, #REG_CPSR
+ moveq r2, #DBGSTACK_USERCPSR_INDEX /* convert register enum to Debug Stack index */
+ beq _retrieve_RegVal
+
+ cmp r1, #REG_FPSCR
+ bne _exit_dbg_outputOneRegValue /* No match, skip */
+
+_output_dummy_fpscr:
+ /* Output Dummy FPSCR value */
+ mov r1, #0
+ b _output2buffer /* Output all zeros for FPSCR */
+
+_retrieve_RegVal:
+ _getdbgregisterfromindex r2, r1 /* Retrieve Register contents into R1 */
+
+_output2buffer:
+
+#ifdef __BIG_ENDIAN__
+ bl word2ascii_be /* Convert and put hex chars into Output Message Buffer */
+#else
+ bl word2ascii_le /* Convert and put hex chars into Output Message Buffer */
+#endif
+
+_exit_dbg_outputOneRegValue:
+ ldmfd sp!, {pc}
+
+/* _dbg__cmd_GetAllRegs
+ * Get All Register Values Command Handler
+ * Output Buffer returns register values in the order: User R0, R1, R2, ..., R15, CPSR
+ * On entry:
+ * r0: parameter buffer pointer (contents after '$' and '<cmdchar>')
+ * <NULL> (no parameters)
+ * On exit:
+ * r0, r1, r2, r3: destroyed
+ */
+_dbg__cmd_GetAllRegs:
+ stmfd sp!, {lr}
+ bl __dbg__cmdParamLen
+ teq r1, #CMD_REG_GETALL_PARAMLEN /* Check for correct length */
+ bne __dbg__procCmdParamError /* Unexpected input, report error */
+
+ _dbg_outputMsgValidResponse /* R0: address of output message buffer data pointer (after response prefix) */
+
+ /* We must return R0-R15, then CPSR */
+ mov r3, #REG_R0 /* Output User Register Values first */
+1: mov r1, r3
+ bl _dbg_outputOneRegValue /* update output buffer */
+ add r3, r3, #1 /* increment index */
+ cmp r3, #REG_PC
+ bls 1b /* process all the registers */
+_get_cpsr:
+ mov r1, #REG_CPSR /* Output User CPSR Value last */
+ bl _dbg_outputOneRegValue /* update output buffer */
+
+ _asciiz r0, r1
+ bl dbg__putDebugMsg /* Send response to the GDB server */
+ ldmfd sp!, {pc}
+
+/* _dbg_setOneRegValue
+ * Given Register Enum (0-F: R0-R15, 0x19: CPSR, OtherVal: dummy), output hex char to buffer
+ * On entry:
+ * r0: parameter buffer pointer
+ * r1: register enum (0-F, 0x19)
+ * On exit:
+ * r0: (Updated) Address of parameter in buffer
+ * r1, r2, r3: destroyed
+ */
+_dbg_setOneRegValue:
+ stmfd sp!, {lr}
+ cmp r1, #REG_PC
+ addls r2, r1, #DBGSTACK_USERREG_INDEX /* Convert register enum to Debug Stack index */
+ bls _store_RegVal
+
+ cmp r1, #REG_CPSR
+ moveq r2, #DBGSTACK_USERCPSR_INDEX /* convert register enum to Debug Stack index */
+ beq _store_RegVal
+
+ cmp r1, #REG_FPSCR
+ bne _exit_dbg_setOneRegValue /* No match, skip */
+ /* FIXME: If we get SetAllRegs with FP reg values, this will not skip pass the FP regs! */
+
+_set_dummy_fpscr:
+ /* Set dummy FPSCR value (ignored) */
+ add r1, r0, #CMD_REG_REGPARAMLEN /* Just increment the pointer */
+ b _done_store_RegVal
+
+_store_RegVal:
+#ifdef __BIG_ENDIAN__
+ bl ascii2word_be
+#else
+ bl ascii2word_le
+#endif
+ /* R0: value, R1: pointer to next char in buffer */
+ _setdbgregisterfromindex r2, r0, r3 /* Set Register contents in R0, using index in R2, and scratch register R3 */
+_done_store_RegVal:
+ mov r0, r1 /* Copy buffer pointer to next parameter to R0 for return value */
+_exit_dbg_setOneRegValue:
+ ldmfd sp!, {pc}
+
+
+/* _dbg__cmd_SetOneReg
+ * Set One Register Value Command Handler
+ * Valid command parameter x is from '0' to 'F' for User Mode Registers R0-R15,
+ * '18' for FPSCR (dummy), and '19' for CPSR
+ * On entry:
+ * r0: parameter buffer pointer (contents after '$' and '<cmdchar>')
+ * x=rrrr
+ * On exit:
+ * r0, r1, r2, r3, r4: destroyed
+ *
+ */
+_dbg__cmd_SetOneReg:
+ stmfd sp!, {lr}
+ bl __dbg__cmdParamLen
+ cmp r1, #CMD_REG_SETONE_MINPARAMLEN /* Check for correct length */
+ blo __dbg__procCmdParamError /* Unexpected input, report error */
+ cmp r1, #CMD_REG_SETONE_MAXPARAMLEN /* Check for correct length */
+ bhi __dbg__procCmdParamError /* Unexpected input, report error */
+
+ bl ascii2hex_varlen_be /* convert ASCII reg enum to Hex (in R0), R1 has address of next buffer char */
+ mov r4, r0 /* Keep enum in R4 */
+ _check_msgassignment r1 /* R1 points to next char in buffer */
+ bne __dbg__procCmdParamError /* Can't find '=' */
+
+_dbg__proc_setRegister:
+ mov r0, r1 /* move parameter buffer pointer to R0 */
+ mov r1, r4 /* and retrieve enum to R1 to call _dbg_setOneRegValue */
+ bl _dbg_setOneRegValue
+ b __dbg__procCmdOk
+
+/* _dbg__cmd_SetAllReg
+ * Set All Register Values Command Handler
+ * On entry:
+ * r0: parameter buffer pointer (contents after '$' and '<cmdchar>')
+ * rrrrRRRRrrrr... (17 registers)
+ * On exit:
+ * r0, r1, r2, r3, r4: destroyed
+ *
+ */
+_dbg__cmd_SetAllRegs:
+ /* Assumes that the registers are in the sequence R0, R1, ... R15, CPSR */
+ stmfd sp!, {lr}
+ bl __dbg__cmdParamLen /* R0: pointer to parameters in buffer */
+ teq r1, #CMD_REG_SETALL_PARAMLEN /* Check for correct length */
+ bne __dbg__procCmdParamError /* Unexpected input, report error */
+ mov r4, #REG_R0 /* R4: register enum, starting with enum for R0 */
+1: mov r1, r4 /* Copy enum to R1; R0 already points to current parameter */
+ bl _dbg_setOneRegValue /* R0: next parameter address pointer */
+ add r4, r4, #1 /* increment index */
+ cmp r4, #REG_PC
+ bls 1b
+
+_set_cpsr:
+#if 0
+ /* GDB was not happy when this was added */
+ /* Read dummy FPSCR value (ignored) */
+#ifdef __BIG_ENDIAN__
+ bl ascii2word_be
+#else
+ bl ascii2word_le
+#endif
+ mov r0, r1 /* Copy buffer pointer to next parameter to R0 for return value */
+#endif
+
+ mov r1, #REG_CPSR /* Use CPSR enum */
+ bl _dbg_setOneRegValue /* R0: next parameter address pointer */
+ ldrb r0, [r0]
+ teq r0, #0 /* Look for ASCIIZ character to terminate loop */
+ beq __dbg__procCmdOk
+ bne __dbg__procCmdParamError /* Unexpected input, report error */
+
+/* _dbg__nop
+ * NOP Command Handler (placeholder)
+ * On entry:
+ * r0: parameter buffer (contents after '$' and '<cmdchar>')
+ * On exit:
+ * r0, r1, r2, r3: destroyed
+ */
+_dbg__nop:
+ stmfd sp!, {lr}
+ b __dbg__procUnimplementedError
+
+/* _dbg__cmd_ReadMem
+ * Read Memory Contents Command Handler
+ * Output Buffer returns memory contents
+ * On entry:
+ * r0: parameter buffer pointer (contents after '$' and '<cmdchar>')
+ * AA..AA,LLLL
+ * On exit:
+ * r0, r1, r2, r3, r4: destroyed
+ */
+_dbg__cmd_ReadMem:
+ stmfd sp!, {lr}
+ bl __dbg__cmdParamLen
+#if 0
+ teq r1, #CMD_MEM_READ_PARAMLEN /* Check for correct length */
+ bne __dbg__procCmdParamError /* Unexpected input, report error */
+ bl ascii2word_be /* convert ASCII address location to Hex (in R0), R1 has address of next buffer char */
+#endif
+ bl ascii2hex_varlen_be /* convert ASCII address to Hex (in R0), R1 has address of next buffer char */
+ mov r3, r0 /* Keep Address location in R3 */
+ _check_msgseparator r1
+ bne __dbg__procCmdParamError /* Can't find ',' */
+ mov r0, r1 /* move buffer pointer to R0 for subsequent processing */
+ bl ascii2hex_varlen_be /* convert ASCII length to Hex (in R0), R1 has address of next buffer char */
+ cmp r0, #CMD_MEM_MAXREADBYTES /* Don't overflow our buffer (2 x CMD_MEM_MAXREADBYTES) */
+ bhi __dbg__procCmdReturnOutputLengthError /* Requested Length greater than buffer size, return error */
+ mov r4, r0 /* Keep numbytes in R4 */
+ /* FIXME: Should validate the address? */
+
+ _dbg_outputMsgValidResponse /* R0: address of output message buffer data pointer (after response prefix) */
+1: ldrb r1, [r3], #1
+ bl byte2ascii /* update output buffer */
+ subs r4, r4, #1
+ bne 1b
+
+ _asciiz r0, r1
+ bl dbg__putDebugMsg /* Send response to the GDB server */
+ ldmfd sp!, {pc}
+
+
+/* _dbg__cmd_WriteMem
+ * Write Memory Contents Command Handler
+ * On entry:
+ * r0: parameter buffer pointer (contents after '$' and '<cmdchar>')
+ * AA..AA,LLLL::bb..bb
+ * On exit:
+ * r0, r1, r2, r3, r4: destroyed
+ */
+_dbg__cmd_WriteMem:
+ stmfd sp!, {lr}
+ bl __dbg__cmdParamLen
+#if 0
+ cmp r1, #CMD_MEM_WRITE_MINPARAMLEN /* Check for correct (minimum) length */
+ blo __dbg__procCmdParamError /* Unexpected input, report error */
+ sub r4, r1, #CMD_MEM_WRITE_MINPARAMLEN /* R4: Number of ASCII Hex chars for byte writes */
+ bl ascii2word_be /* convert ASCII address location to Hex (in R0), R1 has address of next buffer char */
+#endif
+ bl ascii2hex_varlen_be /* convert ASCII address to Hex (in R0), R1 has address of next buffer char */
+ mov r3, r0 /* Keep Address location in R3 */
+ _check_msgseparator r1
+ bne __dbg__procCmdParamError /* Can't find ',' */
+ mov r0, r1 /* move buffer pointer to R0 for subsequent processing */
+ bl ascii2hex_varlen_be /* convert ASCII length to Hex (in R0), R1 has address of next buffer char */
+ cmp r0, r4, asr #1 /* is Number of bytes to write == (number of ASCII Hex Chars / 2)? */
+ bne __dbg__procCmdParamError /* Number of bytes does not match argument length */
+ cmp r0, #CMD_MEM_MAXWRITEBYTES /* Don't overflow our buffer (2 x CMD_MEM_MAXWRITEBYTES) */
+ bhi __dbg__procCmdReturnInputLengthError /* Requested Length greater than buffer size, return error */
+ mov r4, r0 /* Keep numbytes in R4 */
+ /* FIXME: Should validate the address? */
+ _check_msgargument r1
+ bne __dbg__procCmdParamError /* Can't find ':' */
+
+1: mov r0, r1
+ bl ascii2byte /* convert ASCII Hex value into byte value */
+ strb r0, [r3], #1 /* Store into memory location */
+ subs r4, r4, #1
+ bne 1b
+ b __dbg__procCmdOk
+
+/* _dbg__cmd_Detach
+ * Detach User Program Execution Command Handler
+ * Treat this as being equivalent to 'Continue' without any arguments
+ *
+ * On entry:
+ * r0: parameter buffer pointer (contents after '$' and '<cmdchar>')
+ * <NULL> (No Parameters)
+ * On exit:
+ * r0-r7: destroyed
+ * Note: This routine does not return to caller. Instead it switches
+ * operating mode to UNDEF and returns to previously active program
+ */
+_dbg__cmd_Detach:
+ stmfd sp!, {lr} /* In case unexpected parameters were received */
+ bl __dbg__cmdParamLen
+ teq r1, #CMD_DETACH_PARAMLEN /* Check for correct length */
+ bne __dbg__procCmdParamError /* Unexpected input, report error */
+
+ ldmfd sp!, {lr} /* Cleanup stack, since we won't return to the Debugger Run Loop */
+ _dbg_setstate DBG_INIT /* Reset the Debug State */
+ bl __dbg__procOkOnly /* send OK to keep GDB server happy */
+ b _dbg__cont_check_breakpoint_type /* Continue from current PC */
+
+/* _dbg__cmd_Continue
+ * Continue User Program Execution Command Handler
+ * Setup breakpoints before resuming execution of program.
+ *
+ * If Address is specified, update the next instruction address to specified address
+ *
+ * If this is a Normal Breakpoint, then we need to install an Autobreakpoint at next instruction address
+ * and resume from current (Breakpoint) exception address
+ * Else (it is a Manual Breakpoint or Address Specified)
+ * We need to resume from the next instruction address
+ * On entry:
+ * r0: parameter buffer pointer (contents after '$' and '<cmdchar>')
+ * Optional: AA..AA
+ * On exit:
+ * r0-r7: destroyed
+ * Note: This routine does not return to caller. Instead it switches
+ * operating mode to UNDEF and returns to previously active program
+ */
+_dbg__cmd_Continue:
+ /* Don't put anything on the stack, we won't return to the Debugger Run Loop */
+ bl __dbg__cmdParamLen
+ cmp r1, #CMD_CONTINUE_MINPARAMLEN /* Check for correct parameter length */
+ bne _dbg__cont_fromAddr /* Address is specified */
+ bl __dbg__procAckOnly /* send Ack to keep GDB server happy */
+ b _dbg__cont_check_breakpoint_type /* Continue from current PC */
+
+_dbg__cont_fromAddr:
+ bl ascii2hex_varlen_be /* convert ASCII address to Hex (in R0), R1 has address of next buffer char */
+ /* Continue from Specified Address */
+ mov r2, #DBGSTACK_NEXTINSTR_INDEX /* The Next Instruction Pointer for Resume is in index 0 of the Debug Stack */
+ _setdbgregisterfromindex r2, r0, r1 /* Set Register contents in R0, using index in R2, and scratch register R1 */
+ bl __dbg__procAckOnly /* send Ack to keep GDB server happy */
+ b _dbg__cont_is_manual_bkpt_or_address_specified
+
+_dbg__cont_check_breakpoint_type:
+ _dbg_get_bkpt_type r0
+ teq r0, #DBG_MANUAL_BKPT_ARM
+ beq _dbg__cont_is_manual_bkpt_or_address_specified
+ teq r0, #DBG_MANUAL_BKPT_THUMB
+ beq _dbg__cont_is_manual_bkpt_or_address_specified
+
+_dbg__cont_is_normal_breakpoint:
+/* FIXME: _dbg_following_instruction_addr doesn't actually work currently.
+ This breaks normal breakpoints which need to determine the next instruction to execute
+ (for placing the autobreakpoint) prior to returning.
+*/
+ mov r2, #DBGSTACK_USERPC_INDEX /* Retrieve Aborted Instruction PC from the Debug Stack */
+ _getdbgregisterfromindex r2, r0 /* Retrieve Register contents into R0 */
+ mov r2, #DBGSTACK_NEXTINSTR_INDEX /* The Next Instruction Pointer for Resume is in index 0 of the Debug Stack */
+ _setdbgregisterfromindex r2, r0, r1 /* Set Register contents in R0, using index in R2, and scratch register R1 */
+ bl _dbg_following_instruction_addr /* following instruction address returned in r1 */
+ bl dbg__install_singlestep /* Setup Single Step, next instruction address returned in r1 */
+ _dbg_getcurrbkpt_index r0 /* load current breakpoint index in memory */
+ bl _dbg__activate_autobreakpoint /* pass next instruction address in r1 */
+ b _dbg__switch2undefmode
+
+_dbg__cont_is_manual_bkpt_or_address_specified:
+ bl _dbg__activate_breakpoints /* Restore exisiting breakpoints */
+ b _dbg__switch2undefmode
+
+/* _dbg__cmd_Step
+ * Step User Program Execution Command Handler
+ * Setup breakpoints before resuming execution of program.
+ *
+ * If Address is specified, update the next instruction address to specified address
+ *
+ * If this is a Normal Breakpoint, then we need to install a Step Breakpoint at next instruction address
+ * and resume from current (Breakpoint) exception address
+ * Else (it is a Manual Breakpoint or Address specified)
+ * We need to install a Step Breakpoint at the following instruction address (after the next instruction address)
+ * and resume from the next instruction address
+ * On entry:
+ * r0: parameter buffer pointer (contents after '$' and '<cmdchar>')
+ * Optional: AA..AA
+ * On exit:
+ * r0-r7: destroyed
+ * Note: This routine does not return to caller. Instead it switches
+ * operating mode to UNDEF and returns to previously active program
+ */
+_dbg__cmd_Step: