armv7a_mmu: Do not restrict virtual addresses to uint32_t
[fw/openocd] / src / target / cortex_a.c
index 2768e18cf91055f8d19b2572d7b07dd71b6f6973..158de0b2476e98511ab97826c81bd69912058858 100644 (file)
 #include "breakpoints.h"
 #include "cortex_a.h"
 #include "register.h"
+#include "armv7a_mmu.h"
 #include "target_request.h"
 #include "target_type.h"
 #include "arm_opcodes.h"
 #include "arm_semihosting.h"
 #include "transport/transport.h"
+#include "smp.h"
 #include <helper/time_support.h>
 
-#define foreach_smp_target(pos, head) \
-       for (pos = head; (pos != NULL); pos = pos->next)
-
 static int cortex_a_poll(struct target *target);
 static int cortex_a_debug_entry(struct target *target);
 static int cortex_a_restore_context(struct target *target, bool bpwp);
@@ -71,10 +70,8 @@ static int cortex_a_set_hybrid_breakpoint(struct target *target,
        struct breakpoint *breakpoint);
 static int cortex_a_unset_breakpoint(struct target *target,
        struct breakpoint *breakpoint);
-static int cortex_a_dap_read_coreregister_u32(struct target *target,
-       uint32_t *value, int regnum);
-static int cortex_a_dap_write_coreregister_u32(struct target *target,
-       uint32_t value, int regnum);
+static int cortex_a_wait_dscr_bits(struct target *target, uint32_t mask,
+       uint32_t value, uint32_t *dscr);
 static int cortex_a_mmu(struct target *target, int *enabled);
 static int cortex_a_mmu_modify(struct target *target, int enable);
 static int cortex_a_virt2phys(struct target *target,
@@ -113,7 +110,7 @@ static int cortex_a_prep_memaccess(struct target *target, int phys_access)
        int mmu_enabled = 0;
 
        if (phys_access == 0) {
-               dpm_modeswitch(&armv7a->dpm, ARM_MODE_SVC);
+               arm_dpm_modeswitch(&armv7a->dpm, ARM_MODE_SVC);
                cortex_a_mmu(target, &mmu_enabled);
                if (mmu_enabled)
                        cortex_a_mmu_modify(target, 1);
@@ -148,7 +145,7 @@ static int cortex_a_post_memaccess(struct target *target, int phys_access)
                                        0, 0, 3, 0,
                                        cortex_a->cp15_dacr_reg);
                }
-               dpm_modeswitch(&armv7a->dpm, ARM_MODE_ANY);
+               arm_dpm_modeswitch(&armv7a->dpm, ARM_MODE_ANY);
        } else {
                int mmu_enabled = 0;
                cortex_a_mmu(target, &mmu_enabled);
@@ -205,6 +202,7 @@ static int cortex_a_mmu_modify(struct target *target, int enable)
 static int cortex_a_init_debug_access(struct target *target)
 {
        struct armv7a_common *armv7a = target_to_armv7a(target);
+       uint32_t dscr;
        int retval;
 
        /* lock memory-mapped access to debug registers to prevent
@@ -234,6 +232,16 @@ static int cortex_a_init_debug_access(struct target *target)
 
        /* Resync breakpoint registers */
 
+       /* Enable halt for breakpoint, watchpoint and vector catch */
+       retval = mem_ap_read_atomic_u32(armv7a->debug_ap,
+                       armv7a->debug_base + CPUDBG_DSCR, &dscr);
+       if (retval != ERROR_OK)
+               return retval;
+       retval = mem_ap_write_atomic_u32(armv7a->debug_ap,
+                       armv7a->debug_base + CPUDBG_DSCR, dscr | DSCR_HALT_DBG_MODE);
+       if (retval != ERROR_OK)
+               return retval;
+
        /* Since this is likely called from init or reset, update target state information*/
        return cortex_a_poll(target);
 }
@@ -244,21 +252,21 @@ static int cortex_a_wait_instrcmpl(struct target *target, uint32_t *dscr, bool f
         * Writes final value of DSCR into *dscr. Pass force to force always
         * reading DSCR at least once. */
        struct armv7a_common *armv7a = target_to_armv7a(target);
-       int64_t then = timeval_ms();
-       while ((*dscr & DSCR_INSTR_COMP) == 0 || force) {
-               force = false;
-               int retval = mem_ap_read_atomic_u32(armv7a->debug_ap,
+       int retval;
+
+       if (force) {
+               retval = mem_ap_read_atomic_u32(armv7a->debug_ap,
                                armv7a->debug_base + CPUDBG_DSCR, dscr);
                if (retval != ERROR_OK) {
                        LOG_ERROR("Could not read DSCR register");
                        return retval;
                }
-               if (timeval_ms() > then + 1000) {
-                       LOG_ERROR("Timeout waiting for InstrCompl=1");
-                       return ERROR_FAIL;
-               }
        }
-       return ERROR_OK;
+
+       retval = cortex_a_wait_dscr_bits(target, DSCR_INSTR_COMP, DSCR_INSTR_COMP, dscr);
+       if (retval != ERROR_OK)
+               LOG_ERROR("Error waiting for InstrCompl=1");
+       return retval;
 }
 
 /* To reduce needless round-trips, pass in a pointer to the current
@@ -287,163 +295,15 @@ static int cortex_a_exec_opcode(struct target *target,
        if (retval != ERROR_OK)
                return retval;
 
-       int64_t then = timeval_ms();
-       do {
-               retval = mem_ap_read_atomic_u32(armv7a->debug_ap,
-                               armv7a->debug_base + CPUDBG_DSCR, &dscr);
-               if (retval != ERROR_OK) {
-                       LOG_ERROR("Could not read DSCR register");
-                       return retval;
-               }
-               if (timeval_ms() > then + 1000) {
-                       LOG_ERROR("Timeout waiting for cortex_a_exec_opcode");
-                       return ERROR_FAIL;
-               }
-       } while ((dscr & DSCR_INSTR_COMP) == 0);        /* Wait for InstrCompl bit to be set */
-
-       if (dscr_p)
-               *dscr_p = dscr;
-
-       return retval;
-}
-
-static int cortex_a_dap_read_coreregister_u32(struct target *target,
-       uint32_t *value, int regnum)
-{
-       int retval = ERROR_OK;
-       uint8_t reg = regnum&0xFF;
-       uint32_t dscr = 0;
-       struct armv7a_common *armv7a = target_to_armv7a(target);
-
-       if (reg > 17)
-               return retval;
-
-       if (reg < 15) {
-               /* Rn to DCCTX, "MCR p14, 0, Rn, c0, c5, 0"  0xEE00nE15 */
-               retval = cortex_a_exec_opcode(target,
-                               ARMV4_5_MCR(14, 0, reg, 0, 5, 0),
-                               &dscr);
-               if (retval != ERROR_OK)
-                       return retval;
-       } else if (reg == 15) {
-               /* "MOV r0, r15"; then move r0 to DCCTX */
-               retval = cortex_a_exec_opcode(target, 0xE1A0000F, &dscr);
-               if (retval != ERROR_OK)
-                       return retval;
-               retval = cortex_a_exec_opcode(target,
-                               ARMV4_5_MCR(14, 0, 0, 0, 5, 0),
-                               &dscr);
-               if (retval != ERROR_OK)
-                       return retval;
-       } else {
-               /* "MRS r0, CPSR" or "MRS r0, SPSR"
-                * then move r0 to DCCTX
-                */
-               retval = cortex_a_exec_opcode(target, ARMV4_5_MRS(0, reg & 1), &dscr);
-               if (retval != ERROR_OK)
-                       return retval;
-               retval = cortex_a_exec_opcode(target,
-                               ARMV4_5_MCR(14, 0, 0, 0, 5, 0),
-                               &dscr);
-               if (retval != ERROR_OK)
-                       return retval;
-       }
-
-       /* Wait for DTRRXfull then read DTRRTX */
-       int64_t then = timeval_ms();
-       while ((dscr & DSCR_DTR_TX_FULL) == 0) {
-               retval = mem_ap_read_atomic_u32(armv7a->debug_ap,
-                               armv7a->debug_base + CPUDBG_DSCR, &dscr);
-               if (retval != ERROR_OK)
-                       return retval;
-               if (timeval_ms() > then + 1000) {
-                       LOG_ERROR("Timeout waiting for cortex_a_exec_opcode");
-                       return ERROR_FAIL;
-               }
-       }
-
-       retval = mem_ap_read_atomic_u32(armv7a->debug_ap,
-                       armv7a->debug_base + CPUDBG_DTRTX, value);
-       LOG_DEBUG("read DCC 0x%08" PRIx32, *value);
-
-       return retval;
-}
-
-__attribute__((unused))
-static int cortex_a_dap_write_coreregister_u32(struct target *target,
-       uint32_t value, int regnum)
-{
-       int retval = ERROR_OK;
-       uint8_t Rd = regnum&0xFF;
-       uint32_t dscr;
-       struct armv7a_common *armv7a = target_to_armv7a(target);
-
-       LOG_DEBUG("register %i, value 0x%08" PRIx32, regnum, value);
-
-       /* Check that DCCRX is not full */
-       retval = mem_ap_read_atomic_u32(armv7a->debug_ap,
-                       armv7a->debug_base + CPUDBG_DSCR, &dscr);
-       if (retval != ERROR_OK)
+       /* Wait for InstrCompl bit to be set */
+       retval = cortex_a_wait_instrcmpl(target, &dscr, true);
+       if (retval != ERROR_OK) {
+               LOG_ERROR("Error waiting for cortex_a_exec_opcode");
                return retval;
-       if (dscr & DSCR_DTR_RX_FULL) {
-               LOG_ERROR("DSCR_DTR_RX_FULL, dscr 0x%08" PRIx32, dscr);
-               /* Clear DCCRX with MRC(p14, 0, Rd, c0, c5, 0), opcode  0xEE100E15 */
-               retval = cortex_a_exec_opcode(target, ARMV4_5_MRC(14, 0, 0, 0, 5, 0),
-                               &dscr);
-               if (retval != ERROR_OK)
-                       return retval;
        }
 
-       if (Rd > 17)
-               return retval;
-
-       /* Write DTRRX ... sets DSCR.DTRRXfull but exec_opcode() won't care */
-       LOG_DEBUG("write DCC 0x%08" PRIx32, value);
-       retval = mem_ap_write_u32(armv7a->debug_ap,
-                       armv7a->debug_base + CPUDBG_DTRRX, value);
-       if (retval != ERROR_OK)
-               return retval;
-
-       if (Rd < 15) {
-               /* DCCRX to Rn, "MRC p14, 0, Rn, c0, c5, 0", 0xEE10nE15 */
-               retval = cortex_a_exec_opcode(target, ARMV4_5_MRC(14, 0, Rd, 0, 5, 0),
-                               &dscr);
-
-               if (retval != ERROR_OK)
-                       return retval;
-       } else if (Rd == 15) {
-               /* DCCRX to R0, "MRC p14, 0, R0, c0, c5, 0", 0xEE100E15
-                * then "mov r15, r0"
-                */
-               retval = cortex_a_exec_opcode(target, ARMV4_5_MRC(14, 0, 0, 0, 5, 0),
-                               &dscr);
-               if (retval != ERROR_OK)
-                       return retval;
-               retval = cortex_a_exec_opcode(target, 0xE1A0F000, &dscr);
-               if (retval != ERROR_OK)
-                       return retval;
-       } else {
-               /* DCCRX to R0, "MRC p14, 0, R0, c0, c5, 0", 0xEE100E15
-                * then "MSR CPSR_cxsf, r0" or "MSR SPSR_cxsf, r0" (all fields)
-                */
-               retval = cortex_a_exec_opcode(target, ARMV4_5_MRC(14, 0, 0, 0, 5, 0),
-                               &dscr);
-               if (retval != ERROR_OK)
-                       return retval;
-               retval = cortex_a_exec_opcode(target, ARMV4_5_MSR_GP(0, 0xF, Rd & 1),
-                               &dscr);
-               if (retval != ERROR_OK)
-                       return retval;
-
-               /* "Prefetch flush" after modifying execution status in CPSR */
-               if (Rd == 16) {
-                       retval = cortex_a_exec_opcode(target,
-                                       ARMV4_5_MCR(15, 0, 0, 7, 5, 4),
-                                       &dscr);
-                       if (retval != ERROR_OK)
-                               return retval;
-               }
-       }
+       if (dscr_p)
+               *dscr_p = dscr;
 
        return retval;
 }
@@ -494,17 +354,11 @@ static int cortex_a_read_dcc(struct cortex_a_common *a, uint32_t *data,
                dscr = *dscr_p;
 
        /* Wait for DTRRXfull */
-       int64_t then = timeval_ms();
-       while ((dscr & DSCR_DTR_TX_FULL) == 0) {
-               retval = mem_ap_read_atomic_u32(a->armv7a_common.debug_ap,
-                               a->armv7a_common.debug_base + CPUDBG_DSCR,
-                               &dscr);
-               if (retval != ERROR_OK)
-                       return retval;
-               if (timeval_ms() > then + 1000) {
-                       LOG_ERROR("Timeout waiting for read dcc");
-                       return ERROR_FAIL;
-               }
+       retval = cortex_a_wait_dscr_bits(a->armv7a_common.arm.target,
+                       DSCR_DTR_TX_FULL, DSCR_DTR_TX_FULL, &dscr);
+       if (retval != ERROR_OK) {
+               LOG_ERROR("Error waiting for read dcc");
+               return retval;
        }
 
        retval = mem_ap_read_atomic_u32(a->armv7a_common.debug_ap,
@@ -526,19 +380,10 @@ static int cortex_a_dpm_prepare(struct arm_dpm *dpm)
        int retval;
 
        /* set up invariant:  INSTR_COMP is set after ever DPM operation */
-       int64_t then = timeval_ms();
-       for (;; ) {
-               retval = mem_ap_read_atomic_u32(a->armv7a_common.debug_ap,
-                               a->armv7a_common.debug_base + CPUDBG_DSCR,
-                               &dscr);
-               if (retval != ERROR_OK)
-                       return retval;
-               if ((dscr & DSCR_INSTR_COMP) != 0)
-                       break;
-               if (timeval_ms() > then + 1000) {
-                       LOG_ERROR("Timeout waiting for dpm prepare");
-                       return ERROR_FAIL;
-               }
+       retval = cortex_a_wait_instrcmpl(dpm->arm->target, &dscr, true);
+       if (retval != ERROR_OK) {
+               LOG_ERROR("Error waiting for dpm prepare");
+               return retval;
        }
 
        /* this "should never happen" ... */
@@ -858,39 +703,26 @@ static int cortex_a_poll(struct target *target)
                        /* We have a halting debug event */
                        LOG_DEBUG("Target halted");
                        target->state = TARGET_HALTED;
-                       if ((prev_target_state == TARGET_RUNNING)
-                               || (prev_target_state == TARGET_UNKNOWN)
-                               || (prev_target_state == TARGET_RESET)) {
-                               retval = cortex_a_debug_entry(target);
+
+                       retval = cortex_a_debug_entry(target);
+                       if (retval != ERROR_OK)
+                               return retval;
+
+                       if (target->smp) {
+                               retval = update_halt_gdb(target);
                                if (retval != ERROR_OK)
                                        return retval;
-                               if (target->smp) {
-                                       retval = update_halt_gdb(target);
-                                       if (retval != ERROR_OK)
-                                               return retval;
-                               }
+                       }
 
+                       if (prev_target_state == TARGET_DEBUG_RUNNING) {
+                               target_call_event_callbacks(target, TARGET_EVENT_DEBUG_HALTED);
+                       } else { /* prev_target_state is RUNNING, UNKNOWN or RESET */
                                if (arm_semihosting(target, &retval) != 0)
                                        return retval;
 
                                target_call_event_callbacks(target,
                                        TARGET_EVENT_HALTED);
                        }
-                       if (prev_target_state == TARGET_DEBUG_RUNNING) {
-                               LOG_DEBUG(" ");
-
-                               retval = cortex_a_debug_entry(target);
-                               if (retval != ERROR_OK)
-                                       return retval;
-                               if (target->smp) {
-                                       retval = update_halt_gdb(target);
-                                       if (retval != ERROR_OK)
-                                               return retval;
-                               }
-
-                               target_call_event_callbacks(target,
-                                       TARGET_EVENT_DEBUG_HALTED);
-                       }
                }
        } else
                target->state = TARGET_RUNNING;
@@ -900,7 +732,7 @@ static int cortex_a_poll(struct target *target)
 
 static int cortex_a_halt(struct target *target)
 {
-       int retval = ERROR_OK;
+       int retval;
        uint32_t dscr;
        struct armv7a_common *armv7a = target_to_armv7a(target);
 
@@ -913,31 +745,12 @@ static int cortex_a_halt(struct target *target)
        if (retval != ERROR_OK)
                return retval;
 
-       /*
-        * enter halting debug mode
-        */
-       retval = mem_ap_read_atomic_u32(armv7a->debug_ap,
-                       armv7a->debug_base + CPUDBG_DSCR, &dscr);
-       if (retval != ERROR_OK)
-               return retval;
-
-       retval = mem_ap_write_atomic_u32(armv7a->debug_ap,
-                       armv7a->debug_base + CPUDBG_DSCR, dscr | DSCR_HALT_DBG_MODE);
-       if (retval != ERROR_OK)
+       dscr = 0; /* force read of dscr */
+       retval = cortex_a_wait_dscr_bits(target, DSCR_CORE_HALTED,
+                       DSCR_CORE_HALTED, &dscr);
+       if (retval != ERROR_OK) {
+               LOG_ERROR("Error waiting for halt");
                return retval;
-
-       int64_t then = timeval_ms();
-       for (;; ) {
-               retval = mem_ap_read_atomic_u32(armv7a->debug_ap,
-                               armv7a->debug_base + CPUDBG_DSCR, &dscr);
-               if (retval != ERROR_OK)
-                       return retval;
-               if ((dscr & DSCR_CORE_HALTED) != 0)
-                       break;
-               if (timeval_ms() > then + 1000) {
-                       LOG_ERROR("Timeout waiting for halt");
-                       return ERROR_FAIL;
-               }
        }
 
        target->debug_reason = DBG_REASON_DBGRQ;
@@ -965,15 +778,15 @@ static int cortex_a_internal_restore(struct target *target, int current,
                 * 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;
+               armv7m->core_cache->reg_list[ARMV7M_PRIMASK].dirty = true;
+               armv7m->core_cache->reg_list[ARMV7M_PRIMASK].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;
+               armv7m->core_cache->reg_list[ARMV7M_xPSR].dirty = true;
+               armv7m->core_cache->reg_list[ARMV7M_xPSR].valid = true;
        }
 #endif
 
@@ -1007,11 +820,11 @@ static int cortex_a_internal_restore(struct target *target, int current,
        }
        LOG_DEBUG("resume pc = 0x%08" PRIx32, resume_pc);
        buf_set_u32(arm->pc->value, 0, 32, resume_pc);
-       arm->pc->dirty = 1;
-       arm->pc->valid = 1;
+       arm->pc->dirty = true;
+       arm->pc->valid = true;
 
        /* restore dpm_mode at system halt */
-       dpm_modeswitch(&armv7a->dpm, ARM_MODE_ANY);
+       arm_dpm_modeswitch(&armv7a->dpm, ARM_MODE_ANY);
        /* called it now before restoring context because it uses cpu
         * register r0 for restoring cp15 control register */
        retval = cortex_a_restore_cp15_control_reg(target);
@@ -1076,18 +889,12 @@ static int cortex_a_internal_restart(struct target *target)
        if (retval != ERROR_OK)
                return retval;
 
-       int64_t then = timeval_ms();
-       for (;; ) {
-               retval = mem_ap_read_atomic_u32(armv7a->debug_ap,
-                               armv7a->debug_base + CPUDBG_DSCR, &dscr);
-               if (retval != ERROR_OK)
-                       return retval;
-               if ((dscr & DSCR_CORE_RESTARTED) != 0)
-                       break;
-               if (timeval_ms() > then + 1000) {
-                       LOG_ERROR("Timeout waiting for resume");
-                       return ERROR_FAIL;
-               }
+       dscr = 0; /* force read of dscr */
+       retval = cortex_a_wait_dscr_bits(target, DSCR_CORE_RESTARTED,
+                       DSCR_CORE_RESTARTED, &dscr);
+       if (retval != ERROR_OK) {
+               LOG_ERROR("Error waiting for resume");
+               return retval;
        }
 
        target->debug_reason = DBG_REASON_NOTHALTED;
@@ -1158,12 +965,11 @@ static int cortex_a_resume(struct target *target, int current,
 
 static int cortex_a_debug_entry(struct target *target)
 {
-       uint32_t spsr, dscr;
+       uint32_t dscr;
        int retval = ERROR_OK;
        struct cortex_a_common *cortex_a = target_to_cortex_a(target);
        struct armv7a_common *armv7a = target_to_armv7a(target);
        struct arm *arm = &armv7a->arm;
-       struct reg *reg;
 
        LOG_DEBUG("dscr = 0x%08" PRIx32, cortex_a->cpudbg_dscr);
 
@@ -1206,16 +1012,10 @@ static int cortex_a_debug_entry(struct target *target)
                return retval;
 
        if (arm->spsr) {
-               /* read Saved PSR */
-               retval = cortex_a_dap_read_coreregister_u32(target, &spsr, 17);
-               /*  store current spsr */
+               /* read SPSR */
+               retval = arm_dpm_read_reg(&armv7a->dpm, arm->spsr, 17);
                if (retval != ERROR_OK)
                        return retval;
-
-               reg = arm->spsr;
-               buf_set_u32(reg->value, 0, 32, spsr);
-               reg->valid = 1;
-               reg->dirty = 0;
        }
 
 #if 0
@@ -1277,7 +1077,7 @@ static int cortex_a_post_debug_entry(struct target *target)
        cortex_a->curr_mode = armv7a->arm.core_mode;
 
        /* switch to SVC mode to read DACR */
-       dpm_modeswitch(&armv7a->dpm, ARM_MODE_SVC);
+       arm_dpm_modeswitch(&armv7a->dpm, ARM_MODE_SVC);
        armv7a->arm.mrc(target, 15,
                        0, 0, 3, 0,
                        &cortex_a->cp15_dacr_reg);
@@ -1285,7 +1085,7 @@ static int cortex_a_post_debug_entry(struct target *target)
        LOG_DEBUG("cp15_dacr_reg: %8.8" PRIx32,
                        cortex_a->cp15_dacr_reg);
 
-       dpm_modeswitch(&armv7a->dpm, ARM_MODE_ANY);
+       arm_dpm_modeswitch(&armv7a->dpm, ARM_MODE_ANY);
        return ERROR_OK;
 }
 
@@ -1374,6 +1174,8 @@ static int cortex_a_step(struct target *target, int current, target_addr_t addre
                retval = cortex_a_poll(target);
                if (retval != ERROR_OK)
                        return retval;
+               if (target->state == TARGET_HALTED)
+                       break;
                if (timeval_ms() > then + 1000) {
                        LOG_ERROR("timeout waiting for target halt");
                        return ERROR_FAIL;
@@ -1931,14 +1733,22 @@ static int cortex_a_wait_dscr_bits(struct target *target, uint32_t mask,
 {
        /* Waits until the specified bit(s) of DSCR take on a specified value. */
        struct armv7a_common *armv7a = target_to_armv7a(target);
-       int64_t then = timeval_ms();
+       int64_t then;
        int retval;
 
-       while ((*dscr & mask) != value) {
+       if ((*dscr & mask) == value)
+               return ERROR_OK;
+
+       then = timeval_ms();
+       while (1) {
                retval = mem_ap_read_atomic_u32(armv7a->debug_ap,
                                armv7a->debug_base + CPUDBG_DSCR, dscr);
-               if (retval != ERROR_OK)
+               if (retval != ERROR_OK) {
+                       LOG_ERROR("Could not read DSCR register");
                        return retval;
+               }
+               if ((*dscr & mask) == value)
+                       break;
                if (timeval_ms() > then + 1000) {
                        LOG_ERROR("timeout waiting for DSCR bit change");
                        return ERROR_FAIL;
@@ -2999,15 +2809,15 @@ static int cortex_a_init_arch_info(struct target *target,
 
        /* REVISIT v7a setup should be in a v7a-specific routine */
        armv7a_init_arch_info(target, armv7a);
-       target_register_timer_callback(cortex_a_handle_target_request, 1, 1, target);
+       target_register_timer_callback(cortex_a_handle_target_request, 1,
+               TARGET_TIMER_TYPE_PERIODIC, target);
 
        return ERROR_OK;
 }
 
 static int cortex_a_target_create(struct target *target, Jim_Interp *interp)
 {
-       struct cortex_a_common *cortex_a = calloc(1, sizeof(struct cortex_a_common));
-       cortex_a->common_magic = CORTEX_A_COMMON_MAGIC;
+       struct cortex_a_common *cortex_a;
        struct adiv5_private_config *pc;
 
        if (target->private_config == NULL)
@@ -3015,8 +2825,13 @@ static int cortex_a_target_create(struct target *target, Jim_Interp *interp)
 
        pc = (struct adiv5_private_config *)target->private_config;
 
+       cortex_a = calloc(1, sizeof(struct cortex_a_common));
+       if (cortex_a == NULL) {
+               LOG_ERROR("Out of memory");
+               return ERROR_FAIL;
+       }
+       cortex_a->common_magic = CORTEX_A_COMMON_MAGIC;
        cortex_a->armv7a_common.is_armv7r = false;
-
        cortex_a->armv7a_common.arm.arm_vfp_version = ARM_VFP_V3;
 
        return cortex_a_init_arch_info(target, cortex_a, pc->dap);
@@ -3024,14 +2839,19 @@ static int cortex_a_target_create(struct target *target, Jim_Interp *interp)
 
 static int cortex_r4_target_create(struct target *target, Jim_Interp *interp)
 {
-       struct cortex_a_common *cortex_a = calloc(1, sizeof(struct cortex_a_common));
-       cortex_a->common_magic = CORTEX_A_COMMON_MAGIC;
+       struct cortex_a_common *cortex_a;
        struct adiv5_private_config *pc;
 
        pc = (struct adiv5_private_config *)target->private_config;
        if (adiv5_verify_config(pc) != ERROR_OK)
                return ERROR_FAIL;
 
+       cortex_a = calloc(1, sizeof(struct cortex_a_common));
+       if (cortex_a == NULL) {
+               LOG_ERROR("Out of memory");
+               return ERROR_FAIL;
+       }
+       cortex_a->common_magic = CORTEX_A_COMMON_MAGIC;
        cortex_a->armv7a_common.is_armv7r = true;
 
        return cortex_a_init_arch_info(target, cortex_a, pc->dap);
@@ -3040,7 +2860,20 @@ static int cortex_r4_target_create(struct target *target, Jim_Interp *interp)
 static void cortex_a_deinit_target(struct target *target)
 {
        struct cortex_a_common *cortex_a = target_to_cortex_a(target);
-       struct arm_dpm *dpm = &cortex_a->armv7a_common.dpm;
+       struct armv7a_common *armv7a = &cortex_a->armv7a_common;
+       struct arm_dpm *dpm = &armv7a->dpm;
+       uint32_t dscr;
+       int retval;
+
+       if (target_was_examined(target)) {
+               /* Disable halt for breakpoint, watchpoint and vector catch */
+               retval = mem_ap_read_atomic_u32(armv7a->debug_ap,
+                               armv7a->debug_base + CPUDBG_DSCR, &dscr);
+               if (retval == ERROR_OK)
+                       mem_ap_write_atomic_u32(armv7a->debug_ap,
+                                       armv7a->debug_base + CPUDBG_DSCR,
+                                       dscr & ~DSCR_HALT_DBG_MODE);
+       }
 
        free(cortex_a->brp_list);
        free(dpm->dbp);
@@ -3089,7 +2922,7 @@ static int cortex_a_virt2phys(struct target *target,
        if (retval != ERROR_OK)
                return retval;
        return armv7a_mmu_translate_va_pa(target, (uint32_t)virt,
-                                                   (uint32_t *)phys, 1);
+                                                   phys, 1);
 }
 
 COMMAND_HANDLER(cortex_a_handle_cache_info_command)
@@ -3097,7 +2930,7 @@ COMMAND_HANDLER(cortex_a_handle_cache_info_command)
        struct target *target = get_current_target(CMD_CTX);
        struct armv7a_common *armv7a = target_to_armv7a(target);
 
-       return armv7a_handle_cache_info_command(CMD_CTX,
+       return armv7a_handle_cache_info_command(CMD,
                        &armv7a->armv7a_mmu.armv7a_cache);
 }
 
@@ -3112,63 +2945,6 @@ COMMAND_HANDLER(cortex_a_handle_dbginit_command)
 
        return cortex_a_init_debug_access(target);
 }
-COMMAND_HANDLER(cortex_a_handle_smp_off_command)
-{
-       struct target *target = get_current_target(CMD_CTX);
-       /* check target is an smp target */
-       struct target_list *head;
-       struct target *curr;
-       head = target->head;
-       target->smp = 0;
-       if (head != (struct target_list *)NULL) {
-               while (head != (struct target_list *)NULL) {
-                       curr = head->target;
-                       curr->smp = 0;
-                       head = head->next;
-               }
-               /*  fixes the target display to the debugger */
-               target->gdb_service->target = target;
-       }
-       return ERROR_OK;
-}
-
-COMMAND_HANDLER(cortex_a_handle_smp_on_command)
-{
-       struct target *target = get_current_target(CMD_CTX);
-       struct target_list *head;
-       struct target *curr;
-       head = target->head;
-       if (head != (struct target_list *)NULL) {
-               target->smp = 1;
-               while (head != (struct target_list *)NULL) {
-                       curr = head->target;
-                       curr->smp = 1;
-                       head = head->next;
-               }
-       }
-       return ERROR_OK;
-}
-
-COMMAND_HANDLER(cortex_a_handle_smp_gdb_command)
-{
-       struct target *target = get_current_target(CMD_CTX);
-       int retval = ERROR_OK;
-       struct target_list *head;
-       head = target->head;
-       if (head != (struct target_list *)NULL) {
-               if (CMD_ARGC == 1) {
-                       int coreid = 0;
-                       COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], coreid);
-                       if (ERROR_OK != retval)
-                               return retval;
-                       target->gdb_service->core[1] = coreid;
-
-               }
-               command_print(CMD_CTX, "gdb coreid  %" PRId32 " -> %" PRId32, target->gdb_service->core[0]
-                       , target->gdb_service->core[1]);
-       }
-       return ERROR_OK;
-}
 
 COMMAND_HANDLER(handle_cortex_a_mask_interrupts_command)
 {
@@ -3193,7 +2969,7 @@ COMMAND_HANDLER(handle_cortex_a_mask_interrupts_command)
        }
 
        n = Jim_Nvp_value2name_simple(nvp_maskisr_modes, cortex_a->isrmasking_mode);
-       command_print(CMD_CTX, "cortex_a interrupt mask %s", n->name);
+       command_print(CMD, "cortex_a interrupt mask %s", n->name);
 
        return ERROR_OK;
 }
@@ -3219,7 +2995,7 @@ COMMAND_HANDLER(handle_cortex_a_dacrfixup_command)
        }
 
        n = Jim_Nvp_value2name_simple(nvp_dacrfixup_modes, cortex_a->dacrfixup_mode);
-       command_print(CMD_CTX, "cortex_a domain access control fixup %s", n->name);
+       command_print(CMD, "cortex_a domain access control fixup %s", n->name);
 
        return ERROR_OK;
 }
@@ -3239,25 +3015,6 @@ static const struct command_registration cortex_a_exec_command_handlers[] = {
                .help = "Initialize core debug",
                .usage = "",
        },
-       {   .name = "smp_off",
-           .handler = cortex_a_handle_smp_off_command,
-           .mode = COMMAND_EXEC,
-           .help = "Stop smp handling",
-           .usage = "",},
-       {
-               .name = "smp_on",
-               .handler = cortex_a_handle_smp_on_command,
-               .mode = COMMAND_EXEC,
-               .help = "Restart smp handling",
-               .usage = "",
-       },
-       {
-               .name = "smp_gdb",
-               .handler = cortex_a_handle_smp_gdb_command,
-               .mode = COMMAND_EXEC,
-               .help = "display/fix current core played to gdb",
-               .usage = "",
-       },
        {
                .name = "maskisr",
                .handler = handle_cortex_a_mask_interrupts_command,
@@ -3273,6 +3030,12 @@ static const struct command_registration cortex_a_exec_command_handlers[] = {
                        "on memory access",
                .usage = "['on'|'off']",
        },
+       {
+               .chain = armv7a_mmu_command_handlers,
+       },
+       {
+               .chain = smp_command_handlers,
+       },
 
        COMMAND_REGISTRATION_DONE
 };
@@ -3308,6 +3071,7 @@ struct target_type cortexa_target = {
        .deassert_reset = cortex_a_deassert_reset,
 
        /* REVISIT allow exporting VFP3 registers ... */
+       .get_gdb_arch = arm_get_gdb_arch,
        .get_gdb_reg_list = arm_get_gdb_reg_list,
 
        .read_memory = cortex_a_read_memory,
@@ -3387,6 +3151,7 @@ struct target_type cortexr4_target = {
        .deassert_reset = cortex_a_deassert_reset,
 
        /* REVISIT allow exporting VFP3 registers ... */
+       .get_gdb_arch = arm_get_gdb_arch,
        .get_gdb_reg_list = arm_get_gdb_reg_list,
 
        .read_memory = cortex_a_read_phys_memory,