cortex_m: poll S_REGRDY on register r/w

Accordingly to arm documentation [1], chapter C1.6.4, the
operation to read/write from/to core registers can require time,
and the specific flag DHCSR.S_REGRDY has to be polled to verify
that the operation has been completed.
The lack of check on S_REGRDY causes OpenOCD to fail handling
correctly the core registers on a Cortex-M4 emulated in a slow
FPGA, and it could also fail on devices clocked at very low speed
while using a fast adapter.

Poll S_REGRDY as specified in [1] while either reading or writing
the core registers.
A timeout of 0.5s is added. This could still be too small in some
extremely slow cases, but at least now we log the timeout event,
which can help tracking down such odd issue.
During register read include in the polling loop the read of DCRSR
and to flush the JTAG queue only once.
During register write, relax the write in DCRSR by removing the
atomicity that is now useless since followed by the atomic read to
S_REGRDY.
During register read include the read of DCRSR inside the polling
loop to relax the read of S_REGRDY since followed by the atomic
read to DCRSR.

This change has the drawback of adding other transfers to the
adapter while reading/writing the registers, so it is expected to
introduce some speed degradation during step-by-step.

[1] DDI0403E - "ARMv7-M Architecture Reference Manual"

Change-Id: I61f454248f11a3bec6dcf4c58a50c5c996d7ef81
Signed-off-by: Antonio Borneo <borneo.antonio@gmail.com>
Signed-off-by: Tomas Vanek <vanekt@fbl.cz>
Reviewed-on: https://review.openocd.org/c/openocd/+/5319
Tested-by: jenkins
Reviewed-by: Tarek BOCHKATI <tarek.bouchkati@gmail.com>

1 files changed, 48 insertions, 5 deletions

diff --git a/src/target/cortex_m.c b/src/target/cortex_m.c
index 6558252..ba7bc17 100644
--- a/src/target/cortex_m.c
+++ b/src/target/cortex_m.c
@@ -53,6 +53,9 @@
  * any longer.
  */
 
+/* Timeout for register r/w */
+#define DHCSR_S_REGRDY_TIMEOUT (500)
+
 /* Supported Cortex-M Cores */
 static const struct cortex_m_part_info cortex_m_parts[] = {
 	{
@@ -148,9 +151,11 @@ static int cortex_m_read_dhcsr_atomic_sticky(struct target *target)
 static int cortex_m_load_core_reg_u32(struct target *target,
 		uint32_t regsel, uint32_t *value)
 {
+	struct cortex_m_common *cortex_m = target_to_cm(target);
 	struct armv7m_common *armv7m = target_to_armv7m(target);
 	int retval;
-	uint32_t dcrdr;
+	uint32_t dcrdr, tmp_value;
+	int64_t then;
 
 	/* because the DCB_DCRDR is used for the emulated dcc channel
 	 * we have to save/restore the DCB_DCRDR when used */
@@ -164,9 +169,28 @@ static int cortex_m_load_core_reg_u32(struct target *target,
 	if (retval != ERROR_OK)
 		return retval;
 
-	retval = mem_ap_read_atomic_u32(armv7m->debug_ap, DCB_DCRDR, value);
-	if (retval != ERROR_OK)
-		return retval;
+	/* check if value from register is ready and pre-read it */
+	then = timeval_ms();
+	while (1) {
+		retval = mem_ap_read_u32(armv7m->debug_ap, DCB_DHCSR,
+								 &cortex_m->dcb_dhcsr);
+		if (retval != ERROR_OK)
+			return retval;
+		retval = mem_ap_read_atomic_u32(armv7m->debug_ap, DCB_DCRDR,
+										&tmp_value);
+		if (retval != ERROR_OK)
+			return retval;
+		cortex_m_cumulate_dhcsr_sticky(cortex_m, cortex_m->dcb_dhcsr);
+		if (cortex_m->dcb_dhcsr & S_REGRDY)
+			break;
+		if (timeval_ms() > then + DHCSR_S_REGRDY_TIMEOUT) {
+			LOG_ERROR("Timeout waiting for DCRDR transfer ready");
+			return ERROR_TIMEOUT_REACHED;
+		}
+		keep_alive();
+	}
+
+	*value = tmp_value;
 
 	if (target->dbg_msg_enabled) {
 		/* restore DCB_DCRDR - this needs to be in a separate
@@ -181,9 +205,11 @@ static int cortex_m_load_core_reg_u32(struct target *target,
 static int cortex_m_store_core_reg_u32(struct target *target,
 		uint32_t regsel, uint32_t value)
 {
+	struct cortex_m_common *cortex_m = target_to_cm(target);
 	struct armv7m_common *armv7m = target_to_armv7m(target);
 	int retval;
 	uint32_t dcrdr;
+	int64_t then;
 
 	/* because the DCB_DCRDR is used for the emulated dcc channel
 	 * we have to save/restore the DCB_DCRDR when used */
@@ -197,10 +223,27 @@ static int cortex_m_store_core_reg_u32(struct target *target,
 	if (retval != ERROR_OK)
 		return retval;
 
-	retval = mem_ap_write_atomic_u32(armv7m->debug_ap, DCB_DCRSR, regsel | DCRSR_WNR);
+	retval = mem_ap_write_u32(armv7m->debug_ap, DCB_DCRSR, regsel | DCRSR_WNR);
 	if (retval != ERROR_OK)
 		return retval;
 
+	/* check if value is written into register */
+	then = timeval_ms();
+	while (1) {
+		retval = mem_ap_read_atomic_u32(armv7m->debug_ap, DCB_DHCSR,
+										&cortex_m->dcb_dhcsr);
+		if (retval != ERROR_OK)
+			return retval;
+		cortex_m_cumulate_dhcsr_sticky(cortex_m, cortex_m->dcb_dhcsr);
+		if (cortex_m->dcb_dhcsr & S_REGRDY)
+			break;
+		if (timeval_ms() > then + DHCSR_S_REGRDY_TIMEOUT) {
+			LOG_ERROR("Timeout waiting for DCRDR transfer ready");
+			return ERROR_TIMEOUT_REACHED;
+		}
+		keep_alive();
+	}
+
 	if (target->dbg_msg_enabled) {
 		/* restore DCB_DCRDR - this needs to be in a separate
 		 * transaction otherwise the emulated DCC channel breaks */