diff --git a/src/target/armv7m.c b/src/target/armv7m.c index 233fb959c..c172a27dc 100644 --- a/src/target/armv7m.c +++ b/src/target/armv7m.c @@ -694,6 +694,44 @@ int armv7m_blank_check_memory(struct target *target, return ERROR_OK; } +int armv7m_maybe_skip_bkpt_inst(struct target *target, bool *inst_found) +{ + struct armv7m_common *armv7m = target_to_armv7m(target); + struct reg *r = armv7m->core_cache->reg_list + 15; + bool result = false; + + + /* if we halted last time due to a bkpt instruction + * then we have to manually step over it, otherwise + * the core will break again */ + + if (target->debug_reason == DBG_REASON_BREAKPOINT) + { + uint16_t op; + uint32_t pc = buf_get_u32(r->value, 0, 32); + + pc &= ~1; + if (target_read_u16(target, pc, &op) == ERROR_OK) + { + if ((op & 0xFF00) == 0xBE00) + { + pc = buf_get_u32(r->value, 0, 32) + 2; + buf_set_u32(r->value, 0, 32, pc); + r->dirty = true; + r->valid = true; + result = true; + LOG_DEBUG("Skipping over BKPT instruction"); + } + } + } + + if (inst_found) { + *inst_found = result; + } + + return ERROR_OK; +} + /*--------------------------------------------------------------------------*/ /* diff --git a/src/target/armv7m.h b/src/target/armv7m.h index 86caae21b..9787e3054 100644 --- a/src/target/armv7m.h +++ b/src/target/armv7m.h @@ -171,6 +171,8 @@ int armv7m_checksum_memory(struct target *target, int armv7m_blank_check_memory(struct target *target, uint32_t address, uint32_t count, uint32_t* blank); +int armv7m_maybe_skip_bkpt_inst(struct target *target, bool *inst_found); + extern const struct command_registration armv7m_command_handlers[]; #endif /* ARMV7M_H */ diff --git a/src/target/cortex_m3.c b/src/target/cortex_m3.c index 48f811489..762e3184c 100644 --- a/src/target/cortex_m3.c +++ b/src/target/cortex_m3.c @@ -638,6 +638,16 @@ static int cortex_m3_resume(struct target *target, int current, r->valid = true; } + /* if we halted last time due to a bkpt instruction + * then we have to manually step over it, otherwise + * the core will break again */ + + if (!breakpoint_find(target, buf_get_u32(r->value, 0, 32)) + && !debug_execution) + { + armv7m_maybe_skip_bkpt_inst(target, NULL); + } + resume_pc = buf_get_u32(r->value, 0, 32); armv7m_restore_context(target); @@ -690,6 +700,7 @@ static int cortex_m3_step(struct target *target, int current, struct swjdp_common *swjdp = &armv7m->swjdp_info; struct breakpoint *breakpoint = NULL; struct reg *pc = armv7m->core_cache->reg_list + 15; + bool bkpt_inst_found = false; if (target->state != TARGET_HALTED) { @@ -709,14 +720,23 @@ static int cortex_m3_step(struct target *target, int current, cortex_m3_unset_breakpoint(target, breakpoint); } + armv7m_maybe_skip_bkpt_inst(target, &bkpt_inst_found); + target->debug_reason = DBG_REASON_SINGLESTEP; armv7m_restore_context(target); target_call_event_callbacks(target, TARGET_EVENT_RESUMED); - /* set step and clear halt */ - cortex_m3_write_debug_halt_mask(target, C_STEP, C_HALT); + /* if no bkpt instruction is found at pc then we can perform + * a normal step, otherwise we have to manually step over the bkpt + * instruction - as such simulate a step */ + if (bkpt_inst_found == false) + { + /* set step and clear halt */ + cortex_m3_write_debug_halt_mask(target, C_STEP, C_HALT); + } + mem_ap_read_atomic_u32(swjdp, DCB_DHCSR, &cortex_m3->dcb_dhcsr); /* registers are now invalid */ @@ -735,6 +755,7 @@ static int cortex_m3_step(struct target *target, int current, LOG_DEBUG("target stepped dcb_dhcsr = 0x%" PRIx32 " nvic_icsr = 0x%" PRIx32, cortex_m3->dcb_dhcsr, cortex_m3->nvic_icsr); + return ERROR_OK; }