Fix resume when core state has been modified

Sometimes it is necessary to resume into a different state (ARM/Thumb)
than at debug state entry. According to the documentation this should
be possible with "arm core_state arm|thumb" before the resume command,
however the original code also restores the original CPSR, which
overrides whatever state the core was set to. This seems to work on some
cores (e.g. Cortex-A5) but not on others (e.g. Cortex-A9). Using the "BX"
instruction to set resume PC and core state works on Cortex-A9 and
ARM11, but is not sufficient on Cortex-A5, where an explicit write to
the PC (MOV pc, r0) is required additionally.

Change-Id: Ic03153b4b250fbb8cf6c75f8e329fb34829aa35f
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Reviewed-on: http://openocd.zylin.com/3386
Tested-by: jenkins
Reviewed-by: Alexander Stein <alexanders83@web.de>
Reviewed-by: Andreas Fritiofson <andreas.fritiofson@gmail.com>

src/target/arm11.c

@@ -418,11 +418,33 @@ static uint32_t arm11_nextpc(struct arm11_common *arm11, int current, uint32_t a
 {
 	void *value = arm11->arm.pc->value;
 
-	if (!current)
+	/* use the current program counter */
-		buf_set_u32(value, 0, 32, address);
+	if (current)
-	else
 		address = buf_get_u32(value, 0, 32);
 
+	/* Make sure that the gdb thumb fixup does not
+	 * kill the return address
+	 */
+	switch (arm11->arm.core_state) {
+		case ARM_STATE_ARM:
+			address &= 0xFFFFFFFC;
+			break;
+		case ARM_STATE_THUMB:
+			/* When the return address is loaded into PC
+			 * bit 0 must be 1 to stay in Thumb state
+			 */
+			address |= 0x1;
+			break;
+
+		/* catch-all for JAZELLE and THUMB_EE */
+		default:
+			break;
+	}
+
+	buf_set_u32(value, 0, 32, address);
+	arm11->arm.pc->dirty = 1;
+	arm11->arm.pc->valid = 1;
+
 	return address;
 }
 

src/target/arm_dpm.c

@@ -228,6 +228,18 @@ static int dpm_write_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
 	return retval;
 }
 
+/**
+ * Write to program counter and switch the core state (arm/thumb) according to
+ * the address.
+ */
+static int dpm_write_pc_core_state(struct arm_dpm *dpm, struct reg *r)
+{
+	uint32_t value = buf_get_u32(r->value, 0, 32);
+
+	/* read r0 from DCC; then "BX r0" */
+	return dpm->instr_write_data_r0(dpm, ARMV4_5_BX(0), value);
+}
+
 /**
  * Read basic registers of the the current context:  R0 to R15, and CPSR;
  * sets the core mode (such as USR or IRQ) and state (such as ARM or Thumb).
@@ -465,6 +477,19 @@ int arm_dpm_write_dirty_registers(struct arm_dpm *dpm, bool bpwp)
 		goto done;
 	arm->cpsr->dirty = false;
 
+	/* restore the PC, make sure to also switch the core state
+	 * to whatever it was set to with "arm core_state" command.
+	 * target code will have set PC to an appropriate resume address.
+	 */
+	retval = dpm_write_pc_core_state(dpm, arm->pc);
+	if (retval != ERROR_OK)
+		goto done;
+	/* on Cortex-A5 (as found on NXP VF610 SoC), BX instruction
+	 * executed in debug state doesn't appear to set the PC,
+	 * explicitly set it with a "MOV pc, r0". This doesn't influence
+	 * CPSR on Cortex-A9 so it should be OK. Maybe due to different
+	 * debug version?
+	 */
 	retval = dpm_write_reg(dpm, arm->pc, 15);
 	if (retval != ERROR_OK)
 		goto done;