src/target/adi_v5_jtag.c

   1 // SPDX-License-Identifier: GPL-2.0-or-later
   2
   3 /***************************************************************************
   4  *   Copyright (C) 2006 by Magnus Lundin
   5  *   lundin@mlu.mine.nu
   6  *
   7  *   Copyright (C) 2008 by Spencer Oliver
   8  *   spen@spen-soft.co.uk
   9  *
  10  *   Copyright (C) 2009 by Oyvind Harboe
  11  *   oyvind.harboe@zylin.com
  12  *
  13  *   Copyright (C) 2009-2010 by David Brownell
  14  *
  15  *   Copyright (C) 2020-2021, Ampere Computing LLC                              *
  16  ***************************************************************************/
  17
  18 /**
  19  * @file
  20  * This file implements JTAG transport support for cores implementing
  21  the ARM Debug Interface version 5 (ADIv5) and version 6 (ADIv6).
  22  */
  23
  24 #ifdef HAVE_CONFIG_H
  25 #include "config.h"
  26 #endif
  27
  28 #include "arm.h"
  29 #include "arm_adi_v5.h"
  30 #include <helper/time_support.h>
  31 #include <helper/list.h>
  32 #include <jtag/swd.h>
  33
  34 /*#define DEBUG_WAIT*/
  35
  36 /* JTAG instructions/registers for JTAG-DP and SWJ-DP */
  37 #define JTAG_DP_ABORT           0xF8
  38 #define JTAG_DP_DPACC           0xFA
  39 #define JTAG_DP_APACC           0xFB
  40 #define JTAG_DP_IDCODE          0xFE
  41
  42 /* three-bit ACK values for DPACC and APACC reads */
  43 #define JTAG_ACK_WAIT       0x1     /* ADIv5 and ADIv6 */
  44 #define JTAG_ACK_OK_FAULT   0x2     /* ADIv5 */
  45 #define JTAG_ACK_FAULT      0x2     /* ADIv6 */
  46 #define JTAG_ACK_OK         0x4     /* ADIV6 */
  47
  48 static int jtag_ap_q_abort(struct adiv5_dap *dap, uint8_t *ack);
  49
  50 #ifdef DEBUG_WAIT
  51 static const char *dap_reg_name(struct adiv5_dap *dap, uint8_t instr, uint16_t reg_addr)
  52 {
  53         char *reg_name = "UNK";
  54
  55         if (instr == JTAG_DP_DPACC) {
  56                 switch (reg_addr) {
  57                 case DP_ABORT:
  58                         reg_name =  "ABORT";
  59                         break;
  60                 case DP_CTRL_STAT:
  61                         reg_name =  "CTRL/STAT";
  62                         break;
  63                 case DP_SELECT:
  64                         reg_name = "SELECT";
  65                         break;
  66                 case DP_RDBUFF:
  67                         reg_name =  "RDBUFF";
  68                         break;
  69                 case DP_DLCR:
  70                         reg_name =  "DLCR";
  71                         break;
  72                 default:
  73                         reg_name = "UNK";
  74                         break;
  75                 }
  76         }
  77
  78         if (instr == JTAG_DP_APACC) {
  79                 if (reg_addr == MEM_AP_REG_CSW(dap))
  80                         reg_name = "CSW";
  81                 else if (reg_addr == MEM_AP_REG_TAR(dap))
  82                         reg_name = "TAR";
  83                 else if (reg_addr == MEM_AP_REG_TAR64(dap))
  84                         reg_name = "TAR64";
  85                 else if (reg_addr == MEM_AP_REG_DRW(dap))
  86                         reg_name = "DRW";
  87                 else if (reg_addr == MEM_AP_REG_BD0(dap))
  88                         reg_name = "BD0";
  89                 else if (reg_addr == MEM_AP_REG_BD1(dap))
  90                         reg_name = "BD1";
  91                 else if (reg_addr == MEM_AP_REG_BD2(dap))
  92                         reg_name = "BD2";
  93                 else if (reg_addr == MEM_AP_REG_BD3(dap))
  94                         reg_name = "BD3";
  95                 else if (reg_addr == MEM_AP_REG_CFG(dap))
  96                         reg_name = "CFG";
  97                 else if (reg_addr == MEM_AP_REG_BASE(dap))
  98                         reg_name = "BASE";
  99                 else if (reg_addr == MEM_AP_REG_BASE64(dap))
 100                         reg_name = "BASE64";
 101                 else if (reg_addr == AP_REG_IDR(dap))
 102                         reg_name = "IDR";
 103                 else
 104                         reg_name = "UNK";
 105         }
 106
 107         return reg_name;
 108 }
 109 #endif
 110
 111 struct dap_cmd {
 112         struct list_head lh;
 113         uint8_t instr;
 114         uint16_t reg_addr;
 115         uint8_t rnw;
 116         uint8_t *invalue;
 117         uint8_t ack;
 118         uint32_t memaccess_tck;
 119         uint64_t dp_select;
 120
 121         struct scan_field fields[2];
 122         uint8_t out_addr_buf;
 123         uint8_t invalue_buf[4];
 124         uint8_t outvalue_buf[4];
 125 };
 126
 127 #define MAX_DAP_COMMAND_NUM 65536
 128
 129 struct dap_cmd_pool {
 130         struct list_head lh;
 131         struct dap_cmd cmd;
 132 };
 133
 134 static void log_dap_cmd(struct adiv5_dap *dap, const char *header, struct dap_cmd *el)
 135 {
 136 #ifdef DEBUG_WAIT
 137         const char *ack;
 138         switch (el->ack) {
 139         case JTAG_ACK_WAIT:         /* ADIv5 and ADIv6 */
 140                 ack = "WAIT";
 141                 break;
 142         case JTAG_ACK_OK_FAULT:     /* ADIv5, same value as JTAG_ACK_FAULT */
 143         /* case JTAG_ACK_FAULT: */  /* ADIv6 */
 144                 if (is_adiv6(dap))
 145                         ack = "FAULT";
 146                 else
 147                         ack = "OK";
 148                 break;
 149         case JTAG_ACK_OK:           /* ADIv6 */
 150                 if (is_adiv6(dap)) {
 151                         ack = "OK";
 152                         break;
 153                 }
 154                 /* fall-through */
 155         default:
 156                 ack = "INVAL";
 157                 break;
 158         }
 159         LOG_DEBUG("%s: %2s %6s %5s 0x%08x 0x%08x %2s", header,
 160                 el->instr == JTAG_DP_APACC ? "AP" : "DP",
 161                 dap_reg_name(dap, el->instr, el->reg_addr),
 162                 el->rnw == DPAP_READ ? "READ" : "WRITE",
 163                 buf_get_u32(el->outvalue_buf, 0, 32),
 164                 buf_get_u32(el->invalue, 0, 32),
 165                 ack);
 166 #endif
 167 }
 168
 169 static int jtag_limit_queue_size(struct adiv5_dap *dap)
 170 {
 171         if (dap->cmd_pool_size < MAX_DAP_COMMAND_NUM)
 172                 return ERROR_OK;
 173
 174         return dap_run(dap);
 175 }
 176
 177 static struct dap_cmd *dap_cmd_new(struct adiv5_dap *dap, uint8_t instr,
 178                 uint16_t reg_addr, uint8_t rnw,
 179                 uint8_t *outvalue, uint8_t *invalue,
 180                 uint32_t memaccess_tck)
 181 {
 182
 183         struct dap_cmd_pool *pool = NULL;
 184
 185         if (list_empty(&dap->cmd_pool)) {
 186                 pool = calloc(1, sizeof(struct dap_cmd_pool));
 187                 if (!pool)
 188                         return NULL;
 189         } else {
 190                 pool = list_first_entry(&dap->cmd_pool, struct dap_cmd_pool, lh);
 191                 list_del(&pool->lh);
 192         }
 193
 194         INIT_LIST_HEAD(&pool->lh);
 195         dap->cmd_pool_size++;
 196
 197         struct dap_cmd *cmd = &pool->cmd;
 198         INIT_LIST_HEAD(&cmd->lh);
 199         cmd->instr = instr;
 200         cmd->reg_addr = reg_addr;
 201         cmd->rnw = rnw;
 202         if (outvalue)
 203                 memcpy(cmd->outvalue_buf, outvalue, 4);
 204         cmd->invalue = (invalue) ? invalue : cmd->invalue_buf;
 205         cmd->memaccess_tck = memaccess_tck;
 206
 207         return cmd;
 208 }
 209
 210 static void dap_cmd_release(struct adiv5_dap *dap, struct dap_cmd *cmd)
 211 {
 212         struct dap_cmd_pool *pool = container_of(cmd, struct dap_cmd_pool, cmd);
 213         if (dap->cmd_pool_size > MAX_DAP_COMMAND_NUM)
 214                 free(pool);
 215         else
 216                 list_add(&pool->lh, &dap->cmd_pool);
 217
 218         dap->cmd_pool_size--;
 219 }
 220
 221 static void flush_journal(struct adiv5_dap *dap, struct list_head *lh)
 222 {
 223         struct dap_cmd *el, *tmp;
 224
 225         list_for_each_entry_safe(el, tmp, lh, lh) {
 226                 list_del(&el->lh);
 227                 dap_cmd_release(dap, el);
 228         }
 229 }
 230
 231 static void jtag_quit(struct adiv5_dap *dap)
 232 {
 233         struct dap_cmd_pool *el, *tmp;
 234         struct list_head *lh = &dap->cmd_pool;
 235
 236         list_for_each_entry_safe(el, tmp, lh, lh) {
 237                 list_del(&el->lh);
 238                 free(el);
 239         }
 240 }
 241
 242 /***************************************************************************
 243  *
 244  * DPACC and APACC scanchain access through JTAG-DP (or SWJ-DP)
 245  *
 246 ***************************************************************************/
 247
 248 static int adi_jtag_dp_scan_cmd(struct adiv5_dap *dap, struct dap_cmd *cmd, uint8_t *ack)
 249 {
 250         struct jtag_tap *tap = dap->tap;
 251         int retval;
 252
 253         retval = arm_jtag_set_instr(tap, cmd->instr, NULL, TAP_IDLE);
 254         if (retval != ERROR_OK)
 255                 return retval;
 256
 257         /* Scan out a read or write operation using some DP or AP register.
 258          * For APACC access with any sticky error flag set, this is discarded.
 259          */
 260         cmd->fields[0].num_bits = 3;
 261         buf_set_u32(&cmd->out_addr_buf, 0, 3, ((cmd->reg_addr >> 1) & 0x6) | (cmd->rnw & 0x1));
 262         cmd->fields[0].out_value = &cmd->out_addr_buf;
 263         cmd->fields[0].in_value = (ack) ? ack : &cmd->ack;
 264
 265         /* NOTE: if we receive JTAG_ACK_WAIT, the previous operation did not
 266          * complete; data we write is discarded, data we read is unpredictable.
 267          * When overrun detect is active, STICKYORUN is set.
 268          */
 269
 270         cmd->fields[1].num_bits = 32;
 271         cmd->fields[1].out_value = cmd->outvalue_buf;
 272         cmd->fields[1].in_value = cmd->invalue;
 273
 274         jtag_add_dr_scan(tap, 2, cmd->fields, TAP_IDLE);
 275
 276         /* Add specified number of tck clocks after starting AP register
 277          * access or memory bus access, giving the hardware time to complete
 278          * the access.
 279          * They provide more time for the (MEM) AP to complete the read ...
 280          * See "Minimum Response Time" for JTAG-DP, in the ADIv5/ADIv6 spec.
 281          */
 282         if (cmd->instr == JTAG_DP_APACC && cmd->memaccess_tck != 0)
 283                 jtag_add_runtest(cmd->memaccess_tck, TAP_IDLE);
 284
 285         return ERROR_OK;
 286 }
 287
 288 static int adi_jtag_dp_scan_cmd_sync(struct adiv5_dap *dap, struct dap_cmd *cmd, uint8_t *ack)
 289 {
 290         int retval;
 291
 292         retval = adi_jtag_dp_scan_cmd(dap, cmd, ack);
 293         if (retval != ERROR_OK)
 294                 return retval;
 295
 296         return jtag_execute_queue();
 297 }
 298
 299 /**
 300  * Scan DPACC or APACC using target ordered uint8_t buffers.  No endianness
 301  * conversions are performed.  See section 4.4.3 of the ADIv5/ADIv6 spec, which
 302  * discusses operations which access these registers.
 303  *
 304  * Note that only one scan is performed.  If rnw is set, a separate scan
 305  * will be needed to collect the data which was read; the "invalue" collects
 306  * the posted result of a preceding operation, not the current one.
 307  *
 308  * @param dap the DAP
 309  * @param instr JTAG_DP_APACC (AP access) or JTAG_DP_DPACC (DP access)
 310  * @param reg_addr two significant bits; A[3:2]; for APACC access, the
 311  *      SELECT register has more addressing bits.
 312  * @param rnw false iff outvalue will be written to the DP or AP
 313  * @param outvalue points to a 32-bit (little-endian) integer
 314  * @param invalue NULL, or points to a 32-bit (little-endian) integer
 315  * @param ack points to where the three bit JTAG_ACK_* code will be stored
 316  * @param memaccess_tck number of idle cycles to add after AP access
 317  */
 318
 319 static int adi_jtag_dp_scan(struct adiv5_dap *dap,
 320                 uint8_t instr, uint16_t reg_addr, uint8_t rnw,
 321                 uint8_t *outvalue, uint8_t *invalue,
 322                 uint32_t memaccess_tck, uint8_t *ack)
 323 {
 324         struct dap_cmd *cmd;
 325         int retval;
 326
 327         cmd = dap_cmd_new(dap, instr, reg_addr, rnw, outvalue, invalue, memaccess_tck);
 328         if (cmd)
 329                 cmd->dp_select = dap->select;
 330         else
 331                 return ERROR_JTAG_DEVICE_ERROR;
 332
 333         retval = adi_jtag_dp_scan_cmd(dap, cmd, ack);
 334         if (retval == ERROR_OK)
 335                 list_add_tail(&cmd->lh, &dap->cmd_journal);
 336
 337         return retval;
 338 }
 339
 340 /**
 341  * Scan DPACC or APACC out and in from host ordered uint32_t buffers.
 342  * This is exactly like adi_jtag_dp_scan(), except that endianness
 343  * conversions are performed (so the types of invalue and outvalue
 344  * must be different).
 345  */
 346 static int adi_jtag_dp_scan_u32(struct adiv5_dap *dap,
 347                 uint8_t instr, uint16_t reg_addr, uint8_t rnw,
 348                 uint32_t outvalue, uint32_t *invalue,
 349                 uint32_t memaccess_tck, uint8_t *ack)
 350 {
 351         uint8_t out_value_buf[4];
 352         int retval;
 353         uint64_t sel = (reg_addr >> 4) & DP_SELECT_DPBANK;
 354
 355         /* No need to change SELECT or RDBUFF as they are not banked */
 356         if (instr == JTAG_DP_DPACC && reg_addr != DP_SELECT && reg_addr != DP_RDBUFF
 357                         && (!dap->select_valid || sel != (dap->select & DP_SELECT_DPBANK))) {
 358                 /* Use the AP part of dap->select regardless of dap->select_valid:
 359                  * if !dap->select_valid
 360                  * dap->select contains a speculative value likely going to be used
 361                  * in the following swd_queue_ap_bankselect() */
 362                 sel |= dap->select & SELECT_AP_MASK;
 363
 364                 LOG_DEBUG_IO("DP BANK SELECT: %" PRIx32, (uint32_t)sel);
 365
 366                 buf_set_u32(out_value_buf, 0, 32, (uint32_t)sel);
 367
 368                 retval = adi_jtag_dp_scan(dap, JTAG_DP_DPACC,
 369                                 DP_SELECT, DPAP_WRITE, out_value_buf, NULL, 0, NULL);
 370                 if (retval != ERROR_OK)
 371                         return retval;
 372
 373                 dap->select = sel;
 374                 dap->select_valid = true;
 375         }
 376         buf_set_u32(out_value_buf, 0, 32, outvalue);
 377
 378         retval = adi_jtag_dp_scan(dap, instr, reg_addr, rnw,
 379                         out_value_buf, (uint8_t *)invalue, memaccess_tck, ack);
 380         if (retval != ERROR_OK)
 381                 return retval;
 382
 383         if (invalue)
 384                 jtag_add_callback(arm_le_to_h_u32,
 385                                 (jtag_callback_data_t) invalue);
 386
 387         return retval;
 388 }
 389
 390 static int adi_jtag_finish_read(struct adiv5_dap *dap)
 391 {
 392         int retval = ERROR_OK;
 393
 394         if (dap->last_read) {
 395                 retval = adi_jtag_dp_scan_u32(dap, JTAG_DP_DPACC,
 396                                 DP_RDBUFF, DPAP_READ, 0, dap->last_read, 0, NULL);
 397                 dap->last_read = NULL;
 398         }
 399
 400         return retval;
 401 }
 402
 403 static int adi_jtag_scan_inout_check_u32(struct adiv5_dap *dap,
 404                 uint8_t instr, uint16_t reg_addr, uint8_t rnw,
 405                 uint32_t outvalue, uint32_t *invalue, uint32_t memaccess_tck)
 406 {
 407         int retval;
 408
 409         /* Issue the read or write */
 410         retval = adi_jtag_dp_scan_u32(dap, instr, reg_addr,
 411                         rnw, outvalue, NULL, memaccess_tck, NULL);
 412         if (retval != ERROR_OK)
 413                 return retval;
 414
 415         /* For reads,  collect posted value; RDBUFF has no other effect.
 416          * Assumes read gets acked with OK/FAULT, and CTRL_STAT says "OK".
 417          */
 418         if ((rnw == DPAP_READ) && (invalue)) {
 419                 retval = adi_jtag_dp_scan_u32(dap, JTAG_DP_DPACC,
 420                                 DP_RDBUFF, DPAP_READ, 0, invalue, 0, NULL);
 421                 if (retval != ERROR_OK)
 422                         return retval;
 423         }
 424
 425         return jtag_execute_queue();
 426 }
 427
 428 static int jtagdp_overrun_check(struct adiv5_dap *dap)
 429 {
 430         int retval;
 431         struct dap_cmd *el, *tmp, *prev = NULL;
 432         int found_wait = 0;
 433         int64_t time_now;
 434         LIST_HEAD(replay_list);
 435
 436         /* make sure all queued transactions are complete */
 437         retval = jtag_execute_queue();
 438         if (retval != ERROR_OK)
 439                 goto done;
 440
 441         /* skip all completed transactions up to the first WAIT */
 442         list_for_each_entry(el, &dap->cmd_journal, lh) {
 443                 /*
 444                  * JTAG_ACK_OK_FAULT (ADIv5) and JTAG_ACK_FAULT (ADIv6) are equal so
 445                  * the following statement is checking to see if an acknowledgment of
 446                  * OK or FAULT is generated for ADIv5 or ADIv6
 447                  */
 448                 if (el->ack == JTAG_ACK_OK_FAULT || (is_adiv6(dap) && el->ack == JTAG_ACK_OK)) {
 449                         log_dap_cmd(dap, "LOG", el);
 450                 } else if (el->ack == JTAG_ACK_WAIT) {
 451                         found_wait = 1;
 452                         break;
 453                 } else {
 454                         LOG_ERROR("Invalid ACK (%1x) in DAP response", el->ack);
 455                         log_dap_cmd(dap, "ERR", el);
 456                         retval = ERROR_JTAG_DEVICE_ERROR;
 457                         goto done;
 458                 }
 459         }
 460
 461         /*
 462          * If we found a stalled transaction and a previous transaction
 463          * exists, check if it's a READ access.
 464          */
 465         if (found_wait && el != list_first_entry(&dap->cmd_journal, struct dap_cmd, lh)) {
 466                 prev = list_entry(el->lh.prev, struct dap_cmd, lh);
 467                 if (prev->rnw == DPAP_READ) {
 468                         log_dap_cmd(dap, "PND", prev);
 469                         /* search for the next OK transaction, it contains
 470                          * the result of the previous READ */
 471                         tmp = el;
 472                         list_for_each_entry_from(tmp, &dap->cmd_journal, lh) {
 473                                 /* The following check covers OK and FAULT ACKs for both ADIv5 and ADIv6 */
 474                                 if (tmp->ack == JTAG_ACK_OK_FAULT || (is_adiv6(dap) && tmp->ack == JTAG_ACK_OK)) {
 475                                         /* recover the read value */
 476                                         log_dap_cmd(dap, "FND", tmp);
 477                                         if (el->invalue != el->invalue_buf) {
 478                                                 uint32_t invalue = le_to_h_u32(tmp->invalue);
 479                                                 memcpy(el->invalue, &invalue, sizeof(uint32_t));
 480                                         }
 481                                         prev = NULL;
 482                                         break;
 483                                 }
 484                         }
 485
 486                         if (prev) {
 487                                 log_dap_cmd(dap, "LST", el);
 488
 489                                 /*
 490                                 * At this point we're sure that no previous
 491                                 * transaction completed and the DAP/AP is still
 492                                 * in busy state. We know that the next "OK" scan
 493                                 * will return the READ result we need to recover.
 494                                 * To complete the READ, we just keep polling RDBUFF
 495                                 * until the WAIT condition clears
 496                                 */
 497                                 tmp = dap_cmd_new(dap, JTAG_DP_DPACC,
 498                                                 DP_RDBUFF, DPAP_READ, NULL, NULL, 0);
 499                                 if (!tmp) {
 500                                         retval = ERROR_JTAG_DEVICE_ERROR;
 501                                         goto done;
 502                                 }
 503                                 /* synchronously retry the command until it succeeds */
 504                                 time_now = timeval_ms();
 505                                 do {
 506                                         retval = adi_jtag_dp_scan_cmd_sync(dap, tmp, NULL);
 507                                         if (retval != ERROR_OK)
 508                                                 break;
 509                                         /* The following check covers OK and FAULT ACKs for both ADIv5 and ADIv6 */
 510                                         if (tmp->ack == JTAG_ACK_OK_FAULT || (is_adiv6(dap) && tmp->ack == JTAG_ACK_OK)) {
 511                                                 log_dap_cmd(dap, "FND", tmp);
 512                                                 if (el->invalue != el->invalue_buf) {
 513                                                         uint32_t invalue = le_to_h_u32(tmp->invalue);
 514                                                         memcpy(el->invalue, &invalue, sizeof(uint32_t));
 515                                                 }
 516                                                 break;
 517                                         }
 518                                         if (tmp->ack != JTAG_ACK_WAIT) {
 519                                                 LOG_ERROR("Invalid ACK (%1x) in DAP response", tmp->ack);
 520                                                 log_dap_cmd(dap, "ERR", tmp);
 521                                                 retval = ERROR_JTAG_DEVICE_ERROR;
 522                                                 break;
 523                                         }
 524
 525                                 } while (timeval_ms() - time_now < 1000);
 526
 527                                 if (retval == ERROR_OK) {
 528                                         /* timeout happened */
 529                                         if (tmp->ack == JTAG_ACK_WAIT) {
 530                                                 LOG_ERROR("Timeout during WAIT recovery");
 531                                                 dap->select_valid = false;
 532                                                 dap->select1_valid = false;
 533                                                 /* Keep dap->select unchanged, the same AP and AP bank
 534                                                  * is likely going to be used further */
 535                                                 jtag_ap_q_abort(dap, NULL);
 536                                                 /* clear the sticky overrun condition */
 537                                                 adi_jtag_scan_inout_check_u32(dap, JTAG_DP_DPACC,
 538                                                         DP_CTRL_STAT, DPAP_WRITE,
 539                                                         dap->dp_ctrl_stat | SSTICKYORUN, NULL, 0);
 540                                                 retval = ERROR_JTAG_DEVICE_ERROR;
 541                                         }
 542                                 }
 543
 544                                 /* we're done with this command, release it */
 545                                 dap_cmd_release(dap, tmp);
 546
 547                                 if (retval != ERROR_OK)
 548                                         goto done;
 549
 550                         }
 551                         /* make el->invalue point to the default invalue
 552                         * so that we can safely retry it without clobbering
 553                         * the result we just recovered */
 554                         el->invalue = el->invalue_buf;
 555                 }
 556         }
 557
 558         /* move all remaining transactions over to the replay list */
 559         list_for_each_entry_safe_from(el, tmp, &dap->cmd_journal, lh) {
 560                 log_dap_cmd(dap, "REP", el);
 561                 list_move_tail(&el->lh, &replay_list);
 562         }
 563
 564         /* we're done with the journal, flush it */
 565         flush_journal(dap, &dap->cmd_journal);
 566
 567         /* check for overrun condition in the last batch of transactions */
 568         if (found_wait) {
 569                 LOG_INFO("DAP transaction stalled (WAIT) - slowing down and resending");
 570                 /* clear the sticky overrun condition */
 571                 retval = adi_jtag_scan_inout_check_u32(dap, JTAG_DP_DPACC,
 572                                 DP_CTRL_STAT, DPAP_WRITE,
 573                                 dap->dp_ctrl_stat | SSTICKYORUN, NULL, 0);
 574                 if (retval != ERROR_OK)
 575                         goto done;
 576
 577                 /* restore SELECT register first */
 578                 if (!list_empty(&replay_list)) {
 579                         el = list_first_entry(&replay_list, struct dap_cmd, lh);
 580
 581                         uint8_t out_value_buf[4];
 582                         buf_set_u32(out_value_buf, 0, 32, (uint32_t)(el->dp_select));
 583
 584                         tmp = dap_cmd_new(dap, JTAG_DP_DPACC,
 585                                           DP_SELECT, DPAP_WRITE, out_value_buf, NULL, 0);
 586                         if (!tmp) {
 587                                 retval = ERROR_JTAG_DEVICE_ERROR;
 588                                 goto done;
 589                         }
 590                         list_add(&tmp->lh, &replay_list);
 591
 592                         /* TODO: ADIv6 DP SELECT1 handling */
 593
 594                         dap->select_valid = false;
 595                 }
 596
 597                 list_for_each_entry_safe(el, tmp, &replay_list, lh) {
 598                         time_now = timeval_ms();
 599                         do {
 600                                 retval = adi_jtag_dp_scan_cmd_sync(dap, el, NULL);
 601                                 if (retval != ERROR_OK)
 602                                         break;
 603                                 log_dap_cmd(dap, "REC", el);
 604                                 if (el->ack == JTAG_ACK_OK_FAULT || (is_adiv6(dap) && el->ack == JTAG_ACK_OK)) {
 605                                         if (el->invalue != el->invalue_buf) {
 606                                                 uint32_t invalue = le_to_h_u32(el->invalue);
 607                                                 memcpy(el->invalue, &invalue, sizeof(uint32_t));
 608                                         }
 609                                         break;
 610                                 }
 611                                 if (el->ack != JTAG_ACK_WAIT) {
 612                                         LOG_ERROR("Invalid ACK (%1x) in DAP response", el->ack);
 613                                         log_dap_cmd(dap, "ERR", el);
 614                                         retval = ERROR_JTAG_DEVICE_ERROR;
 615                                         break;
 616                                 }
 617                                 LOG_DEBUG("DAP transaction stalled during replay (WAIT) - resending");
 618                                 /* clear the sticky overrun condition */
 619                                 retval = adi_jtag_scan_inout_check_u32(dap, JTAG_DP_DPACC,
 620                                                 DP_CTRL_STAT, DPAP_WRITE,
 621                                                 dap->dp_ctrl_stat | SSTICKYORUN, NULL, 0);
 622                                 if (retval != ERROR_OK)
 623                                         break;
 624                         } while (timeval_ms() - time_now < 1000);
 625
 626                         if (retval == ERROR_OK) {
 627                                 if (el->ack == JTAG_ACK_WAIT) {
 628                                         LOG_ERROR("Timeout during WAIT recovery");
 629                                         dap->select_valid = false;
 630                                         dap->select1_valid = false;
 631                                         /* Keep dap->select unchanged, the same AP and AP bank
 632                                          * is likely going to be used further */
 633                                         jtag_ap_q_abort(dap, NULL);
 634                                         /* clear the sticky overrun condition */
 635                                         adi_jtag_scan_inout_check_u32(dap, JTAG_DP_DPACC,
 636                                                 DP_CTRL_STAT, DPAP_WRITE,
 637                                                 dap->dp_ctrl_stat | SSTICKYORUN, NULL, 0);
 638                                         retval = ERROR_JTAG_DEVICE_ERROR;
 639                                         break;
 640                                 }
 641                         } else
 642                                 break;
 643                 }
 644         }
 645
 646  done:
 647         flush_journal(dap, &replay_list);
 648         flush_journal(dap, &dap->cmd_journal);
 649         return retval;
 650 }
 651
 652 static int jtagdp_transaction_endcheck(struct adiv5_dap *dap)
 653 {
 654         int retval;
 655         uint32_t ctrlstat, pwrmask;
 656
 657         /* too expensive to call keep_alive() here */
 658
 659         /* Post CTRL/STAT read; discard any previous posted read value
 660          * but collect its ACK status.
 661          */
 662         retval = adi_jtag_scan_inout_check_u32(dap, JTAG_DP_DPACC,
 663                         DP_CTRL_STAT, DPAP_READ, 0, &ctrlstat, 0);
 664         if (retval != ERROR_OK)
 665                 goto done;
 666
 667         /* REVISIT also STICKYCMP, for pushed comparisons (nyet used) */
 668
 669         /* Check for STICKYERR */
 670         if (ctrlstat & SSTICKYERR) {
 671                 LOG_DEBUG("jtag-dp: CTRL/STAT 0x%" PRIx32, ctrlstat);
 672                 /* Check power to debug regions */
 673                 pwrmask = CDBGPWRUPREQ | CDBGPWRUPACK | CSYSPWRUPREQ;
 674                 if (!dap->ignore_syspwrupack)
 675                         pwrmask |= CSYSPWRUPACK;
 676                 if ((ctrlstat & pwrmask) != pwrmask) {
 677                         LOG_ERROR("Debug regions are unpowered, an unexpected reset might have happened");
 678                         dap->do_reconnect = true;
 679                 }
 680
 681                 if (ctrlstat & SSTICKYERR)
 682                         LOG_ERROR("JTAG-DP STICKY ERROR");
 683                 if (ctrlstat & SSTICKYORUN)
 684                         LOG_DEBUG("JTAG-DP STICKY OVERRUN");
 685
 686                 /* Clear Sticky Error and Sticky Overrun Bits */
 687                 retval = adi_jtag_scan_inout_check_u32(dap, JTAG_DP_DPACC,
 688                                 DP_CTRL_STAT, DPAP_WRITE,
 689                                 dap->dp_ctrl_stat | SSTICKYERR | SSTICKYORUN, NULL, 0);
 690                 if (retval != ERROR_OK)
 691                         goto done;
 692
 693                 retval = ERROR_JTAG_DEVICE_ERROR;
 694         }
 695
 696  done:
 697         flush_journal(dap, &dap->cmd_journal);
 698         return retval;
 699 }
 700
 701 /*--------------------------------------------------------------------------*/
 702
 703 static int jtag_connect(struct adiv5_dap *dap)
 704 {
 705         dap->do_reconnect = false;
 706         return dap_dp_init(dap);
 707 }
 708
 709 static int jtag_check_reconnect(struct adiv5_dap *dap)
 710 {
 711         if (dap->do_reconnect)
 712                 return jtag_connect(dap);
 713
 714         return ERROR_OK;
 715 }
 716
 717 static int jtag_send_sequence(struct adiv5_dap *dap, enum swd_special_seq seq)
 718 {
 719         int retval;
 720
 721         switch (seq) {
 722         case JTAG_TO_SWD:
 723                 retval =  jtag_add_tms_seq(swd_seq_jtag_to_swd_len,
 724                                 swd_seq_jtag_to_swd, TAP_INVALID);
 725                 break;
 726         case SWD_TO_JTAG:
 727                 retval = jtag_add_tms_seq(swd_seq_swd_to_jtag_len,
 728                                 swd_seq_swd_to_jtag, TAP_RESET);
 729                 break;
 730         default:
 731                 LOG_ERROR("Sequence %d not supported", seq);
 732                 return ERROR_FAIL;
 733         }
 734         if (retval == ERROR_OK)
 735                 retval = jtag_execute_queue();
 736         return retval;
 737 }
 738
 739 static int jtag_dp_q_read(struct adiv5_dap *dap, unsigned reg,
 740                 uint32_t *data)
 741 {
 742         int retval = jtag_limit_queue_size(dap);
 743         if (retval != ERROR_OK)
 744                 return retval;
 745
 746         retval =  adi_jtag_dp_scan_u32(dap, JTAG_DP_DPACC, reg,
 747                         DPAP_READ, 0, dap->last_read, 0, NULL);
 748         dap->last_read = data;
 749         return retval;
 750 }
 751
 752 static int jtag_dp_q_write(struct adiv5_dap *dap, unsigned reg,
 753                 uint32_t data)
 754 {
 755         int retval = jtag_limit_queue_size(dap);
 756         if (retval != ERROR_OK)
 757                 return retval;
 758
 759         retval =  adi_jtag_dp_scan_u32(dap, JTAG_DP_DPACC,
 760                         reg, DPAP_WRITE, data, dap->last_read, 0, NULL);
 761         dap->last_read = NULL;
 762         return retval;
 763 }
 764
 765 /** Select the AP register bank */
 766 static int jtag_ap_q_bankselect(struct adiv5_ap *ap, unsigned reg)
 767 {
 768         int retval;
 769         struct adiv5_dap *dap = ap->dap;
 770         uint64_t sel;
 771
 772         if (is_adiv6(dap))
 773                 sel = ap->ap_num | (reg & 0x00000FF0);
 774         else
 775                 sel = (ap->ap_num << 24) | (reg & ADIV5_DP_SELECT_APBANK);
 776
 777         uint64_t sel_diff = (sel ^ dap->select) & SELECT_AP_MASK;
 778
 779         bool set_select = !dap->select_valid || (sel_diff & 0xffffffffull);
 780         bool set_select1 = is_adiv6(dap) && dap->asize > 32
 781                                                 && (!dap->select1_valid
 782                                                         || sel_diff & (0xffffffffull << 32));
 783
 784         if (set_select && set_select1) {
 785                 /* Prepare DP bank for DP_SELECT1 now to save one write */
 786                 sel |= (DP_SELECT1 >> 4) & DP_SELECT_DPBANK;
 787         } else {
 788                 /* Use the DP part of dap->select regardless of dap->select_valid:
 789                  * if !dap->select_valid
 790                  * dap->select contains a speculative value likely going to be used
 791                  * in the following swd_queue_dp_bankselect().
 792                  * Moreover dap->select_valid should never be false here as a DP bank
 793                  * is always selected before selecting an AP bank */
 794                 sel |= dap->select & DP_SELECT_DPBANK;
 795         }
 796
 797         if (set_select) {
 798                 LOG_DEBUG_IO("AP BANK SELECT: %" PRIx32, (uint32_t)sel);
 799
 800                 retval = jtag_dp_q_write(dap, DP_SELECT, (uint32_t)sel);
 801                 if (retval != ERROR_OK) {
 802                         dap->select_valid = false;
 803                         return retval;
 804                 }
 805         }
 806
 807         if (set_select1) {
 808                 LOG_DEBUG_IO("AP BANK SELECT1: %" PRIx32, (uint32_t)(sel >> 32));
 809
 810                 retval = jtag_dp_q_write(dap, DP_SELECT1, (uint32_t)(sel >> 32));
 811                 if (retval != ERROR_OK) {
 812                         dap->select1_valid = false;
 813                         return retval;
 814                 }
 815         }
 816
 817         dap->select = sel;
 818         return ERROR_OK;
 819 }
 820
 821 static int jtag_ap_q_read(struct adiv5_ap *ap, unsigned reg,
 822                 uint32_t *data)
 823 {
 824         int retval = jtag_limit_queue_size(ap->dap);
 825         if (retval != ERROR_OK)
 826                 return retval;
 827
 828         retval = jtag_check_reconnect(ap->dap);
 829         if (retval != ERROR_OK)
 830                 return retval;
 831
 832         retval = jtag_ap_q_bankselect(ap, reg);
 833         if (retval != ERROR_OK)
 834                 return retval;
 835
 836         retval =  adi_jtag_dp_scan_u32(ap->dap, JTAG_DP_APACC, reg,
 837                         DPAP_READ, 0, ap->dap->last_read, ap->memaccess_tck, NULL);
 838         ap->dap->last_read = data;
 839
 840         return retval;
 841 }
 842
 843 static int jtag_ap_q_write(struct adiv5_ap *ap, unsigned reg,
 844                 uint32_t data)
 845 {
 846         int retval = jtag_limit_queue_size(ap->dap);
 847         if (retval != ERROR_OK)
 848                 return retval;
 849
 850         retval = jtag_check_reconnect(ap->dap);
 851         if (retval != ERROR_OK)
 852                 return retval;
 853
 854         retval = jtag_ap_q_bankselect(ap, reg);
 855         if (retval != ERROR_OK)
 856                 return retval;
 857
 858         retval =  adi_jtag_dp_scan_u32(ap->dap, JTAG_DP_APACC, reg,
 859                         DPAP_WRITE, data, ap->dap->last_read, ap->memaccess_tck, NULL);
 860         ap->dap->last_read = NULL;
 861         return retval;
 862 }
 863
 864 static int jtag_ap_q_abort(struct adiv5_dap *dap, uint8_t *ack)
 865 {
 866         /* for JTAG, this is the only valid ABORT register operation */
 867         int retval =  adi_jtag_dp_scan_u32(dap, JTAG_DP_ABORT,
 868                         0, DPAP_WRITE, 1, NULL, 0, NULL);
 869         if (retval != ERROR_OK)
 870                 return retval;
 871
 872         return jtag_execute_queue();
 873 }
 874
 875 static int jtag_dp_run(struct adiv5_dap *dap)
 876 {
 877         int retval;
 878         int retval2 = ERROR_OK;
 879
 880         retval = adi_jtag_finish_read(dap);
 881         if (retval != ERROR_OK)
 882                 goto done;
 883         retval2 = jtagdp_overrun_check(dap);
 884         retval = jtagdp_transaction_endcheck(dap);
 885
 886  done:
 887         return (retval2 != ERROR_OK) ? retval2 : retval;
 888 }
 889
 890 static int jtag_dp_sync(struct adiv5_dap *dap)
 891 {
 892         return jtagdp_overrun_check(dap);
 893 }
 894
 895 /* FIXME don't export ... just initialize as
 896  * part of DAP setup
 897 */
 898 const struct dap_ops jtag_dp_ops = {
 899         .connect             = jtag_connect,
 900         .send_sequence       = jtag_send_sequence,
 901         .queue_dp_read       = jtag_dp_q_read,
 902         .queue_dp_write      = jtag_dp_q_write,
 903         .queue_ap_read       = jtag_ap_q_read,
 904         .queue_ap_write      = jtag_ap_q_write,
 905         .queue_ap_abort      = jtag_ap_q_abort,
 906         .run                 = jtag_dp_run,
 907         .sync                = jtag_dp_sync,
 908         .quit                = jtag_quit,
 909 };