cortex_a9: fix dap_ap_select() usage

Save, select and restore AP in cortex_a9_step and cortex_a9_init_debug_access.
Fixes a bug where the wrong AP is selected after a reset.

Signed-off-by: Aaron Carroll <aaronc@cse.unsw.edu.au>

diff --git a/src/target/cortex_a9.c b/src/target/cortex_a9.c
index 5850ace7943010725931ff1cad5d514aa883c05a..62bf2630b9b9073dcb9123e11ddd72d936144a29 100644 (file)
--- a/src/target/cortex_a9.c
+++ b/src/target/cortex_a9.c
@@ -81,10 +81,13 @@ static int cortex_a9_init_debug_access(struct target *target)
 {
        struct armv7a_common *armv7a = target_to_armv7a(target);
        struct adiv5_dap *swjdp = &armv7a->dap;
+       uint8_t saved_apsel = dap_ap_get_select(swjdp);
 
        int retval;
        uint32_t dummy;
 
+       dap_ap_select(swjdp, swjdp_debugap);
+
        LOG_DEBUG(" ");
 
        /* Unlocking the debug registers for modification */
@@ -100,12 +103,12 @@ static int cortex_a9_init_debug_access(struct target *target)
                }
        }
        if (retval != ERROR_OK)
-               return retval;
+               goto out;
        /* Clear Sticky Power Down status Bit in PRSR to enable access to
           the registers in the Core Power Domain */
        retval = mem_ap_read_atomic_u32(swjdp, armv7a->debug_base + CPUDBG_PRSR, &dummy);
        if (retval != ERROR_OK)
-               return retval;
+               goto out;
 
        /* Enabling of instruction execution in debug mode is done in debug_entry code */
 
@@ -114,6 +117,8 @@ static int cortex_a9_init_debug_access(struct target *target)
        /* Since this is likely called from init or reset, update target state information*/
        retval = cortex_a9_poll(target);
 
+ out:
+       dap_ap_select(swjdp, saved_apsel);
        return retval;
 }
 
@@ -1129,10 +1134,12 @@ static int cortex_a9_step(struct target *target, int current, uint32_t address,
 {
        struct armv7a_common *armv7a = target_to_armv7a(target);
        struct arm *armv4_5 = &armv7a->armv4_5_common;
+       struct adiv5_dap *swjdp = &armv7a->dap;
        struct breakpoint *breakpoint = NULL;
        struct breakpoint stepbreakpoint;
        struct reg *r;
        int retval;
+       uint8_t saved_apsel = dap_ap_get_select(swjdp);
 
        if (target->state != TARGET_HALTED)
        {
@@ -1140,6 +1147,8 @@ static int cortex_a9_step(struct target *target, int current, uint32_t address,
                return ERROR_TARGET_NOT_HALTED;
        }
 
+       dap_ap_select(swjdp, swjdp_debugap);
+
        /* current = 1: continue on current pc, otherwise continue at <address> */
        r = armv4_5->pc;
        if (!current)
@@ -1176,18 +1185,19 @@ static int cortex_a9_step(struct target *target, int current, uint32_t address,
 
        retval = cortex_a9_resume(target, 1, address, 0, 0);
        if (retval != ERROR_OK)
-               return retval;
+               goto out;
 
        long long then = timeval_ms();
        while (target->state != TARGET_HALTED)
        {
                retval = cortex_a9_poll(target);
                if (retval != ERROR_OK)
-                       return retval;
+                       goto out;
                if (timeval_ms() > then + 1000)
                {
                        LOG_ERROR("timeout waiting for target halt");
-                       return ERROR_FAIL;
+                       retval = ERROR_FAIL;
+                       goto out;
                }
        }
 
@@ -1201,7 +1211,11 @@ static int cortex_a9_step(struct target *target, int current, uint32_t address,
        if (target->state != TARGET_HALTED)
                LOG_DEBUG("target stepped");
 
-       return ERROR_OK;
+       retval = ERROR_OK;
+
+ out:
+       dap_ap_select(swjdp, saved_apsel);
+       return retval;
 }
 
 static int cortex_a9_restore_context(struct target *target, bool bpwp)