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