+ DP_RDBUFF, DPAP_READ, 0, invalue, 0, NULL);
+ if (retval != ERROR_OK)
+ return retval;
+ }
+
+ return jtag_execute_queue();
+}
+
+static int jtagdp_overrun_check(struct adiv5_dap *dap)
+{
+ int retval;
+ struct dap_cmd *el, *tmp, *prev = NULL;
+ int found_wait = 0;
+ int64_t time_now;
+ LIST_HEAD(replay_list);
+
+ /* make sure all queued transactions are complete */
+ retval = jtag_execute_queue();
+ if (retval != ERROR_OK)
+ goto done;
+
+ /* skip all completed transactions up to the first WAIT */
+ list_for_each_entry(el, &dap->cmd_journal, lh) {
+ if (el->ack == JTAG_ACK_OK_FAULT) {
+ log_dap_cmd("LOG", el);
+ } else if (el->ack == JTAG_ACK_WAIT) {
+ found_wait = 1;
+ break;
+ } else {
+ LOG_ERROR("Invalid ACK (%1x) in DAP response", el->ack);
+ log_dap_cmd("ERR", el);
+ retval = ERROR_JTAG_DEVICE_ERROR;
+ goto done;
+ }
+ }
+
+ /*
+ * If we found a stalled transaction and a previous transaction
+ * exists, check if it's a READ access.
+ */
+ if (found_wait && el != list_first_entry(&dap->cmd_journal, struct dap_cmd, lh)) {
+ prev = list_entry(el->lh.prev, struct dap_cmd, lh);
+ if (prev->RnW == DPAP_READ) {
+ log_dap_cmd("PND", prev);
+ /* search for the next OK transaction, it contains
+ * the result of the previous READ */
+ tmp = el;
+ list_for_each_entry_from(tmp, &dap->cmd_journal, lh) {
+ if (tmp->ack == JTAG_ACK_OK_FAULT) {
+ /* recover the read value */
+ log_dap_cmd("FND", tmp);
+ if (el->invalue != el->invalue_buf) {
+ uint32_t invalue = le_to_h_u32(tmp->invalue);
+ memcpy(el->invalue, &invalue, sizeof(uint32_t));
+ }
+ prev = NULL;
+ break;
+ }
+ }
+
+ if (prev != NULL) {
+ log_dap_cmd("LST", el);
+
+ /*
+ * At this point we're sure that no previous
+ * transaction completed and the DAP/AP is still
+ * in busy state. We know that the next "OK" scan
+ * will return the READ result we need to recover.
+ * To complete the READ, we just keep polling RDBUFF
+ * until the WAIT condition clears
+ */
+ tmp = dap_cmd_new(JTAG_DP_DPACC,
+ DP_RDBUFF, DPAP_READ, NULL, NULL, 0);
+ if (tmp == NULL) {
+ retval = ERROR_JTAG_DEVICE_ERROR;
+ goto done;
+ }
+ /* synchronously retry the command until it succeeds */
+ time_now = timeval_ms();
+ do {
+ retval = adi_jtag_dp_scan_cmd_sync(dap, tmp, NULL);
+ if (retval != ERROR_OK)
+ break;
+ if (tmp->ack == JTAG_ACK_OK_FAULT) {
+ log_dap_cmd("FND", tmp);
+ if (el->invalue != el->invalue_buf) {
+ uint32_t invalue = le_to_h_u32(tmp->invalue);
+ memcpy(el->invalue, &invalue, sizeof(uint32_t));
+ }
+ break;
+ }
+ if (tmp->ack != JTAG_ACK_WAIT) {
+ LOG_ERROR("Invalid ACK (%1x) in DAP response", tmp->ack);
+ log_dap_cmd("ERR", tmp);
+ retval = ERROR_JTAG_DEVICE_ERROR;
+ break;
+ }
+
+ } while (timeval_ms() - time_now < 1000);
+
+ if (retval == ERROR_OK) {
+ /* timeout happened */
+ if (tmp->ack != JTAG_ACK_OK_FAULT) {
+ LOG_ERROR("Timeout during WAIT recovery");
+ dap->select = DP_SELECT_INVALID;
+ jtag_ap_q_abort(dap, NULL);
+ /* clear the sticky overrun condition */
+ adi_jtag_scan_inout_check_u32(dap, JTAG_DP_DPACC,
+ DP_CTRL_STAT, DPAP_WRITE,
+ dap->dp_ctrl_stat | SSTICKYORUN, NULL, 0);
+ retval = ERROR_JTAG_DEVICE_ERROR;
+ }
+ }
+
+ /* we're done with this command, release it */
+ free(tmp);
+
+ if (retval != ERROR_OK)
+ goto done;
+
+ }
+ /* make el->invalue point to the default invalue
+ * so that we can safely retry it without clobbering
+ * the result we just recovered */
+ el->invalue = el->invalue_buf;
+ }
+ }
+
+ /* move all remaining transactions over to the replay list */
+ list_for_each_entry_safe_from(el, tmp, &dap->cmd_journal, lh) {
+ log_dap_cmd("REP", el);
+ list_move_tail(&el->lh, &replay_list);
+ }
+
+ /* we're done with the journal, flush it */
+ flush_journal(&dap->cmd_journal);
+
+ /* check for overrun condition in the last batch of transactions */
+ if (found_wait) {
+ LOG_INFO("DAP transaction stalled (WAIT) - slowing down");
+ /* clear the sticky overrun condition */
+ retval = adi_jtag_scan_inout_check_u32(dap, JTAG_DP_DPACC,
+ DP_CTRL_STAT, DPAP_WRITE,
+ dap->dp_ctrl_stat | SSTICKYORUN, NULL, 0);
+ if (retval != ERROR_OK)
+ goto done;
+
+ /* restore SELECT register first */
+ if (!list_empty(&replay_list)) {
+ el = list_first_entry(&replay_list, struct dap_cmd, lh);
+ tmp = dap_cmd_new(JTAG_DP_DPACC,
+ DP_SELECT, DPAP_WRITE, (uint8_t *)&el->dp_select, NULL, 0);
+ if (tmp == NULL) {
+ retval = ERROR_JTAG_DEVICE_ERROR;
+ goto done;
+ }
+ list_add(&tmp->lh, &replay_list);
+
+ dap->select = DP_SELECT_INVALID;
+ }
+
+ list_for_each_entry_safe(el, tmp, &replay_list, lh) {
+ time_now = timeval_ms();
+ do {
+ retval = adi_jtag_dp_scan_cmd_sync(dap, el, NULL);
+ if (retval != ERROR_OK)
+ break;
+ log_dap_cmd("REC", el);
+ if (el->ack == JTAG_ACK_OK_FAULT) {
+ if (el->invalue != el->invalue_buf) {
+ uint32_t invalue = le_to_h_u32(el->invalue);
+ memcpy(el->invalue, &invalue, sizeof(uint32_t));
+ }
+ break;
+ }
+ if (el->ack != JTAG_ACK_WAIT) {
+ LOG_ERROR("Invalid ACK (%1x) in DAP response", el->ack);
+ log_dap_cmd("ERR", el);
+ retval = ERROR_JTAG_DEVICE_ERROR;
+ break;
+ }
+ } while (timeval_ms() - time_now < 1000);
+
+ if (retval == ERROR_OK) {
+ if (el->ack != JTAG_ACK_OK_FAULT) {
+ LOG_ERROR("Timeout during WAIT recovery");
+ dap->select = DP_SELECT_INVALID;
+ jtag_ap_q_abort(dap, NULL);
+ /* clear the sticky overrun condition */
+ adi_jtag_scan_inout_check_u32(dap, JTAG_DP_DPACC,
+ DP_CTRL_STAT, DPAP_WRITE,
+ dap->dp_ctrl_stat | SSTICKYORUN, NULL, 0);
+ retval = ERROR_JTAG_DEVICE_ERROR;
+ break;
+ }
+ } else
+ break;
+ }
+ }
+
+ done:
+ flush_journal(&replay_list);
+ flush_journal(&dap->cmd_journal);