Revert "cortex_m3: add auto maskisr"
[openocd.git] / src / target / cortex_m3.c
index e99e99c..9c3d2d9 100644 (file)
 #include "target_type.h"
 #include "arm_disassembler.h"
 #include "register.h"
-
+#include "arm_opcodes.h"
+#include "arm_semihosting.h"
 
 /* NOTE:  most of this should work fine for the Cortex-M1 and
  * Cortex-M0 cores too, although they're ARMv6-M not ARMv7-M.
+ * Some differences:  M0/M1 doesn't have FBP remapping or the
+ * DWT tracing/profiling support.  (So the cycle counter will
+ * not be usable; the other stuff isn't currently used here.)
+ *
+ * Although there are some workarounds for errata seen only in r0p0
+ * silicon, such old parts are hard to find and thus not much tested
+ * any longer.
+ */
+
+/**
+ * Returns the type of a break point required by address location
  */
+#define BKPT_TYPE_BY_ADDR(addr) ((addr) < 0x20000000 ? BKPT_HARD : BKPT_SOFT)
 
 
 /* forward declarations */
@@ -51,12 +64,7 @@ static void cortex_m3_enable_watchpoints(struct target *target);
 static int cortex_m3_store_core_reg_u32(struct target *target,
                enum armv7m_regtype type, uint32_t num, uint32_t value);
 
-#ifdef ARMV7_GDB_HACKS
-extern uint8_t armv7m_gdb_dummy_cpsr_value[];
-extern struct reg armv7m_gdb_dummy_cpsr_reg;
-#endif
-
-static int cortexm3_dap_read_coreregister_u32(struct swjdp_common *swjdp,
+static int cortexm3_dap_read_coreregister_u32(struct adiv5_dap *swjdp,
                uint32_t *value, int regnum)
 {
        int retval;
@@ -65,19 +73,29 @@ static int cortexm3_dap_read_coreregister_u32(struct swjdp_common *swjdp,
        /* because the DCB_DCRDR is used for the emulated dcc channel
         * we have to save/restore the DCB_DCRDR when used */
 
-       mem_ap_read_u32(swjdp, DCB_DCRDR, &dcrdr);
-
-       swjdp->trans_mode = TRANS_MODE_COMPOSITE;
+       retval = mem_ap_read_u32(swjdp, DCB_DCRDR, &dcrdr);
+       if (retval != ERROR_OK)
+               return retval;
 
        /* mem_ap_write_u32(swjdp, DCB_DCRSR, regnum); */
-       dap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, DCB_DCRSR & 0xFFFFFFF0);
-       dap_ap_write_reg_u32(swjdp, AP_REG_BD0 | (DCB_DCRSR & 0xC), regnum);
+       retval = dap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, DCB_DCRSR & 0xFFFFFFF0);
+       if (retval != ERROR_OK)
+               return retval;
+       retval = dap_queue_ap_write(swjdp, AP_REG_BD0 | (DCB_DCRSR & 0xC), regnum);
+       if (retval != ERROR_OK)
+               return retval;
 
        /* mem_ap_read_u32(swjdp, DCB_DCRDR, value); */
-       dap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, DCB_DCRDR & 0xFFFFFFF0);
-       dap_ap_read_reg_u32(swjdp, AP_REG_BD0 | (DCB_DCRDR & 0xC), value);
+       retval = dap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, DCB_DCRDR & 0xFFFFFFF0);
+       if (retval != ERROR_OK)
+               return retval;
+       retval = dap_queue_ap_read(swjdp, AP_REG_BD0 | (DCB_DCRDR & 0xC), value);
+       if (retval != ERROR_OK)
+               return retval;
 
-       retval = swjdp_transaction_endcheck(swjdp);
+       retval = dap_run(swjdp);
+       if (retval != ERROR_OK)
+               return retval;
 
        /* restore DCB_DCRDR - this needs to be in a seperate
         * transaction otherwise the emulated DCC channel breaks */
@@ -87,7 +105,7 @@ static int cortexm3_dap_read_coreregister_u32(struct swjdp_common *swjdp,
        return retval;
 }
 
-static int cortexm3_dap_write_coreregister_u32(struct swjdp_common *swjdp,
+static int cortexm3_dap_write_coreregister_u32(struct adiv5_dap *swjdp,
                uint32_t value, int regnum)
 {
        int retval;
@@ -96,19 +114,25 @@ static int cortexm3_dap_write_coreregister_u32(struct swjdp_common *swjdp,
        /* because the DCB_DCRDR is used for the emulated dcc channel
         * we have to save/restore the DCB_DCRDR when used */
 
-       mem_ap_read_u32(swjdp, DCB_DCRDR, &dcrdr);
-
-       swjdp->trans_mode = TRANS_MODE_COMPOSITE;
+       retval = mem_ap_read_u32(swjdp, DCB_DCRDR, &dcrdr);
+       if (retval != ERROR_OK)
+               return retval;
 
        /* mem_ap_write_u32(swjdp, DCB_DCRDR, core_regs[i]); */
-       dap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, DCB_DCRDR & 0xFFFFFFF0);
-       dap_ap_write_reg_u32(swjdp, AP_REG_BD0 | (DCB_DCRDR & 0xC), value);
+       retval = dap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, DCB_DCRDR & 0xFFFFFFF0);
+       if (retval != ERROR_OK)
+               return retval;
+       retval = dap_queue_ap_write(swjdp, AP_REG_BD0 | (DCB_DCRDR & 0xC), value);
+       // XXX check retval
 
        /* mem_ap_write_u32(swjdp, DCB_DCRSR, i | DCRSR_WnR); */
-       dap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, DCB_DCRSR & 0xFFFFFFF0);
-       dap_ap_write_reg_u32(swjdp, AP_REG_BD0 | (DCB_DCRSR & 0xC), regnum | DCRSR_WnR);
+       retval = dap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, DCB_DCRSR & 0xFFFFFFF0);
+       if (retval != ERROR_OK)
+               return retval;
+       retval = dap_queue_ap_write(swjdp, AP_REG_BD0 | (DCB_DCRSR & 0xC), regnum | DCRSR_WnR);
+       // XXX check retval
 
-       retval = swjdp_transaction_endcheck(swjdp);
+       retval = dap_run(swjdp);
 
        /* restore DCB_DCRDR - this needs to be in a seperate
         * transaction otherwise the emulated DCC channel breaks */
@@ -122,7 +146,7 @@ static int cortex_m3_write_debug_halt_mask(struct target *target,
                uint32_t mask_on, uint32_t mask_off)
 {
        struct cortex_m3_common *cortex_m3 = target_to_cm3(target);
-       struct swjdp_common *swjdp = &cortex_m3->armv7m.swjdp_info;
+       struct adiv5_dap *swjdp = &cortex_m3->armv7m.dap;
 
        /* mask off status bits */
        cortex_m3->dcb_dhcsr &= ~((0xFFFF << 16) | mask_off);
@@ -135,15 +159,21 @@ static int cortex_m3_write_debug_halt_mask(struct target *target,
 static int cortex_m3_clear_halt(struct target *target)
 {
        struct cortex_m3_common *cortex_m3 = target_to_cm3(target);
-       struct swjdp_common *swjdp = &cortex_m3->armv7m.swjdp_info;
+       struct adiv5_dap *swjdp = &cortex_m3->armv7m.dap;
+       int retval;
 
        /* clear step if any */
        cortex_m3_write_debug_halt_mask(target, C_HALT, C_STEP);
 
        /* Read Debug Fault Status Register */
-       mem_ap_read_atomic_u32(swjdp, NVIC_DFSR, &cortex_m3->nvic_dfsr);
+       retval = mem_ap_read_atomic_u32(swjdp, NVIC_DFSR, &cortex_m3->nvic_dfsr);
+       if (retval != ERROR_OK)
+               return retval;
+
        /* Clear Debug Fault Status */
-       mem_ap_write_atomic_u32(swjdp, NVIC_DFSR, cortex_m3->nvic_dfsr);
+       retval = mem_ap_write_atomic_u32(swjdp, NVIC_DFSR, cortex_m3->nvic_dfsr);
+       if (retval != ERROR_OK)
+               return retval;
        LOG_DEBUG(" NVIC_DFSR 0x%" PRIx32 "", cortex_m3->nvic_dfsr);
 
        return ERROR_OK;
@@ -152,16 +182,28 @@ static int cortex_m3_clear_halt(struct target *target)
 static int cortex_m3_single_step_core(struct target *target)
 {
        struct cortex_m3_common *cortex_m3 = target_to_cm3(target);
-       struct swjdp_common *swjdp = &cortex_m3->armv7m.swjdp_info;
+       struct adiv5_dap *swjdp = &cortex_m3->armv7m.dap;
        uint32_t dhcsr_save;
+       int retval;
 
        /* backup dhcsr reg */
        dhcsr_save = cortex_m3->dcb_dhcsr;
 
-       /* mask interrupts if not done already */
+       /* Mask interrupts before clearing halt, if done already.  This avoids
+        * Erratum 377497 (fixed in r1p0) where setting MASKINTS while clearing
+        * HALT can put the core into an unknown state.
+        */
        if (!(cortex_m3->dcb_dhcsr & C_MASKINTS))
-               mem_ap_write_atomic_u32(swjdp, DCB_DHCSR, DBGKEY | C_MASKINTS | C_HALT | C_DEBUGEN);
-       mem_ap_write_atomic_u32(swjdp, DCB_DHCSR, DBGKEY | C_MASKINTS | C_STEP | C_DEBUGEN);
+       {
+               retval = mem_ap_write_atomic_u32(swjdp, DCB_DHCSR,
+                               DBGKEY | C_MASKINTS | C_HALT | C_DEBUGEN);
+               if (retval != ERROR_OK)
+                       return retval;
+       }
+       retval = mem_ap_write_atomic_u32(swjdp, DCB_DHCSR,
+                               DBGKEY | C_MASKINTS | C_STEP | C_DEBUGEN);
+       if (retval != ERROR_OK)
+               return retval;
        LOG_DEBUG(" ");
 
        /* restore dhcsr reg */
@@ -174,59 +216,95 @@ static int cortex_m3_single_step_core(struct target *target)
 static int cortex_m3_endreset_event(struct target *target)
 {
        int i;
+       int retval;
        uint32_t dcb_demcr;
        struct cortex_m3_common *cortex_m3 = target_to_cm3(target);
-       struct swjdp_common *swjdp = &cortex_m3->armv7m.swjdp_info;
+       struct armv7m_common *armv7m = &cortex_m3->armv7m;
+       struct adiv5_dap *swjdp = &cortex_m3->armv7m.dap;
        struct cortex_m3_fp_comparator *fp_list = cortex_m3->fp_comparator_list;
        struct cortex_m3_dwt_comparator *dwt_list = cortex_m3->dwt_comparator_list;
 
-       mem_ap_read_atomic_u32(swjdp, DCB_DEMCR, &dcb_demcr);
+       /* REVISIT The four debug monitor bits are currently ignored... */
+       retval = mem_ap_read_atomic_u32(swjdp, DCB_DEMCR, &dcb_demcr);
+       if (retval != ERROR_OK)
+               return retval;
        LOG_DEBUG("DCB_DEMCR = 0x%8.8" PRIx32 "",dcb_demcr);
 
-       /* this regsiter is used for emulated dcc channel */
-       mem_ap_write_u32(swjdp, DCB_DCRDR, 0);
+       /* this register is used for emulated dcc channel */
+       retval = mem_ap_write_u32(swjdp, DCB_DCRDR, 0);
+       if (retval != ERROR_OK)
+               return retval;
 
        /* Enable debug requests */
-       mem_ap_read_atomic_u32(swjdp, DCB_DHCSR, &cortex_m3->dcb_dhcsr);
+       retval = mem_ap_read_atomic_u32(swjdp, DCB_DHCSR, &cortex_m3->dcb_dhcsr);
+       if (retval != ERROR_OK)
+               return retval;
        if (!(cortex_m3->dcb_dhcsr & C_DEBUGEN))
-               mem_ap_write_u32(swjdp, DCB_DHCSR, DBGKEY | C_DEBUGEN);
+       {
+               retval = mem_ap_write_u32(swjdp, DCB_DHCSR, DBGKEY | C_DEBUGEN);
+               if (retval != ERROR_OK)
+                       return retval;
+       }
 
        /* clear any interrupt masking */
        cortex_m3_write_debug_halt_mask(target, 0, C_MASKINTS);
 
-       /* Enable trace and dwt */
-       mem_ap_write_u32(swjdp, DCB_DEMCR, TRCENA | VC_HARDERR | VC_BUSERR);
-       /* Monitor bus faults */
-       mem_ap_write_u32(swjdp, NVIC_SHCSR, SHCSR_BUSFAULTENA);
+       /* Enable features controlled by ITM and DWT blocks, and catch only
+        * the vectors we were told to pay attention to.
+        *
+        * Target firmware is responsible for all fault handling policy
+        * choices *EXCEPT* explicitly scripted overrides like "vector_catch"
+        * or manual updates to the NVIC SHCSR and CCR registers.
+        */
+       retval = mem_ap_write_u32(swjdp, DCB_DEMCR, TRCENA | armv7m->demcr);
+       if (retval != ERROR_OK)
+               return retval;
+
+       /* Paranoia: evidently some (early?) chips don't preserve all the
+        * debug state (including FBP, DWT, etc) across reset...
+        */
 
        /* Enable FPB */
-       target_write_u32(target, FP_CTRL, 3);
+       retval = target_write_u32(target, FP_CTRL, 3);
+       if (retval != ERROR_OK)
+               return retval;
+
        cortex_m3->fpb_enabled = 1;
 
        /* Restore FPB registers */
        for (i = 0; i < cortex_m3->fp_num_code + cortex_m3->fp_num_lit; i++)
        {
-               target_write_u32(target, fp_list[i].fpcr_address, fp_list[i].fpcr_value);
+               retval = target_write_u32(target, fp_list[i].fpcr_address, fp_list[i].fpcr_value);
+               if (retval != ERROR_OK)
+                       return retval;
        }
 
        /* Restore DWT registers */
        for (i = 0; i < cortex_m3->dwt_num_comp; i++)
        {
-               target_write_u32(target, dwt_list[i].dwt_comparator_address + 0,
+               retval = target_write_u32(target, dwt_list[i].dwt_comparator_address + 0,
                                dwt_list[i].comp);
-               target_write_u32(target, dwt_list[i].dwt_comparator_address + 4,
+               if (retval != ERROR_OK)
+                       return retval;
+               retval = target_write_u32(target, dwt_list[i].dwt_comparator_address + 4,
                                dwt_list[i].mask);
-               target_write_u32(target, dwt_list[i].dwt_comparator_address + 8,
+               if (retval != ERROR_OK)
+                       return retval;
+               retval = target_write_u32(target, dwt_list[i].dwt_comparator_address + 8,
                                dwt_list[i].function);
+               if (retval != ERROR_OK)
+                       return retval;
        }
-       swjdp_transaction_endcheck(swjdp);
+       retval = dap_run(swjdp);
+       if (retval != ERROR_OK)
+               return retval;
 
-       armv7m_invalidate_core_regs(target);
+       register_cache_invalidate(cortex_m3->armv7m.core_cache);
 
        /* make sure we have latest dhcsr flags */
-       mem_ap_read_atomic_u32(swjdp, DCB_DHCSR, &cortex_m3->dcb_dhcsr);
+       retval = mem_ap_read_atomic_u32(swjdp, DCB_DHCSR, &cortex_m3->dcb_dhcsr);
 
-       return ERROR_OK;
+       return retval;
 }
 
 static int cortex_m3_examine_debug_reason(struct target *target)
@@ -260,35 +338,54 @@ static int cortex_m3_examine_exception_reason(struct target *target)
 {
        uint32_t shcsr, except_sr, cfsr = -1, except_ar = -1;
        struct armv7m_common *armv7m = target_to_armv7m(target);
-       struct swjdp_common *swjdp = &armv7m->swjdp_info;
+       struct adiv5_dap *swjdp = &armv7m->dap;
+       int retval;
 
-       mem_ap_read_u32(swjdp, NVIC_SHCSR, &shcsr);
+       retval = mem_ap_read_u32(swjdp, NVIC_SHCSR, &shcsr);
+       if (retval != ERROR_OK)
+               return retval;
        switch (armv7m->exception_number)
        {
                case 2: /* NMI */
                        break;
                case 3: /* Hard Fault */
-                       mem_ap_read_atomic_u32(swjdp, NVIC_HFSR, &except_sr);
+                       retval = mem_ap_read_atomic_u32(swjdp, NVIC_HFSR, &except_sr);
+                       if (retval != ERROR_OK)
+                               return retval;
                        if (except_sr & 0x40000000)
                        {
-                               mem_ap_read_u32(swjdp, NVIC_CFSR, &cfsr);
+                               retval = mem_ap_read_u32(swjdp, NVIC_CFSR, &cfsr);
+                               if (retval != ERROR_OK)
+                                       return retval;
                        }
                        break;
                case 4: /* Memory Management */
-                       mem_ap_read_u32(swjdp, NVIC_CFSR, &except_sr);
-                       mem_ap_read_u32(swjdp, NVIC_MMFAR, &except_ar);
+                       retval = mem_ap_read_u32(swjdp, NVIC_CFSR, &except_sr);
+                       if (retval != ERROR_OK)
+                               return retval;
+                       retval = mem_ap_read_u32(swjdp, NVIC_MMFAR, &except_ar);
+                       if (retval != ERROR_OK)
+                               return retval;
                        break;
                case 5: /* Bus Fault */
-                       mem_ap_read_u32(swjdp, NVIC_CFSR, &except_sr);
-                       mem_ap_read_u32(swjdp, NVIC_BFAR, &except_ar);
+                       retval = mem_ap_read_u32(swjdp, NVIC_CFSR, &except_sr);
+                       if (retval != ERROR_OK)
+                               return retval;
+                       retval = mem_ap_read_u32(swjdp, NVIC_BFAR, &except_ar);
+                       if (retval != ERROR_OK)
+                               return retval;
                        break;
                case 6: /* Usage Fault */
-                       mem_ap_read_u32(swjdp, NVIC_CFSR, &except_sr);
+                       retval = mem_ap_read_u32(swjdp, NVIC_CFSR, &except_sr);
+                       if (retval != ERROR_OK)
+                               return retval;
                        break;
                case 11:        /* SVCall */
                        break;
                case 12:        /* Debug Monitor */
-                       mem_ap_read_u32(swjdp, NVIC_DFSR, &except_sr);
+                       retval = mem_ap_read_u32(swjdp, NVIC_DFSR, &except_sr);
+                       if (retval != ERROR_OK)
+                               return retval;
                        break;
                case 14:        /* PendSV */
                        break;
@@ -298,12 +395,33 @@ static int cortex_m3_examine_exception_reason(struct target *target)
                        except_sr = 0;
                        break;
        }
-       swjdp_transaction_endcheck(swjdp);
-       LOG_DEBUG("%s SHCSR 0x%" PRIx32 ", SR 0x%" PRIx32 ", CFSR 0x%" PRIx32 ", AR 0x%" PRIx32 "", armv7m_exception_string(armv7m->exception_number), \
-               shcsr, except_sr, cfsr, except_ar);
-       return ERROR_OK;
+       retval = dap_run(swjdp);
+       if (retval == ERROR_OK)
+               LOG_DEBUG("%s SHCSR 0x%" PRIx32 ", SR 0x%" PRIx32
+                       ", CFSR 0x%" PRIx32 ", AR 0x%" PRIx32,
+                       armv7m_exception_string(armv7m->exception_number),
+                       shcsr, except_sr, cfsr, except_ar);
+       return retval;
 }
 
+/* PSP is used in some thread modes */
+static const int armv7m_psp_reg_map[17] = {
+       ARMV7M_R0, ARMV7M_R1, ARMV7M_R2, ARMV7M_R3,
+       ARMV7M_R4, ARMV7M_R5, ARMV7M_R6, ARMV7M_R7,
+       ARMV7M_R8, ARMV7M_R9, ARMV7M_R10, ARMV7M_R11,
+       ARMV7M_R12, ARMV7M_PSP, ARMV7M_R14, ARMV7M_PC,
+       ARMV7M_xPSR,
+};
+
+/* MSP is used in handler and some thread modes */
+static const int armv7m_msp_reg_map[17] = {
+       ARMV7M_R0, ARMV7M_R1, ARMV7M_R2, ARMV7M_R3,
+       ARMV7M_R4, ARMV7M_R5, ARMV7M_R6, ARMV7M_R7,
+       ARMV7M_R8, ARMV7M_R9, ARMV7M_R10, ARMV7M_R11,
+       ARMV7M_R12, ARMV7M_MSP, ARMV7M_R14, ARMV7M_PC,
+       ARMV7M_xPSR,
+};
+
 static int cortex_m3_debug_entry(struct target *target)
 {
        int i;
@@ -311,12 +429,16 @@ static int cortex_m3_debug_entry(struct target *target)
        int retval;
        struct cortex_m3_common *cortex_m3 = target_to_cm3(target);
        struct armv7m_common *armv7m = &cortex_m3->armv7m;
-       struct swjdp_common *swjdp = &armv7m->swjdp_info;
+       struct arm *arm = &armv7m->arm;
+       struct adiv5_dap *swjdp = &armv7m->dap;
+       struct reg *r;
 
        LOG_DEBUG(" ");
 
        cortex_m3_clear_halt(target);
-       mem_ap_read_atomic_u32(swjdp, DCB_DHCSR, &cortex_m3->dcb_dhcsr);
+       retval = mem_ap_read_atomic_u32(swjdp, DCB_DHCSR, &cortex_m3->dcb_dhcsr);
+       if (retval != ERROR_OK)
+               return retval;
 
        if ((retval = armv7m->examine_debug_reason(target)) != ERROR_OK)
                return retval;
@@ -331,20 +453,24 @@ static int cortex_m3_debug_entry(struct target *target)
                        armv7m->read_core_reg(target, i);
        }
 
-       xPSR = buf_get_u32(armv7m->core_cache->reg_list[ARMV7M_xPSR].value, 0, 32);
+       r = armv7m->core_cache->reg_list + ARMV7M_xPSR;
+       xPSR = buf_get_u32(r->value, 0, 32);
 
 #ifdef ARMV7_GDB_HACKS
+       /* FIXME this breaks on scan chains with more than one Cortex-M3.
+        * Instead, each CM3 should have its own dummy value...
+        */
        /* copy real xpsr reg for gdb, setting thumb bit */
        buf_set_u32(armv7m_gdb_dummy_cpsr_value, 0, 32, xPSR);
        buf_set_u32(armv7m_gdb_dummy_cpsr_value, 5, 1, 1);
-       armv7m_gdb_dummy_cpsr_reg.valid = armv7m->core_cache->reg_list[ARMV7M_xPSR].valid;
-       armv7m_gdb_dummy_cpsr_reg.dirty = armv7m->core_cache->reg_list[ARMV7M_xPSR].dirty;
+       armv7m_gdb_dummy_cpsr_reg.valid = r->valid;
+       armv7m_gdb_dummy_cpsr_reg.dirty = r->dirty;
 #endif
 
        /* For IT instructions xPSR must be reloaded on resume and clear on debug exec */
        if (xPSR & 0xf00)
        {
-               armv7m->core_cache->reg_list[ARMV7M_xPSR].dirty = armv7m->core_cache->reg_list[ARMV7M_xPSR].valid;
+               r->dirty = r->valid;
                cortex_m3_store_core_reg_u32(target, ARMV7M_REGISTER_CORE_GP, 16, xPSR &~ 0xff);
        }
 
@@ -353,10 +479,27 @@ static int cortex_m3_debug_entry(struct target *target)
        {
                armv7m->core_mode = ARMV7M_MODE_HANDLER;
                armv7m->exception_number = (xPSR & 0x1FF);
+
+               arm->core_mode = ARM_MODE_HANDLER;
+               arm->map = armv7m_msp_reg_map;
        }
        else
        {
-               armv7m->core_mode = buf_get_u32(armv7m->core_cache->reg_list[ARMV7M_CONTROL].value, 0, 1);
+               unsigned control = buf_get_u32(armv7m->core_cache
+                               ->reg_list[ARMV7M_CONTROL].value, 0, 2);
+
+               /* is this thread privileged? */
+               armv7m->core_mode = control & 1;
+               arm->core_mode = armv7m->core_mode
+                               ? ARM_MODE_USER_THREAD
+                               : ARM_MODE_THREAD;
+
+               /* which stack is it using? */
+               if (control & 2)
+                       arm->map = armv7m_psp_reg_map;
+               else
+                       arm->map = armv7m_msp_reg_map;
+
                armv7m->exception_number = 0;
        }
 
@@ -367,21 +510,26 @@ static int cortex_m3_debug_entry(struct target *target)
 
        LOG_DEBUG("entered debug state in core mode: %s at PC 0x%" PRIx32 ", target->state: %s",
                armv7m_mode_strings[armv7m->core_mode],
-               *(uint32_t*)(armv7m->core_cache->reg_list[15].value),
+               *(uint32_t*)(arm->pc->value),
                target_state_name(target));
 
        if (armv7m->post_debug_entry)
-               armv7m->post_debug_entry(target);
+       {
+               retval = armv7m->post_debug_entry(target);
+               if (retval != ERROR_OK)
+                       return retval;
+       }
 
        return ERROR_OK;
 }
 
 static int cortex_m3_poll(struct target *target)
 {
-       int retval;
+       int detected_failure = ERROR_OK;
+       int retval = ERROR_OK;
        enum target_state prev_target_state = target->state;
        struct cortex_m3_common *cortex_m3 = target_to_cm3(target);
-       struct swjdp_common *swjdp = &cortex_m3->armv7m.swjdp_info;
+       struct adiv5_dap *swjdp = &cortex_m3->armv7m.dap;
 
        /* Read from Debug Halting Control and Status Register */
        retval = mem_ap_read_atomic_u32(swjdp, DCB_DHCSR, &cortex_m3->dcb_dhcsr);
@@ -391,10 +539,32 @@ static int cortex_m3_poll(struct target *target)
                return retval;
        }
 
+       /* Recover from lockup.  See ARMv7-M architecture spec,
+        * section B1.5.15 "Unrecoverable exception cases".
+        */
+       if (cortex_m3->dcb_dhcsr & S_LOCKUP) {
+               LOG_ERROR("%s -- clearing lockup after double fault",
+                               target_name(target));
+               cortex_m3_write_debug_halt_mask(target, C_HALT, 0);
+               target->debug_reason = DBG_REASON_DBGRQ;
+
+               /* We have to execute the rest (the "finally" equivalent, but
+                * still throw this exception again).
+                */
+               detected_failure = ERROR_FAIL;
+
+               /* refresh status bits */
+               retval = mem_ap_read_atomic_u32(swjdp, DCB_DHCSR, &cortex_m3->dcb_dhcsr);
+               if (retval != ERROR_OK)
+                       return retval;
+       }
+
        if (cortex_m3->dcb_dhcsr & S_RESET_ST)
        {
                /* check if still in reset */
-               mem_ap_read_atomic_u32(swjdp, DCB_DHCSR, &cortex_m3->dcb_dhcsr);
+               retval = mem_ap_read_atomic_u32(swjdp, DCB_DHCSR, &cortex_m3->dcb_dhcsr);
+               if (retval != ERROR_OK)
+                       return retval;
 
                if (cortex_m3->dcb_dhcsr & S_RESET_ST)
                {
@@ -405,8 +575,11 @@ static int cortex_m3_poll(struct target *target)
 
        if (target->state == TARGET_RESET)
        {
-               /* Cannot switch context while running so endreset is called with target->state == TARGET_RESET */
-               LOG_DEBUG("Exit from reset with dcb_dhcsr 0x%" PRIx32 "", cortex_m3->dcb_dhcsr);
+               /* Cannot switch context while running so endreset is
+                * called with target->state == TARGET_RESET
+                */
+               LOG_DEBUG("Exit from reset with dcb_dhcsr 0x%" PRIx32,
+                               cortex_m3->dcb_dhcsr);
                cortex_m3_endreset_event(target);
                target->state = TARGET_RUNNING;
                prev_target_state = TARGET_RUNNING;
@@ -421,6 +594,9 @@ static int cortex_m3_poll(struct target *target)
                        if ((retval = cortex_m3_debug_entry(target)) != ERROR_OK)
                                return retval;
 
+                       if (arm_semihosting(target, &retval) != 0)
+                               return retval;
+
                        target_call_event_callbacks(target, TARGET_EVENT_HALTED);
                }
                if (prev_target_state == TARGET_DEBUG_RUNNING)
@@ -443,11 +619,14 @@ static int cortex_m3_poll(struct target *target)
                if (cortex_m3->dcb_dhcsr & S_RETIRE_ST)
                {
                        target->state = TARGET_RUNNING;
-                       return ERROR_OK;
+                       retval = ERROR_OK;
                }
        }
 
-       return ERROR_OK;
+       /* Did we detect a failure condition that we cleared? */
+       if (detected_failure != ERROR_OK)
+               retval = detected_failure;
+       return retval;
 }
 
 static int cortex_m3_halt(struct target *target)
@@ -495,34 +674,50 @@ static int cortex_m3_halt(struct target *target)
 static int cortex_m3_soft_reset_halt(struct target *target)
 {
        struct cortex_m3_common *cortex_m3 = target_to_cm3(target);
-       struct swjdp_common *swjdp = &cortex_m3->armv7m.swjdp_info;
+       struct adiv5_dap *swjdp = &cortex_m3->armv7m.dap;
        uint32_t dcb_dhcsr = 0;
        int retval, timeout = 0;
 
-       /* Enter debug state on reset, cf. end_reset_event() */
-       mem_ap_write_u32(swjdp, DCB_DEMCR, TRCENA | VC_HARDERR | VC_BUSERR | VC_CORERESET);
+       /* Enter debug state on reset; restore DEMCR in endreset_event() */
+       retval = mem_ap_write_u32(swjdp, DCB_DEMCR,
+                       TRCENA | VC_HARDERR | VC_BUSERR | VC_CORERESET);
+       if (retval != ERROR_OK)
+               return retval;
 
-       /* Request a reset */
-       mem_ap_write_atomic_u32(swjdp, NVIC_AIRCR, AIRCR_VECTKEY | AIRCR_VECTRESET);
+       /* Request a core-only reset */
+       retval = mem_ap_write_atomic_u32(swjdp, NVIC_AIRCR,
+                       AIRCR_VECTKEY | AIRCR_VECTRESET);
+       if (retval != ERROR_OK)
+               return retval;
        target->state = TARGET_RESET;
 
        /* registers are now invalid */
-       armv7m_invalidate_core_regs(target);
+       register_cache_invalidate(cortex_m3->armv7m.core_cache);
 
        while (timeout < 100)
        {
                retval = mem_ap_read_atomic_u32(swjdp, DCB_DHCSR, &dcb_dhcsr);
                if (retval == ERROR_OK)
                {
-                       mem_ap_read_atomic_u32(swjdp, NVIC_DFSR, &cortex_m3->nvic_dfsr);
-                       if ((dcb_dhcsr & S_HALT) && (cortex_m3->nvic_dfsr & DFSR_VCATCH))
+                       retval = mem_ap_read_atomic_u32(swjdp, NVIC_DFSR,
+                                       &cortex_m3->nvic_dfsr);
+                       if (retval != ERROR_OK)
+                               return retval;
+                       if ((dcb_dhcsr & S_HALT)
+                                       && (cortex_m3->nvic_dfsr & DFSR_VCATCH))
                        {
-                               LOG_DEBUG("system reset-halted, dcb_dhcsr 0x%" PRIx32 ", nvic_dfsr 0x%" PRIx32 "", dcb_dhcsr, cortex_m3->nvic_dfsr);
+                               LOG_DEBUG("system reset-halted, DHCSR 0x%08x, "
+                                       "DFSR 0x%08x",
+                                       (unsigned) dcb_dhcsr,
+                                       (unsigned) cortex_m3->nvic_dfsr);
                                cortex_m3_poll(target);
+                               /* FIXME restore user's vector catch config */
                                return ERROR_OK;
                        }
                        else
-                               LOG_DEBUG("waiting for system reset-halt, dcb_dhcsr 0x%" PRIx32 ", %i ms", dcb_dhcsr, timeout);
+                               LOG_DEBUG("waiting for system reset-halt, "
+                                       "DHCSR 0x%08x, %d ms",
+                                       (unsigned) dcb_dhcsr, timeout);
                }
                timeout++;
                alive_sleep(1);
@@ -538,7 +733,7 @@ static void cortex_m3_enable_breakpoints(struct target *target)
        /* set any pending breakpoints */
        while (breakpoint)
        {
-               if (breakpoint->set == 0)
+               if (!breakpoint->set)
                        cortex_m3_set_breakpoint(target, breakpoint);
                breakpoint = breakpoint->next;
        }
@@ -550,6 +745,7 @@ static int cortex_m3_resume(struct target *target, int current,
        struct armv7m_common *armv7m = target_to_armv7m(target);
        struct breakpoint *breakpoint = NULL;
        uint32_t resume_pc;
+       struct reg *r;
 
        if (target->state != TARGET_HALTED)
        {
@@ -566,30 +762,50 @@ static int cortex_m3_resume(struct target *target, int current,
 
        if (debug_execution)
        {
+               r = armv7m->core_cache->reg_list + ARMV7M_PRIMASK;
+
                /* Disable interrupts */
-               /* We disable interrupts in the PRIMASK register instead of masking with C_MASKINTS,
-                * This is probably the same issue as Cortex-M3 Errata  377493:
-                * C_MASKINTS in parallel with disabled interrupts can cause local faults to not be taken. */
-               buf_set_u32(armv7m->core_cache->reg_list[ARMV7M_PRIMASK].value, 0, 32, 1);
-               armv7m->core_cache->reg_list[ARMV7M_PRIMASK].dirty = 1;
-               armv7m->core_cache->reg_list[ARMV7M_PRIMASK].valid = 1;
+               /* We disable interrupts in the PRIMASK register instead of
+                * masking with C_MASKINTS.  This is probably the same issue
+                * as Cortex-M3 Erratum 377493 (fixed in r1p0):  C_MASKINTS
+                * in parallel with disabled interrupts can cause local faults
+                * to not be taken.
+                *
+                * REVISIT this clearly breaks non-debug execution, since the
+                * PRIMASK register state isn't saved/restored...  workaround
+                * by never resuming app code after debug execution.
+                */
+               buf_set_u32(r->value, 0, 1, 1);
+               r->dirty = true;
+               r->valid = true;
 
                /* Make sure we are in Thumb mode */
-               buf_set_u32(armv7m->core_cache->reg_list[ARMV7M_xPSR].value, 0, 32,
-                       buf_get_u32(armv7m->core_cache->reg_list[ARMV7M_xPSR].value, 0, 32) | (1 << 24));
-               armv7m->core_cache->reg_list[ARMV7M_xPSR].dirty = 1;
-               armv7m->core_cache->reg_list[ARMV7M_xPSR].valid = 1;
+               r = armv7m->core_cache->reg_list + ARMV7M_xPSR;
+               buf_set_u32(r->value, 24, 1, 1);
+               r->dirty = true;
+               r->valid = true;
        }
 
        /* current = 1: continue on current pc, otherwise continue at <address> */
+       r = armv7m->arm.pc;
        if (!current)
        {
-               buf_set_u32(armv7m->core_cache->reg_list[15].value, 0, 32, address);
-               armv7m->core_cache->reg_list[15].dirty = 1;
-               armv7m->core_cache->reg_list[15].valid = 1;
+               buf_set_u32(r->value, 0, 32, address);
+               r->dirty = true;
+               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(armv7m->core_cache->reg_list[15].value, 0, 32);
+       resume_pc = buf_get_u32(r->value, 0, 32);
 
        armv7m_restore_context(target);
 
@@ -614,7 +830,8 @@ static int cortex_m3_resume(struct target *target, int current,
        target->debug_reason = DBG_REASON_NOTHALTED;
 
        /* registers are now invalid */
-       armv7m_invalidate_core_regs(target);
+       register_cache_invalidate(armv7m->core_cache);
+
        if (!debug_execution)
        {
                target->state = TARGET_RUNNING;
@@ -637,8 +854,10 @@ static int cortex_m3_step(struct target *target, int current,
 {
        struct cortex_m3_common *cortex_m3 = target_to_cm3(target);
        struct armv7m_common *armv7m = &cortex_m3->armv7m;
-       struct swjdp_common *swjdp = &armv7m->swjdp_info;
+       struct adiv5_dap *swjdp = &armv7m->dap;
        struct breakpoint *breakpoint = NULL;
+       struct reg *pc = armv7m->arm.pc;
+       bool bkpt_inst_found = false;
 
        if (target->state != TARGET_HALTED)
        {
@@ -648,132 +867,130 @@ static int cortex_m3_step(struct target *target, int current,
 
        /* current = 1: continue on current pc, otherwise continue at <address> */
        if (!current)
-               buf_set_u32(cortex_m3->armv7m.core_cache->reg_list[15].value,
-                               0, 32, address);
+               buf_set_u32(pc->value, 0, 32, address);
+
+       uint32_t pc_value = buf_get_u32(pc->value, 0, 32);
 
        /* the front-end may request us not to handle breakpoints */
        if (handle_breakpoints) {
-               breakpoint = breakpoint_find(target, buf_get_u32(armv7m
-                               ->core_cache->reg_list[15].value, 0, 32));
+               breakpoint = breakpoint_find(target, pc_value);
                if (breakpoint)
                        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);
-       mem_ap_read_atomic_u32(swjdp, DCB_DHCSR, &cortex_m3->dcb_dhcsr);
+       /* 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);
+       }
+
+       int retval;
+       retval = mem_ap_read_atomic_u32(swjdp, DCB_DHCSR, &cortex_m3->dcb_dhcsr);
+       if (retval != ERROR_OK)
+               return retval;
 
        /* registers are now invalid */
-       armv7m_invalidate_core_regs(target);
+       register_cache_invalidate(cortex_m3->armv7m.core_cache);
 
        if (breakpoint)
                cortex_m3_set_breakpoint(target, breakpoint);
 
-       LOG_DEBUG("target stepped dcb_dhcsr = 0x%" PRIx32 " nvic_icsr = 0x%" PRIx32 "", cortex_m3->dcb_dhcsr, cortex_m3->nvic_icsr);
+       LOG_DEBUG("target stepped dcb_dhcsr = 0x%" PRIx32
+                       " nvic_icsr = 0x%" PRIx32,
+                       cortex_m3->dcb_dhcsr, cortex_m3->nvic_icsr);
 
-       cortex_m3_debug_entry(target);
+       retval = cortex_m3_debug_entry(target);
+       if (retval != ERROR_OK)
+               return retval;
        target_call_event_callbacks(target, TARGET_EVENT_HALTED);
 
-       LOG_DEBUG("target stepped dcb_dhcsr = 0x%" PRIx32 " nvic_icsr = 0x%" PRIx32 "", cortex_m3->dcb_dhcsr, cortex_m3->nvic_icsr);
+       LOG_DEBUG("target stepped dcb_dhcsr = 0x%" PRIx32
+                       " nvic_icsr = 0x%" PRIx32,
+                       cortex_m3->dcb_dhcsr, cortex_m3->nvic_icsr);
+
        return ERROR_OK;
 }
 
 static int cortex_m3_assert_reset(struct target *target)
 {
        struct cortex_m3_common *cortex_m3 = target_to_cm3(target);
-       struct swjdp_common *swjdp = &cortex_m3->armv7m.swjdp_info;
-       int assert_srst = 1;
+       struct adiv5_dap *swjdp = &cortex_m3->armv7m.dap;
+       enum cortex_m3_soft_reset_config reset_config = cortex_m3->soft_reset_config;
 
        LOG_DEBUG("target->state: %s",
                target_state_name(target));
 
        enum reset_types jtag_reset_config = jtag_get_reset_config();
 
-       /*
-        * We can reset Cortex-M3 targets using just the NVIC without
-        * requiring SRST, getting a SoC reset (or a core-only reset)
-        * instead of a system reset.
-        */
-       if (!(jtag_reset_config & RESET_HAS_SRST))
-               assert_srst = 0;
+       if (target_has_event_action(target, TARGET_EVENT_RESET_ASSERT)) {
+               /* allow scripts to override the reset event */
+
+               target_handle_event(target, TARGET_EVENT_RESET_ASSERT);
+               register_cache_invalidate(cortex_m3->armv7m.core_cache);
+               target->state = TARGET_RESET;
+
+               return ERROR_OK;
+       }
 
        /* Enable debug requests */
-       mem_ap_read_atomic_u32(swjdp, DCB_DHCSR, &cortex_m3->dcb_dhcsr);
+       int retval;
+       retval = mem_ap_read_atomic_u32(swjdp, DCB_DHCSR, &cortex_m3->dcb_dhcsr);
+       if (retval != ERROR_OK)
+               return retval;
        if (!(cortex_m3->dcb_dhcsr & C_DEBUGEN))
-               mem_ap_write_u32(swjdp, DCB_DHCSR, DBGKEY | C_DEBUGEN);
+       {
+               retval = mem_ap_write_u32(swjdp, DCB_DHCSR, DBGKEY | C_DEBUGEN);
+               if (retval != ERROR_OK)
+                       return retval;
+       }
 
-       mem_ap_write_u32(swjdp, DCB_DCRDR, 0);
+       retval = mem_ap_write_u32(swjdp, DCB_DCRDR, 0);
+       if (retval != ERROR_OK)
+               return retval;
 
        if (!target->reset_halt)
        {
                /* Set/Clear C_MASKINTS in a separate operation */
                if (cortex_m3->dcb_dhcsr & C_MASKINTS)
-                       mem_ap_write_atomic_u32(swjdp, DCB_DHCSR, DBGKEY | C_DEBUGEN | C_HALT);
+               {
+                       retval = mem_ap_write_atomic_u32(swjdp, DCB_DHCSR,
+                                       DBGKEY | C_DEBUGEN | C_HALT);
+                       if (retval != ERROR_OK)
+                               return retval;
+               }
 
                /* clear any debug flags before resuming */
                cortex_m3_clear_halt(target);
 
                /* clear C_HALT in dhcsr reg */
                cortex_m3_write_debug_halt_mask(target, 0, C_HALT);
-
-               /* Enter debug state on reset, cf. end_reset_event() */
-               mem_ap_write_u32(swjdp, DCB_DEMCR, TRCENA | VC_HARDERR | VC_BUSERR);
        }
        else
        {
-               /* Enter debug state on reset, cf. end_reset_event() */
-               mem_ap_write_atomic_u32(swjdp, DCB_DEMCR, TRCENA | VC_HARDERR | VC_BUSERR | VC_CORERESET);
-       }
-
-       /*
-        * When nRST is asserted on most Stellaris devices, it clears some of
-        * the debug state.  The ARMv7M and Cortex-M3 TRMs say that's wrong;
-        * and OpenOCD depends on those TRMs.  So we won't use SRST on those
-        * chips.  (Only power-on reset should affect debug state, beyond a
-        * few specified bits; not the chip's nRST input, wired to SRST.)
-        *
-        * REVISIT current errata specs don't seem to cover this issue.
-        * Do we have more details than this email?
-        *   https://lists.berlios.de/pipermail
-        *      /openocd-development/2008-August/003065.html
-        */
-       if (strcmp(target->variant, "lm3s") == 0)
-       {
-               /* Check for silicon revisions with the issue. */
-               uint32_t did0;
-
-               if (target_read_u32(target, 0x400fe000, &did0) == ERROR_OK)
-               {
-                       switch ((did0 >> 16) & 0xff)
-                       {
-                               case 0:
-                                       /* all Sandstorm suffer issue */
-                                       assert_srst = 0;
-                                       break;
-
-                               case 1:
-                               case 3:
-                                       /* Fury and DustDevil rev A have
-                                        * this nRST problem.  It should
-                                        * be fixed in rev B silicon.
-                                        */
-                                       if (((did0 >> 8) & 0xff) == 0)
-                                               assert_srst = 0;
-                                       break;
-                               case 4:
-                                       /* Tempest should be fine. */
-                                       break;
-                       }
-               }
+               /* Halt in debug on reset; endreset_event() restores DEMCR.
+                *
+                * REVISIT catching BUSERR presumably helps to defend against
+                * bad vector table entries.  Should this include MMERR or
+                * other flags too?
+                */
+               retval = mem_ap_write_atomic_u32(swjdp, DCB_DEMCR,
+                               TRCENA | VC_HARDERR | VC_BUSERR | VC_CORERESET);
+               if (retval != ERROR_OK)
+                       return retval;
        }
 
-       if (assert_srst)
+       if (jtag_reset_config & RESET_HAS_SRST)
        {
                /* default to asserting srst */
                if (jtag_reset_config & RESET_SRST_PULLS_TRST)
@@ -788,13 +1005,23 @@ static int cortex_m3_assert_reset(struct target *target)
        else
        {
                /* Use a standard Cortex-M3 software reset mechanism.
-                * SYSRESETREQ will reset SoC peripherals outside the
-                * core, like watchdog timers, if the SoC wires it up
-                * correctly.  Else VECRESET can reset just the core.
+                * We default to using VECRESET as it is supported on all current cores.
+                * This has the disadvantage of not resetting the peripherals, so a
+                * reset-init event handler is needed to perform any peripheral resets.
                 */
-               mem_ap_write_atomic_u32(swjdp, NVIC_AIRCR,
-                               AIRCR_VECTKEY | AIRCR_SYSRESETREQ);
-               LOG_DEBUG("Using Cortex-M3 SYSRESETREQ");
+               retval = mem_ap_write_atomic_u32(swjdp, NVIC_AIRCR,
+                               AIRCR_VECTKEY | ((reset_config == CORTEX_M3_RESET_SYSRESETREQ)
+                               ? AIRCR_SYSRESETREQ : AIRCR_VECTRESET));
+               if (retval != ERROR_OK)
+                       return retval;
+
+               LOG_DEBUG("Using Cortex-M3 %s", (reset_config == CORTEX_M3_RESET_SYSRESETREQ)
+                               ? "SYSRESETREQ" : "VECTRESET");
+
+               if (reset_config == CORTEX_M3_RESET_VECTRESET) {
+                       LOG_WARNING("Only resetting the Cortex-M3 core, use a reset-init event "
+                                       "handler to reset any peripherals");
+               }
 
                {
                        /* I do not know why this is necessary, but it
@@ -802,18 +1029,19 @@ static int cortex_m3_assert_reset(struct target *target)
                         * after reset) on LM3S6918 -- Michael Schwingen
                         */
                        uint32_t tmp;
-                       mem_ap_read_atomic_u32(swjdp, NVIC_AIRCR, &tmp);
+                       retval = mem_ap_read_atomic_u32(swjdp, NVIC_AIRCR, &tmp);
+                       if (retval != ERROR_OK)
+                               return retval;
                }
        }
 
        target->state = TARGET_RESET;
        jtag_add_sleep(50000);
 
-       armv7m_invalidate_core_regs(target);
+       register_cache_invalidate(cortex_m3->armv7m.core_cache);
 
        if (target->reset_halt)
        {
-               int retval;
                if ((retval = target_halt(target)) != ERROR_OK)
                        return retval;
        }
@@ -849,7 +1077,7 @@ cortex_m3_set_breakpoint(struct target *target, struct breakpoint *breakpoint)
 
        if (cortex_m3->auto_bp_type)
        {
-               breakpoint->type = (breakpoint->address < 0x20000000) ? BKPT_HARD : BKPT_SOFT;
+               breakpoint->type = BKPT_TYPE_BY_ADDR(breakpoint->address);
        }
 
        if (breakpoint->type == BKPT_HARD)
@@ -858,9 +1086,8 @@ cortex_m3_set_breakpoint(struct target *target, struct breakpoint *breakpoint)
                        fp_num++;
                if (fp_num >= cortex_m3->fp_num_code)
                {
-                       LOG_DEBUG("ERROR Can not find free FP Comparator");
-                       LOG_WARNING("ERROR Can not find free FP Comparator");
-                       exit(-1);
+                       LOG_ERROR("Can not find free FPB Comparator!");
+                       return ERROR_FAIL;
                }
                breakpoint->set = fp_num + 1;
                hilo = (breakpoint->address & 0x2) ? FPCR_REPLACE_BKPT_HIGH : FPCR_REPLACE_BKPT_LOW;
@@ -877,16 +1104,25 @@ cortex_m3_set_breakpoint(struct target *target, struct breakpoint *breakpoint)
        else if (breakpoint->type == BKPT_SOFT)
        {
                uint8_t code[4];
-               buf_set_u32(code, 0, 32, ARMV7M_T_BKPT(0x11));
-               if ((retval = target_read_memory(target, breakpoint->address & 0xFFFFFFFE, breakpoint->length, 1, breakpoint->orig_instr)) != ERROR_OK)
-               {
+
+               /* NOTE: on ARMv6-M and ARMv7-M, BKPT(0xab) is used for
+                * semihosting; don't use that.  Otherwise the BKPT
+                * parameter is arbitrary.
+                */
+               buf_set_u32(code, 0, 32, ARMV5_T_BKPT(0x11));
+               retval = target_read_memory(target,
+                               breakpoint->address & 0xFFFFFFFE,
+                               breakpoint->length, 1,
+                               breakpoint->orig_instr);
+               if (retval != ERROR_OK)
                        return retval;
-               }
-               if ((retval = target_write_memory(target, breakpoint->address & 0xFFFFFFFE, breakpoint->length, 1, code)) != ERROR_OK)
-               {
+               retval = target_write_memory(target,
+                               breakpoint->address & 0xFFFFFFFE,
+                               breakpoint->length, 1,
+                               code);
+               if (retval != ERROR_OK)
                        return retval;
-               }
-               breakpoint->set = 0x11; /* Any nice value but 0 */
+               breakpoint->set = true;
        }
 
        LOG_DEBUG("BPID: %d, Type: %d, Address: 0x%08" PRIx32 " Length: %d (set=%d)",
@@ -949,7 +1185,7 @@ cortex_m3_unset_breakpoint(struct target *target, struct breakpoint *breakpoint)
                        }
                }
        }
-       breakpoint->set = 0;
+       breakpoint->set = false;
 
        return ERROR_OK;
 }
@@ -961,7 +1197,7 @@ cortex_m3_add_breakpoint(struct target *target, struct breakpoint *breakpoint)
 
        if (cortex_m3->auto_bp_type)
        {
-               breakpoint->type = (breakpoint->address < 0x20000000) ? BKPT_HARD : BKPT_SOFT;
+               breakpoint->type = BKPT_TYPE_BY_ADDR(breakpoint->address);
 #ifdef ARMV7_GDB_HACKS
                if (breakpoint->length != 2) {
                        /* XXX Hack: Replace all breakpoints with length != 2 with
@@ -972,16 +1208,18 @@ cortex_m3_add_breakpoint(struct target *target, struct breakpoint *breakpoint)
 #endif
        }
 
-       if ((breakpoint->type == BKPT_HARD) && (breakpoint->address >= 0x20000000))
-       {
-               LOG_INFO("flash patch comparator requested outside code memory region");
-               return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
-       }
+       if(breakpoint->type != BKPT_TYPE_BY_ADDR(breakpoint->address)) {
+               if (breakpoint->type == BKPT_HARD)
+               {
+                       LOG_INFO("flash patch comparator requested outside code memory region");
+                       return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+               }
 
-       if ((breakpoint->type == BKPT_SOFT) && (breakpoint->address < 0x20000000))
-       {
-               LOG_INFO("soft breakpoint requested in code (flash) memory region");
-               return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+               if (breakpoint->type == BKPT_SOFT)
+               {
+                       LOG_INFO("soft breakpoint requested in code (flash) memory region");
+                       return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+               }
        }
 
        if ((breakpoint->type == BKPT_HARD) && (cortex_m3->fp_code_available < 1))
@@ -998,9 +1236,8 @@ cortex_m3_add_breakpoint(struct target *target, struct breakpoint *breakpoint)
 
        if (breakpoint->type == BKPT_HARD)
                cortex_m3->fp_code_available--;
-       cortex_m3_set_breakpoint(target, breakpoint);
 
-       return ERROR_OK;
+       return cortex_m3_set_breakpoint(target, breakpoint);
 }
 
 static int
@@ -1017,7 +1254,7 @@ cortex_m3_remove_breakpoint(struct target *target, struct breakpoint *breakpoint
 
        if (cortex_m3->auto_bp_type)
        {
-               breakpoint->type = (breakpoint->address < 0x20000000) ? BKPT_HARD : BKPT_SOFT;
+               breakpoint->type = BKPT_TYPE_BY_ADDR(breakpoint->address);
        }
 
        if (breakpoint->set)
@@ -1128,7 +1365,7 @@ cortex_m3_unset_watchpoint(struct target *target, struct watchpoint *watchpoint)
        target_write_u32(target, comparator->dwt_comparator_address + 8,
                        comparator->function);
 
-       watchpoint->set = 0;
+       watchpoint->set = false;
 
        return ERROR_OK;
 }
@@ -1138,13 +1375,6 @@ cortex_m3_add_watchpoint(struct target *target, struct watchpoint *watchpoint)
 {
        struct cortex_m3_common *cortex_m3 = target_to_cm3(target);
 
-       /* REVISIT why check? DWT can be updated with core running ... */
-       if (target->state != TARGET_HALTED)
-       {
-               LOG_WARNING("target not halted");
-               return ERROR_TARGET_NOT_HALTED;
-       }
-
        if (cortex_m3->dwt_comp_available < 1)
        {
                LOG_DEBUG("no comparators?");
@@ -1221,7 +1451,7 @@ static void cortex_m3_enable_watchpoints(struct target *target)
        /* set any pending watchpoints */
        while (watchpoint)
        {
-               if (watchpoint->set == 0)
+               if (!watchpoint->set)
                        cortex_m3_set_watchpoint(target, watchpoint);
                watchpoint = watchpoint->next;
        }
@@ -1232,7 +1462,7 @@ static int cortex_m3_load_core_reg_u32(struct target *target,
 {
        int retval;
        struct armv7m_common *armv7m = target_to_armv7m(target);
-       struct swjdp_common *swjdp = &armv7m->swjdp_info;
+       struct adiv5_dap *swjdp = &armv7m->dap;
 
        /* NOTE:  we "know" here that the register identifiers used
         * in the v7m header match the Cortex-M3 Debug Core Register
@@ -1296,7 +1526,7 @@ static int cortex_m3_store_core_reg_u32(struct target *target,
        int retval;
        uint32_t reg;
        struct armv7m_common *armv7m = target_to_armv7m(target);
-       struct swjdp_common *swjdp = &armv7m->swjdp_info;
+       struct adiv5_dap *swjdp = &armv7m->dap;
 
 #ifdef ARMV7_GDB_HACKS
        /* If the LR register is being modified, make sure it will put us
@@ -1319,8 +1549,11 @@ static int cortex_m3_store_core_reg_u32(struct target *target,
                retval = cortexm3_dap_write_coreregister_u32(swjdp, value, num);
                if (retval != ERROR_OK)
                {
-                       LOG_ERROR("JTAG failure %i", retval);
-                       armv7m->core_cache->reg_list[num].dirty = armv7m->core_cache->reg_list[num].valid;
+                       struct reg *r;
+
+                       LOG_ERROR("JTAG failure");
+                       r = armv7m->core_cache->reg_list + num;
+                       r->dirty = r->valid;
                        return ERROR_JTAG_DEVICE_ERROR;
                }
                LOG_DEBUG("write core reg %i value 0x%" PRIx32 "", (int)num, value);
@@ -1371,17 +1604,12 @@ static int cortex_m3_read_memory(struct target *target, uint32_t address,
                uint32_t size, uint32_t count, uint8_t *buffer)
 {
        struct armv7m_common *armv7m = target_to_armv7m(target);
-       struct swjdp_common *swjdp = &armv7m->swjdp_info;
-       int retval;
-
-       /* sanitize arguments */
-       if (((size != 4) && (size != 2) && (size != 1)) || (count == 0) || !(buffer))
-               return ERROR_INVALID_ARGUMENTS;
+       struct adiv5_dap *swjdp = &armv7m->dap;
+       int retval = ERROR_INVALID_ARGUMENTS;
 
        /* cortex_m3 handles unaligned memory access */
-
-       switch (size)
-       {
+       if (count && buffer) {
+               switch (size) {
                case 4:
                        retval = mem_ap_read_buf_u32(swjdp, buffer, 4 * count, address);
                        break;
@@ -1391,27 +1619,21 @@ static int cortex_m3_read_memory(struct target *target, uint32_t address,
                case 1:
                        retval = mem_ap_read_buf_u8(swjdp, buffer, count, address);
                        break;
-               default:
-                       LOG_ERROR("BUG: we shouldn't get here");
-                       exit(-1);
+               }
        }
 
        return retval;
 }
 
 static int cortex_m3_write_memory(struct target *target, uint32_t address,
-               uint32_t size, uint32_t count, uint8_t *buffer)
+               uint32_t size, uint32_t count, const uint8_t *buffer)
 {
        struct armv7m_common *armv7m = target_to_armv7m(target);
-       struct swjdp_common *swjdp = &armv7m->swjdp_info;
-       int retval;
-
-       /* sanitize arguments */
-       if (((size != 4) && (size != 2) && (size != 1)) || (count == 0) || !(buffer))
-               return ERROR_INVALID_ARGUMENTS;
+       struct adiv5_dap *swjdp = &armv7m->dap;
+       int retval = ERROR_INVALID_ARGUMENTS;
 
-       switch (size)
-       {
+       if (count && buffer) {
+               switch (size) {
                case 4:
                        retval = mem_ap_write_buf_u32(swjdp, buffer, 4 * count, address);
                        break;
@@ -1421,16 +1643,14 @@ static int cortex_m3_write_memory(struct target *target, uint32_t address,
                case 1:
                        retval = mem_ap_write_buf_u8(swjdp, buffer, count, address);
                        break;
-               default:
-                       LOG_ERROR("BUG: we shouldn't get here");
-                       exit(-1);
+               }
        }
 
        return retval;
 }
 
 static int cortex_m3_bulk_write_memory(struct target *target, uint32_t address,
-               uint32_t count, uint8_t *buffer)
+               uint32_t count, const uint8_t *buffer)
 {
        return cortex_m3_write_memory(target, address, 4, count, buffer);
 }
@@ -1476,6 +1696,9 @@ struct dwt_reg {
 
 static struct dwt_reg dwt_base_regs[] = {
        { DWT_CTRL, "dwt_ctrl", 32, },
+       /* NOTE that Erratum 532314 (fixed r2p0) affects CYCCNT:  it wrongly
+        * increments while the core is asleep.
+        */
        { DWT_CYCCNT, "dwt_cyccnt", 32, },
        /* plus some 8 bit counters, useful for profiling with TPIU */
 };
@@ -1492,7 +1715,10 @@ static struct dwt_reg dwt_comp[] = {
 #undef DWT_COMPARATOR
 };
 
-static int dwt_reg_type = -1;
+static const struct reg_arch_type dwt_reg_type = {
+       .get = cortex_m3_dwt_get_reg,
+       .set = cortex_m3_dwt_set_reg,
+};
 
 static void
 cortex_m3_dwt_addreg(struct target *t, struct reg *r, struct dwt_reg *d)
@@ -1509,7 +1735,7 @@ cortex_m3_dwt_addreg(struct target *t, struct reg *r, struct dwt_reg *d)
        r->size = d->size;
        r->value = &state->value;
        r->arch_info = state;
-       r->arch_type = dwt_reg_type;
+       r->type = &dwt_reg_type;
 }
 
 static void
@@ -1526,10 +1752,6 @@ cortex_m3_dwt_setup(struct cortex_m3_common *cm3, struct target *target)
                return;
        }
 
-       if (dwt_reg_type < 0)
-               dwt_reg_type = register_reg_arch_type(cortex_m3_dwt_get_reg,
-                               cortex_m3_dwt_set_reg);
-
        cm3->dwt_num_comp = (dwtcr >> 28) & 0xF;
        cm3->dwt_comp_available = cm3->dwt_num_comp;
        cm3->dwt_comparator_list = calloc(cm3->dwt_num_comp,
@@ -1588,7 +1810,7 @@ static int cortex_m3_examine(struct target *target)
        uint32_t cpuid, fpcr;
        int i;
        struct cortex_m3_common *cortex_m3 = target_to_cm3(target);
-       struct swjdp_common *swjdp = &cortex_m3->armv7m.swjdp_info;
+       struct adiv5_dap *swjdp = &cortex_m3->armv7m.dap;
 
        if ((retval = ahbap_debugport_init(swjdp)) != ERROR_OK)
                return retval;
@@ -1603,7 +1825,8 @@ static int cortex_m3_examine(struct target *target)
                        return retval;
 
                if (((cpuid >> 4) & 0xc3f) == 0xc23)
-                       LOG_DEBUG("CORTEX-M3 processor detected");
+                       LOG_DEBUG("Cortex-M3 r%" PRId8 "p%" PRId8 " processor detected",
+                               (uint8_t)((cpuid >> 20) & 0xf), (uint8_t)((cpuid >> 0) & 0xf));
                LOG_DEBUG("cpuid: 0x%8.8" PRIx32 "", cpuid);
 
                /* NOTE: FPB and DWT are both optional. */
@@ -1625,14 +1848,21 @@ static int cortex_m3_examine(struct target *target)
 
                /* Setup DWT */
                cortex_m3_dwt_setup(cortex_m3, target);
+
+               /* These hardware breakpoints only work for code in flash! */
+               LOG_INFO("%s: hardware has %d breakpoints, %d watchpoints",
+                               target_name(target),
+                               cortex_m3->fp_num_code,
+                               cortex_m3->dwt_num_comp);
        }
 
        return ERROR_OK;
 }
 
-static int cortex_m3_dcc_read(struct swjdp_common *swjdp, uint8_t *value, uint8_t *ctrl)
+static int cortex_m3_dcc_read(struct adiv5_dap *swjdp, uint8_t *value, uint8_t *ctrl)
 {
        uint16_t dcrdr;
+       int retval;
 
        mem_ap_read_buf_u16(swjdp, (uint8_t*)&dcrdr, 1, DCB_DCRDR);
        *ctrl = (uint8_t)dcrdr;
@@ -1645,7 +1875,9 @@ static int cortex_m3_dcc_read(struct swjdp_common *swjdp, uint8_t *value, uint8_
        if (dcrdr & (1 << 0))
        {
                dcrdr = 0;
-               mem_ap_write_buf_u16(swjdp, (uint8_t*)&dcrdr, 1, DCB_DCRDR);
+               retval = mem_ap_write_buf_u16(swjdp, (uint8_t*)&dcrdr, 1, DCB_DCRDR);
+               if (retval != ERROR_OK)
+                       return retval;
        }
 
        return ERROR_OK;
@@ -1655,7 +1887,7 @@ static int cortex_m3_target_request_data(struct target *target,
                uint32_t size, uint8_t *buffer)
 {
        struct armv7m_common *armv7m = target_to_armv7m(target);
-       struct swjdp_common *swjdp = &armv7m->swjdp_info;
+       struct adiv5_dap *swjdp = &armv7m->dap;
        uint8_t data;
        uint8_t ctrl;
        uint32_t i;
@@ -1675,7 +1907,7 @@ static int cortex_m3_handle_target_request(void *priv)
        if (!target_was_examined(target))
                return ERROR_OK;
        struct armv7m_common *armv7m = target_to_armv7m(target);
-       struct swjdp_common *swjdp = &armv7m->swjdp_info;
+       struct adiv5_dap *swjdp = &armv7m->dap;
 
        if (!target->dbg_msg_enabled)
                return ERROR_OK;
@@ -1719,12 +1951,17 @@ static int cortex_m3_init_arch_info(struct target *target,
        cortex_m3->jtag_info.tap = tap;
        cortex_m3->jtag_info.scann_size = 4;
 
-       armv7m->swjdp_info.dp_select_value = -1;
-       armv7m->swjdp_info.ap_csw_value = -1;
-       armv7m->swjdp_info.ap_tar_value = -1;
-       armv7m->swjdp_info.jtag_info = &cortex_m3->jtag_info;
-       armv7m->swjdp_info.memaccess_tck = 8;
-       armv7m->swjdp_info.tar_autoincr_block = (1 << 12);      /* Cortex-M3 has 4096 bytes autoincrement range */
+       /* default reset mode is to use srst if fitted
+        * if not it will use CORTEX_M3_RESET_VECTRESET */
+       cortex_m3->soft_reset_config = CORTEX_M3_RESET_VECTRESET;
+
+       armv7m->arm.dap = &armv7m->dap;
+
+       /* Leave (only) generic DAP stuff for debugport_init(); */
+       armv7m->dap.jtag_info = &cortex_m3->jtag_info;
+       armv7m->dap.memaccess_tck = 8;
+       /* Cortex-M3 has 4096 bytes autoincrement range */
+       armv7m->dap.tar_autoincr_block = (1 << 12);
 
        /* register arch-specific functions */
        armv7m->examine_debug_reason = cortex_m3_examine_debug_reason;
@@ -1732,7 +1969,6 @@ static int cortex_m3_init_arch_info(struct target *target,
        armv7m->post_debug_entry = NULL;
 
        armv7m->pre_restore_context = NULL;
-       armv7m->post_restore_context = NULL;
 
        armv7m->load_core_reg_u32 = cortex_m3_load_core_reg_u32;
        armv7m->store_core_reg_u32 = cortex_m3_store_core_reg_u32;
@@ -1775,50 +2011,6 @@ static int cortex_m3_verify_pointer(struct command_context *cmd_ctx,
  * cortexm3_target structure, which is only used with CM3 targets.
  */
 
-/*
- * REVISIT Thumb2 disassembly should work for all ARMv7 cores, as well
- * as at least ARM-1156T2.  The interesting thing about Cortex-M is
- * that *only* Thumb2 disassembly matters.  There are also some small
- * additions to Thumb2 that are specific to ARMv7-M.
- */
-COMMAND_HANDLER(handle_cortex_m3_disassemble_command)
-{
-       int retval;
-       struct target *target = get_current_target(cmd_ctx);
-       struct cortex_m3_common *cortex_m3 = target_to_cm3(target);
-       uint32_t address;
-       unsigned long count = 1;
-       struct arm_instruction cur_instruction;
-
-       retval = cortex_m3_verify_pointer(cmd_ctx, cortex_m3);
-       if (retval != ERROR_OK)
-               return retval;
-
-       errno = 0;
-       switch (argc) {
-       case 2:
-               COMMAND_PARSE_NUMBER(ulong, args[1], count);
-               /* FALL THROUGH */
-       case 1:
-               COMMAND_PARSE_NUMBER(u32, args[0], address);
-               break;
-       default:
-               command_print(cmd_ctx,
-                       "usage: cortex_m3 disassemble <address> [<count>]");
-               return ERROR_OK;
-       }
-
-       while (count--) {
-               retval = thumb2_opcode(target, address, &cur_instruction);
-               if (retval != ERROR_OK)
-                       return retval;
-               command_print(cmd_ctx, "%s", cur_instruction.text);
-               address += cur_instruction.instruction_size;
-       }
-
-       return ERROR_OK;
-}
-
 static const struct {
        char name[10];
        unsigned mask;
@@ -1835,121 +2027,184 @@ static const struct {
 
 COMMAND_HANDLER(handle_cortex_m3_vector_catch_command)
 {
-       struct target *target = get_current_target(cmd_ctx);
+       struct target *target = get_current_target(CMD_CTX);
        struct cortex_m3_common *cortex_m3 = target_to_cm3(target);
        struct armv7m_common *armv7m = &cortex_m3->armv7m;
-       struct swjdp_common *swjdp = &armv7m->swjdp_info;
+       struct adiv5_dap *swjdp = &armv7m->dap;
        uint32_t demcr = 0;
        int retval;
-       retval = cortex_m3_verify_pointer(cmd_ctx, cortex_m3);
+
+       retval = cortex_m3_verify_pointer(CMD_CTX, cortex_m3);
        if (retval != ERROR_OK)
                return retval;
 
-       mem_ap_read_atomic_u32(swjdp, DCB_DEMCR, &demcr);
+       retval = mem_ap_read_atomic_u32(swjdp, DCB_DEMCR, &demcr);
+       if (retval != ERROR_OK)
+               return retval;
 
-       if (argc > 0) {
+       if (CMD_ARGC > 0) {
                unsigned catch = 0;
 
-               if (argc == 1) {
-                       if (strcmp(args[0], "all") == 0) {
+               if (CMD_ARGC == 1) {
+                       if (strcmp(CMD_ARGV[0], "all") == 0) {
                                catch = VC_HARDERR | VC_INTERR | VC_BUSERR
                                        | VC_STATERR | VC_CHKERR | VC_NOCPERR
                                        | VC_MMERR | VC_CORERESET;
                                goto write;
-                       } else if (strcmp(args[0], "none") == 0) {
+                       } else if (strcmp(CMD_ARGV[0], "none") == 0) {
                                goto write;
                        }
                }
-               while (argc-- > 0) {
+               while (CMD_ARGC-- > 0) {
                        unsigned i;
                        for (i = 0; i < ARRAY_SIZE(vec_ids); i++) {
-                               if (strcmp(args[argc], vec_ids[i].name) != 0)
+                               if (strcmp(CMD_ARGV[CMD_ARGC], vec_ids[i].name) != 0)
                                        continue;
                                catch |= vec_ids[i].mask;
                                break;
                        }
                        if (i == ARRAY_SIZE(vec_ids)) {
-                               LOG_ERROR("No CM3 vector '%s'", args[argc]);
+                               LOG_ERROR("No CM3 vector '%s'", CMD_ARGV[CMD_ARGC]);
                                return ERROR_INVALID_ARGUMENTS;
                        }
                }
 write:
+               /* For now, armv7m->demcr only stores vector catch flags. */
+               armv7m->demcr = catch;
+
                demcr &= ~0xffff;
                demcr |= catch;
 
-               /* write, but don't assume it stuck */
-               mem_ap_write_u32(swjdp, DCB_DEMCR, demcr);
-               mem_ap_read_atomic_u32(swjdp, DCB_DEMCR, &demcr);
+               /* write, but don't assume it stuck (why not??) */
+               retval = mem_ap_write_u32(swjdp, DCB_DEMCR, demcr);
+               if (retval != ERROR_OK)
+                       return retval;
+               retval = mem_ap_read_atomic_u32(swjdp, DCB_DEMCR, &demcr);
+               if (retval != ERROR_OK)
+                       return retval;
+
+               /* FIXME be sure to clear DEMCR on clean server shutdown.
+                * Otherwise the vector catch hardware could fire when there's
+                * no debugger hooked up, causing much confusion...
+                */
        }
 
        for (unsigned i = 0; i < ARRAY_SIZE(vec_ids); i++)
-               command_print(cmd_ctx, "%9s: %s", vec_ids[i].name,
+       {
+               command_print(CMD_CTX, "%9s: %s", vec_ids[i].name,
                        (demcr & vec_ids[i].mask) ? "catch" : "ignore");
+       }
 
        return ERROR_OK;
 }
 
 COMMAND_HANDLER(handle_cortex_m3_mask_interrupts_command)
 {
-       struct target *target = get_current_target(cmd_ctx);
+       struct target *target = get_current_target(CMD_CTX);
        struct cortex_m3_common *cortex_m3 = target_to_cm3(target);
        int retval;
 
-       retval = cortex_m3_verify_pointer(cmd_ctx, cortex_m3);
+       retval = cortex_m3_verify_pointer(CMD_CTX, cortex_m3);
        if (retval != ERROR_OK)
                return retval;
 
        if (target->state != TARGET_HALTED)
        {
-               command_print(cmd_ctx, "target must be stopped for \"%s\" command", CMD_NAME);
+               command_print(CMD_CTX, "target must be stopped for \"%s\" command", CMD_NAME);
                return ERROR_OK;
        }
 
-       if (argc > 0)
+       if (CMD_ARGC > 0)
        {
-               if (!strcmp(args[0], "on"))
-               {
-                       cortex_m3_write_debug_halt_mask(target, C_HALT | C_MASKINTS, 0);
-               }
-               else if (!strcmp(args[0], "off"))
-               {
-                       cortex_m3_write_debug_halt_mask(target, C_HALT, C_MASKINTS);
-               }
-               else
-               {
-                       command_print(cmd_ctx, "usage: cortex_m3 maskisr ['on'|'off']");
-               }
+               bool enable;
+               COMMAND_PARSE_ON_OFF(CMD_ARGV[0], enable);
+               uint32_t mask_on = C_HALT | (enable ? C_MASKINTS : 0);
+               uint32_t mask_off = enable ? 0 : C_MASKINTS;
+               cortex_m3_write_debug_halt_mask(target, mask_on, mask_off);
        }
 
-       command_print(cmd_ctx, "cortex_m3 interrupt mask %s",
+       command_print(CMD_CTX, "cortex_m3 interrupt mask %s",
                        (cortex_m3->dcb_dhcsr & C_MASKINTS) ? "on" : "off");
 
        return ERROR_OK;
 }
 
-static int cortex_m3_register_commands(struct command_context *cmd_ctx)
+COMMAND_HANDLER(handle_cortex_m3_reset_config_command)
 {
+       struct target *target = get_current_target(CMD_CTX);
+       struct cortex_m3_common *cortex_m3 = target_to_cm3(target);
        int retval;
-       struct command *cortex_m3_cmd;
+       char *reset_config;
+
+       retval = cortex_m3_verify_pointer(CMD_CTX, cortex_m3);
+       if (retval != ERROR_OK)
+               return retval;
 
-       retval = armv7m_register_commands(cmd_ctx);
+       if (CMD_ARGC > 0)
+       {
+               if (strcmp(*CMD_ARGV, "sysresetreq") == 0)
+                       cortex_m3->soft_reset_config = CORTEX_M3_RESET_SYSRESETREQ;
+               else if (strcmp(*CMD_ARGV, "vectreset") == 0)
+                       cortex_m3->soft_reset_config = CORTEX_M3_RESET_VECTRESET;
+       }
 
-       cortex_m3_cmd = register_command(cmd_ctx, NULL, "cortex_m3",
-                       NULL, COMMAND_ANY, "cortex_m3 specific commands");
+       switch (cortex_m3->soft_reset_config)
+       {
+               case CORTEX_M3_RESET_SYSRESETREQ:
+                       reset_config = "sysresetreq";
+                       break;
 
-       register_command(cmd_ctx, cortex_m3_cmd, "disassemble",
-                       handle_cortex_m3_disassemble_command, COMMAND_EXEC,
-                       "disassemble Thumb2 instructions <address> [<count>]");
-       register_command(cmd_ctx, cortex_m3_cmd, "maskisr",
-                       handle_cortex_m3_mask_interrupts_command, COMMAND_EXEC,
-                       "mask cortex_m3 interrupts ['on'|'off']");
-       register_command(cmd_ctx, cortex_m3_cmd, "vector_catch",
-                       handle_cortex_m3_vector_catch_command, COMMAND_EXEC,
-                       "catch hardware vectors ['all'|'none'|<list>]");
+               case CORTEX_M3_RESET_VECTRESET:
+                       reset_config = "vectreset";
+                       break;
 
-       return retval;
+               default:
+                       reset_config = "unknown";
+                       break;
+       }
+
+       command_print(CMD_CTX, "cortex_m3 reset_config %s", reset_config);
+
+       return ERROR_OK;
 }
 
+static const struct command_registration cortex_m3_exec_command_handlers[] = {
+       {
+               .name = "maskisr",
+               .handler = handle_cortex_m3_mask_interrupts_command,
+               .mode = COMMAND_EXEC,
+               .help = "mask cortex_m3 interrupts",
+               .usage = "['on'|'off']",
+       },
+       {
+               .name = "vector_catch",
+               .handler = handle_cortex_m3_vector_catch_command,
+               .mode = COMMAND_EXEC,
+               .help = "configure hardware vectors to trigger debug entry",
+               .usage = "['all'|'none'|('bus_err'|'chk_err'|...)*]",
+       },
+       {
+               .name = "reset_config",
+               .handler = handle_cortex_m3_reset_config_command,
+               .mode = COMMAND_ANY,
+               .help = "configure software reset handling",
+               .usage = "['srst'|'sysresetreq'|'vectreset']",
+       },
+       COMMAND_REGISTRATION_DONE
+};
+static const struct command_registration cortex_m3_command_handlers[] = {
+       {
+               .chain = armv7m_command_handlers,
+       },
+       {
+               .name = "cortex_m3",
+               .mode = COMMAND_EXEC,
+               .help = "Cortex-M3 command group",
+               .chain = cortex_m3_exec_command_handlers,
+       },
+       COMMAND_REGISTRATION_DONE
+};
+
 struct target_type cortexm3_target =
 {
        .name = "cortex_m3",
@@ -1982,7 +2237,7 @@ struct target_type cortexm3_target =
        .add_watchpoint = cortex_m3_add_watchpoint,
        .remove_watchpoint = cortex_m3_remove_watchpoint,
 
-       .register_commands = cortex_m3_register_commands,
+       .commands = cortex_m3_command_handlers,
        .target_create = cortex_m3_target_create,
        .init_target = cortex_m3_init_target,
        .examine = cortex_m3_examine,