diff --git a/src/target/arm_adi_v5.c b/src/target/arm_adi_v5.c
index 1c80fcc2f..1e324efae 100644
--- a/src/target/arm_adi_v5.c
+++ b/src/target/arm_adi_v5.c
@@ -619,6 +619,8 @@ int mem_ap_sel_write_buf_noincr(struct adiv5_dap *swjdp, uint8_t ap,
 /*--------------------------------------------------------------------------*/
 
 
+#define DAP_POWER_DOMAIN_TIMEOUT (10)
+
 /* FIXME don't import ... just initialize as
  * part of DAP transport setup
 */
@@ -640,8 +642,6 @@ extern const struct dap_ops jtag_dp_ops;
  */
 int ahbap_debugport_init(struct adiv5_dap *dap)
 {
-	uint32_t ctrlstat;
-	int cnt = 0;
 	int retval;
 
 	LOG_DEBUG(" ");
@@ -681,35 +681,20 @@ int ahbap_debugport_init(struct adiv5_dap *dap)
 	if (retval != ERROR_OK)
 		return retval;
 
-	retval = dap_queue_dp_read(dap, DP_CTRL_STAT, &ctrlstat);
-	if (retval != ERROR_OK)
-		return retval;
-	retval = dap_run(dap);
-	if (retval != ERROR_OK)
-		return retval;
-
 	/* Check that we have debug power domains activated */
-	while (!(ctrlstat & CDBGPWRUPACK) && (cnt++ < 10)) {
-		LOG_DEBUG("DAP: wait CDBGPWRUPACK");
-		retval = dap_queue_dp_read(dap, DP_CTRL_STAT, &ctrlstat);
-		if (retval != ERROR_OK)
-			return retval;
-		retval = dap_run(dap);
-		if (retval != ERROR_OK)
-			return retval;
-		alive_sleep(10);
-	}
+	LOG_DEBUG("DAP: wait CDBGPWRUPACK");
+	retval = dap_dp_poll_register(dap, DP_CTRL_STAT,
+				      CDBGPWRUPACK, CDBGPWRUPACK,
+				      DAP_POWER_DOMAIN_TIMEOUT);
+	if (retval != ERROR_OK)
+		return retval;
 
-	while (!(ctrlstat & CSYSPWRUPACK) && (cnt++ < 10)) {
-		LOG_DEBUG("DAP: wait CSYSPWRUPACK");
-		retval = dap_queue_dp_read(dap, DP_CTRL_STAT, &ctrlstat);
-		if (retval != ERROR_OK)
-			return retval;
-		retval = dap_run(dap);
-		if (retval != ERROR_OK)
-			return retval;
-		alive_sleep(10);
-	}
+	LOG_DEBUG("DAP: wait CSYSPWRUPACK");
+	retval = dap_dp_poll_register(dap, DP_CTRL_STAT,
+				      CSYSPWRUPACK, CSYSPWRUPACK,
+				      DAP_POWER_DOMAIN_TIMEOUT);
+	if (retval != ERROR_OK)
+		return retval;
 
 	retval = dap_queue_dp_read(dap, DP_CTRL_STAT, NULL);
 	if (retval != ERROR_OK)
diff --git a/src/target/arm_adi_v5.h b/src/target/arm_adi_v5.h
index 9f0dea911..131e9d960 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)
 {