aboutsummaryrefslogtreecommitdiff
path: root/AT91SAM7S256/armdebug
diff options
context:
space:
mode:
authorTat-Chee Wan (USM)2011-04-04 17:32:42 +0800
committerTat-Chee Wan (USM)2011-04-04 17:32:42 +0800
commit41528b49b9c8e0c20663f3f7becc6276eeff94b0 (patch)
tree9fb75ff2e4fbaf70dd406ef347687e3f0d37b85e /AT91SAM7S256/armdebug
parenta49b11a219f6876e63844c2b89f45122f64ab702 (diff)
parent373c665e5015040f402771a106e802c909c81d17 (diff)
Merge branch 'master' of ssh://svc.cs.usm.my/~/gitrepo-bare/armdebug
Diffstat (limited to 'AT91SAM7S256/armdebug')
-rw-r--r--AT91SAM7S256/armdebug/Debugger/debug_internals.h32
-rw-r--r--AT91SAM7S256/armdebug/Debugger/debug_macros.h27
-rw-r--r--AT91SAM7S256/armdebug/Debugger/debug_stub.S814
3 files changed, 779 insertions, 94 deletions
diff --git a/AT91SAM7S256/armdebug/Debugger/debug_internals.h b/AT91SAM7S256/armdebug/Debugger/debug_internals.h
index a5121d7..daab9b4 100644
--- a/AT91SAM7S256/armdebug/Debugger/debug_internals.h
+++ b/AT91SAM7S256/armdebug/Debugger/debug_internals.h
@@ -155,6 +155,26 @@
#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.
@@ -187,6 +207,7 @@
#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 */
@@ -197,6 +218,15 @@
#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
@@ -216,7 +246,7 @@ ENUM_END(dbg_state_t)
* The enums must be consecutive, starting from 0
*/
ENUM_BEGIN
-ENUM_VALASSIGN(DBG_AUTO_BKPT,0) /**< Auto Thumb Breakpoint (Instruction resume after breakpoint). */
+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. */
diff --git a/AT91SAM7S256/armdebug/Debugger/debug_macros.h b/AT91SAM7S256/armdebug/Debugger/debug_macros.h
index e71d9ca..75d6299 100644
--- a/AT91SAM7S256/armdebug/Debugger/debug_macros.h
+++ b/AT91SAM7S256/armdebug/Debugger/debug_macros.h
@@ -52,7 +52,8 @@
add \instrmask, \instrmask, \indexreg, lsl #3
ldm \instrmask, {\instrreg, \codehandler} /* LSHW: IID, MSHW: IBM */
mov \instrmask, \instrreg, lsr #16
- and \instrreg, \instrreg, #HLFWRD0
+ mov \instrreg, \instrreg, lsl #16
+ mov \instrreg, \instrreg, lsr #16 /* Keep HLFWORD0 containing instruction */
.endm
/* _dbg_armDecodeEntry
@@ -247,8 +248,7 @@
* indexreg contains debugger stack index value (0-max index)
*/
.macro _regenum2index indexenum, indexreg
- mov \indexreg, #indexenum
- add \indexreg, \indexreg, #DBGSTACK_USERREG_INDEX /* Convert register index to Debug Stack index, keep in R1 */
+ add \indexreg, \indexenum, #DBGSTACK_USERREG_INDEX /* Convert register index to Debug Stack index */
.endm
/* _getdbgregisterfromindex
@@ -381,27 +381,6 @@
strb \reg, [r1]
.endm
-/* _dbg_getabortedinstr_addr
- * Get aborted instruction address
- * On exit:
- * reg: aborted instruction address
- */
- .macro _dbg_getabortedinstr_addr reg
- ldr \reg, =__debugger_stack_bottom__
- ldr \reg, [\reg]
- .endm
-
-/* _dbg_setabortedinstr_addr
- * Set aborted instruction address
- * On exit:
- * r1: destroyed
- */
- .macro _dbg_setabortedinstr_addr reg
- ldr r1, =__debugger_stack_bottom__
- str \reg, [r1]
- .endm
-
-
/*@}*/
#endif /* __DEBUG_MACROS_H__ */
diff --git a/AT91SAM7S256/armdebug/Debugger/debug_stub.S b/AT91SAM7S256/armdebug/Debugger/debug_stub.S
index 3743b98..e99b144 100644
--- a/AT91SAM7S256/armdebug/Debugger/debug_stub.S
+++ b/AT91SAM7S256/armdebug/Debugger/debug_stub.S
@@ -251,6 +251,50 @@ debug_cmdJumpTable:
.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
+ * 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
@@ -272,11 +316,11 @@ debug_cmdJumpTable:
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 */
+ .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_next_instruction_addr */
+/* .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_handler /* B or BL. Note v4t does not have BLX instr */
+ .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 */
@@ -295,7 +339,7 @@ debug_thumbDecodeTable:
.hword 0xe000, 0xf800
.word _thumb_b_handler /* B */
.hword 0xf000, 0xf000
- .word _thumb_long_b_handler /* Long BL or BLX (4 bytes) Note: b11 (H) indicates 1st or 2nd instr */
+ .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 */
@@ -571,14 +615,15 @@ _process_auto_breakpoint:
bl _dbg__restore_singlestep
bl _dbg__clear_singlestep
_dbg_set_bkpt_type DBG_AUTO_BKPT
- /* b _dbg__switch2undefmode */
+ b _dbg__switch2undefmode
/* _dbg__switch2undefmode
* Common internal routine to return execution to user program
*/
-_dbg__switch2undefmode:
+_dbg__switch2undefmode_withAck:
bl __dbg__procAckOnly /* send Ack to keep GDB server happy */
bl dbg__runloopTasks /* Service run loop tasks */
+_dbg__switch2undefmode:
bl _dbg__flush_icache
msr cpsr_c, #(MODE_UND | CPSR_FIQ | CPSR_IRQ) /* Configure Undef Mode */
_dbg_setmode FALSE /* Debug Mode = False */
@@ -1150,7 +1195,7 @@ _dbg__cmd_WriteMem:
* r0: parameter buffer pointer (contents after '$' and '<cmdchar>')
* Optional: AA..AA
* On exit:
- * r0, r1, r2: destroyed
+ * r0-r7: destroyed
* Note: This routine does not return to caller. Instead it switches
* operating mode to UNDEF and returns to previously active program
*/
@@ -1175,7 +1220,7 @@ _dbg__cont_check_breakpoint_type:
beq _dbg__cont_is_manual_bkpt_or_address_specified
_dbg__cont_is_normal_breakpoint:
-/* FIXME: _dbg_next_instruction_addr doesn't actually work currently.
+/* 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.
*/
@@ -1183,15 +1228,15 @@ _dbg__cont_is_normal_breakpoint:
_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_next_instruction_addr /* next instruction address returned in 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
+ b _dbg__switch2undefmode_withAck
_dbg__cont_is_manual_bkpt_or_address_specified:
bl _dbg__activate_breakpoints /* Restore exisiting breakpoints */
- b _dbg__switch2undefmode
+ b _dbg__switch2undefmode_withAck
/* _dbg__cmd_Step
* Step User Program Execution Command Handler
@@ -1208,7 +1253,7 @@ _dbg__cont_is_manual_bkpt_or_address_specified:
* r0: parameter buffer pointer (contents after '$' and '<cmdchar>')
* Optional: AA..AA
* On exit:
- * r0, r1, r2: destroyed
+ * r0-r7: destroyed
* Note: This routine does not return to caller. Instead it switches
* operating mode to UNDEF and returns to previously active program
*/
@@ -1233,7 +1278,7 @@ _dbg__step_check_breakpoint_type:
beq _dbg__step_is_manual_bkpt
_dbg__step_is_normal_breakpoint:
-/* FIXME: _dbg_next_instruction_addr doesn't actually work currently.
+/* 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.
*/
@@ -1245,15 +1290,14 @@ _dbg__step_is_normal_breakpoint:
_dbg__step_is_manual_bkpt:
mov r2, #DBGSTACK_NEXTINSTR_INDEX /* The Next Instruction Pointer for Resume is in index 0 of the Debug Stack */
- _getdbgregisterfromindex r2, r1 /* Retrieve Register contents into R1 */
+ _getdbgregisterfromindex r2, r0 /* Retrieve Register contents into R1 */
/* b _dbg__step_is_manual_bkpt_or_address_specified */
_dbg__step_is_manual_bkpt_or_address_specified:
- /* R1 contains the instruction address to be executed upon return */
- bl _dbg_next_instruction_addr /* following instruction address returned in 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 */
bl _dbg__activate_singlestep
- b _dbg__switch2undefmode
+ b _dbg__switch2undefmode_withAck
/* _dbg__proc_brkpt_params
@@ -1806,10 +1850,14 @@ void __single_step (void)
****************************************************************************/
-/* _dbg_next_instruction_addr
- * Determine the address of the next instruction to execute.
+/* _dbg_following_instruction_addr
+ * Determine the address of the following instruction to execute.
+ * On entry:
+ * R0: Address of the instruction to be (re)executed
* On exit:
- * R1: Instruction Address (31 bits, b0 = THUMB flag)
+ * R0: Destroyed
+ * R1: Following Instruction Address (31 bits, b0 = THUMB flag)
+ * R2-R7: Destroyed
*
* Here we make use of the Debugger Stack which contains the address of the aborted instruction that will be reexecuted
* when we resume the program.
@@ -1822,49 +1870,144 @@ void __single_step (void)
* We need to check the aborted instruction type, to see if it is a branch instruction, before we can determine
* the next instruction address (for inserting a Breakpoint).
*/
-_dbg_next_instruction_addr:
- /* FIXME: This needs a total rewrite */
- mov r1, #0
- bx lr
+_dbg_following_instruction_addr:
+ stmfd sp!, {lr}
/* We assume that any BKPT instructions in the code will be Manual Breakpoints,
* i.e., the Debugger does not leave stray Single Step / Auto / Normal breakpoints in memory
*/
-
- mov r2, #DBGSTACK_USERCPSR_INDEX /* Retrieve User CPSR */
- _getdbgregisterfromindex r2, r0 /* Retrieve Register contents into R0 */
- and r4, r0, #CPSR_THUMB /* store Thumb Mode status in R4 */
- mov r5, r0, lsr #28 /* store CPSR condition flags in R5[3:0] */
-
- _dbg_getabortedinstr_addr r2 /* Retrieve aborted instruction address */
-1: teq r4, #0 /* Check if it is ARM or Thumb instruction */
- ldrneh r0, [r2]
- ldrne r1, =(BKPT16_INSTR | BKPT16_MANUAL_BKPT) /* check for Thumb Manual Breakpoint Instruction */
- ldreq r0, [r2]
- ldreq r1, =(BKPT32_INSTR | BKPT32_MANUAL_BKPT) /* check for ARM Manual Breakpoint Instruction */
- teq r0, r1
- bne 2f /* Not Manual breakpoint */
- teq r4, #0 /* Check if it is ARM or Thumb instruction */
- addne r2, r2, #2 /* Is Manual Breakpoint, Skip to next Thumb instruction */
- addeq r2, r2, #4 /* Is Manual Breakpoint, Skip to next ARM instruction */
- _dbg_setabortedinstr_addr r2 /* Update aborted instruction address */
- b 1b /* To protect against a sequence of Manual Breakpoint Instructions */
-
-/* Here, r0 contains the instruction which will be reexecuted when program resumes. We need to dissect it to see if
- * it is a branch instruction.
+ mov r6, r0 /* Keep instruction address in R6 */
+ mov r2, #DBGSTACK_USERCPSR_INDEX /* Retrieve User CPSR */
+ _getdbgregisterfromindex r2, r1 /* Retrieve Register contents into R1 */
+ and r4, r1, #CPSR_THUMB /* store Thumb Mode status in R4 */
+ mov r5, r1, lsr #28 /* store CPSR condition flags in R5[3:0] */
+
+_dbg_get_aborted_instr:
+1: teq r4, #0 /* Check if it is ARM or Thumb instruction */
+ ldrneh r0, [r6] /* Load Thumb instruction opcode using Addr in R6 into R0 */
+ ldrne r2, =(BKPT16_INSTR | BKPT16_MANUAL_BKPT) /* check for Thumb Manual Breakpoint Instruction */
+ ldreq r0, [r6] /* Load ARM instruction opcode using Addr in R6 into R0 */
+ ldreq r2, =(BKPT32_INSTR | BKPT32_MANUAL_BKPT) /* check for ARM Manual Breakpoint Instruction */
+ teq r0, r2 /* Is instruction opcode (R0) == Manual Breakpoint opcode (R2)? */
+ bne 2f /* Not Manual breakpoint */
+ teq r4, #0 /* Check if it is ARM or Thumb Manual Breakpoint */
+ addne r6, r6, #2 /* Is Manual Breakpoint, Skip to next Thumb instruction */
+ addeq r6, r6, #4 /* Is Manual Breakpoint, Skip to next ARM instruction */
+ b 1b /* To protect against a sequence of Manual Breakpoint Instructions */
+
+/* Here, R0 contains the instruction opcode which will be (re)executed when program resumes.
+ * We need to dissect it to see if it is a branch instruction.
* For ARM instructions, we also need to evaluate the current (breakpointed) instruction to see if it'll execute.
- * If not, then the next instruction is the instruction following the current instruction.
+ * If not, then the following instruction is at the address following the address of the opcode in R0 (Default Following Instruction Address).
*/
2:
- /* Use R6 to store candidate next instruction address */
- teq r4, #0 /* Check if it is ARM or Thumb instruction */
- beq _next_instr_is_arm
-_next_instr_is_thumb:
- add r6, r2, #2 /* set next Thumb instruction address */
- /*_is_thumb_branch_instr r0 */ /* check if the current instruction is a branch instruction */
-_next_instr_is_arm:
- add r6, r2, #4 /* Is ARM, set next ARM instruction address */
-@@@@@@@@@
- bx lr
+ teq r4, #0 /* Check if it is ARM or Thumb instruction */
+ beq _following_instr_is_arm
+_following_instr_is_thumb:
+ add r6, r6, #2 /* Store following Thumb instruction address to R1 */
+ orr r6, r6, #BKPT_STATE_THUMB_FLAG /* Set b0 to indicate Thumb instruction */
+ /* R0: Candidate Instruction Opcode
+ * R5[3:0]: CPSR condition codes
+ * R6: Default Following Instruction Address (PC+2)
+ */
+ bl _eval_thumb_instruction /* following address is either ARM or Thumb */
+ /* We must set this the Thumb bit only within the instruction handler since BX would switch modes */
+ b _exit_dbg_following_instruction_addr
+
+_following_instr_is_arm:
+ add r6, r6, #4 /* Store following ARM instruction address to R1 */
+ /* R0: Candidate Instruction Opcode
+ * R5[3:0]: CPSR condition codes
+ * R6: Default Following Instruction Address (PC+4)
+ */
+ bl _eval_arm_instruction
+
+_exit_dbg_following_instruction_addr:
+ mov r1, r0 /* Return Actual Following Instruction Address in R1 (B0 set to indicate Thumb mode) */
+ ldmfd sp!, {pc}
+
+
+/* _eval_arm_instruction
+ * Evaluate ARM instruction to determine following instruction address
+ * On entry:
+ * R0: instruction to be executed
+ * R5[3:0]: CPSR condition codes
+ * R6: Default Following Instruction Address (PC+4)
+ * On exit:
+ * R0: following instruction address (B0 set to indicate Thumb mode)
+ * R1-R7: destroyed
+ */
+_eval_arm_instruction:
+ stmfd sp!, {lr}
+ mov r4, r0 /* Keep Instruction Opcode in R4 */
+ bl _dbg_check_arm_condcode
+ teq r0, #FALSE
+ moveq r0, r6 /* False (don't execute), so use Default Following Instruction Address */
+ bne _exit_eval_arm_instruction
+
+_will_execute_arm_instr:
+ mov r0, r4 /* Copy instruction opcode to R0 as Code Handler parameter */
+ mov r1, #0 /* initialize ARM Decode Entry Table index register */
+1:
+ _dbg_armDecodeEntry r2, r3, r4, r1 /* instrreg (R2), instrmask (R3), codehandler (R4), indexreg (R1) */
+ teq r2, #0 /* Check for Null Entry (End of Table marker) */
+ moveq r0, r6 /* End of Table, no match found, so use Default Following Instruction Address */
+ beq _exit_eval_arm_instruction
+ and r7, r0, r3 /* Use R7 to check masked instruction opcode (from R0) to see if it matches template (in R2) */
+ teq r7, r2
+ addne r1, r1, #1 /* No match, so keep looking */
+ bne 1b
+
+_call_arm_code_handler:
+ mov r1, r6 /* Copy Default Following Instruction Address to R1 as Code Handler Parameter */
+ mov lr, pc
+ bx r4 /* Call Code Handler with R0: Instruction Opcode, R1: Default Following Instruction Address */
+_exit_eval_arm_instruction:
+ /* Returned Following Address Instruction in R0 (B0 set to indicate Thumb mode) */
+ ldmfd sp!, {pc}
+
+/* _eval_thumb_instruction
+ * Evaluate Thumb instruction to determine following instruction address
+ * On entry:
+ * R0: instruction to be executed
+ * R5[3:0]: CPSR condition codes
+ * R6: Default Following Instruction Address (PC+2)
+ * On exit:
+ * R0: following instruction address (B0 set to indicate Thumb mode)
+ * R1-R7: destroyed
+ */
+_eval_thumb_instruction:
+ stmfd sp!, {lr}
+#if 0
+ mov r4, r0 /* Keep Instruction Opcode in R4 */
+ /* Only B<cond> instructions are conditionally executed, deal with it in that Code Handler */
+ bl _dbg_check_thumb_condcode
+ teq r0, #FALSE
+ moveq r0, r6 /* False (don't execute), so use Default Following Instruction Address */
+ bne _exit_eval_thumb_instruction
+
+_will_execute_thumb_instr:
+ mov r0, r4 /* Copy instruction opcode to R0 as Code Handler parameter */
+#endif
+ mov r1, #0 /* initialize Thumb Decode Entry Table index register */
+1:
+ _dbg_thumbDecodeEntry r2, r3, r4, r1 /* instrreg (R2), instrmask (R3), codehandler (R4), indexreg (R1) */
+ teq r2, #0 /* Check for Null Entry (End of Table marker) */
+ moveq r0, r6 /* End of Table, no match found, so use Default Following Instruction Address */
+ beq _exit_eval_thumb_instruction
+
+ and r7, r0, r3 /* Use R5 to check masked instruction opcode (from R0) to see if it matches template (in R2) */
+ teq r7, r2
+ addne r1, r1, #1 /* No match, so keep looking */
+ bne 1b
+
+_call_thumb_code_handler:
+ mov r1, r6 /* Copy Default Following Instruction Address to R1 as Code Handler Parameter */
+ mov lr, pc
+ bx r4 /* Call Code Handler with R0: Instruction Opcode, R1: Default Following Instruction Address */
+_exit_eval_thumb_instruction:
+ /* Returned Following Address Instruction in R0 */
+ ldmfd sp!, {pc}
+
/****************************************************************************
*
@@ -1879,6 +2022,7 @@ _next_instr_is_arm:
* R5[3:0]: CPSR condition codes
* On exit:
* R0: will_execute (boolean)
+ * R1-R3: Destroyed
*/
_dbg_check_arm_condcode:
@@ -1978,21 +2122,553 @@ _dbg_check_arm_condcode_exit:
ldmfd sp!, {r6, pc}
-_arm_data_instr_handler: /* Data Processing instr with Rd = R15 */
-_arm_bx_blx_handler: /* BX or BLX */
-_arm_ldr_pc_handler: /* LDR with Rd = PC */
-_arm_ldm_pc_handler: /* LDM {pc} */
-_arm_b_bl_handler: /* B or BL. Note v4t does not have BLX instr */
-_arm_coproc_swi_handler: /* Coprocessor instr or SWI */
- bx lr
+/* _arm_rmshifted_val
+ * Calculate value of Shifted Rm (operand)
+ * On entry:
+ * R0[11:0]: Shifted Rm operand
+ * On exit:
+ * R0: value of Shifted Rm
+ * R1, R2, R3: destroyed
+ */
+_arm_rmshifted_val:
+ stmfd sp!, {lr}
+ ldr r3, =(NIBBLE2|BYTE0)
+ and r3, r0, r3 /* 12 bit Shifted Register operand, copied to R3 */
+ and r2, r3, #NIBBLE0 /* Register Rn Enum in R2 */
+ _regenum2index r2, r2 /* Convert Enum into Index in R2 */
+ _getdbgregisterfromindex r2, r0 /* Retrieve Register Rn contents from Index (R2) into R0 */
+
+ tst r3, #0x10 /* B4: Immediate (0) or Register (1) shift count */
+ /* check bitshift op */
+ and r3, r3, #0x60 /* shift type */
+ mov r3, r3, lsr #5 /* convert into shift type jumptable index */
+ bne _arm_get_reg_shift /* Flags set previously via TST r3 (B4) */
+_arm_calc_const_shift:
+ movs r1, r3, lsr #7 /* Immediate shift count, 5 bit unsigned value in R1 */
+ bne _arm_calc_shifted_rm_val /* Non-zero shift count, process normally */
+ /* Must check for RRX == ROR #0 */
+ teq r3, #0x3 /* ROR == 0x3 */
+ addeq r3, r3, #1
+ b _arm_calc_shifted_rm_val
+
+_arm_get_reg_shift:
+ mov r2, r3, lsr #8 /* Register-based shift count, 4 bit register enum in R2 */
+ _regenum2index r2, r2 /* Convert Enum into Index in R2 */
+ _getdbgregisterfromindex r2, r1 /* Retrieve Register value (shift count) from Index (R2) into R1 */
+
+_arm_calc_shifted_rm_val:
+ _dbg_jumpTableHandler debug_regShiftJumpTable, r2, r3 /* Calculate RmShifted value from R0: Rn Register val, R1: Shift/Rotate val */
+ ldmfd sp!, {pc}
+
+/* Rm Shifted Shift Type Jump Table Routines
+ * On entry:
+ * R0: Register Rm
+ * R1: Shift/Rotate Amount
+ * On exit:
+ * R0: RmShifted result
+ * R1, R2: destroyed
+ *
+ */
+_reg_lsl:
+ lsl r0, r0, r1
+ bx lr
+
+_reg_lsr:
+ lsr r0, r0, r1
+ bx lr
+
+_reg_asr:
+ asr r0, r0, r1
+ bx lr
+
+_reg_ror:
+ ror r0, r0, r1
+ bx lr
+
+_reg_rrx:
+ moveq r2, #DBGSTACK_USERCPSR_INDEX /* convert register enum to Debug Stack index */
+ _getdbgregisterfromindex r2, r1 /* Retrieve CPSR contents from Index (R2) into R1 */
+ ands r1, r1, #CPSR_CFLAG /* Keep C Flag */
+ movne r1, #0x80000000 /* Set B31 if C Flag set */
+ lsr r0, r0, #1 /* Rm >> 1 */
+ orr r0, r0, r1 /* Put C flag into B31 */
+ bx lr
+
+
+/* _arm_data_instr_handler
+ * ARM Data Processing Instruction with Rd == R15
+ * On entry:
+ * R0: instruction to be executed
+ * R1: Default Following Instruction Address (PC+4)
+ * R5[3:0]: CPSR condition codes
+ * On exit:
+ * R0: following instruction address
+ * R1, R2, R3, R4, R5, R6: Destroyed
+ */
+_arm_data_instr_handler:
+ stmfd sp!, {lr}
+ mov r6, r1 /* save Following Instruction Address in R6 */
+ ldr r1, =0x0FBF0000
+ and r4, r0, r1 /* Keep instruction Opcode in R4 */
+ ldr r1, =0x010F0000
+ cmp r4, r1 /* Check for MSR / MRS instruction */
+
+_arm_is_msr_mrs_instr:
+ moveq r0, r6 /* Copy default next instruciton address to R0 */
+ beq _exit_arm_data_instr_handler /* Return default next instruction address */
+
+ /* Not MSR / MRS, so process normally */
+_arm_normal_data_instr:
+ and r4, r0, #0x01E00000 /* Mask Instruction Opcode into R4 */
+ and r5, r0, #0x000F0000 /* Store Rn (Operand 1) Register Enum into R5[19:16] */
+ lsr r5, r5, #16 /* Shift into R5[3:0] */
+
+_arm_check_operand2_type:
+ tst r0, #0x02000000 /* Check for Immediate (1) or Register (0) Operand 2 */
+ beq _arm_op2_is_reg
+
+_arm_op2_is_imm:
+ and r1, r0, #BYTE0 /* 8 bit unsigned constant in R1 */
+ and r2, r0, #NIBBLE2 /* (rotate count / 2) in R2[11:8] */
+ lsr r2, r2, #7 /* actual rotate count in R2[4:0] */
+ ror r1, r1, r2 /* Rotated constant in R1 */
+ b _arm_get_operand1_val
+
+_arm_op2_is_reg:
+ ldr r1, =(NIBBLE2|BYTE0)
+ and r0, r0, r1 /* 12 bit register operand in R1 */
+ bl _arm_rmshifted_val /* R0 contains the Rm shifted val */
+ mov r1, r0 /* move to R1 for later processing */
+
+_arm_get_operand1_val:
+ _regenum2index r5, r1 /* Convert Enum into Index in R1 */
+ _getdbgregisterfromindex r1, r0 /* Retrieve Register contents from Index (R1) into R4 */
+ teq r5, #REG_PC /* Check if it is PC relative */
+ addeq r0, r0, #8 /* adjust for PC relative (+8) */
+
+_arm_calc_data_instr_val:
+ mov r2, r6 /* Retrieve Following Instruction Address in R6 to R2 */
+ /* Calculate data instruction value from R0: Rn Register (Op1) val, R1: Operand 2 val, R2: Default Next Instr Addr */
+ _dbg_jumpTableHandler debug_dataInstrJumpTable, r3, r4 /* Next Instruction Address in R0 */
+_exit_arm_data_instr_handler:
+ ldmfd sp!, {pc}
+
+
+/* Data Processing Instruction Jump Table Routines
+ * On entry:
+ * R0: Register Rn (Operand 1) value
+ * R1: Operand 2 value
+ * R2: Default Next Instruction Address (PC+4)
+ * On exit:
+ * R0: Calculated result
+ * R1, R2, R3: Destroyed
+ *
+ */
+_opcode_and:
+ and r0, r0, r1
+ bx lr
+
+_opcode_eor:
+ eor r0, r0, r1
+ bx lr
+
+_opcode_sub:
+ sub r0, r0, r1
+ bx lr
+
+_opcode_rsb:
+ rsb r0, r0, r1
+ bx lr
+
+_opcode_add:
+ add r0, r0, r1
+ bx lr
+
+_opcode_adc:
+ /* Op1 + Op2 + C */
+ moveq r2, #DBGSTACK_USERCPSR_INDEX /* convert register enum to Debug Stack index */
+ _getdbgregisterfromindex r2, r3 /* Retrieve CPSR contents from Index (R2) into R3 */
+ ands r3, r3, #CPSR_CFLAG /* Keep C Flag, NE if C set */
+ add r0, r0, r1
+ addne r0, r0, #1 /* Add C if set */
+ bx lr
+
+_opcode_sbc:
+ /* Op1 - Op2 + C - 1 */
+ moveq r2, #DBGSTACK_USERCPSR_INDEX /* convert register enum to Debug Stack index */
+ _getdbgregisterfromindex r2, r3 /* Retrieve CPSR contents from Index (R2) into R3 */
+ ands r3, r3, #CPSR_CFLAG /* Keep C Flag, NE if C set */
+ sub r0, r0, r1
+ subeq r0, r0, #1 /* If C clear, subtract 1, else (C - 1) = 0 */
+ bx lr
+
+_opcode_rsc:
+ /* Op2 - Op1 + C - 1 */
+ moveq r2, #DBGSTACK_USERCPSR_INDEX /* convert register enum to Debug Stack index */
+ _getdbgregisterfromindex r2, r3 /* Retrieve CPSR contents from Index (R2) into R3 */
+ ands r3, r3, #CPSR_CFLAG /* Keep C Flag, NE if C set */
+ rsb r0, r0, r1
+ subeq r0, r0, #1 /* If C clear, subtract 1, else (C - 1) = 0 */
+ bx lr
+
+_opcode_tst:
+_opcode_teq:
+_opcode_cmp:
+_opcode_cmn:
+ mov r0, r2 /* Next Instruction Address is not modified */
+ bx lr
+
+_opcode_orr:
+ orr r0, r0, r1
+ bx lr
+
+_opcode_mov:
+ mov r0, r1 /* Operand 1 is ignored */
+ bx lr
+
+_opcode_bic:
+ bic r0, r0, r1
+ bx lr
+
+_opcode_mvn:
+ mvn r0, r1 /* Operand 1 is ignored */
+ bx lr
+
+
+/* _arm_bx_blx_handler
+ * BX or BLX Rm Handler. Note v4t does not have BLX instr
+ * On entry:
+ * R0: instruction to be executed
+ * R1: Default Following Instruction Address (PC+4)
+ * R5[3:0]: CPSR condition codes
+ * On exit:
+ * R0: following instruction address (B0 set to indicate Thumb mode)
+ * R1, R2: destroyed
+ */
+_arm_bx_blx_handler:
+ stmfd sp!, {lr}
+ and r0, r0, #NIBBLE0 /* Register Rn Enum in R0 */
+ _regenum2index r0, r1 /* Convert Enum into Index in R1 */
+ _getdbgregisterfromindex r1, r0 /* Retrieve Register contents from Index (R1) into R0 */
+ /* Here, the register value would have B0 set to indicate switch to Thumb mode */
+#if 0
+ bic r0, #0x01 /* Clear R0[0] since it is used to indicates Thumb mode */
+#endif
+ ldmfd sp!, {pc}
+
+/* _arm_ldr_pc_handler
+ * LDR with Rd = PC
+ * On entry:
+ * R0: instruction to be executed
+ * R1: Default Following Instruction Address (PC+4)
+ * R5[3:0]: CPSR condition codes
+ * On exit:
+ * R0: following instruction address
+ * R1, R2, R3, R4, R5: destroyed
+ */
+
+_arm_ldr_pc_handler:
+ stmfd sp!, {lr}
+ mov r5, r0 /* Keep a copy of the instruction in R5 */
+ and r0, r0, #NIBBLE4 /* Register Rn Enum in R0[19:16] */
+ lsr r0, r0, #16 /* Move Rn Enum to R0[3:0] */
+ _regenum2index r0, r1 /* Convert Enum into Index in R1 */
+ _getdbgregisterfromindex r1, r4 /* Retrieve Register contents from Index (R1) into R4 */
+ teq r0, #REG_PC /* Check if it is PC relative */
+ addeq r4, r4, #8 /* adjust for PC relative (+8) */
+ tst r5, #0x01000000 /* Pre (1) or Post (0) Indexed */
+ beq _exit_arm_ldr_pc_handler /* If Post-Indexed, just return value of Rn */
+ /* Pre-Indexed */
+ ldr r0, =(NIBBLE2|BYTE0)
+ and r0, r5, r0 /* 12 bit Immediate value or Shifted Reg operand */
+ tst r5, #0x02000000 /* Immediate (0) or Register (1) */
+ beq _calc_ldr_pc_offset /* Immediate value is already in R0 */
+
+_get_shiftedreg_val:
+ bl _arm_rmshifted_val /* Convert Rm shifted operand into value in R0 */
+
+_calc_ldr_pc_offset:
+ tst r5, #0x00800000 /* Add (1) or Subtract (0) */
+ addne r4, r4, r0 /* If Add, R2 = Rn + value */
+ subeq r4, r4, r0 /* If Sub, R2 = Rn - value */
+
+_exit_arm_ldr_pc_handler:
+ mov r0, r4 /* Return next instruction address in R0 */
+ ldmfd sp!, {pc}
+
+
+/* _arm_ldm_pc_handler
+ * LDM {pc}
+ * On entry:
+ * R0: instruction to be executed
+ * R1: Default Following Instruction Address (PC+4)
+ * R5[3:0]: CPSR condition codes
+ * On exit:
+ * R0: following instruction address
+ * R1, R2, R3: destroyed
+ *
+ * FIXME: The algorithm from eCos arm_stub.c does not deal with the Pre/Post-Indexed addressing (P) bit.
+ */
+_arm_ldm_pc_handler:
+ stmfd sp!, {lr}
+ and r3, r0, #0x000F0000 /* Store Rn (Operand 1) Register Enum into R3[19:16] */
+ lsr r3, r3, #16 /* Shift into R3[3:0] */
+
+_arm_get_Rn_val:
+ _regenum2index r3, r2 /* Convert Enum into Index in R2 */
+ _getdbgregisterfromindex r2, r1 /* Retrieve Register contents from Index (R2) into R1 */
+ teq r3, #REG_PC /* Check if it is PC relative */
+ addeq r1, r1, #8 /* adjust Rn (R1) for PC relative (+8) */
+
+_arm_get_regcount:
+ mov r2, #0 /* Initialize reg_count (R2) to 0 */
+ mov r3, r0, lsl #16 /* Keep HLFWORD0 containing vector bits in R3[31:16] */
+ /* This shortens the checking to a max of 16 iterations, since the PC bit should be set */
+1: movs r3, r3, lsl #1 /* count number of '1' bits */
+ addcs r2, r2, #1 /* increment reg_count (R2) if C Flag set */
+ bne 1b /* continue until vector is empty */
+
+_arm_check_updown_offset:
+ tst r0, #0x00800000 /* Check Up (1) or Down (0) */
+ addne r1, r1, r2, lsl #2 /* Ascending: Rn (R1) += reg_count (R2) x 4 */
+ subeq r1, r1, #4 /* Descending: Rn (R1) -= 4 */
+ ldr r0, [r1] /* Retrieve stack content for new PC value */
+ ldmfd sp!, {pc}
+
+
+
+/* _arm_b_bl_blx_handler
+ * B, BL or BLX <offset>. Note v4t does not have BLX instr
+ * On entry:
+ * R0: instruction to be executed
+ * R1: Default Following Instruction Address (PC+4)
+ * R5[3:0]: CPSR condition codes
+ * On exit:
+ * R0: following instruction address
+ * R1, R2, R3: destroyed
+ */
+
+_arm_b_bl_blx_handler:
+ stmfd sp!, {lr}
+#ifndef __ARM6OR7__
+ /* armv5t or later, has BLX support */
+ mov r3, r0 /* Copy instruction to R3 */
+#endif
+
+_arm_b_bl_blx_get_offset:
+ and r0, r0, #(BYTE2|BYTE1|BYTE0) /* Encoded Branch offset in R0[23:0] */
+ lsl r0, r0, #(32-24) /* Shift to R0[31:8] */
+ asr r0, r0, #(32-25) /* Actual Signed offset = Encode Offset x 2 in R0[25:0] */
+ add r1, r1, #4 /* R1: (PC+4) + 4 */
+ add r0, r0, r1 /* Calculate Branch Target Address R0: (PC+8) + signed offset */
+
+#ifndef __ARM6OR7__
+ /* armv5t or later, has BLX support */
+ and r2, r3, #0xFE000000 /* Mask out Condition Code and Opcode */
+ teq r2, #0xFA000000 /* Look for BLX */
+ bne _exit_arm_b_bl_blx_handler /* No, it is a B/BL instruction */
+ tst r3, #0x01000000 /* H bit for Thumb Halfword Address */
+ orrne r0, r0, #0x02 /* Set Halfword Address R0[1] */
+ orr r0, r0, #0x01 /* Set R0[0] since it is used to indicates Thumb mode */
+#endif
+
+_exit_arm_b_bl_blx_handler:
+ ldmfd sp!, {pc}
+
+/* _arm_coproc_swi_handler
+ * SVC (SWI) or Coprocessor instruction
+ * On entry:
+ * R0: instruction to be executed
+ * R1: Default Following Instruction Address (PC+4)
+ * R5[3:0]: CPSR condition codes
+ * On exit:
+ * R0: following instruction address
+ * R1, R2: destroyed
+ */
-_thumb_bx_blx_handler: /* BX or BLX. Note: b7 (H1) is not matched in the mask */
-_thumb_poppc_handler: /* PUSH/POP, specifically POP {Rlist,PC} */
-_thumb_bcond_swi_handler: /* B<cond> or SWI */
-_thumb_b_handler: /* B */
-_thumb_long_b_handler: /* Long BL or BLX (4 bytes) Note: b11 (H) indicates 1st or 2nd instr */
+_arm_coproc_swi_handler:
+ and r2, r0, #0x0F000000
+ teq r2, #0x0F000000 /* SVC (SWI) instruction */
+
+_arm_swi_instr:
+ ldreq r0, =SVC_VECTOR /* Return SVC Vector Address */
+ beq _exit_arm_coproc_swi_handler
+
+_arm_coproc_instr:
+ mov r0, r1 /* Use default Following Instruction Address */
+_exit_arm_coproc_swi_handler:
+ bx lr
+
+
+/* _thumb_bx_blx_handler
+ * BX or BLX Handler. Note: b7 (H1) is not matched in the mask (should be 0); armv4t does not support BLX.
+ * On entry:
+ * R0: instruction to be executed
+ * R1: Default Following Instruction Address (PC+2)
+ * R5[3:0]: CPSR condition codes
+ * On exit:
+ * R0: following instruction address (B0 set to indicate Thumb mode)
+ * R1: destroyed
+ */
+_thumb_bx_blx_handler:
+ stmfd sp!, {lr}
+ and r0, r0, #0x78 /* Register Rn Enum in R0[6:3] (Hi-Reg indicated by B6) */
+ mov r0, r0, lsr #3 /* Shift Rn Enum to R0[3:0] */
+ _regenum2index r0, r1 /* Convert Enum into Index in R1 */
+ _getdbgregisterfromindex r1, r0 /* Retrieve Register contents from Index (R1) into R0 */
+ /* Here, the register value would have R0[0] set to indicate switch to Thumb mode */
+ ldmfd sp!, {pc}
+
+/* _thumb_poppc_handler
+ * PUSH/POP, specifically POP {Rlist,PC}
+ * On entry:
+ * R0: instruction to be executed
+ * R1: Default Following Instruction Address (PC+2)
+ * R5[3:0]: CPSR condition codes
+ * On exit:
+ * R0: following instruction address (B0 set to indicate Thumb mode)
+ * R1: destroyed
+ */
+_thumb_poppc_handler:
+ stmfd sp!, {lr}
+
+_thumb_get_SP_val:
+ mov r2, #DBGSTACK_USERSP_INDEX /* Set Register Index (R2) to SP Index */
+ _getdbgregisterfromindex r2, r1 /* Retrieve SP contents from Index (R2) into R1 */
+
+_thumb_get_regcount:
+ mov r2, #0 /* Initialize reg_count (R2) to 0 */
+ mov r3, r0, lsl #24 /* Keep BYTE0 containing vector bits in R3[31:24] */
+ /* This shortens the checking to a max of 8 iterations */
+1: movs r3, r3, lsl #1 /* count number of '1' bits */
+ addcs r2, r2, #1 /* increment reg_count (R2) if C Flag set */
+ add r1, r1, #4 /* Walk the stack to locate the PUSHed LR (POP PC) value */
+ bne 1b /* continue until vector is empty */
+ ldr r0, [r1] /* Retrieve new PC value */
+#if 0
+ /* PC Value should have B0 set */
+ orr r0, r0, #0x01 /* Force R0[0] since it is used to indicates Thumb mode */
+#endif
+ ldmfd sp!, {pc}
+
+
+/* _thumb_bcond_swi_handler
+ * B<cond> or SWI (SVC)
+ * On entry:
+ * R0: instruction to be executed
+ * R1: Default Following Instruction Address (PC+2)
+ * R5[3:0]: CPSR condition codes
+ * On exit:
+ * R0: following instruction address (B0 set to indicate Thumb mode)
+ * R1-R6: destroyed
+ */
+_thumb_bcond_swi_handler:
+ mov r4, r0 /* Keep instruction in R4 */
+ and r2, r0, #0x0F00 /* Keep Condition Code R2[11:8] */
+ teq r2, #0x0F00 /* SVC (SWI) instruction */
+_thumb_swi_instr:
+ ldreq r0, =SVC_VECTOR /* Return SVC Vector Address */
+ beq _exit_thumb_bcond_swi_handler /* Switch to ARM mode for SVC */
+_thum_bcond_unused_instr:
+ teq r2, #0x0E00
+ moveq r0, r6 /* False (don't execute), so use Default Following Instruction Address */
+ beq _exit_thumb_bcond_instr
+
+_thumb_bcond_instr:
+ mov r6, r1 /* Store Default Following Instruction Address in R6 */
+ lsl r0, r2, #(32-12) /* Shift condition code in R2[11:8] to R0[31:28] to match ARM cond-code format */
+ bl _dbg_check_arm_condcode /* Use ARM condition code checking routine to test (R4, R6 unchanged) */
+ teq r0, #FALSE
+ moveq r0, r6 /* False (don't execute), so use Default Following Instruction Address */
+ beq _exit_thumb_bcond_instr
+
+_thumb_calc_bcond_offset:
+ lsl r0, r4, #(32-8) /* Shift 8-bit offset in R4[7:0] to R0[31:24] */
+ asr r0, r0, #(32-9) /* Convert into 9-bit signed offset in R0[8:0] */
+ add r0, r6, r0 /* PC+2 + signed offset */
+ add r0, r0, #2 /* PC+4 + signed offset */
+_exit_thumb_bcond_instr:
+ orr r0, r0, #0x01 /* Set R0[0] since it is used to indicates Thumb mode */
+_exit_thumb_bcond_swi_handler:
bx lr
+/* _thumb_b_handler
+ * B
+ * On entry:
+ * R0: instruction to be executed
+ * R1: Default Following Instruction Address (PC+2)
+ * R5[3:0]: CPSR condition codes
+ * On exit:
+ * R0: following instruction address (B0 set to indicate Thumb mode)
+ * R1: destroyed
+ * Note: The signed offset is 12-bits (encoded value x 2)
+ */
+_thumb_b_handler:
+ stmfd sp!, {lr}
+ lsl r0, r0, #(32-11) /* Shift 11-bit offset in R0[10:0] to R0[31:21] */
+ asr r0, r0, #(32-12) /* Convert into 12-bit signed offset in R0[11:0] */
+ add r0, r1, r0 /* PC+2 + signed offset */
+ add r0, r0, #2 /* PC+4 + signed offset */
+ orr r0, r0, #0x01 /* Set R0[0] since it is used to indicates Thumb mode */
+ ldmfd sp!, {pc}
+
+
+/* _thumb_long_bl_blx_handler
+ * Long BL or BLX (4 bytes) Note: b11 (H) indicates 1st or 2nd instr; armv4t does not support BLX.
+ * On entry:
+ * R0: instruction to be executed
+ * R1: Default Following Instruction Address (PC+2)
+ * R5[3:0]: CPSR condition codes
+ * On exit:
+ * R0: following instruction address (B0 set to indicate Thumb mode)
+ * R1, R2, R3: destroyed
+ * Note: The BL instruction (0xFxxx) should be in pairs (Dual 16-bit instructions).
+ * The first instruction should have (H=0) to indicate the upper 11 bits of the encoded offset
+ * The second instruction should have (H=1) to indicate the lower 11 bits of the encoded offset
+ * The signed offset is 23 bits (encoded value x 2)
+ *
+ * Note2: The BLX instruction (0xExxx) encodes the first instruciton using BL (0xFxxx) with H=0,
+ * while the second instruction has a different opcode value (0xExxx), with H=1.
+ * BLX is only used to switch to an ARM target.
+ */
+_thumb_long_bl_blx_handler:
+ stmfd sp!, {lr}
+ tst r0, #0x0800 /* Check H bit */
+ bne _return_default_thumb_following_instr /* H=1 as first instruction shouldn't happen */
+
+_thumb_is_1st_bl_blx_instruction:
+ lsl r0, r0, #(32-11) /* Shift 11-bit offset in R0[10:0] to R0[31:21] */
+ asr r0, r0, #(32-23) /* Convert into 12-bit signed offset in R0[22:12] */
+ ldrh r3, [r1] /* Get second instruction in pair into R3 */
+ tst r3, #0x0800 /* Check H bit */
+ beq _exit_thumb_long_bl_blx_handler /* H=0 as second instruction shouldn't happen */
+
+ lsl r2, r3, #(32-11) /* Shift 11-bit offset in R3[10:0] to R2[31:21] */
+ lsr r2, r2, #(32-12) /* Convert into 12-bit unsigned offset in R2[11:0] */
+ orr r0, r0, r2 /* Combine offsets */
+ add r0, r1, r0 /* PC+2 + signed offset */
+ add r0, r0, #2 /* PC+4 + signed offset */
+
+_thumb_is_2nd_bl_blx_instruction:
+ and r3, r3, #0x0F000 /* Keep instruction opcode in R3 */
+
+ cmp r3, #0x0F000 /* Look for BL */
+ orreq r0, r0, #0x01 /* Match, set R0[0] since it is used to indicates Thumb mode */
+ beq _exit_thumb_long_bl_blx_handler
+
+#ifndef __ARM6OR7__
+ /* v5t or higher architecture */
+ cmp r3, #0x0E000 /* Look for BLX */
+ biceq r0, r0, #0x03 /* Match, Force ARM address */
+ beq _exit_thumb_long_bl_blx_handler
+#endif
+
+_return_default_thumb_following_instr:
+ mov r0, r1 /* Return default Following Instruction Address */
+
+_exit_thumb_long_bl_blx_handler:
+ ldmfd sp!, {pc}
+
/****************************************************************************
*