arm_stub.c

eCos1.31版

                    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<<i)) reg_count++;
                    }                    
                    if (ins & 0x00800000) {
                        // Add offset
                        Rn += reg_count*4;
                    } else {
                        // Subtract offset
                        Rn -= 4;
                    }
                    return ((unsigned long *)*(unsigned long *)Rn);
                }
            }
        } else {
            // Branch
            if (ins_will_execute(ins)) {
                offset = (ins & 0x00FFFFFF) << 2;
                if (ins & 0x00800000) offset |= 0xFC000000;  // sign extend
                new_pc = (unsigned long)(pc+2) + offset;
                return ((unsigned long *)new_pc);
            } else {
                // Falls through
                return (pc+1);
            }
        }
    case 0x3:  // Coprocessor & SWI
        return (pc+1);
    }
}

// FIXME: target_ins also needs to check for CPSR/THUMB being set and
//        set the thumb bit accordingly.

static unsigned long
target_thumb_ins(unsigned long pc, unsigned short ins)
{
    unsigned long new_pc = MAKE_THUMB_ADDR(pc+2); // default is fall-through 
                                        // to next thumb instruction
    unsigned long offset, arm_ins;

    switch ((ins & 0xf000) >> 12) {
    case 0x4:
        // Check for BX
        if ((ins & 0xff87) == 0x4700)
            new_pc = (unsigned long)get_register((ins & 0x00078) >> 3);
        break;
    case 0xd:
        // Bcc
        // Use ARM function to check condition
        arm_ins = ((unsigned long)(ins & 0x0f00)) << 20;
        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 (4byte instruction!)
        // First instruction (bit 11 == 0) holds top-part of offset
        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);
        offset |= (ins & 0x07ff) << 1;
        new_pc = MAKE_THUMB_ADDR((unsigned long)(pc+4) + offset);
        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;
    }
}

/* Clear the single-step state. */

void __clear_single_step (void)
{
    if (ss_saved_pc != 0) {
        // Restore instruction according to type
        if (IS_THUMB_ADDR(ss_saved_pc)) {
            // Thumb instruction
            unsigned long t_pc = UNMAKE_THUMB_ADDR(ss_saved_pc);
            *(unsigned short*)t_pc = ss_saved_thumb_instr;
        } else {
            // ARM instruction
            *(unsigned long*)ss_saved_pc = ss_saved_instr;
        }
        ss_saved_pc = 0;
    }
}

#if !defined(CYGPKG_CYGMON)
void __install_breakpoints (void)
{
//    FIXME();
}

void __clear_breakpoints (void)
{
//    FIXME();
}
#endif // !CYGPKG_CYGMON

/* If the breakpoint we hit is in the breakpoint() instruction, return a
   non-zero value. */

int
__is_breakpoint_function ()
{
    return get_register (PC) == (target_register_t)&CYG_LABEL_NAME(breakinst);
}


/* Skip the current instruction.  Since this is only called by the
   stub when the PC points to a breakpoint or trap instruction,
   we can safely just skip 4. */

void __skipinst (void)
{
    unsigned long pc = get_register(PC);
    unsigned long cpsr = get_register(PS);

    if (cpsr & CPSR_THUMB_ENABLE)
        pc += 2;
    else
        pc += 4;

    put_register(PC, pc);
}

//-----------------------------------------------------------------------
// Thumb-aware GDB interrupt handler.
// This is a brute-force replacement of the ones in hal_stub.c. Need to
// find a better way of handling it... Maybe... Probably only ARM/thumb
// that is this weird.

typedef struct
{
    cyg_uint32 targetAddr;
    union {
        cyg_uint32 arm_instr;
        cyg_uint16 thumb_instr;
    } savedInstr;
} instrBuffer;

static instrBuffer break_buffer;

volatile int cyg_hal_gdb_running_step = 0;

void 
cyg_hal_gdb_interrupt (target_register_t pc)
{
    // Clear flag that we Continued instead of Stepping
    cyg_hal_gdb_running_step = 0;
    // and override existing break? So that a ^C takes effect...
    if (0 != break_buffer.targetAddr)
        cyg_hal_gdb_remove_break( break_buffer.targetAddr );

    if (0 == break_buffer.targetAddr) {
        cyg_uint32 cpsr = get_register(PS);

        if (cpsr & CPSR_THUMB_ENABLE) {
            break_buffer.targetAddr = MAKE_THUMB_ADDR((cyg_uint32)pc);
            break_buffer.savedInstr.thumb_instr = *(cyg_uint16*)pc;
            *(cyg_uint16*)pc = HAL_BREAKINST_THUMB;
        } else {
            break_buffer.targetAddr = (cyg_uint32)pc;
            break_buffer.savedInstr.arm_instr = *(cyg_uint32*)pc;
            *(cyg_uint32*)pc = HAL_BREAKINST_ARM;
        }

        __data_cache(CACHE_FLUSH);
        __instruction_cache(CACHE_FLUSH);
    }
}

void 
cyg_hal_gdb_place_break (target_register_t pc)
{
    // Clear flag that we Continued instead of Stepping
    cyg_hal_gdb_running_step = 0;
    // Unconditionally place an ARM breakpoint not a THUMB; there is
    // no saved regset for get_register in this call:
    if (0 == break_buffer.targetAddr) {
        break_buffer.targetAddr = (cyg_uint32)pc;
        break_buffer.savedInstr.arm_instr = *(cyg_uint32*)pc;
        *(cyg_uint32*)pc = HAL_BREAKINST_ARM;
        
        __data_cache(CACHE_FLUSH);
        __instruction_cache(CACHE_FLUSH);
    }
}

int 
cyg_hal_gdb_remove_break (target_register_t pc)
{
    if ( cyg_hal_gdb_running_step )
        return 0; // Do not remove the break: we must hit it!

    if (pc == UNMAKE_THUMB_ADDR(break_buffer.targetAddr)) {
        if (IS_THUMB_ADDR(break_buffer.targetAddr)) {
            *(cyg_uint16*)pc = break_buffer.savedInstr.thumb_instr;
        } else {
            *(cyg_uint32*)pc = break_buffer.savedInstr.arm_instr;
        }
        break_buffer.targetAddr = 0;

        __data_cache(CACHE_FLUSH);
        __instruction_cache(CACHE_FLUSH);
        return 1;
    }
    return 0;
}

int
cyg_hal_gdb_break_is_set (void)
{
    if (0 != break_buffer.targetAddr) {
        return 1;
    }
    return 0;
}



#endif // CYGDBG_HAL_DEBUG_GDB_INCLUDE_STUBS