From f8c74550cf2a06f62499e4abbcd1c82afb9a3d42 Mon Sep 17 00:00:00 2001 From: Tat-Chee Wan (USM) Date: Tue, 5 Jul 2011 09:37:12 +0800 Subject: refactoring, moved opcode parsing logic to separate file --- Debugger/debug_opcodes.S | 1443 ++++++++++++++++++++++++++++++++++++++++++++++ Debugger/debug_stub.S | 1438 +-------------------------------------------- 2 files changed, 1449 insertions(+), 1432 deletions(-) create mode 100644 Debugger/debug_opcodes.S (limited to 'Debugger') diff --git a/Debugger/debug_opcodes.S b/Debugger/debug_opcodes.S new file mode 100644 index 0000000..e460e83 --- /dev/null +++ b/Debugger/debug_opcodes.S @@ -0,0 +1,1443 @@ +/** @file debug_opcodes.S + * @brief ARM Debugger Opcode Parsing Routines + * + */ + +/* Copyright (C) 2007-2011 the NxOS developers + * + * Module Developed by: TC Wan + * + * See AUTHORS for a full list of the developers. + * + * See COPYING for redistribution license + * + */ +/**************************************************************************** +// Selected Routines from the eCos arm_stub.c related to next instruction address +// determination in ARM processors. + +//======================================================================== +// +// arm_stub.c +// +// Helper functions for stub, generic to all ARM processors +// +//======================================================================== +// ####ECOSGPLCOPYRIGHTBEGIN#### +// ------------------------------------------- +// This file is part of eCos, the Embedded Configurable Operating System. +// Copyright (C) 1998, 1999, 2000, 2001, 2002, 2003 Free Software Foundation, Inc. +// +// eCos is free software; you can redistribute it and/or modify it under +// the terms of the GNU General Public License as published by the Free +// Software Foundation; either version 2 or (at your option) any later +// version. +// +// eCos 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. See the GNU General Public License +// for more details. +// +// You should have received a copy of the GNU General Public License +// along with eCos; if not, write to the Free Software Foundation, Inc., +// 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. +// +// As a special exception, if other files instantiate templates or use +// macros or inline functions from this file, or you compile this file +// and link it with other works to produce a work based on this file, +// this file does not by itself cause the resulting work to be covered by +// the GNU General Public License. However the source code for this file +// must still be made available in accordance with section (3) of the GNU +// General Public License v2. +// +// This exception does not invalidate any other reasons why a work based +// on this file might be covered by the GNU General Public License. +// ------------------------------------------- +// ####ECOSGPLCOPYRIGHTEND#### +//======================================================================== +//#####DESCRIPTIONBEGIN#### +// +// Author(s): Red Hat, gthomas +// Contributors: Red Hat, gthomas, jskov +// Date: 1998-11-26 +// Purpose: +// Description: Helper functions for stub, generic to all ARM processors +// Usage: +// +//####DESCRIPTIONEND#### +// +//======================================================================== + + +static int +ins_will_execute(unsigned long ins) +{ + unsigned long psr = get_register(PS); // condition codes + int res = 0; + switch ((ins & 0xF0000000) >> 28) { + case 0x0: // EQ + res = (psr & PS_Z) != 0; + break; + case 0x1: // NE + res = (psr & PS_Z) == 0; + break; + case 0x2: // CS + res = (psr & PS_C) != 0; + break; + case 0x3: // CC + res = (psr & PS_C) == 0; + break; + case 0x4: // MI + res = (psr & PS_N) != 0; + break; + case 0x5: // PL + res = (psr & PS_N) == 0; + break; + case 0x6: // VS + res = (psr & PS_V) != 0; + break; + case 0x7: // VC + res = (psr & PS_V) == 0; + break; + case 0x8: // HI + res = ((psr & PS_C) != 0) && ((psr & PS_Z) == 0); + break; + case 0x9: // LS + res = ((psr & PS_C) == 0) || ((psr & PS_Z) != 0); + break; + case 0xA: // GE + res = ((psr & (PS_N|PS_V)) == (PS_N|PS_V)) || + ((psr & (PS_N|PS_V)) == 0); + break; + case 0xB: // LT + res = ((psr & (PS_N|PS_V)) == PS_N) || + ((psr & (PS_N|PS_V)) == PS_V); + break; + case 0xC: // GT + res = ((psr & (PS_N|PS_V)) == (PS_N|PS_V)) || + ((psr & (PS_N|PS_V)) == 0); + res = ((psr & PS_Z) == 0) && res; + break; + case 0xD: // LE + res = ((psr & (PS_N|PS_V)) == PS_N) || + ((psr & (PS_N|PS_V)) == PS_V); + res = ((psr & PS_Z) == PS_Z) || res; + break; + case 0xE: // AL + res = TRUE; + break; + case 0xF: // NV + if (((ins & 0x0E000000) >> 24) == 0xA) + res = TRUE; + else + res = FALSE; + break; + } + return res; +} + +static unsigned long +RmShifted(int shift) +{ + unsigned long Rm = get_register(shift & 0x00F); + int shift_count; + if ((shift & 0x010) == 0) { + shift_count = (shift & 0xF80) >> 7; + } else { + shift_count = get_register((shift & 0xF00) >> 8); + } + switch ((shift & 0x060) >> 5) { + case 0x0: // Logical left + Rm <<= shift_count; + break; + case 0x1: // Logical right + Rm >>= shift_count; + break; + case 0x2: // Arithmetic right + Rm = (unsigned long)((long)Rm >> shift_count); + break; + case 0x3: // Rotate right + if (shift_count == 0) { + // Special case, RORx + Rm >>= 1; + if (get_register(PS) & PS_C) Rm |= 0x80000000; + } else { + Rm = (Rm >> shift_count) | (Rm << (32-shift_count)); + } + break; + } + return Rm; +} + +// Decide the next instruction to be executed for a given instruction +static unsigned long * +target_ins(unsigned long *pc, unsigned long ins) +{ + unsigned long new_pc, offset, op2; + unsigned long Rn; + int i, reg_count, c; + + switch ((ins & 0x0C000000) >> 26) { + case 0x0: + // BX or BLX + if ((ins & 0x0FFFFFD0) == 0x012FFF10) { + new_pc = (unsigned long)get_register(ins & 0x0000000F); + return ((unsigned long *)new_pc); + } + // Data processing + new_pc = (unsigned long)(pc+1); + if ((ins & 0x0000F000) == 0x0000F000) { + // Destination register is PC + if ((ins & 0x0FBF0000) != 0x010F0000) { + Rn = (unsigned long)get_register((ins & 0x000F0000) >> 16); + if ((ins & 0x000F0000) == 0x000F0000) Rn += 8; // PC prefetch! + if ((ins & 0x02000000) == 0) { + op2 = RmShifted(ins & 0x00000FFF); + } else { + op2 = ins & 0x000000FF; + i = (ins & 0x00000F00) >> 8; // Rotate count + op2 = (op2 >> (i*2)) | (op2 << (32-(i*2))); + } + switch ((ins & 0x01E00000) >> 21) { + case 0x0: // AND + new_pc = Rn & op2; + break; + case 0x1: // EOR + new_pc = Rn ^ op2; + break; + case 0x2: // SUB + new_pc = Rn - op2; + break; + case 0x3: // RSB + new_pc = op2 - Rn; + break; + case 0x4: // ADD + new_pc = Rn + op2; + break; + case 0x5: // ADC + c = (get_register(PS) & PS_C) != 0; + new_pc = Rn + op2 + c; + break; + case 0x6: // SBC + c = (get_register(PS) & PS_C) != 0; + new_pc = Rn - op2 + c - 1; + break; + case 0x7: // RSC + c = (get_register(PS) & PS_C) != 0; + new_pc = op2 - Rn +c - 1; + break; + case 0x8: // TST + case 0x9: // TEQ + case 0xA: // CMP + case 0xB: // CMN + break; // PC doesn't change + case 0xC: // ORR + new_pc = Rn | op2; + break; + case 0xD: // MOV + new_pc = op2; + break; + case 0xE: // BIC + new_pc = Rn & ~op2; + break; + case 0xF: // MVN + new_pc = ~op2; + break; + } + } + } + return ((unsigned long *)new_pc); + case 0x1: + if ((ins & 0x02000010) == 0x02000010) { + // Undefined! + return (pc+1); + } else { + if ((ins & 0x00100000) == 0) { + // STR + return (pc+1); + } else { + // LDR + if ((ins & 0x0000F000) != 0x0000F000) { + // Rd not PC + return (pc+1); + } else { + Rn = (unsigned long)get_register((ins & 0x000F0000) >> 16); + if ((ins & 0x000F0000) == 0x000F0000) Rn += 8; // PC prefetch! + if (ins & 0x01000000) { + // Add/subtract offset before + if ((ins & 0x02000000) == 0) { + // Immediate offset + if (ins & 0x00800000) { + // Add offset + Rn += (ins & 0x00000FFF); + } else { + // Subtract offset + Rn -= (ins & 0x00000FFF); + } + } else { + // Offset is in a register + if (ins & 0x00800000) { + // Add offset + Rn += RmShifted(ins & 0x00000FFF); + } else { + // Subtract offset + Rn -= RmShifted(ins & 0x00000FFF); + } + } + } + return ((unsigned long *)*(unsigned long *)Rn); + } + } + } + return (pc+1); + case 0x2: // Branch, LDM/STM + if ((ins & 0x02000000) == 0) { + // LDM/STM + if ((ins & 0x00100000) == 0) { + // STM + return (pc+1); + } else { + // LDM + if ((ins & 0x00008000) == 0) { + // PC not in list + return (pc+1); + } else { + Rn = (unsigned long)get_register((ins & 0x000F0000) >> 16); + if ((ins & 0x000F0000) == 0x000F0000) Rn += 8; // PC prefetch! + offset = ins & 0x0000FFFF; + reg_count = 0; + for (i = 0; i < 15; i++) { + if (offset & (1<> 12) { + case 0x4: + // Check for BX or BLX + if ((ins & 0xff07) == 0x4700) + new_pc = (unsigned long)get_register((ins & 0x00078) >> 3); + break; + case 0xb: + // push/pop + // Look for "pop {...,pc}" + if ((ins & 0xf00) == 0xd00) { + // find PC + sp = (unsigned long)get_register(SP); + + for (offset = i = 0; i < 8; i++) + if (ins & (1 << i)) + offset += 4; + + new_pc = *(cyg_uint32 *)(sp + offset); + + if (!v5T_semantics()) + new_pc = MAKE_THUMB_ADDR(new_pc); + } + break; + case 0xd: + // Bcc | SWI + // Use ARM function to check condition + arm_ins = ((unsigned long)(ins & 0x0f00)) << 20; + if ((arm_ins & 0xF0000000) == 0xF0000000) { + // SWI + new_pc = CYGNUM_HAL_VECTOR_SOFTWARE_INTERRUPT * 4; + } else if (ins_will_execute(arm_ins)) { + offset = (ins & 0x00FF) << 1; + if (ins & 0x0080) offset |= 0xFFFFFE00; // sign extend + new_pc = MAKE_THUMB_ADDR((unsigned long)(pc+4) + offset); + } + break; + case 0xe: + // check for B + if ((ins & 0x0800) == 0) { + offset = (ins & 0x07FF) << 1; + if (ins & 0x0400) offset |= 0xFFFFF800; // sign extend + new_pc = MAKE_THUMB_ADDR((unsigned long)(pc+4) + offset); + } + break; + case 0xf: + // BL/BLX (4byte instruction!) + // First instruction (bit 11 == 0) holds top-part of offset + if ((ins & 0x0800) == 0) { + offset = (ins & 0x07FF) << 12; + if (ins & 0x0400) offset |= 0xFF800000; // sign extend + // Get second instruction + // Second instruction (bit 11 == 1) holds bottom-part of offset + ins = *(unsigned short*)(pc+2); + // Check for BL/BLX + if ((ins & 0xE800) == 0xE800) { + offset |= (ins & 0x07ff) << 1; + new_pc = (unsigned long)(pc+4) + offset; + // If its BLX, force a full word alignment + // Otherwise, its a thumb address. + if (!(ins & 0x1000)) + new_pc &= ~3; + else + new_pc = MAKE_THUMB_ADDR(new_pc); + } + } + break; + } + + return new_pc; +} + +void __single_step (void) +{ + unsigned long pc = get_register(PC); + unsigned long cpsr = get_register(PS); + + // Calculate address of next instruction to be executed + if (cpsr & CPSR_THUMB_ENABLE) { + // thumb + ss_saved_pc = target_thumb_ins(pc, *(unsigned short*)pc); + } else { + // ARM + unsigned long curins = *(unsigned long*)pc; + if (ins_will_execute(curins)) { + // Decode instruction to decide what the next PC will be + ss_saved_pc = (unsigned long) target_ins((unsigned long*)pc, + curins); + } else { + // The current instruction will not execute (the conditions + // don't hold) + ss_saved_pc = pc+4; + } + } + + // Set breakpoint according to type + if (IS_THUMB_ADDR(ss_saved_pc)) { + // Thumb instruction + unsigned long t_pc = UNMAKE_THUMB_ADDR(ss_saved_pc); + ss_saved_thumb_instr = *(unsigned short*)t_pc; + *(unsigned short*)t_pc = HAL_BREAKINST_THUMB; + } else { + // ARM instruction + ss_saved_instr = *(unsigned long*)ss_saved_pc; + *(unsigned long*)ss_saved_pc = HAL_BREAKINST_ARM; + } +} + + ****************************************************************************/ + +#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) + */ + +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: Link (L:b7) is not checked 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 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) + * - - - - - 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, 0x13, 0x14 + +.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 it is ARM or Thumb instruction */ + beq _following_instr_is_arm +_following_instr_is_thumb: + add r6, r6, #2 /* Store default following Thumb instruction address to R6 */ + orr r6, r6, #BKPT_STATE_THUMB_FLAG /* Set b0 to indicate Thumb instruction */ + /* 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_is_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 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 */ + 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 + mov r3, r4, lsr #28 /* convert 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 set/clear checking algorithm + teq r1, #0 + beq _dbg_check_arm_condcode_exit +*/ + teq r1, #0xFF + bne _dbg_check_bits_set + + +/* + * Complex Checks: + * + * will_execute = TRUE [default condition] + * If (N == V) bit set + * will_execute = (N == V) + * else + * will_execute = (N != V) + * + * If (ANDOR bit) set + * z_match = ~(Z XOR Z set) + * If (AND bit set) + * will_execute = will_execute && z_match + * else + * will_execute = will_execute || z_match + */ +_dbg_cond_complex_check: + 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 */ + + 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)? */ + 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_andor + + /* 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) */ + +_cond_check_andor: + tst r1, #COMPLEX_CONDCODE_ANDOR_MASK + beq _dbg_check_arm_condcode_exit /* No additional checks needed, exit */ + + /* Use R2 to store Z Flag, R3 to store Z set Mask */ + and r2, r5, #COMPLEX_CONDCODE_ZFLAG /* r2 = Z flag */ + and r3, r1, #COMPLEX_CONDCODE_ZSET_MASK /* r3 = Z set */ + eors r2, r2, r3 /* r2 = (Z xor Z set): 0 if matched, non-zero if failed match */ + moveq r3, #TRUE /* Zero, so z flag matched */ + movne r3, #FALSE /* Non-zero, so z flag failed match */ + + tst r1, #COMPLEX_CONDCODE_AND_MASK /* Is AND 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: + * + * will_execute = TRUE [default condition, equivalent to 0x00 (AL) ] + * If (SetBitMask is Non-Zero) + * will_execute = ((cond_code & SetBitMask) == SetBitMask) + * If will_execute && (ClearBitMask is Non-Zero) + * will_execute = will_execute && ((cond_code & ClearBitMask) == 0) + */ + +_dbg_check_bits_set: + movs r2, r1, lsr #4 /* R2: bits set */ + beq _dbg_check_bits_clear /* No bits set mask enabled, skip check */ + and r3, r5, r2 /* Check bits set IF bitmask non-zero */ + teq r2, r3 /* ((cond_code & SetBitMask) == SetBitMask)? */ + movne r0, #FALSE /* No, so will_execute = FALSE */ + bne _dbg_check_arm_condcode_exit /* Check failed (need to be TRUE to check bits clear), return */ + +_dbg_check_bits_clear: + ands r1, r1, #NIBBLE0 /* R1: bits clear */ + beq _dbg_check_arm_condcode_exit + ands r3, r5, r1 /* Check bits clear IF bitmask non-zero */ + movne r0, #FALSE /* (cond_code & ClearBitMask) != 0, so will_execute = FALSE */ + +_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: destroyed + */ +_arm_bx_blx_handler: + stmfd sp!, {lr} + and r0, r4, #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: + * 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 . 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-25) /* Actual Signed offset = Encode Offset x 2 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, #0x01 /* 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: destroyed + */ +_thumb_bx_blx_handler: + stmfd sp!, {lr} + and r0, r4, #THUMB_BLX_INSTR_REG_RNMASK /* 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: + * 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 r2, #0 /* Initialize reg_count (R2) to 0 */ + mov r3, r4, 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 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, #0x01 /* 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, #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: + * 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, #0x01 /* Set R0[0] since it is used to indicates Thumb mode */ + +_exit_thumb_long_bl_blx_handler: + ldmfd sp!, {pc} + diff --git a/Debugger/debug_stub.S b/Debugger/debug_stub.S index e03896c..a0797c6 100644 --- a/Debugger/debug_stub.S +++ b/Debugger/debug_stub.S @@ -144,6 +144,9 @@ #include "debug_internals.h" #include "debug_macros.h" + /* Opcode Parser function reference */ + .extern dbg_following_instruction_addr + /* Hexutils function references */ .extern hex2char .extern char2hex @@ -280,158 +283,6 @@ 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 - * 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: Link (L:b7) is not checked 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 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) - * - - - - - 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, 0x13, 0x14 - .code 32 .text .align 4 @@ -1252,14 +1103,10 @@ _dbg__cont_check_breakpoint_type: 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. -*/ _getdbgregister DBGSTACK_USERPC_INDEX, r0 /* Retrieve Aborted Instruction PC from the Debug Stack into R0 */ _setdbgregister DBGSTACK_NEXTINSTR_INDEX, r0, r1 /* Set Next Instruction Pointer for Resume using contents of R0, 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 */ + 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 @@ -1308,10 +1155,6 @@ _dbg__step_check_breakpoint_type: beq _dbg__step_is_manual_bkpt _dbg__step_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. -*/ _getdbgregister DBGSTACK_USERPC_INDEX, r0 /* Retrieve Aborted Instruction PC from the Debug Stack into R0 */ _setdbgregister DBGSTACK_NEXTINSTR_INDEX, r0, r1 /* Set Next Instruction Pointer for Resume usinh contents in R0, and scratch register R1 */ b _dbg__step_is_manual_bkpt_or_address_specified @@ -1321,7 +1164,7 @@ _dbg__step_is_manual_bkpt: /* b _dbg__step_is_manual_bkpt_or_address_specified */ _dbg__step_is_manual_bkpt_or_address_specified: - bl _dbg_following_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 #if 0 @@ -1440,1275 +1283,6 @@ _dbg__cmd_RemoveBreakpoint: bl _dbg__clear_one_breakpoint /* R0: address of breakpoint to clear */ b __dbg__procCmdOk - - - -/**************************************************************************** -// Selected Routines from the eCos arm_stub.c related to next instruction address -// determination in ARM processors. - -//======================================================================== -// -// arm_stub.c -// -// Helper functions for stub, generic to all ARM processors -// -//======================================================================== -// ####ECOSGPLCOPYRIGHTBEGIN#### -// ------------------------------------------- -// This file is part of eCos, the Embedded Configurable Operating System. -// Copyright (C) 1998, 1999, 2000, 2001, 2002, 2003 Free Software Foundation, Inc. -// -// eCos is free software; you can redistribute it and/or modify it under -// the terms of the GNU General Public License as published by the Free -// Software Foundation; either version 2 or (at your option) any later -// version. -// -// eCos 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. See the GNU General Public License -// for more details. -// -// You should have received a copy of the GNU General Public License -// along with eCos; if not, write to the Free Software Foundation, Inc., -// 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. -// -// As a special exception, if other files instantiate templates or use -// macros or inline functions from this file, or you compile this file -// and link it with other works to produce a work based on this file, -// this file does not by itself cause the resulting work to be covered by -// the GNU General Public License. However the source code for this file -// must still be made available in accordance with section (3) of the GNU -// General Public License v2. -// -// This exception does not invalidate any other reasons why a work based -// on this file might be covered by the GNU General Public License. -// ------------------------------------------- -// ####ECOSGPLCOPYRIGHTEND#### -//======================================================================== -//#####DESCRIPTIONBEGIN#### -// -// Author(s): Red Hat, gthomas -// Contributors: Red Hat, gthomas, jskov -// Date: 1998-11-26 -// Purpose: -// Description: Helper functions for stub, generic to all ARM processors -// Usage: -// -//####DESCRIPTIONEND#### -// -//======================================================================== - - -static int -ins_will_execute(unsigned long ins) -{ - unsigned long psr = get_register(PS); // condition codes - int res = 0; - switch ((ins & 0xF0000000) >> 28) { - case 0x0: // EQ - res = (psr & PS_Z) != 0; - break; - case 0x1: // NE - res = (psr & PS_Z) == 0; - break; - case 0x2: // CS - res = (psr & PS_C) != 0; - break; - case 0x3: // CC - res = (psr & PS_C) == 0; - break; - case 0x4: // MI - res = (psr & PS_N) != 0; - break; - case 0x5: // PL - res = (psr & PS_N) == 0; - break; - case 0x6: // VS - res = (psr & PS_V) != 0; - break; - case 0x7: // VC - res = (psr & PS_V) == 0; - break; - case 0x8: // HI - res = ((psr & PS_C) != 0) && ((psr & PS_Z) == 0); - break; - case 0x9: // LS - res = ((psr & PS_C) == 0) || ((psr & PS_Z) != 0); - break; - case 0xA: // GE - res = ((psr & (PS_N|PS_V)) == (PS_N|PS_V)) || - ((psr & (PS_N|PS_V)) == 0); - break; - case 0xB: // LT - res = ((psr & (PS_N|PS_V)) == PS_N) || - ((psr & (PS_N|PS_V)) == PS_V); - break; - case 0xC: // GT - res = ((psr & (PS_N|PS_V)) == (PS_N|PS_V)) || - ((psr & (PS_N|PS_V)) == 0); - res = ((psr & PS_Z) == 0) && res; - break; - case 0xD: // LE - res = ((psr & (PS_N|PS_V)) == PS_N) || - ((psr & (PS_N|PS_V)) == PS_V); - res = ((psr & PS_Z) == PS_Z) || res; - break; - case 0xE: // AL - res = TRUE; - break; - case 0xF: // NV - if (((ins & 0x0E000000) >> 24) == 0xA) - res = TRUE; - else - res = FALSE; - break; - } - return res; -} - -static unsigned long -RmShifted(int shift) -{ - unsigned long Rm = get_register(shift & 0x00F); - int shift_count; - if ((shift & 0x010) == 0) { - shift_count = (shift & 0xF80) >> 7; - } else { - shift_count = get_register((shift & 0xF00) >> 8); - } - switch ((shift & 0x060) >> 5) { - case 0x0: // Logical left - Rm <<= shift_count; - break; - case 0x1: // Logical right - Rm >>= shift_count; - break; - case 0x2: // Arithmetic right - Rm = (unsigned long)((long)Rm >> shift_count); - break; - case 0x3: // Rotate right - if (shift_count == 0) { - // Special case, RORx - Rm >>= 1; - if (get_register(PS) & PS_C) Rm |= 0x80000000; - } else { - Rm = (Rm >> shift_count) | (Rm << (32-shift_count)); - } - break; - } - return Rm; -} - -// Decide the next instruction to be executed for a given instruction -static unsigned long * -target_ins(unsigned long *pc, unsigned long ins) -{ - unsigned long new_pc, offset, op2; - unsigned long Rn; - int i, reg_count, c; - - switch ((ins & 0x0C000000) >> 26) { - case 0x0: - // BX or BLX - if ((ins & 0x0FFFFFD0) == 0x012FFF10) { - new_pc = (unsigned long)get_register(ins & 0x0000000F); - return ((unsigned long *)new_pc); - } - // Data processing - new_pc = (unsigned long)(pc+1); - if ((ins & 0x0000F000) == 0x0000F000) { - // Destination register is PC - if ((ins & 0x0FBF0000) != 0x010F0000) { - Rn = (unsigned long)get_register((ins & 0x000F0000) >> 16); - if ((ins & 0x000F0000) == 0x000F0000) Rn += 8; // PC prefetch! - if ((ins & 0x02000000) == 0) { - op2 = RmShifted(ins & 0x00000FFF); - } else { - op2 = ins & 0x000000FF; - i = (ins & 0x00000F00) >> 8; // Rotate count - op2 = (op2 >> (i*2)) | (op2 << (32-(i*2))); - } - switch ((ins & 0x01E00000) >> 21) { - case 0x0: // AND - new_pc = Rn & op2; - break; - case 0x1: // EOR - new_pc = Rn ^ op2; - break; - case 0x2: // SUB - new_pc = Rn - op2; - break; - case 0x3: // RSB - new_pc = op2 - Rn; - break; - case 0x4: // ADD - new_pc = Rn + op2; - break; - case 0x5: // ADC - c = (get_register(PS) & PS_C) != 0; - new_pc = Rn + op2 + c; - break; - case 0x6: // SBC - c = (get_register(PS) & PS_C) != 0; - new_pc = Rn - op2 + c - 1; - break; - case 0x7: // RSC - c = (get_register(PS) & PS_C) != 0; - new_pc = op2 - Rn +c - 1; - break; - case 0x8: // TST - case 0x9: // TEQ - case 0xA: // CMP - case 0xB: // CMN - break; // PC doesn't change - case 0xC: // ORR - new_pc = Rn | op2; - break; - case 0xD: // MOV - new_pc = op2; - break; - case 0xE: // BIC - new_pc = Rn & ~op2; - break; - case 0xF: // MVN - new_pc = ~op2; - break; - } - } - } - return ((unsigned long *)new_pc); - case 0x1: - if ((ins & 0x02000010) == 0x02000010) { - // Undefined! - return (pc+1); - } else { - if ((ins & 0x00100000) == 0) { - // STR - return (pc+1); - } else { - // LDR - if ((ins & 0x0000F000) != 0x0000F000) { - // Rd not PC - return (pc+1); - } else { - Rn = (unsigned long)get_register((ins & 0x000F0000) >> 16); - if ((ins & 0x000F0000) == 0x000F0000) Rn += 8; // PC prefetch! - if (ins & 0x01000000) { - // Add/subtract offset before - if ((ins & 0x02000000) == 0) { - // Immediate offset - if (ins & 0x00800000) { - // Add offset - Rn += (ins & 0x00000FFF); - } else { - // Subtract offset - Rn -= (ins & 0x00000FFF); - } - } else { - // Offset is in a register - if (ins & 0x00800000) { - // Add offset - Rn += RmShifted(ins & 0x00000FFF); - } else { - // Subtract offset - Rn -= RmShifted(ins & 0x00000FFF); - } - } - } - return ((unsigned long *)*(unsigned long *)Rn); - } - } - } - return (pc+1); - case 0x2: // Branch, LDM/STM - if ((ins & 0x02000000) == 0) { - // LDM/STM - if ((ins & 0x00100000) == 0) { - // STM - return (pc+1); - } else { - // LDM - if ((ins & 0x00008000) == 0) { - // PC not in list - return (pc+1); - } else { - Rn = (unsigned long)get_register((ins & 0x000F0000) >> 16); - if ((ins & 0x000F0000) == 0x000F0000) Rn += 8; // PC prefetch! - offset = ins & 0x0000FFFF; - reg_count = 0; - for (i = 0; i < 15; i++) { - if (offset & (1<> 12) { - case 0x4: - // Check for BX or BLX - if ((ins & 0xff07) == 0x4700) - new_pc = (unsigned long)get_register((ins & 0x00078) >> 3); - break; - case 0xb: - // push/pop - // Look for "pop {...,pc}" - if ((ins & 0xf00) == 0xd00) { - // find PC - sp = (unsigned long)get_register(SP); - - for (offset = i = 0; i < 8; i++) - if (ins & (1 << i)) - offset += 4; - - new_pc = *(cyg_uint32 *)(sp + offset); - - if (!v5T_semantics()) - new_pc = MAKE_THUMB_ADDR(new_pc); - } - break; - case 0xd: - // Bcc | SWI - // Use ARM function to check condition - arm_ins = ((unsigned long)(ins & 0x0f00)) << 20; - if ((arm_ins & 0xF0000000) == 0xF0000000) { - // SWI - new_pc = CYGNUM_HAL_VECTOR_SOFTWARE_INTERRUPT * 4; - } else if (ins_will_execute(arm_ins)) { - offset = (ins & 0x00FF) << 1; - if (ins & 0x0080) offset |= 0xFFFFFE00; // sign extend - new_pc = MAKE_THUMB_ADDR((unsigned long)(pc+4) + offset); - } - break; - case 0xe: - // check for B - if ((ins & 0x0800) == 0) { - offset = (ins & 0x07FF) << 1; - if (ins & 0x0400) offset |= 0xFFFFF800; // sign extend - new_pc = MAKE_THUMB_ADDR((unsigned long)(pc+4) + offset); - } - break; - case 0xf: - // BL/BLX (4byte instruction!) - // First instruction (bit 11 == 0) holds top-part of offset - if ((ins & 0x0800) == 0) { - offset = (ins & 0x07FF) << 12; - if (ins & 0x0400) offset |= 0xFF800000; // sign extend - // Get second instruction - // Second instruction (bit 11 == 1) holds bottom-part of offset - ins = *(unsigned short*)(pc+2); - // Check for BL/BLX - if ((ins & 0xE800) == 0xE800) { - offset |= (ins & 0x07ff) << 1; - new_pc = (unsigned long)(pc+4) + offset; - // If its BLX, force a full word alignment - // Otherwise, its a thumb address. - if (!(ins & 0x1000)) - new_pc &= ~3; - else - new_pc = MAKE_THUMB_ADDR(new_pc); - } - } - break; - } - - return new_pc; -} - -void __single_step (void) -{ - unsigned long pc = get_register(PC); - unsigned long cpsr = get_register(PS); - - // Calculate address of next instruction to be executed - if (cpsr & CPSR_THUMB_ENABLE) { - // thumb - ss_saved_pc = target_thumb_ins(pc, *(unsigned short*)pc); - } else { - // ARM - unsigned long curins = *(unsigned long*)pc; - if (ins_will_execute(curins)) { - // Decode instruction to decide what the next PC will be - ss_saved_pc = (unsigned long) target_ins((unsigned long*)pc, - curins); - } else { - // The current instruction will not execute (the conditions - // don't hold) - ss_saved_pc = pc+4; - } - } - - // Set breakpoint according to type - if (IS_THUMB_ADDR(ss_saved_pc)) { - // Thumb instruction - unsigned long t_pc = UNMAKE_THUMB_ADDR(ss_saved_pc); - ss_saved_thumb_instr = *(unsigned short*)t_pc; - *(unsigned short*)t_pc = HAL_BREAKINST_THUMB; - } else { - // ARM instruction - ss_saved_instr = *(unsigned long*)ss_saved_pc; - *(unsigned long*)ss_saved_pc = HAL_BREAKINST_ARM; - } -} - - ****************************************************************************/ - -/* _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). - */ -_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 it is ARM or Thumb instruction */ - beq _following_instr_is_arm -_following_instr_is_thumb: - add r6, r6, #2 /* Store default following Thumb instruction address to R6 */ - orr r6, r6, #BKPT_STATE_THUMB_FLAG /* Set b0 to indicate Thumb instruction */ - /* 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_is_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 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 */ - 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 - mov r3, r4, lsr #28 /* convert 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 set/clear checking algorithm - teq r1, #0 - beq _dbg_check_arm_condcode_exit -*/ - teq r1, #0xFF - bne _dbg_check_bits_set - - -/* - * Complex Checks: - * - * will_execute = TRUE [default condition] - * If (N == V) bit set - * will_execute = (N == V) - * else - * will_execute = (N != V) - * - * If (ANDOR bit) set - * z_match = ~(Z XOR Z set) - * If (AND bit set) - * will_execute = will_execute && z_match - * else - * will_execute = will_execute || z_match - */ -_dbg_cond_complex_check: - 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 */ - - 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)? */ - 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_andor - - /* 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) */ - -_cond_check_andor: - tst r1, #COMPLEX_CONDCODE_ANDOR_MASK - beq _dbg_check_arm_condcode_exit /* No additional checks needed, exit */ - - /* Use R2 to store Z Flag, R3 to store Z set Mask */ - and r2, r5, #COMPLEX_CONDCODE_ZFLAG /* r2 = Z flag */ - and r3, r1, #COMPLEX_CONDCODE_ZSET_MASK /* r3 = Z set */ - eors r2, r2, r3 /* r2 = (Z xor Z set): 0 if matched, non-zero if failed match */ - moveq r3, #TRUE /* Zero, so z flag matched */ - movne r3, #FALSE /* Non-zero, so z flag failed match */ - - tst r1, #COMPLEX_CONDCODE_AND_MASK /* Is AND 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: - * - * will_execute = TRUE [default condition, equivalent to 0x00 (AL) ] - * If (SetBitMask is Non-Zero) - * will_execute = ((cond_code & SetBitMask) == SetBitMask) - * If will_execute && (ClearBitMask is Non-Zero) - * will_execute = will_execute && ((cond_code & ClearBitMask) == 0) - */ - -_dbg_check_bits_set: - movs r2, r1, lsr #4 /* R2: bits set */ - beq _dbg_check_bits_clear /* No bits set mask enabled, skip check */ - and r3, r5, r2 /* Check bits set IF bitmask non-zero */ - teq r2, r3 /* ((cond_code & SetBitMask) == SetBitMask)? */ - movne r0, #FALSE /* No, so will_execute = FALSE */ - bne _dbg_check_arm_condcode_exit /* Check failed (need to be TRUE to check bits clear), return */ - -_dbg_check_bits_clear: - ands r1, r1, #NIBBLE0 /* R1: bits clear */ - beq _dbg_check_arm_condcode_exit - ands r3, r5, r1 /* Check bits clear IF bitmask non-zero */ - movne r0, #FALSE /* (cond_code & ClearBitMask) != 0, so will_execute = FALSE */ - -_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: destroyed - */ -_arm_bx_blx_handler: - stmfd sp!, {lr} - and r0, r4, #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: - * 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 . 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-25) /* Actual Signed offset = Encode Offset x 2 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, #0x01 /* 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: destroyed - */ -_thumb_bx_blx_handler: - stmfd sp!, {lr} - and r0, r4, #THUMB_BLX_INSTR_REG_RNMASK /* 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: - * 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 r2, #0 /* Initialize reg_count (R2) to 0 */ - mov r3, r4, 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 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, #0x01 /* 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, #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: - * 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, #0x01 /* Set R0[0] since it is used to indicates Thumb mode */ - -_exit_thumb_long_bl_blx_handler: - ldmfd sp!, {pc} - - /**************************************************************************** * * Breakpoint Manipulation Routines -- cgit v1.2.3