/** @file debug_opcodes.S
 *  @brief ARM Debugger Opcode Parsing 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_stub.h"
#include "debug_internals.h"
#include "debug_macros.h"

.data
.align 4
/* 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)
 */

/* WARNING: The sequence of matching instructions is important!
 *			Always check from more specific to more general IBMs
 *			for instructions sharing common opcode prefix bits.
 */
debug_armDecodeTable:
	.word	0x012fff10, 0x0ffffff0, _arm_bx_blx_handler		/* [Prefix:00] BX or BLX. Note v4t does not have BLX instr */
	.word	0x0000f000, 0x0c00f000, _arm_data_instr_handler	/* [Prefix:00] Data Processing instr with Rd = R15 */
/*	.word	0x06000010, 0x0e000010, _arm_undef_handler	*/	/* [Prefix:01] Undefined instr: shouldn't occur, as it would've been trapped already. See _dbg_following_instruction_addr */
	.word	0x0410f000, 0x0410f000, _arm_ldr_pc_handler		/* [Prefix:01] LDR with Rd = PC */
	.word	0x08108000, 0x0e108000, _arm_ldm_pc_handler		/* [Prefix:10] LDM {pc} */
	.word	0x0a000000, 0x0e000000, _arm_b_bl_blx_handler	/* [Prefix:10] B, BL or BLX. Note v4t does not have BLX instr */
	.word	0x0c000000, 0x0c000000, _arm_coproc_swi_handler	/* [Prefix:11] Coprocessor instr or SWI */
	.word	0x0,0x0,0x0										/* Null Entry */

/* Thumb Instruction Decode Table
 * 	.hword IID, IBM
 * 	.word IHA (8 bytes)
 */

/* WARNING: The sequence of matching instructions is important!
 *			Always check from more specific to more general IBMs
 *			for instructions sharing common opcode prefix bits.
 */
debug_thumbDecodeTable:
	.hword	0x4700, 0xff07
	.word	_thumb_bx_blx_handler				/* [Prefix:01] BX or BLX. Note: Link (L:b7) is not checked in the mask */
	.hword	0xbd00, 0xff00
	.word	_thumb_poppc_handler				/* [Prefix:10] PUSH/POP, specifically POP {Rlist,PC} */
	.hword	0xd000, 0xf000
	.word	_thumb_bcond_swi_handler			/* [Prefix:11] B<cond> or SWI */
	.hword	0xe000, 0xf800
	.word	_thumb_b_handler					/* [Prefix:11] B */
	.hword	0xf000, 0xf000
	.word	_thumb_long_bl_blx_handler			/* [Prefix:11] 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
 */


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, 0xFF, 0xFF, 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 HI, LS, GE, LT, GT and LE instructions only
 * Condition Code stored in the following order:
 *	 b7   b6   b5    b4   b3    b2     b1    b0
 *	AND  CHKZ CHKC CHKNV   -  Z set  C set  N==V		(bit set = 1)
 *   OR    -    -     -    -  Z clr  C clr  N!=V		(bit clr = 0)
 *
 *		HI: C set AND Z clr
 *		LS: C clr  OR Z set
 *		GE: N == V
 *		LT: N != V
 *		GT: Z clr AND (N == V)
 *		LE: Z set OR (N != V)
 */

#define COMPLEX_CONDCODE_START	    0x08
#define COMPLEX_CONDCODE_NEQV_MASK  0x01
#define COMPLEX_CONDCODE_CSET_MASK  0x02
#define COMPLEX_CONDCODE_ZSET_MASK  0x04
#define COMPLEX_CONDCODE_CHKNV_MASK 0x10
#define COMPLEX_CONDCODE_CHKC_MASK  0x20
#define COMPLEX_CONDCODE_CHKZ_MASK  0x40
#define COMPLEX_CONDCODE_ANDOR_MASK 0x80

#define COMPLEX_CONDCODE_NFLAG		0x08
#define COMPLEX_CONDCODE_ZFLAG		0x04
#define COMPLEX_CONDCODE_CFLAG		0x02
#define COMPLEX_CONDCODE_VFLAG		0x01


debug_armComplexCCTable:
			/* HI,   LS,   GE,   LT,   GT,   LE */
	.byte	 0xE2, 0x64, 0x11, 0x10, 0xD1, 0x54

.code 32
.text
.align 	4

/* 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:
 *          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.
 *
 *	If it is a Manual Breakpoint inserted into the code, then we will need to update the aborted instruction
 *	address to skip the current aborted instruction and resume execution at the next instruction address,
 *  and the next instruction address to be returned to the calling routine is the following instruction
 *  address instead.
 *
 *	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).
 */
	.global dbg_following_instruction_addr
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     r6, r0                                      /* Keep instruction address in R6 */
    _getdbgregister DBGSTACK_USERCPSR_INDEX, r1         /* Retrieve User CPSR into R1 */
    and     r0, r1, #CPSR_THUMB                         /* store Thumb Mode status in R0 */
    mov     r5, r1, lsr #28                             /* store CPSR condition flags in R5[3:0] */

_dbg_get_aborted_instr:
1:  teq     r0, #0                                      /* Check if it is ARM or Thumb instruction */
    ldrneh  r4, [r6]                                    /* Load Thumb instruction opcode using Addr in R6 into R4 */
    ldrne   r2, =(BKPT16_INSTR | BKPT16_MANUAL_BKPT)    /* check for Thumb Manual Breakpoint Instruction */
    ldreq   r4, [r6]                                    /* Load ARM instruction opcode using Addr in R6 into R4 */
    ldreq   r2, =(BKPT32_INSTR | BKPT32_MANUAL_BKPT)    /* check for ARM Manual Breakpoint Instruction */
    teq     r4, r2                                      /* Is instruction opcode (R4) == Manual Breakpoint opcode (R2)? */
    bne     2f                                          /* Not Manual breakpoint */
    teq     r0, #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, R4 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 following instruction is at the address following the address of the opcode in R4 (Default Following Instruction Address in R6).
 */
2:
    teq     r0, #0                                      /* Check if current instruction is ARM or Thumb instruction */
    beq     _following_instr_addr_for_arm
_following_instr_addr_for_thumb:
    add     r6, r6, #2                                  /* Store default following Thumb instruction address to R6 */
#if 0
	/* Flag Thumb instruction only within the instruction handler */
    orr     r6, r6, #BKPT_STATE_THUMB_FLAG              /* Set b0 to indicate Thumb instruction */
#endif
    /* R4: 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_addr_for_arm:
    add     r6, r6, #4                                  /* Store default following ARM instruction address to R6 */
    /* R4: 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:
 *          R4: Opcode of 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}
    bl      _dbg_check_arm_condcode		/* Returns R0: will_execute (boolean) */
    teq     r0, #FALSE
    moveq   r0, r6                      /* If False (don't execute), so use Default Following Instruction Address */
    beq     _exit_eval_arm_instruction	/* and Return to caller */

_will_execute_arm_instr:
    mov     r0, #0                      /* initialize ARM Decode Entry Table index register */
1:
    _dbg_armDecodeEntry r1, r2, r3, r0  /* instrreg (R1), instrmask (R2), codehandler (R3), indexreg (R0) */
    teq     r1, #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, r4, r2                  /* Use R7 to check masked instruction opcode (from R4) to see if it matches template (in R1) */
    teq     r7, r1
    addne   r0, r0, #1                  /* No match, so keep looking */
    bne     1b

_call_arm_code_handler:
    mov     lr, pc
    bx      r3                          /* Call Code Handler with R4: Instruction Opcode, R5[3:0]: CPSR, R6: 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:
 *          R4: Opcode of 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}
	/* Only B<cond> instructions are conditionally executed, deal with it in that Code Handler */
    mov     r0, #0                      /* initialize Thumb Decode Entry Table index register */
1:
    _dbg_thumbDecodeEntry r1, r2, r3, r0  /* instrreg (R1), instrmask (R2), codehandler (R3), indexreg (R0) */
    teq     r1, #0                      /* Check for Null Entry (End of Table marker) */
    moveq   r0, r6                      /* End of Table, no match found, so use Default Following Instruction Address */
    orreq  	r0, r0, #BKPT_STATE_THUMB_FLAG  /* Set R0[0] to flag Thumb mode */
    beq     _exit_eval_thumb_instruction

    and     r7, r4, r2                  /* Use R5 to check masked instruction opcode (from R4) to see if it matches template (in R1) */
    teq     r7, r1
    addne   r0, r0, #1                  /* No match, so keep looking */
    bne     1b

_call_thumb_code_handler:
    mov     lr, pc
    bx      r3                          /* Call Code Handler with R4: Instruction Opcode, R5[3:0]: CPSR, R6: Default Following Instruction Address */
_exit_eval_thumb_instruction:
    /* Returned Following Address Instruction in R0 */
    ldmfd   sp!, {pc}


/****************************************************************************
 *
 * Instruction Decode Routines
 *
 ****************************************************************************/

/* _dbg_check_arm_condcode
 *		Check ARM conditional execution code
 *		On entry:
 *			R4: Opcode of instruction to be executed
 *			R5[3:0]: CPSR condition codes
 *		On exit:
 *			R0: will_execute (boolean)
 *          R1-R3: Destroyed
 */

_dbg_check_arm_condcode:
	mov		r0, #TRUE								/* Default will_execute value */
	mov		r3, r4, lsr #28							/* convert opcode's condition code to index (0-F) */
	ldr		r2, =debug_armCondCodeTable
	ldrb	r1, [r2, r3]							/* Get condition code mask */
/*
 * The following check is unnecessary as it is covered by the _dbg_cond_simple_checks checking algorithm
 	teq		r1, #0
	beq		_dbg_check_arm_condcode_exit
*/
	teq		r1, #0xFF
	bne		_dbg_cond_simple_checks


/*
 * Complex Checks:
 *		We assume that CHKNV and CHKC are mutually exclusive.
 *		In addition, it is possible for CHKNV, CHKC and CHKZ to
 *		be cleared, in which case it'll return True (default)
 *
 *
 * will_execute = TRUE [default condition]
 * If (CHKNV) set
 *		// Only N/V, and Z flags are involved
 *		NEQV_Flag = (N == V)
 *		will_execute = (NEQV_Flag == NEQV_Mask)
 *
 * If (CHKC) set
 *		// Only C and Z flags are involved
 *		will_execute = (C_Flag == CSet_Mask)
 *
 * If (CHKZ) set
 *		z_match = (Z_Flag == ZSet_Mask)
 *		If (AND bit set)
 *			will_execute = will_execute && z_match
 *		else
 *			will_execute = will_execute || z_match
 *
 */
_dbg_cond_complex_checks:
	sub		r3, r3, #COMPLEX_CONDCODE_START			/* Convert complex condition code in R3 to new index (0-3) */
	ldr		r2, =debug_armComplexCCTable
	ldrb	r1, [r2, r3]							/* Get complex condition code bitmap in R1 */

_cond_check_nv:
	tst		r1, #COMPLEX_CONDCODE_CHKNV_MASK
	beq		_cond_check_c							/* CHECKNV not set, so skip */
	ands	r2, r5, #(COMPLEX_CONDCODE_NFLAG | COMPLEX_CONDCODE_VFLAG)	/* Is (N == V == 0)? */
	teqne	r2, #(COMPLEX_CONDCODE_NFLAG | COMPLEX_CONDCODE_VFLAG)		/* No, Is (N == V == 1)? */

	moveq	r2, #COMPLEX_CONDCODE_NEQV_MASK			/* EQ: Either (N == V == 0) or (N == V == 1), set R2: COMPLEX_CONDCODE_NEQV_MASK */
	movne	r2, #0									/* NE: N != V, clear R2 */
	and		r3, r1, #COMPLEX_CONDCODE_NEQV_MASK		/* R3: Extract NEQV Mask Value */
	teq		r2, r3									/* Does N/V Condition match NEQV Mask value? */
	movne	r0, #FALSE								/* No, so will_execute = FALSE (for now) */
	b		_cond_check_z

#if 0
	bne		_cond_nnev								/* No, so (N != V) */

	/* EQ: Either (N == V == 0) or (N == V == 1) */
_cond_neqv:
	tst		r1, #COMPLEX_CONDCODE_NEQV_MASK			/* Is (N == V) mask set? */
	moveq	r0, #FALSE								/* No, so will_execute = FALSE (for now) */
	b		_cond_check_z

	/* Else, N != V */
_cond_nnev:
	tst		r1, #COMPLEX_CONDCODE_NEQV_MASK			/* Is (N == V) mask set? */
	movne	r0, #FALSE								/* Yes, so will_execute = FALSE (for now) */
	b		_cond_check_z
#endif

_cond_check_c:
	tst		r1, #COMPLEX_CONDCODE_CHKC_MASK
	beq		_cond_check_z							/* CHECKC not set, so skip */

	/* Use R2 to store C Flag, R3 to store CSet Mask */
	and		r2, r5, #COMPLEX_CONDCODE_CFLAG			/* r2 = C flag */
	and		r3, r1, #COMPLEX_CONDCODE_CSET_MASK		/* r3 = CSet mask */
	teq		r2, r3									/* Does C flag == CSet mask */
	movne	r0, #FALSE								/* No, so C flag failed match */

_cond_check_z:
	tst		r1, #COMPLEX_CONDCODE_CHKZ_MASK
	beq		_dbg_check_arm_condcode_exit			/* No additional checks needed, exit */

	/* Use R2 to store Z Flag, R3 to store ZSet Mask */
	and		r2, r5, #COMPLEX_CONDCODE_ZFLAG			/* r2 = Z flag */
	and		r3, r1, #COMPLEX_CONDCODE_ZSET_MASK		/* r3 = ZSet mask */
	teq		r2, r3									/* Does Z flag == ZSet mask */
	moveq	r3, #TRUE								/* Zero, so z flag matched */
	movne	r3, #FALSE								/* Non-zero, so z flag failed match */

_cond_andor:
	tst		r1, #COMPLEX_CONDCODE_ANDOR_MASK		/* Is ANDOR mask set? */
	andne	r0, r0, r3								/* Yes, so AND with will_execute */
	orreq	r0, r0, r3								/* No, so OR with will_execute */
	b		_dbg_check_arm_condcode_exit			/* Return will_execute (R0) */

/*
 * Simple Checks:
 *		We take advantage of the fact that only 1 bit would be set
 *		in the bitmask, by generating the corresponding actual
 *		CondSet[7:4], CondClr[3:0] value for comparison.
 *
 * will_execute = TRUE [default condition, equivalent to 0x00 (AL) ]
 * Generate CondSetClr[7:0] from CPSR[3:0]
 * will_execute = ((CondSetClr & BitMask) == BitMask)
 *
 */
_dbg_cond_simple_checks:
	eor		r2, r5, #NIBBLE0						/* R2: CondClr[3:0] = Invert CPSR[3:0] */
	orr		r2, r2, r5, lsl #4						/* R2: CondSet[7:4] | CondClr[3:0] */
	and		r2, r2, r1								/* R2: CondSetClr[7:0] & Bitmask */
	teq		r2, r1									/* ((cond_code & SetBitMask) == SetBitMask)? */
	movne	r0, #FALSE								/* Not equal, check failed */

_dbg_check_arm_condcode_exit:
	bx		lr										/* Return to caller */

/* _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: 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:
    _getdbgregister DBGSTACK_USERCPSR_INDEX, r1	/* Retrieve CPSR contents 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:
 *          R4: Opcode of instruction to be executed
 *          R5[3:0]: CPSR condition codes
 *          R6: Default Following Instruction Address (PC+4)
 *      On exit:
 *          R0: following instruction address
 *          R1-R7: Destroyed
 */
_arm_data_instr_handler:
    stmfd   sp!, {lr}
    ldr     r1, =ARM_DATA_INSTR_MASK
    and     r3, r4, r1                      /* Keep base instruction Opcode in R3 */
    ldr     r1, =ARM_DATA_INSTR_MSRMRS
    teq     r3, 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_check_operand2_type:
    tst     r4, #ARM_DATA_INSTR_IMMREG      /* Check for Immediate (1) or Register (0) Operand 2 */
    beq     _arm_op2_is_reg

_arm_op2_is_imm:
    and     r1, r4, #BYTE0                  /* 8 bit unsigned constant in R1 */
    and     r2, r4, #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, r4, r1                      /* 12 bit register operand in R1 */
    bl      _arm_rmshifted_val              /* R0 contains the Rm shifted val */
    mov     r1, r0                          /* R1: Operand2 val */

_arm_get_operand1_val:
	bl		_dbg_data_instr_retrieve_op1val	/* R0: Register Rn (Operand1) val */

_arm_calc_data_instr_val:
    and     r3, r4, #ARM_DATA_INSTR_NORMAL  /* Mask Instruction Opcode into R3[24:21] */
    lsr     r3, r3, #21                     /* Shift Data Processing Opcode into R3[3:0] */
    /* Calculate data instruction value from R0: Register Rn (Operand1) val, R1: Operand2 val, R5[3:0]: CPSR, R6: Default Next Instr Addr */
    _dbg_jumpTableHandler   debug_dataInstrJumpTable, r2, r3    /* Next Instruction Address in R0 */
_exit_arm_data_instr_handler:
    ldmfd   sp!, {pc}

/* _dbg_data_instr_retrieve_op1val
 * Retrieve Data Instruction Operand 1 value
 * On entry:
 *      R4: Opcode of instruction to be executed
 *      R5[3:0]: CPSR condition codes
 *      R6: Default Next Instruction Address (PC+4)
 * On exit:
 *      R0: Register Rn (Operand 1) value
 *      R2, R3: Destroyed
 *
 */
_dbg_data_instr_retrieve_op1val:
    and     r3, r4, #NIBBLE4             	/* Store Rn (Operand1) Register Enum into R3[19:16] */
    lsr     r3, r3, #16                     /* Shift into R3[3:0] */
    _regenum2index  r3, r2                  /* Convert Enum into Index in R2 */
    _getdbgregisterfromindex r2, r0         /* Retrieve Register contents from Index (R2) into R0 */
    teq     r3, #REG_PC                     /* Check if it is PC relative */
    addeq   r0, r0, #8                      /* R0: Register Rn (Operand1) val; adjust for PC relative (+8) */
	bx		lr

/* Data Processing Instruction Jump Table Routines
 * On entry:
 *      R0: Register Rn (Operand 1) value
 *      R1: Operand 2 value
 *      R5[3:0]: CPSR condition codes
 *      R6: 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 */
    tst     r5, #(CPSR_CFLAG>> 28)          /* R5[3:0] is shifted CPSR value: Test C Flag */
    add     r0, r0, r1
    addne   r0, r0, #1                      /* Add C if set */
    bx      lr

_opcode_sbc:
    /* Op1 - Op2 + C - 1 */
    tst     r5, #(CPSR_CFLAG>> 28)          /* R5[3:0] is shifted CPSR value: Test C Flag */
    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 */
    tst     r5, #(CPSR_CFLAG>> 28)          /* R5[3:0] is shifted CPSR value: Test C Flag */
    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, r6          /* 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:
 *          R4: Opcode of 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,R2: destroyed
 */
_arm_bx_blx_handler:
    stmfd   sp!, {lr}
    and     r2, r4, #NIBBLE0                /* Register Rn Enum in R2 */
    _regenum2index  r2, r1                  /* Convert Enum into Index in R1 */
    _getdbgregisterfromindex r1, r0         /* Retrieve Register contents from Index (R1) into R0 */
	teq		r2, #REG_PC
	addeq	r0, r0, #8						/* Adjust PC relative register value (for BX PC) */
	/* Here, the register value would have B0 set to indicate switch to Thumb mode  */
    ldmfd   sp!, {pc}

/* _arm_ldr_pc_handler
 *      LDR with Rd = PC
 *      On entry:
 *          R4: Opcode of instruction to be executed
 *          R5[3:0]: CPSR condition codes
 *          R6: Default Following Instruction Address (PC+4)
 *      On exit:
 *          R0: following instruction address
 *          R1, R2, R3, R4, R5: destroyed
 */

_arm_ldr_pc_handler:
    stmfd   sp!, {lr}

	mov		r1, #0							/* R1: Post-Indexed Offset (cleared) */
    tst     r4, #ARM_LDR_INSTR_PREPOST      /* Pre (1) or Post (0) Indexed */
	beq		_get_rn_val						/* If Post-Indexed, just use Rn directly */

    /* Pre-Indexed */
    ldr     r0, =(NIBBLE2|BYTE0)
    and     r0, r4, r0                      /* R0: 12 bit Immediate value or Shifted Reg operand */
    tst     r4, #ARM_LDR_INSTR_REGIMM       /* Register (1) or Immediate (0) */
    beq     _calc_ldr_pc_offset             /* Immediate value is already in R0 */

_get_shiftedreg_val:
    bl      _arm_rmshifted_val              /* Convert Rm shifted operand in R0 into value in R0 */

_calc_ldr_pc_offset:
	mov		r1, r0							/* Keep Offset in R1 */
_get_rn_val:
	bl		_dbg_data_instr_retrieve_op1val	/* R0: Register Rn (Operand1) val */
_calc_op1val_with_offset:
    tst     r4, #ARM_LDR_INSTR_UPDOWN       /* Add (1) or Subtract (0) */
    addne   r0, r0, r1                      /* If Add, R0 = Rn + Offset */
    subeq   r0, r0, r1                      /* If Sub, R0 = Rn - Offset */

_get_ldr_pc_val_from_mem:
	ldr		r0, [r0]						/* Retrieve value from Memory at address given in R0 */
    ldmfd   sp!, {pc}

/* _arm_ldm_pc_handler
 *      LDM {pc}
 *      On entry:
 *          R4: Opcode of instruction to be executed
 *          R5[3:0]: CPSR condition codes
 *          R6: Default Following Instruction Address (PC+4)
 *      On exit:
 *          R0: following instruction address
 *          R2, R3: destroyed
 *
 *      Note: The algorithm from eCos arm_stub.c does not deal with the Pre/Post-Indexed addressing (P) bit.
 *			The algorithm here loads different content using LDM based on the value of the P bit.
 */
_arm_ldm_pc_handler:
    stmfd   sp!, {lr}
	bl		_dbg_data_instr_retrieve_op1val	/* R0: Register Rn (Operand1) val */

_arm_get_regcount:
    mov     r2, #0                          /* Initialize reg_count (R2) to 0 */
    mov     r3, r4, 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 */

	/* Pre-Incr:  Rn += reg_count x 4
	 * Post-Incr: Rn += (reg_count - 1) x 4
	 * Pre-Decr:  Rn -= 4
	 * Post-Decr: Rn
	 */

_arm_check_updown_offset:
    tst     r4, #ARM_LDM_INSTR_UPDOWN       /* Check Up (1) or Down (0) */
    beq		_arm_check_prepost_decr

_arm_check_prepost_incr:
    tst     r4, #ARM_LDM_INSTR_PREPOST      /* Check Pre (1) or Post (0) */
    subeq	r2, r2, #1						/* Post-Incr: Decrement reg_count in R2 */
    add		r0, r0, r2, lsl #2              /* Increment Offset: Rn (R0) += reg_count (R2) x 4 */
	b		_get_ldm_pc_val_from_mem

_arm_check_prepost_decr:
    tst     r4, #ARM_LDM_INSTR_PREPOST      /* Check Pre (1) or Post (0) */
	/* Post-Decr: Rn unchanged */
    subne   r0, r0, #4                      /* Pre-Decr: Rn (R0) -= 4 */

_get_ldm_pc_val_from_mem:
    ldr     r0, [r0]                        /* 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:
 *          R4: Opcode of instruction to be executed
 *          R5[3:0]: CPSR condition codes
 *          R6: Default Following Instruction Address (PC+4)
 *      On exit:
 *          R0: following instruction address
 *          R1: destroyed
 */

_arm_b_bl_blx_handler:
    stmfd   sp!, {lr}

_arm_b_bl_blx_get_offset:
    and     r0, r4, #(BYTE2|BYTE1|BYTE0)    /* Encoded Branch offset in R4[23:0] */
    lsl		r0, r0, #(32-24)				/* Shift to R0[31:8] */
    asr		r0, r0, #(32-26)				/* Actual Signed offset = Encode Offset x 4 in R0[25:0] */
	add		r1, r6, #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		r1, r4, #ARM_BLX_INSTR_MASK		/* Mask out Condition Code and Opcode */
	teq		r1, #ARM_BLX_INSTR_BLX			/* Look for BLX */
	bne		_exit_arm_b_bl_blx_handler		/* No, it is a B/BL instruction */
	tst		r4, #ARM_BLX_INSTR_HBIT			/* H bit for Thumb Halfword Address */
	orrne	r0, r0, #0x02					/* Set Halfword Address R0[1] */
    orr   	r0, r0, #BKPT_STATE_THUMB_FLAG  /* Set R0[0] since BLX instr used to switch to Thumb mode */
#endif

_exit_arm_b_bl_blx_handler:
    ldmfd   sp!, {pc}

/* _arm_coproc_swi_handler
 *      SVC (SWI) or Coprocessor instruction
 *      On entry:
 *          R4: Opcode of instruction to be executed
 *          R5[3:0]: CPSR condition codes
 *          R6: Default Following Instruction Address (PC+4)
 *      On exit:
 *          R0: following instruction address
 */

_arm_coproc_swi_handler:
    and     r0, r4, #ARM_SWI_INSTR_MASK
    teq     r0, #ARM_SWI_INSTR_VAL          /* SVC (SWI) instruction */

    ldreq   r0, =SVC_VECTOR                 /* SWI: Return SVC Vector Address */
    movne   r0, r6                          /* CoProc: Use default Following Instruction Address */
_exit_arm_coproc_swi_handler:
	bx	    lr

/* _thumb_bx_blx_handler
 *      BX or BLX Handler. Note: Link (L:b7) is not checked in the mask; armv4t does not support BLX.
 *      On entry:
 *          R4: Opcode of 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, R2: destroyed
 */
_thumb_bx_blx_handler:
    stmfd   sp!, {lr}
    and     r2, r4, #THUMB_BLX_INSTR_REG_RNMASK	/* Register Rn Enum in R2[6:3] (Hi-Reg indicated by B6) */
    mov		r2, r2, lsr #3					/* Shift Rn Enum to R2[3:0] */
    _regenum2index  r2, r1                  /* Convert Enum into Index in R1 */
    _getdbgregisterfromindex r1, r0         /* Retrieve Register contents from Index (R1) into R0 */
	teq		r2, #REG_PC
	addeq	r0, r0, #4						/* Adjust PC relative register value (for BX PC) */
	/* 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:
 *          R4: Opcode of 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-R3: destroyed
 */
_thumb_poppc_handler:
    stmfd   sp!, {lr}

_thumb_get_SP_val:
    _getdbgregister DBGSTACK_USERSP_INDEX, r1	/* Retrieve SP contents into R1 */

_thumb_get_regcount:
    mov     r3, r4, lsl #24					/* Keep BYTE0 containing vector bits in R3[31:24] */
	/* POP is equivalent to LDMFD. Load PC is encoded in b8,
	 * the 8-bit vector is for Lo registers.
	 * This shortens the checking to a max of 8 iterations
	 */
1:  movs    r3, r3, lsl #1                  /* count number of '1' bits */
	addcs	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, #BKPT_STATE_THUMB_FLAG	/* 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:
 *          R4: Opcode of 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-R3: destroyed
 */
_thumb_bcond_swi_handler:
    stmfd   sp!, {lr}
    and     r2, r4, #THUMB_BCOND_SWI_INSTR_CONDMASK		/* Keep Condition Code R2[11:8] */
    teq     r2, #THUMB_BCOND_SWI_INSTR_SWI              /* 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, #THUMB_BCOND_SWI_COND_UNUSED
    moveq   r0, r6                          /* False (don't execute), so use Default Following Instruction Address */
    beq     _exit_thumb_bcond_instr

_thumb_bcond_instr:
	stmfd	sp!, {r4}						/* Preserve Opcode in R4 */
	lsl		r4, 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) */
	ldmfd	sp!, {r4}						/* Restore Opcode in R4 */
    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, #BKPT_STATE_THUMB_FLAG  /* Set R0[0] since it is used to indicates Thumb mode */
_exit_thumb_bcond_swi_handler:
    ldmfd   sp!, {pc}

/* _thumb_b_handler
 *      B
 *      On entry:
 *          R4: Opcode of 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: destroyed
 *		Note: The signed offset is 12-bits (encoded value x 2)
 */
_thumb_b_handler:
    stmfd   sp!, {lr}
    lsl		r0, r4, #(32-11)		/* Shift 11-bit offset in R4[10:0] to R0[31:21] */
    asr		r0, r0, #(32-12)		/* Convert into 12-bit signed offset in R0[11:0] */
    add		r0, r6, r0				/* PC+2 + signed offset */
    add		r0, r0, #2				/* PC+4 + signed offset */
    orr     r0, r0, #BKPT_STATE_THUMB_FLAG /* 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:
 *          R4: Opcode of 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, R2, R3: destroyed
 *			R6: Subseqent Instruction Address (PC+4) if first instruction is valid, else unchanged (PC+2)
 *		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}
_thumb_check_1st_bl_blx_instruction:
	tst		r4, #THUMB_BLX_INSTR_IMM_HBIT	/* Check H bit */
	bne		_return_default_thumb_following_instr	/* H=1 as first instruction shouldn't happen */
_thumb_check_2nd_bl_blx_instruction:
    ldrh	r3, [r6]						/* Get second instruction in pair at PC+2 into R3 */
	add		r6, r6, #2						/* Skip to Subsequent Instruction (PC+4) */
	tst		r3, #THUMB_BLX_INSTR_IMM_HBIT	/* Check H bit */
	beq		_return_default_thumb_following_instr	/* H=0 as second instruction shouldn't happen */

_thumb_concat_branch_offset:
    lsl		r0, r4, #(32-11)			/* Shift first instruction 11-bit offset in R4[10:0] to R0[31:21] */
    asr		r0, r0, #(32-23)			/* Convert into 12-bit signed offset in R0[22:12] */
    lsl		r2, r3, #(32-11)			/* Shift second instruction 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, r6, r0					/* PC+4 + signed offset */

_thumb_check_bl_blx_pair:
    and		r3, r3, #THUMB_BLX_INSTR_IMM_MASK	/* Keep second instruction opcode in R3 */
	teq		r3, #THUMB_BLX_INSTR_IMM_BL			/* Look for BL */
	beq		_flag_thumb_instr_addr				/* Return BL target address in R0 */

#ifndef __ARM6OR7__
	/* v5t or higher architecture */
	teq		r3, #THUMB_BLX_INSTR_IMM_BLX		/* Look for BLX */
	biceq	r0, r0, #0x03						/* Match, Force ARM address */
    beq		_exit_thumb_long_bl_blx_handler
#endif

_return_default_thumb_following_instr:
	/* FIXME: This assumes that once the 4-byte sequence check fails, it will return PC+4,
	 * regardless of whether the second instruction is a valid BL/BLX instruction or not.
	 */
	mov		r0, r6								/* Return default Following/Subsequent Instruction Address  */
_flag_thumb_instr_addr:
    orr		r0, r0, #BKPT_STATE_THUMB_FLAG      /* Set R0[0] since it is used to indicates Thumb mode */

_exit_thumb_long_bl_blx_handler:
    ldmfd   sp!, {pc}

