Cortex-M: fix stale DHCSR cache values

[fw/openocd] / src / target / cortex_m.c

diff --git a/src/target/cortex_m.c b/src/target/cortex_m.c
index f674cc527c0a6a7c7e5fa6fa616436c3607663f6..11dbf24f28eb871fffd7ed05ebfb6875a37acd53 100644 (file)
--- a/src/target/cortex_m.c
+++ b/src/target/cortex_m.c
@@ -242,7 +242,7 @@ static int cortex_m_endreset_event(struct target *target)
        if (retval != ERROR_OK)
                return retval;
        if (!(cortex_m->dcb_dhcsr & C_DEBUGEN)) {
-               retval = mem_ap_write_u32(armv7m->debug_ap, DCB_DHCSR, DBGKEY | C_DEBUGEN);
+               retval = cortex_m_write_debug_halt_mask(target, 0, C_HALT | C_STEP | C_MASKINTS);
                if (retval != ERROR_OK)
                        return retval;
        }
@@ -682,7 +682,7 @@ void cortex_m_enable_breakpoints(struct target *target)
 }
 
 static int cortex_m_resume(struct target *target, int current,
-       uint32_t address, int handle_breakpoints, int debug_execution)
+       target_addr_t address, int handle_breakpoints, int debug_execution)
 {
        struct armv7m_common *armv7m = target_to_armv7m(target);
        struct breakpoint *breakpoint = NULL;
@@ -750,7 +750,7 @@ static int cortex_m_resume(struct target *target, int current,
                /* Single step past breakpoint at current address */
                breakpoint = breakpoint_find(target, resume_pc);
                if (breakpoint) {
-                       LOG_DEBUG("unset breakpoint at 0x%8.8" PRIx32 " (ID: %" PRIu32 ")",
+                       LOG_DEBUG("unset breakpoint at " TARGET_ADDR_FMT " (ID: %" PRIu32 ")",
                                breakpoint->address,
                                breakpoint->unique_id);
                        cortex_m_unset_breakpoint(target, breakpoint);
@@ -782,7 +782,7 @@ static int cortex_m_resume(struct target *target, int current,
 
 /* int irqstepcount = 0; */
 static int cortex_m_step(struct target *target, int current,
-       uint32_t address, int handle_breakpoints)
+       target_addr_t address, int handle_breakpoints)
 {
        struct cortex_m_common *cortex_m = target_to_cm(target);
        struct armv7m_common *armv7m = &cortex_m->armv7m;
@@ -1005,12 +1005,12 @@ static int cortex_m_assert_reset(struct target *target)
        /* Store important errors instead of failing and proceed to reset assert */
 
        if (retval != ERROR_OK || !(cortex_m->dcb_dhcsr & C_DEBUGEN))
-               retval = mem_ap_write_u32(armv7m->debug_ap, DCB_DHCSR, DBGKEY | C_DEBUGEN);
+               retval = cortex_m_write_debug_halt_mask(target, 0, C_HALT | C_STEP | C_MASKINTS);
 
        /* If the processor is sleeping in a WFI or WFE instruction, the
         * C_HALT bit must be asserted to regain control */
        if (retval == ERROR_OK && (cortex_m->dcb_dhcsr & S_SLEEP))
-               retval = mem_ap_write_u32(armv7m->debug_ap, DCB_DHCSR, DBGKEY | C_HALT | C_DEBUGEN);
+               retval = cortex_m_write_debug_halt_mask(target, C_HALT, 0);
 
        mem_ap_write_u32(armv7m->debug_ap, DCB_DCRDR, 0);
        /* Ignore less important errors */
@@ -1018,8 +1018,7 @@ static int cortex_m_assert_reset(struct target *target)
        if (!target->reset_halt) {
                /* Set/Clear C_MASKINTS in a separate operation */
                if (cortex_m->dcb_dhcsr & C_MASKINTS)
-                       mem_ap_write_atomic_u32(armv7m->debug_ap, DCB_DHCSR,
-                                       DBGKEY | C_DEBUGEN | C_HALT);
+                       cortex_m_write_debug_halt_mask(target, 0, C_MASKINTS);
 
                /* clear any debug flags before resuming */
                cortex_m_clear_halt(target);
@@ -1198,7 +1197,7 @@ int cortex_m_set_breakpoint(struct target *target, struct breakpoint *breakpoint
                breakpoint->set = true;
        }
 
-       LOG_DEBUG("BPID: %" PRIu32 ", Type: %d, Address: 0x%08" PRIx32 " Length: %d (set=%d)",
+       LOG_DEBUG("BPID: %" PRIu32 ", Type: %d, Address: " TARGET_ADDR_FMT " Length: %d (set=%d)",
                breakpoint->unique_id,
                (int)(breakpoint->type),
                breakpoint->address,
@@ -1219,7 +1218,7 @@ int cortex_m_unset_breakpoint(struct target *target, struct breakpoint *breakpoi
                return ERROR_OK;
        }
 
-       LOG_DEBUG("BPID: %" PRIu32 ", Type: %d, Address: 0x%08" PRIx32 " Length: %d (set=%d)",
+       LOG_DEBUG("BPID: %" PRIu32 ", Type: %d, Address: " TARGET_ADDR_FMT " Length: %d (set=%d)",
                breakpoint->unique_id,
                (int)(breakpoint->type),
                breakpoint->address,
@@ -1664,7 +1663,7 @@ static int cortex_m_store_core_reg_u32(struct target *target,
        return ERROR_OK;
 }
 
-static int cortex_m_read_memory(struct target *target, uint32_t address,
+static int cortex_m_read_memory(struct target *target, target_addr_t address,
        uint32_t size, uint32_t count, uint8_t *buffer)
 {
        struct armv7m_common *armv7m = target_to_armv7m(target);
@@ -1678,7 +1677,7 @@ static int cortex_m_read_memory(struct target *target, uint32_t address,
        return mem_ap_read_buf(armv7m->debug_ap, buffer, size, count, address);
 }
 
-static int cortex_m_write_memory(struct target *target, uint32_t address,
+static int cortex_m_write_memory(struct target *target, target_addr_t address,
        uint32_t size, uint32_t count, const uint8_t *buffer)
 {
        struct armv7m_common *armv7m = target_to_armv7m(target);
@@ -1709,6 +1708,7 @@ void cortex_m_deinit_target(struct target *target)
        cortex_m_dwt_free(target);
        armv7m_free_reg_cache(target);
 
+       free(target->private_config);
        free(cortex_m);
 }
 
@@ -1912,11 +1912,15 @@ int cortex_m_examine(struct target *target)
                        return retval;
                }
 
-               /* Search for the MEM-AP */
-               retval = dap_find_ap(swjdp, AP_TYPE_AHB_AP, &armv7m->debug_ap);
-               if (retval != ERROR_OK) {
-                       LOG_ERROR("Could not find MEM-AP to control the core");
-                       return retval;
+               if (cortex_m->apsel < 0) {
+                       /* Search for the MEM-AP */
+                       retval = dap_find_ap(swjdp, AP_TYPE_AHB_AP, &armv7m->debug_ap);
+                       if (retval != ERROR_OK) {
+                               LOG_ERROR("Could not find MEM-AP to control the core");
+                               return retval;
+                       }
+               } else {
+                       armv7m->debug_ap = dap_ap(swjdp, cortex_m->apsel);
                }
 
                /* Leave (only) generic DAP stuff for debugport_init(); */
@@ -2180,6 +2184,13 @@ static int cortex_m_target_create(struct target *target, Jim_Interp *interp)
        cortex_m->common_magic = CORTEX_M_COMMON_MAGIC;
        cortex_m_init_arch_info(target, cortex_m, target->tap);
 
+       if (target->private_config != NULL) {
+               struct adiv5_private_config *pc =
+                               (struct adiv5_private_config *)target->private_config;
+               cortex_m->apsel = pc->ap_num;
+       } else
+               cortex_m->apsel = -1;
+
        return ERROR_OK;
 }
 
@@ -2441,6 +2452,7 @@ struct target_type cortexm_target = {
 
        .commands = cortex_m_command_handlers,
        .target_create = cortex_m_target_create,
+       .target_jim_configure = adiv5_jim_configure,
        .init_target = cortex_m_init_target,
        .examine = cortex_m_examine,
        .deinit_target = cortex_m_deinit_target,