arm_adi_v5: Do not ignore register polling timeout

Previous to this commit 'ahbap_debugport_init' would ignore if timeout
happened or not when waiting for CDBGPWRUPACK and CSYSPWRUPACK and would
continue initialization regardless. It also would not reset the
timeout counter after finishing polling for CDBGPWRUPACK and starting
for CSYSPWRUPACK which could potentially cause some problems.

Also refactor code of both snippets into a more generic function to
avoid duplication.

Change-Id: I16e4f50e6819e08c4126e71ef8cec7db559d608e
Signed-off-by: Andrey Smirnov <andrew.smirnov@gmail.com>
Reviewed-on: http://openocd.zylin.com/2086
Reviewed-by: Andreas Fritiofson <andreas.fritiofson@gmail.com>
Tested-by: jenkins

1 files changed, 29 insertions, 0 deletions

diff --git a/src/target/arm_adi_v5.h b/src/target/arm_adi_v5.h
index 9f0dea91..131e9d96 100644
--- a/src/target/arm_adi_v5.h
+++ b/src/target/arm_adi_v5.h
@@ -401,6 +401,35 @@ static inline int dap_dp_read_atomic(struct adiv5_dap *dap, unsigned reg,
 	return dap_run(dap);
 }
 
+static inline int dap_dp_poll_register(struct adiv5_dap *dap, unsigned reg,
+				       uint32_t mask, uint32_t value, int timeout)
+{
+	assert(timeout > 0);
+	assert((value & mask) == value);
+
+	int ret;
+	uint32_t regval;
+	LOG_DEBUG("DAP: poll %x, mask 0x08%" PRIx32 ", value 0x%08" PRIx32,
+		  reg, mask, value);
+	do {
+		ret = dap_dp_read_atomic(dap, reg, &regval);
+		if (ret != ERROR_OK)
+			return ret;
+
+		if ((regval & mask) == value)
+			break;
+
+		alive_sleep(10);
+	} while (--timeout);
+
+	if (!timeout) {
+		LOG_DEBUG("DAP: poll %x timeout", reg);
+		return ERROR_FAIL;
+	} else {
+		return ERROR_OK;
+	}
+}
+
 /** Accessor for currently selected DAP-AP number (0..255) */
 static inline uint8_t dap_ap_get_select(struct adiv5_dap *swjdp)
 {