+#define JTAG_ACK_WAIT 0x1 /* ADIv5 and ADIv6 */
+#define JTAG_ACK_OK_FAULT 0x2 /* ADIv5 */
+#define JTAG_ACK_FAULT 0x2 /* ADIv6 */
+#define JTAG_ACK_OK 0x4 /* ADIV6 */
+
+static int jtag_ap_q_abort(struct adiv5_dap *dap, uint8_t *ack);
+
+#ifdef DEBUG_WAIT
+static const char *dap_reg_name(struct adiv5_dap *dap, uint8_t instr, uint16_t reg_addr)
+{
+ char *reg_name = "UNK";
+
+ if (instr == JTAG_DP_DPACC) {
+ switch (reg_addr) {
+ case DP_ABORT:
+ reg_name = "ABORT";
+ break;
+ case DP_CTRL_STAT:
+ reg_name = "CTRL/STAT";
+ break;
+ case DP_SELECT:
+ reg_name = "SELECT";
+ break;
+ case DP_RDBUFF:
+ reg_name = "RDBUFF";
+ break;
+ case DP_DLCR:
+ reg_name = "DLCR";
+ break;
+ default:
+ reg_name = "UNK";
+ break;
+ }
+ }
+
+ if (instr == JTAG_DP_APACC) {
+ if (reg_addr == MEM_AP_REG_CSW(dap))
+ reg_name = "CSW";
+ else if (reg_addr == MEM_AP_REG_TAR(dap))
+ reg_name = "TAR";
+ else if (reg_addr == MEM_AP_REG_TAR64(dap))
+ reg_name = "TAR64";
+ else if (reg_addr == MEM_AP_REG_DRW(dap))
+ reg_name = "DRW";
+ else if (reg_addr == MEM_AP_REG_BD0(dap))
+ reg_name = "BD0";
+ else if (reg_addr == MEM_AP_REG_BD1(dap))
+ reg_name = "BD1";
+ else if (reg_addr == MEM_AP_REG_BD2(dap))
+ reg_name = "BD2";
+ else if (reg_addr == MEM_AP_REG_BD3(dap))
+ reg_name = "BD3";
+ else if (reg_addr == MEM_AP_REG_CFG(dap))
+ reg_name = "CFG";
+ else if (reg_addr == MEM_AP_REG_BASE(dap))
+ reg_name = "BASE";
+ else if (reg_addr == MEM_AP_REG_BASE64(dap))
+ reg_name = "BASE64";
+ else if (reg_addr == AP_REG_IDR(dap))
+ reg_name = "IDR";
+ else
+ reg_name = "UNK";
+ }
+
+ return reg_name;
+}
+#endif
+
+struct dap_cmd {
+ struct list_head lh;
+ uint8_t instr;
+ uint16_t reg_addr;
+ uint8_t rnw;
+ uint8_t *invalue;
+ uint8_t ack;
+ uint32_t memaccess_tck;
+ uint64_t dp_select;
+
+ struct scan_field fields[2];
+ uint8_t out_addr_buf;
+ uint8_t invalue_buf[4];
+ uint8_t outvalue_buf[4];
+};
+
+#define MAX_DAP_COMMAND_NUM 65536
+
+struct dap_cmd_pool {
+ struct list_head lh;
+ struct dap_cmd cmd;
+};
+
+static void log_dap_cmd(struct adiv5_dap *dap, const char *header, struct dap_cmd *el)
+{
+#ifdef DEBUG_WAIT
+ const char *ack;
+ switch (el->ack) {
+ case JTAG_ACK_WAIT: /* ADIv5 and ADIv6 */
+ ack = "WAIT";
+ break;
+ case JTAG_ACK_OK_FAULT: /* ADIv5, same value as JTAG_ACK_FAULT */
+ /* case JTAG_ACK_FAULT: */ /* ADIv6 */
+ if (is_adiv6(dap))
+ ack = "FAULT";
+ else
+ ack = "OK";
+ break;
+ case JTAG_ACK_OK: /* ADIv6 */
+ if (is_adiv6(dap)) {
+ ack = "OK";
+ break;
+ }
+ /* fall-through */
+ default:
+ ack = "INVAL";
+ break;
+ }
+ LOG_DEBUG("%s: %2s %6s %5s 0x%08x 0x%08x %2s", header,
+ el->instr == JTAG_DP_APACC ? "AP" : "DP",
+ dap_reg_name(dap, el->instr, el->reg_addr),
+ el->rnw == DPAP_READ ? "READ" : "WRITE",
+ buf_get_u32(el->outvalue_buf, 0, 32),
+ buf_get_u32(el->invalue, 0, 32),
+ ack);
+#endif
+}
+
+static int jtag_limit_queue_size(struct adiv5_dap *dap)
+{
+ if (dap->cmd_pool_size < MAX_DAP_COMMAND_NUM)
+ return ERROR_OK;
+
+ return dap_run(dap);
+}
+
+static struct dap_cmd *dap_cmd_new(struct adiv5_dap *dap, uint8_t instr,
+ uint16_t reg_addr, uint8_t rnw,
+ uint8_t *outvalue, uint8_t *invalue,
+ uint32_t memaccess_tck)
+{
+
+ struct dap_cmd_pool *pool = NULL;
+
+ if (list_empty(&dap->cmd_pool)) {
+ pool = calloc(1, sizeof(struct dap_cmd_pool));
+ if (!pool)
+ return NULL;
+ } else {
+ pool = list_first_entry(&dap->cmd_pool, struct dap_cmd_pool, lh);
+ list_del(&pool->lh);
+ }
+
+ INIT_LIST_HEAD(&pool->lh);
+ dap->cmd_pool_size++;
+
+ struct dap_cmd *cmd = &pool->cmd;
+ INIT_LIST_HEAD(&cmd->lh);
+ cmd->instr = instr;
+ cmd->reg_addr = reg_addr;
+ cmd->rnw = rnw;
+ if (outvalue)
+ memcpy(cmd->outvalue_buf, outvalue, 4);
+ cmd->invalue = (invalue) ? invalue : cmd->invalue_buf;
+ cmd->memaccess_tck = memaccess_tck;
+
+ return cmd;
+}
+
+static void dap_cmd_release(struct adiv5_dap *dap, struct dap_cmd *cmd)
+{
+ struct dap_cmd_pool *pool = container_of(cmd, struct dap_cmd_pool, cmd);
+ if (dap->cmd_pool_size > MAX_DAP_COMMAND_NUM)
+ free(pool);
+ else
+ list_add(&pool->lh, &dap->cmd_pool);
+
+ dap->cmd_pool_size--;
+}
+
+static void flush_journal(struct adiv5_dap *dap, struct list_head *lh)
+{
+ struct dap_cmd *el, *tmp;
+
+ list_for_each_entry_safe(el, tmp, lh, lh) {
+ list_del(&el->lh);
+ dap_cmd_release(dap, el);
+ }
+}
+
+static void jtag_quit(struct adiv5_dap *dap)
+{
+ struct dap_cmd_pool *el, *tmp;
+ struct list_head *lh = &dap->cmd_pool;
+
+ list_for_each_entry_safe(el, tmp, lh, lh) {
+ list_del(&el->lh);
+ free(el);
+ }
+}