- /* Post CTRL/STAT read; discard any previous posted read value
- * but collect its ACK status.
- */
- retval = adi_jtag_scan_inout_check_u32(dap, JTAG_DP_DPACC,
- DP_CTRL_STAT, DPAP_READ, 0, &ctrlstat, 0);
- if (retval != ERROR_OK)
- return retval;
- retval = jtag_execute_queue();
- if (retval != ERROR_OK)
- return retval;
+ 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;
+ }
+ }