ARMV7M: handle bkpt instruction on resume/step

Skip over a bkpt instruction if found on resume/step.
Only software breakpoints known to OpenOCD are currently handled.

So this handles the special case of either a user added bkpt
or library added, eg. semi-hosting support.

Signed-off-by: Spencer Oliver <ntfreak@users.sourceforge.net>

diff --git a/src/target/armv7m.c b/src/target/armv7m.c
index 233fb959cdf2e77a2bacff15e921412a0d5b8634..c172a27dc9233da0ecf5718c2c2683d09610b4aa 100644 (file)
--- 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 86caae21b44a888756f930bb1e986442dfea2f48..9787e305442b6479ec9c43316abd1988db0df5e9 100644 (file)
--- 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 48f811489f870929b396f31c0613d60d42268b7a..762e3184c901151aedc230f8011919c3f07def2e 100644 (file)
--- 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;
 }