adi_v5_jtag.c

Go to the documentation of this file.

// SPDX-License-Identifier: GPL-2.0-or-later
 
/***************************************************************************
 *   Copyright (C) 2006 by Magnus Lundin
 *   lundin@mlu.mine.nu
 *
 *   Copyright (C) 2008 by Spencer Oliver
 *   spen@spen-soft.co.uk
 *
 *   Copyright (C) 2009 by Oyvind Harboe
 *   oyvind.harboe@zylin.com
 *
 *   Copyright (C) 2009-2010 by David Brownell
 *
 *   Copyright (C) 2020-2021, Ampere Computing LLC                              *
 ***************************************************************************/
 
#ifdef HAVE_CONFIG_H
#include "config.h"
#endif
 
#include "arm.h"
#include "arm_adi_v5.h"
#include <helper/time_support.h>
#include <helper/list.h>
#include <jtag/swd.h>
 
/*#define DEBUG_WAIT*/
 
/* JTAG instructions/registers for JTAG-DP and SWJ-DP */
#define JTAG_DP_ABORT       0xF8
#define JTAG_DP_DPACC       0xFA
#define JTAG_DP_APACC       0xFB
#define JTAG_DP_IDCODE      0xFE
 
/* three-bit ACK values for DPACC and APACC reads */
#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);
    }
}
 
/***************************************************************************
 *
 * DPACC and APACC scanchain access through JTAG-DP (or SWJ-DP)
 *
***************************************************************************/
 
static int adi_jtag_dp_scan_cmd(struct adiv5_dap *dap, struct dap_cmd *cmd, uint8_t *ack)
{
    struct jtag_tap *tap = dap->tap;
    int retval;
 
    retval = arm_jtag_set_instr(tap, cmd->instr, NULL, TAP_IDLE);
    if (retval != ERROR_OK)
        return retval;
 
    /* Scan out a read or write operation using some DP or AP register.
     * For APACC access with any sticky error flag set, this is discarded.
     */
    cmd->fields[0].num_bits = 3;
    buf_set_u32(&cmd->out_addr_buf, 0, 3, ((cmd->reg_addr >> 1) & 0x6) | (cmd->rnw & 0x1));
    cmd->fields[0].out_value = &cmd->out_addr_buf;
    cmd->fields[0].in_value = (ack) ? ack : &cmd->ack;
 
    /* NOTE: if we receive JTAG_ACK_WAIT, the previous operation did not
     * complete; data we write is discarded, data we read is unpredictable.
     * When overrun detect is active, STICKYORUN is set.
     */
 
    cmd->fields[1].num_bits = 32;
    cmd->fields[1].out_value = cmd->outvalue_buf;
    cmd->fields[1].in_value = cmd->invalue;
 
    jtag_add_dr_scan(tap, 2, cmd->fields, TAP_IDLE);
 
    /* Add specified number of tck clocks after starting AP register
     * access or memory bus access, giving the hardware time to complete
     * the access.
     * They provide more time for the (MEM) AP to complete the read ...
     * See "Minimum Response Time" for JTAG-DP, in the ADIv5/ADIv6 spec.
     */
    if (cmd->instr == JTAG_DP_APACC && cmd->memaccess_tck != 0)
        jtag_add_runtest(cmd->memaccess_tck, TAP_IDLE);
 
    return ERROR_OK;
}
 
static int adi_jtag_dp_scan_cmd_sync(struct adiv5_dap *dap, struct dap_cmd *cmd, uint8_t *ack)
{
    int retval;
 
    retval = adi_jtag_dp_scan_cmd(dap, cmd, ack);
    if (retval != ERROR_OK)
        return retval;
 
    return jtag_execute_queue();
}
 
static int adi_jtag_dp_scan(struct adiv5_dap *dap,
        uint8_t instr, uint16_t reg_addr, uint8_t rnw,
        uint8_t *outvalue, uint8_t *invalue,
        uint32_t memaccess_tck, uint8_t *ack)
{
    struct dap_cmd *cmd;
    int retval;
 
    cmd = dap_cmd_new(dap, instr, reg_addr, rnw, outvalue, invalue, memaccess_tck);
    if (cmd)
        cmd->dp_select = dap->select;
    else
        return ERROR_JTAG_DEVICE_ERROR;
 
    retval = adi_jtag_dp_scan_cmd(dap, cmd, ack);
    if (retval == ERROR_OK)
        list_add_tail(&cmd->lh, &dap->cmd_journal);
 
    return retval;
}
 
static int adi_jtag_dp_scan_u32(struct adiv5_dap *dap,
        uint8_t instr, uint16_t reg_addr, uint8_t rnw,
        uint32_t outvalue, uint32_t *invalue,
        uint32_t memaccess_tck, uint8_t *ack)
{
    uint8_t out_value_buf[4];
    int retval;
    uint64_t sel = (reg_addr >> 4) & DP_SELECT_DPBANK;
 
    /* No need to change SELECT or RDBUFF as they are not banked */
    if (instr == JTAG_DP_DPACC && reg_addr != DP_SELECT && reg_addr != DP_RDBUFF
            && (!dap->select_valid || sel != (dap->select & DP_SELECT_DPBANK))) {
        /* Use the AP part of dap->select regardless of dap->select_valid:
         * if !dap->select_valid
         * dap->select contains a speculative value likely going to be used
         * in the following swd_queue_ap_bankselect() */
        sel |= dap->select & SELECT_AP_MASK;
 
        LOG_DEBUG_IO("DP BANK SELECT: %" PRIx32, (uint32_t)sel);
 
        buf_set_u32(out_value_buf, 0, 32, (uint32_t)sel);
 
        retval = adi_jtag_dp_scan(dap, JTAG_DP_DPACC,
                DP_SELECT, DPAP_WRITE, out_value_buf, NULL, 0, NULL);
        if (retval != ERROR_OK)
            return retval;
 
        dap->select = sel;
        dap->select_valid = true;
    }
    buf_set_u32(out_value_buf, 0, 32, outvalue);
 
    retval = adi_jtag_dp_scan(dap, instr, reg_addr, rnw,
            out_value_buf, (uint8_t *)invalue, memaccess_tck, ack);
    if (retval != ERROR_OK)
        return retval;
 
    if (invalue)
        jtag_add_callback(arm_le_to_h_u32,
                (jtag_callback_data_t) invalue);
 
    return retval;
}
 
static int adi_jtag_finish_read(struct adiv5_dap *dap)
{
    int retval = ERROR_OK;
 
    if (dap->last_read) {
        retval = adi_jtag_dp_scan_u32(dap, JTAG_DP_DPACC,
                DP_RDBUFF, DPAP_READ, 0, dap->last_read, 0, NULL);
        dap->last_read = NULL;
    }
 
    return retval;
}
 
static int adi_jtag_scan_inout_check_u32(struct adiv5_dap *dap,
        uint8_t instr, uint16_t reg_addr, uint8_t rnw,
        uint32_t outvalue, uint32_t *invalue, uint32_t memaccess_tck)
{
    int retval;
 
    /* Issue the read or write */
    retval = adi_jtag_dp_scan_u32(dap, instr, reg_addr,
            rnw, outvalue, NULL, memaccess_tck, NULL);
    if (retval != ERROR_OK)
        return retval;
 
    /* For reads,  collect posted value; RDBUFF has no other effect.
     * Assumes read gets acked with OK/FAULT, and CTRL_STAT says "OK".
     */
    if ((rnw == DPAP_READ) && (invalue)) {
        retval = adi_jtag_dp_scan_u32(dap, JTAG_DP_DPACC,
                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) {
        /*
         * JTAG_ACK_OK_FAULT (ADIv5) and JTAG_ACK_FAULT (ADIv6) are equal so
         * the following statement is checking to see if an acknowledgment of
         * OK or FAULT is generated for ADIv5 or ADIv6
         */
        if (el->ack == JTAG_ACK_OK_FAULT || (is_adiv6(dap) && el->ack == JTAG_ACK_OK)) {
            log_dap_cmd(dap, "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(dap, "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(dap, "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) {
                /* The following check covers OK and FAULT ACKs for both ADIv5 and ADIv6 */
                if (tmp->ack == JTAG_ACK_OK_FAULT || (is_adiv6(dap) && tmp->ack == JTAG_ACK_OK)) {
                    /* recover the read value */
                    log_dap_cmd(dap, "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) {
                log_dap_cmd(dap, "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(dap, JTAG_DP_DPACC,
                        DP_RDBUFF, DPAP_READ, NULL, NULL, 0);
                if (!tmp) {
                    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;
                    /* The following check covers OK and FAULT ACKs for both ADIv5 and ADIv6 */
                    if (tmp->ack == JTAG_ACK_OK_FAULT || (is_adiv6(dap) && tmp->ack == JTAG_ACK_OK)) {
                        log_dap_cmd(dap, "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(dap, "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_WAIT) {
                        LOG_ERROR("Timeout during WAIT recovery");
                        dap->select_valid = false;
                        dap->select1_valid = false;
                        /* Keep dap->select unchanged, the same AP and AP bank
                         * is likely going to be used further */
                        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 */
                dap_cmd_release(dap, 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(dap, "REP", el);
        list_move_tail(&el->lh, &replay_list);
    }
 
    /* we're done with the journal, flush it */
    flush_journal(dap, &dap->cmd_journal);
 
    /* check for overrun condition in the last batch of transactions */
    if (found_wait) {
        LOG_INFO("DAP transaction stalled (WAIT) - slowing down and resending");
        /* 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);
 
            uint8_t out_value_buf[4];
            buf_set_u32(out_value_buf, 0, 32, (uint32_t)(el->dp_select));
 
            tmp = dap_cmd_new(dap, JTAG_DP_DPACC,
                      DP_SELECT, DPAP_WRITE, out_value_buf, NULL, 0);
            if (!tmp) {
                retval = ERROR_JTAG_DEVICE_ERROR;
                goto done;
            }
            list_add(&tmp->lh, &replay_list);
 
            /* TODO: ADIv6 DP SELECT1 handling */
 
            dap->select_valid = false;
        }
 
        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(dap, "REC", el);
                if (el->ack == JTAG_ACK_OK_FAULT || (is_adiv6(dap) && el->ack == JTAG_ACK_OK)) {
                    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(dap, "ERR", el);
                    retval = ERROR_JTAG_DEVICE_ERROR;
                    break;
                }
                LOG_DEBUG("DAP transaction stalled during replay (WAIT) - resending");
                /* 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)
                    break;
            } while (timeval_ms() - time_now < 1000);
 
            if (retval == ERROR_OK) {
                if (el->ack == JTAG_ACK_WAIT) {
                    LOG_ERROR("Timeout during WAIT recovery");
                    dap->select_valid = false;
                    dap->select1_valid = false;
                    /* Keep dap->select unchanged, the same AP and AP bank
                     * is likely going to be used further */
                    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(dap, &replay_list);
    flush_journal(dap, &dap->cmd_journal);
    return retval;
}
 
static int jtagdp_transaction_endcheck(struct adiv5_dap *dap)
{
    int retval;
    uint32_t ctrlstat, pwrmask;
 
    /* too expensive to call keep_alive() here */
 
    /* 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)
        goto done;
 
    /* REVISIT also STICKYCMP, for pushed comparisons (nyet used) */
 
    /* Check for STICKYERR */
    if (ctrlstat & SSTICKYERR) {
        LOG_DEBUG("jtag-dp: CTRL/STAT 0x%" PRIx32, ctrlstat);
        /* Check power to debug regions */
        pwrmask = CDBGPWRUPREQ | CDBGPWRUPACK | CSYSPWRUPREQ;
        if (!dap->ignore_syspwrupack)
            pwrmask |= CSYSPWRUPACK;
        if ((ctrlstat & pwrmask) != pwrmask) {
            LOG_ERROR("Debug regions are unpowered, an unexpected reset might have happened");
            dap->do_reconnect = true;
        }
 
        if (ctrlstat & SSTICKYERR)
            LOG_ERROR("JTAG-DP STICKY ERROR");
        if (ctrlstat & SSTICKYORUN)
            LOG_DEBUG("JTAG-DP STICKY OVERRUN");
 
        /* Clear Sticky Error and Sticky Overrun Bits */
        retval = adi_jtag_scan_inout_check_u32(dap, JTAG_DP_DPACC,
                DP_CTRL_STAT, DPAP_WRITE,
                dap->dp_ctrl_stat | SSTICKYERR | SSTICKYORUN, NULL, 0);
        if (retval != ERROR_OK)
            goto done;
 
        retval = ERROR_JTAG_DEVICE_ERROR;
    }
 
 done:
    flush_journal(dap, &dap->cmd_journal);
    return retval;
}
 
/*--------------------------------------------------------------------------*/
 
static int jtag_connect(struct adiv5_dap *dap)
{
    dap->do_reconnect = false;
    return dap_dp_init(dap);
}
 
static int jtag_check_reconnect(struct adiv5_dap *dap)
{
    if (dap->do_reconnect)
        return jtag_connect(dap);
 
    return ERROR_OK;
}
 
static int jtag_send_sequence(struct adiv5_dap *dap, enum swd_special_seq seq)
{
    int retval;
 
    switch (seq) {
    case JTAG_TO_SWD:
        retval =  jtag_add_tms_seq(swd_seq_jtag_to_swd_len,
                swd_seq_jtag_to_swd, TAP_INVALID);
        break;
    case SWD_TO_JTAG:
        retval = jtag_add_tms_seq(swd_seq_swd_to_jtag_len,
                swd_seq_swd_to_jtag, TAP_RESET);
        break;
    default:
        LOG_ERROR("Sequence %d not supported", seq);
        return ERROR_FAIL;
    }
    if (retval == ERROR_OK)
        retval = jtag_execute_queue();
    return retval;
}
 
static int jtag_dp_q_read(struct adiv5_dap *dap, unsigned int reg,
        uint32_t *data)
{
    int retval = jtag_limit_queue_size(dap);
    if (retval != ERROR_OK)
        return retval;
 
    retval =  adi_jtag_dp_scan_u32(dap, JTAG_DP_DPACC, reg,
            DPAP_READ, 0, dap->last_read, 0, NULL);
    dap->last_read = data;
    return retval;
}
 
static int jtag_dp_q_write(struct adiv5_dap *dap, unsigned int reg,
        uint32_t data)
{
    int retval = jtag_limit_queue_size(dap);
    if (retval != ERROR_OK)
        return retval;
 
    retval =  adi_jtag_dp_scan_u32(dap, JTAG_DP_DPACC,
            reg, DPAP_WRITE, data, dap->last_read, 0, NULL);
    dap->last_read = NULL;
    return retval;
}
 
static int jtag_ap_q_bankselect(struct adiv5_ap *ap, unsigned int reg)
{
    int retval;
    struct adiv5_dap *dap = ap->dap;
    uint64_t sel;
 
    if (is_adiv6(dap))
        sel = ap->ap_num | (reg & 0x00000FF0);
    else
        sel = (ap->ap_num << 24) | (reg & ADIV5_DP_SELECT_APBANK);
 
    uint64_t sel_diff = (sel ^ dap->select) & SELECT_AP_MASK;
 
    bool set_select = !dap->select_valid || (sel_diff & 0xffffffffull);
    bool set_select1 = is_adiv6(dap) && dap->asize > 32
                        && (!dap->select1_valid
                            || sel_diff & (0xffffffffull << 32));
 
    if (set_select && set_select1) {
        /* Prepare DP bank for DP_SELECT1 now to save one write */
        sel |= (DP_SELECT1 >> 4) & DP_SELECT_DPBANK;
    } else {
        /* Use the DP part of dap->select regardless of dap->select_valid:
         * if !dap->select_valid
         * dap->select contains a speculative value likely going to be used
         * in the following swd_queue_dp_bankselect().
         * Moreover dap->select_valid should never be false here as a DP bank
         * is always selected before selecting an AP bank */
        sel |= dap->select & DP_SELECT_DPBANK;
    }
 
    if (set_select) {
        LOG_DEBUG_IO("AP BANK SELECT: %" PRIx32, (uint32_t)sel);
 
        retval = jtag_dp_q_write(dap, DP_SELECT, (uint32_t)sel);
        if (retval != ERROR_OK) {
            dap->select_valid = false;
            return retval;
        }
    }
 
    if (set_select1) {
        LOG_DEBUG_IO("AP BANK SELECT1: %" PRIx32, (uint32_t)(sel >> 32));
 
        retval = jtag_dp_q_write(dap, DP_SELECT1, (uint32_t)(sel >> 32));
        if (retval != ERROR_OK) {
            dap->select1_valid = false;
            return retval;
        }
    }
 
    dap->select = sel;
    return ERROR_OK;
}
 
static int jtag_ap_q_read(struct adiv5_ap *ap, unsigned int reg,
        uint32_t *data)
{
    int retval = jtag_limit_queue_size(ap->dap);
    if (retval != ERROR_OK)
        return retval;
 
    retval = jtag_check_reconnect(ap->dap);
    if (retval != ERROR_OK)
        return retval;
 
    retval = jtag_ap_q_bankselect(ap, reg);
    if (retval != ERROR_OK)
        return retval;
 
    retval =  adi_jtag_dp_scan_u32(ap->dap, JTAG_DP_APACC, reg,
            DPAP_READ, 0, ap->dap->last_read, ap->memaccess_tck, NULL);
    ap->dap->last_read = data;
 
    return retval;
}
 
static int jtag_ap_q_write(struct adiv5_ap *ap, unsigned int reg,
        uint32_t data)
{
    int retval = jtag_limit_queue_size(ap->dap);
    if (retval != ERROR_OK)
        return retval;
 
    retval = jtag_check_reconnect(ap->dap);
    if (retval != ERROR_OK)
        return retval;
 
    retval = jtag_ap_q_bankselect(ap, reg);
    if (retval != ERROR_OK)
        return retval;
 
    retval =  adi_jtag_dp_scan_u32(ap->dap, JTAG_DP_APACC, reg,
            DPAP_WRITE, data, ap->dap->last_read, ap->memaccess_tck, NULL);
    ap->dap->last_read = NULL;
    return retval;
}
 
static int jtag_ap_q_abort(struct adiv5_dap *dap, uint8_t *ack)
{
    /* for JTAG, this is the only valid ABORT register operation */
    int retval =  adi_jtag_dp_scan_u32(dap, JTAG_DP_ABORT,
            0, DPAP_WRITE, 1, NULL, 0, NULL);
    if (retval != ERROR_OK)
        return retval;
 
    return jtag_execute_queue();
}
 
static int jtag_dp_run(struct adiv5_dap *dap)
{
    int retval;
    int retval2 = ERROR_OK;
 
    retval = adi_jtag_finish_read(dap);
    if (retval != ERROR_OK)
        goto done;
    retval2 = jtagdp_overrun_check(dap);
    retval = jtagdp_transaction_endcheck(dap);
 
 done:
    return (retval2 != ERROR_OK) ? retval2 : retval;
}
 
static int jtag_dp_sync(struct adiv5_dap *dap)
{
    return jtagdp_overrun_check(dap);
}
 
/* FIXME don't export ... just initialize as
 * part of DAP setup
*/
const struct dap_ops jtag_dp_ops = {
    .connect             = jtag_connect,
    .send_sequence       = jtag_send_sequence,
    .queue_dp_read       = jtag_dp_q_read,
    .queue_dp_write      = jtag_dp_q_write,
    .queue_ap_read       = jtag_ap_q_read,
    .queue_ap_write      = jtag_ap_q_write,
    .queue_ap_abort      = jtag_ap_q_abort,
    .run                 = jtag_dp_run,
    .sync                = jtag_dp_sync,
    .quit                = jtag_quit,
};