aarch64: disable interrupts when stepping [WIP]

On live hardware, interrupts will happen while the core is
held for stepping. The next step will most of the time execute an
interrupt service instead of the next line of code, which is not
what you expect. Disable interrupts through DSCR before resuming
for a step, and re-enable them again after the step happened.

This should be made configurable, like on cortex_a target.

Change-Id: I94d8ffb58cf7579dedb66bc756b7eb6828b6e8e4
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index d38e0428443d9a1042216191d770ffac1a56cc2e..88f9b5a43663336ed532f97523c17c48c58af94c 100644 (file)
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -1241,6 +1241,28 @@ static int aarch64_post_debug_entry(struct target *target)
        return ERROR_OK;
 }
 
+static int aarch64_set_dscr_bits(struct target *target, unsigned long bit_mask, unsigned long value)
+{
+       struct armv8_common *armv8 = target_to_armv8(target);
+       uint32_t dscr;
+
+       /* Read DSCR */
+       int retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+                       armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
+       if (ERROR_OK != retval)
+               return retval;
+
+       /* clear bitfield */
+       dscr &= ~bit_mask;
+       /* put new value */
+       dscr |= value & bit_mask;
+
+       /* write new DSCR */
+       retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+                       armv8->debug_base + CPUV8_DBG_DSCR, dscr);
+       return retval;
+}
+
 static int aarch64_step(struct target *target, int current, target_addr_t address,
        int handle_breakpoints)
 {
@@ -1267,6 +1289,11 @@ static int aarch64_step(struct target *target, int current, target_addr_t addres
        if (retval != ERROR_OK)
                return retval;
 
+       /* disable interrupts while stepping */
+       retval = aarch64_set_dscr_bits(target, 0x3 << 22, 0x3 << 22);
+       if (retval != ERROR_OK)
+               return ERROR_OK;
+
        /* resume the target */
        retval = aarch64_resume(target, current, address, 0, 0);
        if (retval != ERROR_OK)
@@ -1289,6 +1316,11 @@ static int aarch64_step(struct target *target, int current, target_addr_t addres
        if (retval != ERROR_OK)
                return retval;
 
+       /* restore interrupts */
+       retval = aarch64_set_dscr_bits(target, 0x3 << 22, 0);
+       if (retval != ERROR_OK)
+               return ERROR_OK;
+
        return ERROR_OK;
 }