git.gag.com Git - fw/openocd/blob - src/target/adi_v5_jtag.c

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