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  *   This program is free software; you can redistribute it and/or modify
  14  *   it under the terms of the GNU General Public License as published by
  15  *   the Free Software Foundation; either version 2 of the License, or
  16  *   (at your option) any later version.
  17  *
  18  *   This program is distributed in the hope that it will be useful,
  19  *   but WITHOUT ANY WARRANTY; without even the implied warranty of
  20  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  21  *   GNU General Public License for more details.
  22  *
  23  *   You should have received a copy of the GNU General Public License
  24  *   along with this program.  If not, see <http://www.gnu.org/licenses/>.
  25  ***************************************************************************/
  26
  27 /**
  28  * @file
  29  * This file implements JTAG transport support for cores implementing
  30  the ARM Debug Interface version 5 (ADIv5).
  31  */
  32
  33 #ifdef HAVE_CONFIG_H
  34 #include "config.h"
  35 #endif
  36
  37 #include "arm.h"
  38 #include "arm_adi_v5.h"
  39 #include <helper/time_support.h>
  40 #include <helper/list.h>
  41 #include <jtag/swd.h>
  42
  43 /*#define DEBUG_WAIT*/
  44
  45 /* JTAG instructions/registers for JTAG-DP and SWJ-DP */
  46 #define JTAG_DP_ABORT           0x8
  47 #define JTAG_DP_DPACC           0xA
  48 #define JTAG_DP_APACC           0xB
  49 #define JTAG_DP_IDCODE          0xE
  50
  51 /* three-bit ACK values for DPACC and APACC reads */
  52 #define JTAG_ACK_OK_FAULT       0x2
  53 #define JTAG_ACK_WAIT           0x1
  54
  55 static int jtag_ap_q_abort(struct adiv5_dap *dap, uint8_t *ack);
  56
  57 #ifdef DEBUG_WAIT
  58 static const char *dap_reg_name(int instr, int reg_addr)
  59 {
  60         char *reg_name = "UNK";
  61
  62         if (instr == JTAG_DP_DPACC) {
  63                 switch (reg_addr) {
  64                 case DP_ABORT:
  65                         reg_name =  "ABORT";
  66                         break;
  67                 case DP_CTRL_STAT:
  68                         reg_name =  "CTRL/STAT";
  69                         break;
  70                 case DP_SELECT:
  71                         reg_name = "SELECT";
  72                         break;
  73                 case DP_RDBUFF:
  74                         reg_name =  "RDBUFF";
  75                         break;
  76                 case DP_DLCR:
  77                         reg_name =  "DLCR";
  78                         break;
  79                 default:
  80                         reg_name = "UNK";
  81                         break;
  82                 }
  83         }
  84
  85         if (instr == JTAG_DP_APACC) {
  86                 switch (reg_addr) {
  87                 case MEM_AP_REG_CSW:
  88                         reg_name = "CSW";
  89                         break;
  90                 case MEM_AP_REG_TAR:
  91                         reg_name = "TAR";
  92                         break;
  93                 case MEM_AP_REG_DRW:
  94                         reg_name = "DRW";
  95                         break;
  96                 case MEM_AP_REG_BD0:
  97                         reg_name = "BD0";
  98                         break;
  99                 case MEM_AP_REG_BD1:
 100                         reg_name = "BD1";
 101                         break;
 102                 case MEM_AP_REG_BD2:
 103                         reg_name = "BD2";
 104                         break;
 105                 case MEM_AP_REG_BD3:
 106                         reg_name = "BD3";
 107                         break;
 108                 case MEM_AP_REG_CFG:
 109                         reg_name = "CFG";
 110                         break;
 111                 case MEM_AP_REG_BASE:
 112                         reg_name = "BASE";
 113                         break;
 114                 case AP_REG_IDR:
 115                         reg_name = "IDR";
 116                         break;
 117                 default:
 118                         reg_name = "UNK";
 119                         break;
 120                 }
 121         }
 122
 123         return reg_name;
 124 }
 125 #endif
 126
 127 struct dap_cmd {
 128         struct list_head lh;
 129         uint8_t instr;
 130         uint8_t reg_addr;
 131         uint8_t RnW;
 132         uint8_t *invalue;
 133         uint8_t ack;
 134         uint32_t memaccess_tck;
 135         uint32_t dp_select;
 136
 137         struct scan_field fields[2];
 138         uint8_t out_addr_buf;
 139         uint8_t invalue_buf[4];
 140         uint8_t outvalue_buf[4];
 141 };
 142
 143 #define MAX_DAP_COMMAND_NUM 65536
 144
 145 struct dap_cmd_pool {
 146         struct list_head lh;
 147         struct dap_cmd cmd;
 148 };
 149
 150 static void log_dap_cmd(const char *header, struct dap_cmd *el)
 151 {
 152 #ifdef DEBUG_WAIT
 153         LOG_DEBUG("%s: %2s %6s %5s 0x%08x 0x%08x %2s", header,
 154                 el->instr == JTAG_DP_APACC ? "AP" : "DP",
 155                 dap_reg_name(el->instr, el->reg_addr),
 156                 el->RnW == DPAP_READ ? "READ" : "WRITE",
 157                 buf_get_u32(el->outvalue_buf, 0, 32),
 158                 buf_get_u32(el->invalue, 0, 32),
 159                 el->ack == JTAG_ACK_OK_FAULT ? "OK" :
 160                         (el->ack == JTAG_ACK_WAIT ? "WAIT" : "INVAL"));
 161 #endif
 162 }
 163
 164 static int jtag_limit_queue_size(struct adiv5_dap *dap)
 165 {
 166         if (dap->cmd_pool_size < MAX_DAP_COMMAND_NUM)
 167                 return ERROR_OK;
 168
 169         return dap_run(dap);
 170 }
 171
 172 static struct dap_cmd *dap_cmd_new(struct adiv5_dap *dap, uint8_t instr,
 173                 uint8_t reg_addr, uint8_t RnW,
 174                 uint8_t *outvalue, uint8_t *invalue,
 175                 uint32_t memaccess_tck)
 176 {
 177
 178         struct dap_cmd_pool *pool = NULL;
 179
 180         if (list_empty(&dap->cmd_pool)) {
 181                 pool = calloc(1, sizeof(struct dap_cmd_pool));
 182                 if (pool == NULL)
 183                         return NULL;
 184         } else {
 185                 pool = list_first_entry(&dap->cmd_pool, struct dap_cmd_pool, lh);
 186                 list_del(&pool->lh);
 187         }
 188
 189         INIT_LIST_HEAD(&pool->lh);
 190         dap->cmd_pool_size++;
 191
 192         struct dap_cmd *cmd = &pool->cmd;
 193         INIT_LIST_HEAD(&cmd->lh);
 194         cmd->instr = instr;
 195         cmd->reg_addr = reg_addr;
 196         cmd->RnW = RnW;
 197         if (outvalue != NULL)
 198                 memcpy(cmd->outvalue_buf, outvalue, 4);
 199         cmd->invalue = (invalue != NULL) ? invalue : cmd->invalue_buf;
 200         cmd->memaccess_tck = memaccess_tck;
 201
 202         return cmd;
 203 }
 204
 205 static void dap_cmd_release(struct adiv5_dap *dap, struct dap_cmd *cmd)
 206 {
 207         struct dap_cmd_pool *pool = container_of(cmd, struct dap_cmd_pool, cmd);
 208         if (dap->cmd_pool_size > MAX_DAP_COMMAND_NUM)
 209                 free(pool);
 210         else
 211                 list_add(&pool->lh, &dap->cmd_pool);
 212
 213         dap->cmd_pool_size--;
 214 }
 215
 216 static void flush_journal(struct adiv5_dap *dap, struct list_head *lh)
 217 {
 218         struct dap_cmd *el, *tmp;
 219
 220         list_for_each_entry_safe(el, tmp, lh, lh) {
 221                 list_del(&el->lh);
 222                 dap_cmd_release(dap, el);
 223         }
 224 }
 225
 226 static void jtag_quit(struct adiv5_dap *dap)
 227 {
 228         struct dap_cmd_pool *el, *tmp;
 229         struct list_head *lh = &dap->cmd_pool;
 230
 231         list_for_each_entry_safe(el, tmp, lh, lh) {
 232                 list_del(&el->lh);
 233                 free(el);
 234         }
 235 }
 236
 237 /***************************************************************************
 238  *
 239  * DPACC and APACC scanchain access through JTAG-DP (or SWJ-DP)
 240  *
 241 ***************************************************************************/
 242
 243 static int adi_jtag_dp_scan_cmd(struct adiv5_dap *dap, struct dap_cmd *cmd, uint8_t *ack)
 244 {
 245         struct jtag_tap *tap = dap->tap;
 246         int retval;
 247
 248         retval = arm_jtag_set_instr(tap, cmd->instr, NULL, TAP_IDLE);
 249         if (retval != ERROR_OK)
 250                 return retval;
 251
 252         /* Scan out a read or write operation using some DP or AP register.
 253          * For APACC access with any sticky error flag set, this is discarded.
 254          */
 255         cmd->fields[0].num_bits = 3;
 256         buf_set_u32(&cmd->out_addr_buf, 0, 3, ((cmd->reg_addr >> 1) & 0x6) | (cmd->RnW & 0x1));
 257         cmd->fields[0].out_value = &cmd->out_addr_buf;
 258         cmd->fields[0].in_value = (ack != NULL) ? ack : &cmd->ack;
 259
 260         /* NOTE: if we receive JTAG_ACK_WAIT, the previous operation did not
 261          * complete; data we write is discarded, data we read is unpredictable.
 262          * When overrun detect is active, STICKYORUN is set.
 263          */
 264
 265         cmd->fields[1].num_bits = 32;
 266         cmd->fields[1].out_value = cmd->outvalue_buf;
 267         cmd->fields[1].in_value = cmd->invalue;
 268
 269         jtag_add_dr_scan(tap, 2, cmd->fields, TAP_IDLE);
 270
 271         /* Add specified number of tck clocks after starting memory bus
 272          * access, giving the hardware time to complete the access.
 273          * They provide more time for the (MEM) AP to complete the read ...
 274          * See "Minimum Response Time" for JTAG-DP, in the ADIv5 spec.
 275          */
 276         if (cmd->instr == JTAG_DP_APACC) {
 277                 if (((cmd->reg_addr == MEM_AP_REG_DRW)
 278                         || ((cmd->reg_addr & 0xF0) == MEM_AP_REG_BD0))
 279                         && (cmd->memaccess_tck != 0))
 280                         jtag_add_runtest(cmd->memaccess_tck, TAP_IDLE);
 281         }
 282
 283         return ERROR_OK;
 284 }
 285
 286 static int adi_jtag_dp_scan_cmd_sync(struct adiv5_dap *dap, struct dap_cmd *cmd, uint8_t *ack)
 287 {
 288         int retval;
 289
 290         retval = adi_jtag_dp_scan_cmd(dap, cmd, ack);
 291         if (retval != ERROR_OK)
 292                 return retval;
 293
 294         return jtag_execute_queue();
 295 }
 296
 297 /**
 298  * Scan DPACC or APACC using target ordered uint8_t buffers.  No endianness
 299  * conversions are performed.  See section 4.4.3 of the ADIv5 spec, which
 300  * discusses operations which access these registers.
 301  *
 302  * Note that only one scan is performed.  If RnW is set, a separate scan
 303  * will be needed to collect the data which was read; the "invalue" collects
 304  * the posted result of a preceding operation, not the current one.
 305  *
 306  * @param dap the DAP
 307  * @param instr JTAG_DP_APACC (AP access) or JTAG_DP_DPACC (DP access)
 308  * @param reg_addr two significant bits; A[3:2]; for APACC access, the
 309  *      SELECT register has more addressing bits.
 310  * @param RnW false iff outvalue will be written to the DP or AP
 311  * @param outvalue points to a 32-bit (little-endian) integer
 312  * @param invalue NULL, or points to a 32-bit (little-endian) integer
 313  * @param ack points to where the three bit JTAG_ACK_* code will be stored
 314  * @param memaccess_tck number of idle cycles to add after AP access
 315  */
 316
 317 static int adi_jtag_dp_scan(struct adiv5_dap *dap,
 318                 uint8_t instr, uint8_t reg_addr, uint8_t RnW,
 319                 uint8_t *outvalue, uint8_t *invalue,
 320                 uint32_t memaccess_tck, uint8_t *ack)
 321 {
 322         struct dap_cmd *cmd;
 323         int retval;
 324
 325         cmd = dap_cmd_new(dap, instr, reg_addr, RnW, outvalue, invalue, memaccess_tck);
 326         if (cmd != NULL)
 327                 cmd->dp_select = dap->select;
 328         else
 329                 return ERROR_JTAG_DEVICE_ERROR;
 330
 331         retval = adi_jtag_dp_scan_cmd(dap, cmd, ack);
 332         if (retval == ERROR_OK)
 333                 list_add_tail(&cmd->lh, &dap->cmd_journal);
 334
 335         return retval;
 336 }
 337
 338 /**
 339  * Scan DPACC or APACC out and in from host ordered uint32_t buffers.
 340  * This is exactly like adi_jtag_dp_scan(), except that endianness
 341  * conversions are performed (so the types of invalue and outvalue
 342  * must be different).
 343  */
 344 static int adi_jtag_dp_scan_u32(struct adiv5_dap *dap,
 345                 uint8_t instr, uint8_t reg_addr, uint8_t RnW,
 346                 uint32_t outvalue, uint32_t *invalue,
 347                 uint32_t memaccess_tck, uint8_t *ack)
 348 {
 349         uint8_t out_value_buf[4];
 350         int retval;
 351
 352         buf_set_u32(out_value_buf, 0, 32, outvalue);
 353
 354         retval = adi_jtag_dp_scan(dap, instr, reg_addr, RnW,
 355                         out_value_buf, (uint8_t *)invalue, memaccess_tck, ack);
 356         if (retval != ERROR_OK)
 357                 return retval;
 358
 359         if (invalue)
 360                 jtag_add_callback(arm_le_to_h_u32,
 361                                 (jtag_callback_data_t) invalue);
 362
 363         return retval;
 364 }
 365
 366 static int adi_jtag_finish_read(struct adiv5_dap *dap)
 367 {
 368         int retval = ERROR_OK;
 369
 370         if (dap->last_read != NULL) {
 371                 retval = adi_jtag_dp_scan_u32(dap, JTAG_DP_DPACC,
 372                                 DP_RDBUFF, DPAP_READ, 0, dap->last_read, 0, NULL);
 373                 dap->last_read = NULL;
 374         }
 375
 376         return retval;
 377 }
 378
 379 static int adi_jtag_scan_inout_check_u32(struct adiv5_dap *dap,
 380                 uint8_t instr, uint8_t reg_addr, uint8_t RnW,
 381                 uint32_t outvalue, uint32_t *invalue, uint32_t memaccess_tck)
 382 {
 383         int retval;
 384
 385         /* Issue the read or write */
 386         retval = adi_jtag_dp_scan_u32(dap, instr, reg_addr,
 387                         RnW, outvalue, NULL, memaccess_tck, NULL);
 388         if (retval != ERROR_OK)
 389                 return retval;
 390
 391         /* For reads,  collect posted value; RDBUFF has no other effect.
 392          * Assumes read gets acked with OK/FAULT, and CTRL_STAT says "OK".
 393          */
 394         if ((RnW == DPAP_READ) && (invalue != NULL)) {
 395                 retval = adi_jtag_dp_scan_u32(dap, JTAG_DP_DPACC,
 396                                 DP_RDBUFF, DPAP_READ, 0, invalue, 0, NULL);
 397                 if (retval != ERROR_OK)
 398                         return retval;
 399         }
 400
 401         return jtag_execute_queue();
 402 }
 403
 404 static int jtagdp_overrun_check(struct adiv5_dap *dap)
 405 {
 406         int retval;
 407         struct dap_cmd *el, *tmp, *prev = NULL;
 408         int found_wait = 0;
 409         int64_t time_now;
 410         LIST_HEAD(replay_list);
 411
 412         /* make sure all queued transactions are complete */
 413         retval = jtag_execute_queue();
 414         if (retval != ERROR_OK)
 415                 goto done;
 416
 417         /* skip all completed transactions up to the first WAIT */
 418         list_for_each_entry(el, &dap->cmd_journal, lh) {
 419                 if (el->ack == JTAG_ACK_OK_FAULT) {
 420                         log_dap_cmd("LOG", el);
 421                 } else if (el->ack == JTAG_ACK_WAIT) {
 422                         found_wait = 1;
 423                         break;
 424                 } else {
 425                         LOG_ERROR("Invalid ACK (%1x) in DAP response", el->ack);
 426                         log_dap_cmd("ERR", el);
 427                         retval = ERROR_JTAG_DEVICE_ERROR;
 428                         goto done;
 429                 }
 430         }
 431
 432         /*
 433          * If we found a stalled transaction and a previous transaction
 434          * exists, check if it's a READ access.
 435          */
 436         if (found_wait && el != list_first_entry(&dap->cmd_journal, struct dap_cmd, lh)) {
 437                 prev = list_entry(el->lh.prev, struct dap_cmd, lh);
 438                 if (prev->RnW == DPAP_READ) {
 439                         log_dap_cmd("PND", prev);
 440                         /* search for the next OK transaction, it contains
 441                          * the result of the previous READ */
 442                         tmp = el;
 443                         list_for_each_entry_from(tmp, &dap->cmd_journal, lh) {
 444                                 if (tmp->ack == JTAG_ACK_OK_FAULT) {
 445                                         /* recover the read value */
 446                                         log_dap_cmd("FND", tmp);
 447                                         if (el->invalue != el->invalue_buf) {
 448                                                 uint32_t invalue = le_to_h_u32(tmp->invalue);
 449                                                 memcpy(el->invalue, &invalue, sizeof(uint32_t));
 450                                         }
 451                                         prev = NULL;
 452                                         break;
 453                                 }
 454                         }
 455
 456                         if (prev != NULL) {
 457                                 log_dap_cmd("LST", el);
 458
 459                                 /*
 460                                 * At this point we're sure that no previous
 461                                 * transaction completed and the DAP/AP is still
 462                                 * in busy state. We know that the next "OK" scan
 463                                 * will return the READ result we need to recover.
 464                                 * To complete the READ, we just keep polling RDBUFF
 465                                 * until the WAIT condition clears
 466                                 */
 467                                 tmp = dap_cmd_new(dap, JTAG_DP_DPACC,
 468                                                 DP_RDBUFF, DPAP_READ, NULL, NULL, 0);
 469                                 if (tmp == NULL) {
 470                                         retval = ERROR_JTAG_DEVICE_ERROR;
 471                                         goto done;
 472                                 }
 473                                 /* synchronously retry the command until it succeeds */
 474                                 time_now = timeval_ms();
 475                                 do {
 476                                         retval = adi_jtag_dp_scan_cmd_sync(dap, tmp, NULL);
 477                                         if (retval != ERROR_OK)
 478                                                 break;
 479                                         if (tmp->ack == JTAG_ACK_OK_FAULT) {
 480                                                 log_dap_cmd("FND", tmp);
 481                                                 if (el->invalue != el->invalue_buf) {
 482                                                         uint32_t invalue = le_to_h_u32(tmp->invalue);
 483                                                         memcpy(el->invalue, &invalue, sizeof(uint32_t));
 484                                                 }
 485                                                 break;
 486                                         }
 487                                         if (tmp->ack != JTAG_ACK_WAIT) {
 488                                                 LOG_ERROR("Invalid ACK (%1x) in DAP response", tmp->ack);
 489                                                 log_dap_cmd("ERR", tmp);
 490                                                 retval = ERROR_JTAG_DEVICE_ERROR;
 491                                                 break;
 492                                         }
 493
 494                                 } while (timeval_ms() - time_now < 1000);
 495
 496                                 if (retval == ERROR_OK) {
 497                                         /* timeout happened */
 498                                         if (tmp->ack != JTAG_ACK_OK_FAULT) {
 499                                                 LOG_ERROR("Timeout during WAIT recovery");
 500                                                 dap->select = DP_SELECT_INVALID;
 501                                                 jtag_ap_q_abort(dap, NULL);
 502                                                 /* clear the sticky overrun condition */
 503                                                 adi_jtag_scan_inout_check_u32(dap, JTAG_DP_DPACC,
 504                                                         DP_CTRL_STAT, DPAP_WRITE,
 505                                                         dap->dp_ctrl_stat | SSTICKYORUN, NULL, 0);
 506                                                 retval = ERROR_JTAG_DEVICE_ERROR;
 507                                         }
 508                                 }
 509
 510                                 /* we're done with this command, release it */
 511                                 dap_cmd_release(dap, tmp);
 512
 513                                 if (retval != ERROR_OK)
 514                                         goto done;
 515
 516                         }
 517                         /* make el->invalue point to the default invalue
 518                         * so that we can safely retry it without clobbering
 519                         * the result we just recovered */
 520                         el->invalue = el->invalue_buf;
 521                 }
 522         }
 523
 524         /* move all remaining transactions over to the replay list */
 525         list_for_each_entry_safe_from(el, tmp, &dap->cmd_journal, lh) {
 526                 log_dap_cmd("REP", el);
 527                 list_move_tail(&el->lh, &replay_list);
 528         }
 529
 530         /* we're done with the journal, flush it */
 531         flush_journal(dap, &dap->cmd_journal);
 532
 533         /* check for overrun condition in the last batch of transactions */
 534         if (found_wait) {
 535                 LOG_INFO("DAP transaction stalled (WAIT) - slowing down");
 536                 /* clear the sticky overrun condition */
 537                 retval = 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                 if (retval != ERROR_OK)
 541                         goto done;
 542
 543                 /* restore SELECT register first */
 544                 if (!list_empty(&replay_list)) {
 545                         el = list_first_entry(&replay_list, struct dap_cmd, lh);
 546                         tmp = dap_cmd_new(dap, JTAG_DP_DPACC,
 547                                           DP_SELECT, DPAP_WRITE, (uint8_t *)&el->dp_select, NULL, 0);
 548                         if (tmp == NULL) {
 549                                 retval = ERROR_JTAG_DEVICE_ERROR;
 550                                 goto done;
 551                         }
 552                         list_add(&tmp->lh, &replay_list);
 553
 554                         dap->select = DP_SELECT_INVALID;
 555                 }
 556
 557                 list_for_each_entry_safe(el, tmp, &replay_list, lh) {
 558                         time_now = timeval_ms();
 559                         do {
 560                                 retval = adi_jtag_dp_scan_cmd_sync(dap, el, NULL);
 561                                 if (retval != ERROR_OK)
 562                                         break;
 563                                 log_dap_cmd("REC", el);
 564                                 if (el->ack == JTAG_ACK_OK_FAULT) {
 565                                         if (el->invalue != el->invalue_buf) {
 566                                                 uint32_t invalue = le_to_h_u32(el->invalue);
 567                                                 memcpy(el->invalue, &invalue, sizeof(uint32_t));
 568                                         }
 569                                         break;
 570                                 }
 571                                 if (el->ack != JTAG_ACK_WAIT) {
 572                                         LOG_ERROR("Invalid ACK (%1x) in DAP response", el->ack);
 573                                         log_dap_cmd("ERR", el);
 574                                         retval = ERROR_JTAG_DEVICE_ERROR;
 575                                         break;
 576                                 }
 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 };