git.gag.com Git - fw/openocd/blob - src/target/arm_adi_v5.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, write to the                         *
  25  *   Free Software Foundation, Inc.,                                       *
  26  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  27  ***************************************************************************/
  28
  29 /**
  30  * @file
  31  * This file implements support for the ARM Debug Interface version 5 (ADIv5)
  32  * debugging architecture.  Compared with previous versions, this includes
  33  * a low pin-count Serial Wire Debug (SWD) alternative to JTAG for message
  34  * transport, and focusses on memory mapped resources as defined by the
  35  * CoreSight architecture.
  36  *
  37  * A key concept in ADIv5 is the Debug Access Port, or DAP.  A DAP has two
  38  * basic components:  a Debug Port (DP) transporting messages to and from a
  39  * debugger, and an Access Port (AP) accessing resources.  Three types of DP
  40  * are defined.  One uses only JTAG for communication, and is called JTAG-DP.
  41  * One uses only SWD for communication, and is called SW-DP.  The third can
  42  * use either SWD or JTAG, and is called SWJ-DP.  The most common type of AP
  43  * is used to access memory mapped resources and is called a MEM-AP.  Also a
  44  * JTAG-AP is also defined, bridging to JTAG resources; those are uncommon.
  45  *
  46  * This programming interface allows DAP pipelined operations through a
  47  * transaction queue.  This primarily affects AP operations (such as using
  48  * a MEM-AP to access memory or registers).  If the current transaction has
  49  * not finished by the time the next one must begin, and the ORUNDETECT bit
  50  * is set in the DP_CTRL_STAT register, the SSTICKYORUN status is set and
  51  * further AP operations will fail.  There are two basic methods to avoid
  52  * such overrun errors.  One involves polling for status instead of using
  53  * transaction piplining.  The other involves adding delays to ensure the
  54  * AP has enough time to complete one operation before starting the next
  55  * one.  (For JTAG these delays are controlled by memaccess_tck.)
  56  */
  57
  58 /*
  59  * Relevant specifications from ARM include:
  60  *
  61  * ARM(tm) Debug Interface v5 Architecture Specification    ARM IHI 0031A
  62  * CoreSight(tm) v1.0 Architecture Specification            ARM IHI 0029B
  63  *
  64  * CoreSight(tm) DAP-Lite TRM, ARM DDI 0316D
  65  * Cortex-M3(tm) TRM, ARM DDI 0337G
  66  */
  67
  68 #ifdef HAVE_CONFIG_H
  69 #include "config.h"
  70 #endif
  71
  72 #include "arm.h"
  73 #include "arm_adi_v5.h"
  74 #include <helper/time_support.h>
  75
  76
  77 /* ARM ADI Specification requires at least 10 bits used for TAR autoincrement  */
  78
  79 /*
  80         uint32_t tar_block_size(uint32_t address)
  81         Return the largest block starting at address that does not cross a tar block size alignment boundary
  82 */
  83 static uint32_t max_tar_block_size(uint32_t tar_autoincr_block, uint32_t address)
  84 {
  85         return (tar_autoincr_block - ((tar_autoincr_block - 1) & address)) >> 2;
  86 }
  87
  88 /***************************************************************************
  89  *                                                                         *
  90  * DPACC and APACC scanchain access through JTAG-DP                        *
  91  *                                                                         *
  92 ***************************************************************************/
  93
  94 /**
  95  * Scan DPACC or APACC using target ordered uint8_t buffers.  No endianness
  96  * conversions are performed.  See section 4.4.3 of the ADIv5 spec, which
  97  * discusses operations which access these registers.
  98  *
  99  * Note that only one scan is performed.  If RnW is set, a separate scan
 100  * will be needed to collect the data which was read; the "invalue" collects
 101  * the posted result of a preceding operation, not the current one.
 102  *
 103  * @param swjdp the DAP
 104  * @param instr JTAG_DP_APACC (AP access) or JTAG_DP_DPACC (DP access)
 105  * @param reg_addr two significant bits; A[3:2]; for APACC access, the
 106  *      SELECT register has more addressing bits.
 107  * @param RnW false iff outvalue will be written to the DP or AP
 108  * @param outvalue points to a 32-bit (little-endian) integer
 109  * @param invalue NULL, or points to a 32-bit (little-endian) integer
 110  * @param ack points to where the three bit JTAG_ACK_* code will be stored
 111  */
 112 static int adi_jtag_dp_scan(struct adiv5_dap *swjdp,
 113                 uint8_t instr, uint8_t reg_addr, uint8_t RnW,
 114                 uint8_t *outvalue, uint8_t *invalue, uint8_t *ack)
 115 {
 116         struct arm_jtag *jtag_info = swjdp->jtag_info;
 117         struct scan_field fields[2];
 118         uint8_t out_addr_buf;
 119
 120         jtag_set_end_state(TAP_IDLE);
 121         arm_jtag_set_instr(jtag_info, instr, NULL);
 122
 123         /* Scan out a read or write operation using some DP or AP register.
 124          * For APACC access with any sticky error flag set, this is discarded.
 125          */
 126         fields[0].tap = jtag_info->tap;
 127         fields[0].num_bits = 3;
 128         buf_set_u32(&out_addr_buf, 0, 3, ((reg_addr >> 1) & 0x6) | (RnW & 0x1));
 129         fields[0].out_value = &out_addr_buf;
 130         fields[0].in_value = ack;
 131
 132         /* NOTE: if we receive JTAG_ACK_WAIT, the previous operation did not
 133          * complete; data we write is discarded, data we read is unpredictable.
 134          * When overrun detect is active, STICKYORUN is set.
 135          */
 136
 137         fields[1].tap = jtag_info->tap;
 138         fields[1].num_bits = 32;
 139         fields[1].out_value = outvalue;
 140         fields[1].in_value = invalue;
 141
 142         jtag_add_dr_scan(2, fields, jtag_get_end_state());
 143
 144         /* Add specified number of tck clocks after starting memory bus
 145          * access, giving the hardware time to complete the access.
 146          * They provide more time for the (MEM) AP to complete the read ...
 147          * See "Minimum Response Time" for JTAG-DP, in the ADIv5 spec.
 148          */
 149         if ((instr == JTAG_DP_APACC)
 150                         && ((reg_addr == AP_REG_DRW)
 151                                 || ((reg_addr & 0xF0) == AP_REG_BD0))
 152                         && (swjdp->memaccess_tck != 0))
 153                 jtag_add_runtest(swjdp->memaccess_tck,
 154                                 jtag_set_end_state(TAP_IDLE));
 155
 156         return jtag_get_error();
 157 }
 158
 159 /**
 160  * Scan DPACC or APACC out and in from host ordered uint32_t buffers.
 161  * This is exactly like adi_jtag_dp_scan(), except that endianness
 162  * conversions are performed (so the types of invalue and outvalue
 163  * must be different).
 164  */
 165 static int adi_jtag_dp_scan_u32(struct adiv5_dap *swjdp,
 166                 uint8_t instr, uint8_t reg_addr, uint8_t RnW,
 167                 uint32_t outvalue, uint32_t *invalue, uint8_t *ack)
 168 {
 169         uint8_t out_value_buf[4];
 170         int retval;
 171
 172         buf_set_u32(out_value_buf, 0, 32, outvalue);
 173
 174         retval = adi_jtag_dp_scan(swjdp, instr, reg_addr, RnW,
 175                         out_value_buf, (uint8_t *)invalue, ack);
 176         if (retval != ERROR_OK)
 177                 return retval;
 178
 179         if (invalue)
 180                 jtag_add_callback(arm_le_to_h_u32,
 181                                 (jtag_callback_data_t) invalue);
 182
 183         return retval;
 184 }
 185
 186 /**
 187  * Utility to write AP registers.
 188  */
 189 static inline int adi_jtag_ap_write_check(struct adiv5_dap *dap,
 190                 uint8_t reg_addr, uint8_t *outvalue)
 191 {
 192         return adi_jtag_dp_scan(dap, JTAG_DP_APACC, reg_addr, DPAP_WRITE,
 193                         outvalue, NULL, NULL);
 194 }
 195
 196 static int adi_jtag_scan_inout_check_u32(struct adiv5_dap *swjdp,
 197                 uint8_t instr, uint8_t reg_addr, uint8_t RnW,
 198                 uint32_t outvalue, uint32_t *invalue)
 199 {
 200         int retval;
 201
 202         /* Issue the read or write */
 203         retval = adi_jtag_dp_scan_u32(swjdp, instr, reg_addr,
 204                         RnW, outvalue, NULL, NULL);
 205         if (retval != ERROR_OK)
 206                 return retval;
 207
 208         /* For reads,  collect posted value; RDBUFF has no other effect.
 209          * Assumes read gets acked with OK/FAULT, and CTRL_STAT says "OK".
 210          */
 211         if ((RnW == DPAP_READ) && (invalue != NULL))
 212                 retval = adi_jtag_dp_scan_u32(swjdp, JTAG_DP_DPACC,
 213                                 DP_RDBUFF, DPAP_READ, 0, invalue, &swjdp->ack);
 214         return retval;
 215 }
 216
 217 static int jtagdp_transaction_endcheck(struct adiv5_dap *swjdp)
 218 {
 219         int retval;
 220         uint32_t ctrlstat;
 221
 222         /* too expensive to call keep_alive() here */
 223
 224 #if 0
 225         /* Danger!!!! BROKEN!!!! */
 226         adi_jtag_scan_inout_check_u32(swjdp, JTAG_DP_DPACC,
 227                         DP_CTRL_STAT, DPAP_READ, 0, &ctrlstat);
 228         /* Danger!!!! BROKEN!!!! Why will jtag_execute_queue() fail here????
 229         R956 introduced the check on return value here and now Michael Schwingen reports
 230         that this code no longer works....
 231
 232         https://lists.berlios.de/pipermail/openocd-development/2008-September/003107.html
 233         */
 234         if ((retval = jtag_execute_queue()) != ERROR_OK)
 235         {
 236                 LOG_ERROR("BUG: Why does this fail the first time????");
 237         }
 238         /* Why??? second time it works??? */
 239 #endif
 240
 241         /* Post CTRL/STAT read; discard any previous posted read value
 242          * but collect its ACK status.
 243          */
 244         adi_jtag_scan_inout_check_u32(swjdp, JTAG_DP_DPACC,
 245                         DP_CTRL_STAT, DPAP_READ, 0, &ctrlstat);
 246         if ((retval = jtag_execute_queue()) != ERROR_OK)
 247                 return retval;
 248
 249         swjdp->ack = swjdp->ack & 0x7;
 250
 251         /* common code path avoids calling timeval_ms() */
 252         if (swjdp->ack != JTAG_ACK_OK_FAULT)
 253         {
 254                 long long then = timeval_ms();
 255
 256                 while (swjdp->ack != JTAG_ACK_OK_FAULT)
 257                 {
 258                         if (swjdp->ack == JTAG_ACK_WAIT)
 259                         {
 260                                 if ((timeval_ms()-then) > 1000)
 261                                 {
 262                                         /* NOTE:  this would be a good spot
 263                                          * to use JTAG_DP_ABORT.
 264                                          */
 265                                         LOG_WARNING("Timeout (1000ms) waiting "
 266                                                 "for ACK=OK/FAULT "
 267                                                 "in JTAG-DP transaction");
 268                                         return ERROR_JTAG_DEVICE_ERROR;
 269                                 }
 270                         }
 271                         else
 272                         {
 273                                 LOG_WARNING("Invalid ACK %#x "
 274                                                 "in JTAG-DP transaction",
 275                                                 swjdp->ack);
 276                                 return ERROR_JTAG_DEVICE_ERROR;
 277                         }
 278
 279                         adi_jtag_scan_inout_check_u32(swjdp, JTAG_DP_DPACC,
 280                                         DP_CTRL_STAT, DPAP_READ, 0, &ctrlstat);
 281                         if ((retval = dap_run(swjdp)) != ERROR_OK)
 282                                 return retval;
 283                         swjdp->ack = swjdp->ack & 0x7;
 284                 }
 285         }
 286
 287         /* REVISIT also STICKYCMP, for pushed comparisons (nyet used) */
 288
 289         /* Check for STICKYERR and STICKYORUN */
 290         if (ctrlstat & (SSTICKYORUN | SSTICKYERR))
 291         {
 292                 LOG_DEBUG("jtag-dp: CTRL/STAT error, 0x%" PRIx32, ctrlstat);
 293                 /* Check power to debug regions */
 294                 if ((ctrlstat & 0xf0000000) != 0xf0000000)
 295                          ahbap_debugport_init(swjdp);
 296                 else
 297                 {
 298                         uint32_t mem_ap_csw, mem_ap_tar;
 299
 300                         /* Maybe print information about last intended
 301                          * MEM-AP access; but not if autoincrementing.
 302                          * *Real* CSW and TAR values are always shown.
 303                          */
 304                         if (swjdp->ap_tar_value != (uint32_t) -1)
 305                                 LOG_DEBUG("MEM-AP Cached values: "
 306                                         "ap_bank 0x%" PRIx32
 307                                         ", ap_csw 0x%" PRIx32
 308                                         ", ap_tar 0x%" PRIx32,
 309                                         swjdp->ap_bank_value,
 310                                         swjdp->ap_csw_value,
 311                                         swjdp->ap_tar_value);
 312
 313                         if (ctrlstat & SSTICKYORUN)
 314                                 LOG_ERROR("JTAG-DP OVERRUN - check clock, "
 315                                         "memaccess, or reduce jtag speed");
 316
 317                         if (ctrlstat & SSTICKYERR)
 318                                 LOG_ERROR("JTAG-DP STICKY ERROR");
 319
 320                         /* Clear Sticky Error Bits */
 321                         adi_jtag_scan_inout_check_u32(swjdp, JTAG_DP_DPACC,
 322                                         DP_CTRL_STAT, DPAP_WRITE,
 323                                         swjdp->dp_ctrl_stat | SSTICKYORUN
 324                                                 | SSTICKYERR, NULL);
 325                         adi_jtag_scan_inout_check_u32(swjdp, JTAG_DP_DPACC,
 326                                         DP_CTRL_STAT, DPAP_READ, 0, &ctrlstat);
 327                         if ((retval = dap_run(swjdp)) != ERROR_OK)
 328                                 return retval;
 329
 330                         LOG_DEBUG("jtag-dp: CTRL/STAT 0x%" PRIx32, ctrlstat);
 331
 332                         retval = dap_queue_ap_read(swjdp,
 333                                         AP_REG_CSW, &mem_ap_csw);
 334                         if (retval != ERROR_OK)
 335                                 return retval;
 336
 337                         retval = dap_queue_ap_read(swjdp,
 338                                         AP_REG_TAR, &mem_ap_tar);
 339                         if (retval != ERROR_OK)
 340                                 return retval;
 341
 342                         if ((retval = dap_run(swjdp)) != ERROR_OK)
 343                                 return retval;
 344                         LOG_ERROR("MEM_AP_CSW 0x%" PRIx32 ", MEM_AP_TAR 0x%"
 345                                         PRIx32, mem_ap_csw, mem_ap_tar);
 346
 347                 }
 348                 if ((retval = dap_run(swjdp)) != ERROR_OK)
 349                         return retval;
 350                 return ERROR_JTAG_DEVICE_ERROR;
 351         }
 352
 353         return ERROR_OK;
 354 }
 355
 356 /***************************************************************************
 357  *                                                                         *
 358  * DP and MEM-AP  register access  through APACC and DPACC                 *
 359  *                                                                         *
 360 ***************************************************************************/
 361
 362 /**
 363  * Select one of the APs connected to the specified DAP.  The
 364  * selection is implicitly used with future AP transactions.
 365  * This is a NOP if the specified AP is already selected.
 366  *
 367  * @param swjdp The DAP
 368  * @param apsel Number of the AP to (implicitly) use with further
 369  *      transactions.  This normally identifies a MEM-AP.
 370  */
 371 void dap_ap_select(struct adiv5_dap *swjdp,uint8_t apsel)
 372 {
 373         uint32_t select = (apsel << 24) & 0xFF000000;
 374
 375         if (select != swjdp->apsel)
 376         {
 377                 swjdp->apsel = select;
 378                 /* Switching AP invalidates cached values.
 379                  * Values MUST BE UPDATED BEFORE AP ACCESS.
 380                  */
 381                 swjdp->ap_bank_value = -1;
 382                 swjdp->ap_csw_value = -1;
 383                 swjdp->ap_tar_value = -1;
 384         }
 385 }
 386
 387 /**
 388  * Queue transactions setting up transfer parameters for the
 389  * currently selected MEM-AP.
 390  *
 391  * Subsequent transfers using registers like AP_REG_DRW or AP_REG_BD2
 392  * initiate data reads or writes using memory or peripheral addresses.
 393  * If the CSW is configured for it, the TAR may be automatically
 394  * incremented after each transfer.
 395  *
 396  * @todo Rename to reflect it being specifically a MEM-AP function.
 397  *
 398  * @param swjdp The DAP connected to the MEM-AP.
 399  * @param csw MEM-AP Control/Status Word (CSW) register to assign.  If this
 400  *      matches the cached value, the register is not changed.
 401  * @param tar MEM-AP Transfer Address Register (TAR) to assign.  If this
 402  *      matches the cached address, the register is not changed.
 403  *
 404  * @return ERROR_OK if the transaction was properly queued, else a fault code.
 405  */
 406 int dap_setup_accessport(struct adiv5_dap *swjdp, uint32_t csw, uint32_t tar)
 407 {
 408         int retval;
 409
 410         csw = csw | CSW_DBGSWENABLE | CSW_MASTER_DEBUG | CSW_HPROT;
 411         if (csw != swjdp->ap_csw_value)
 412         {
 413                 /* LOG_DEBUG("DAP: Set CSW %x",csw); */
 414                 retval = dap_queue_ap_write(swjdp, AP_REG_CSW, csw);
 415                 if (retval != ERROR_OK)
 416                         return retval;
 417                 swjdp->ap_csw_value = csw;
 418         }
 419         if (tar != swjdp->ap_tar_value)
 420         {
 421                 /* LOG_DEBUG("DAP: Set TAR %x",tar); */
 422                 retval = dap_queue_ap_write(swjdp, AP_REG_TAR, tar);
 423                 if (retval != ERROR_OK)
 424                         return retval;
 425                 swjdp->ap_tar_value = tar;
 426         }
 427         /* Disable TAR cache when autoincrementing */
 428         if (csw & CSW_ADDRINC_MASK)
 429                 swjdp->ap_tar_value = -1;
 430         return ERROR_OK;
 431 }
 432
 433 /**
 434  * Asynchronous (queued) read of a word from memory or a system register.
 435  *
 436  * @param swjdp The DAP connected to the MEM-AP performing the read.
 437  * @param address Address of the 32-bit word to read; it must be
 438  *      readable by the currently selected MEM-AP.
 439  * @param value points to where the word will be stored when the
 440  *      transaction queue is flushed (assuming no errors).
 441  *
 442  * @return ERROR_OK for success.  Otherwise a fault code.
 443  */
 444 int mem_ap_read_u32(struct adiv5_dap *swjdp, uint32_t address,
 445                 uint32_t *value)
 446 {
 447         int retval;
 448
 449         /* Use banked addressing (REG_BDx) to avoid some link traffic
 450          * (updating TAR) when reading several consecutive addresses.
 451          */
 452         retval = dap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF,
 453                         address & 0xFFFFFFF0);
 454         if (retval != ERROR_OK)
 455                 return retval;
 456
 457         return dap_queue_ap_read(swjdp, AP_REG_BD0 | (address & 0xC), value);
 458 }
 459
 460 /**
 461  * Synchronous read of a word from memory or a system register.
 462  * As a side effect, this flushes any queued transactions.
 463  *
 464  * @param swjdp The DAP connected to the MEM-AP performing the read.
 465  * @param address Address of the 32-bit word to read; it must be
 466  *      readable by the currently selected MEM-AP.
 467  * @param value points to where the result will be stored.
 468  *
 469  * @return ERROR_OK for success; *value holds the result.
 470  * Otherwise a fault code.
 471  */
 472 int mem_ap_read_atomic_u32(struct adiv5_dap *swjdp, uint32_t address,
 473                 uint32_t *value)
 474 {
 475         int retval;
 476
 477         retval = mem_ap_read_u32(swjdp, address, value);
 478         if (retval != ERROR_OK)
 479                 return retval;
 480
 481         return dap_run(swjdp);
 482 }
 483
 484 /**
 485  * Asynchronous (queued) write of a word to memory or a system register.
 486  *
 487  * @param swjdp The DAP connected to the MEM-AP.
 488  * @param address Address to be written; it must be writable by
 489  *      the currently selected MEM-AP.
 490  * @param value Word that will be written to the address when transaction
 491  *      queue is flushed (assuming no errors).
 492  *
 493  * @return ERROR_OK for success.  Otherwise a fault code.
 494  */
 495 int mem_ap_write_u32(struct adiv5_dap *swjdp, uint32_t address,
 496                 uint32_t value)
 497 {
 498         int retval;
 499
 500         /* Use banked addressing (REG_BDx) to avoid some link traffic
 501          * (updating TAR) when writing several consecutive addresses.
 502          */
 503         retval = dap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF,
 504                         address & 0xFFFFFFF0);
 505         if (retval != ERROR_OK)
 506                 return retval;
 507
 508         return dap_queue_ap_write(swjdp, AP_REG_BD0 | (address & 0xC),
 509                         value);
 510 }
 511
 512 /**
 513  * Synchronous write of a word to memory or a system register.
 514  * As a side effect, this flushes any queued transactions.
 515  *
 516  * @param swjdp The DAP connected to the MEM-AP.
 517  * @param address Address to be written; it must be writable by
 518  *      the currently selected MEM-AP.
 519  * @param value Word that will be written.
 520  *
 521  * @return ERROR_OK for success; the data was written.  Otherwise a fault code.
 522  */
 523 int mem_ap_write_atomic_u32(struct adiv5_dap *swjdp, uint32_t address,
 524                 uint32_t value)
 525 {
 526         int retval = mem_ap_write_u32(swjdp, address, value);
 527
 528         if (retval != ERROR_OK)
 529                 return retval;
 530
 531         return dap_run(swjdp);
 532 }
 533
 534 /*****************************************************************************
 535 *                                                                            *
 536 * mem_ap_write_buf(struct adiv5_dap *swjdp, uint8_t *buffer, int count, uint32_t address) *
 537 *                                                                            *
 538 * Write a buffer in target order (little endian)                             *
 539 *                                                                            *
 540 *****************************************************************************/
 541 int mem_ap_write_buf_u32(struct adiv5_dap *swjdp, uint8_t *buffer, int count, uint32_t address)
 542 {
 543         int wcount, blocksize, writecount, errorcount = 0, retval = ERROR_OK;
 544         uint32_t adr = address;
 545         uint8_t* pBuffer = buffer;
 546
 547         count >>= 2;
 548         wcount = count;
 549
 550         /* if we have an unaligned access - reorder data */
 551         if (adr & 0x3u)
 552         {
 553                 for (writecount = 0; writecount < count; writecount++)
 554                 {
 555                         int i;
 556                         uint32_t outvalue;
 557                         memcpy(&outvalue, pBuffer, sizeof(uint32_t));
 558
 559                         for (i = 0; i < 4; i++)
 560                         {
 561                                 *((uint8_t*)pBuffer + (adr & 0x3)) = outvalue;
 562                                 outvalue >>= 8;
 563                                 adr++;
 564                         }
 565                         pBuffer += sizeof(uint32_t);
 566                 }
 567         }
 568
 569         while (wcount > 0)
 570         {
 571                 /* Adjust to write blocks within boundaries aligned to the TAR autoincremnent size*/
 572                 blocksize = max_tar_block_size(swjdp->tar_autoincr_block, address);
 573                 if (wcount < blocksize)
 574                         blocksize = wcount;
 575
 576                 /* handle unaligned data at 4k boundary */
 577                 if (blocksize == 0)
 578                         blocksize = 1;
 579
 580                 dap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_SINGLE, address);
 581
 582                 for (writecount = 0; writecount < blocksize; writecount++)
 583                 {
 584                         retval = dap_queue_ap_write(swjdp, AP_REG_DRW,
 585                                 *(uint32_t *) (buffer + 4 * writecount));
 586                         if (retval != ERROR_OK)
 587                                 break;
 588                 }
 589
 590                 if (dap_run(swjdp) == ERROR_OK)
 591                 {
 592                         wcount = wcount - blocksize;
 593                         address = address + 4 * blocksize;
 594                         buffer = buffer + 4 * blocksize;
 595                 }
 596                 else
 597                 {
 598                         errorcount++;
 599                 }
 600
 601                 if (errorcount > 1)
 602                 {
 603                         LOG_WARNING("Block write error address 0x%" PRIx32 ", wcount 0x%x", address, wcount);
 604                         /* REVISIT return the *actual* fault code */
 605                         return ERROR_JTAG_DEVICE_ERROR;
 606                 }
 607         }
 608
 609         return retval;
 610 }
 611
 612 static int mem_ap_write_buf_packed_u16(struct adiv5_dap *swjdp,
 613                 uint8_t *buffer, int count, uint32_t address)
 614 {
 615         int retval = ERROR_OK;
 616         int wcount, blocksize, writecount, i;
 617
 618         wcount = count >> 1;
 619
 620         while (wcount > 0)
 621         {
 622                 int nbytes;
 623
 624                 /* Adjust to write blocks within boundaries aligned to the TAR autoincremnent size*/
 625                 blocksize = max_tar_block_size(swjdp->tar_autoincr_block, address);
 626
 627                 if (wcount < blocksize)
 628                         blocksize = wcount;
 629
 630                 /* handle unaligned data at 4k boundary */
 631                 if (blocksize == 0)
 632                         blocksize = 1;
 633
 634                 dap_setup_accessport(swjdp, CSW_16BIT | CSW_ADDRINC_PACKED, address);
 635                 writecount = blocksize;
 636
 637                 do
 638                 {
 639                         nbytes = MIN((writecount << 1), 4);
 640
 641                         if (nbytes < 4)
 642                         {
 643                                 if (mem_ap_write_buf_u16(swjdp, buffer,
 644                                                 nbytes, address) != ERROR_OK)
 645                                 {
 646                                         LOG_WARNING("Block write error address "
 647                                                 "0x%" PRIx32 ", count 0x%x",
 648                                                 address, count);
 649                                         return ERROR_JTAG_DEVICE_ERROR;
 650                                 }
 651
 652                                 address += nbytes >> 1;
 653                         }
 654                         else
 655                         {
 656                                 uint32_t outvalue;
 657                                 memcpy(&outvalue, buffer, sizeof(uint32_t));
 658
 659                                 for (i = 0; i < nbytes; i++)
 660                                 {
 661                                         *((uint8_t*)buffer + (address & 0x3)) = outvalue;
 662                                         outvalue >>= 8;
 663                                         address++;
 664                                 }
 665
 666                                 memcpy(&outvalue, buffer, sizeof(uint32_t));
 667                                 retval = dap_queue_ap_write(swjdp,
 668                                                 AP_REG_DRW, outvalue);
 669                                 if (retval != ERROR_OK)
 670                                         break;
 671
 672                                 if (dap_run(swjdp) != ERROR_OK)
 673                                 {
 674                                         LOG_WARNING("Block write error address "
 675                                                 "0x%" PRIx32 ", count 0x%x",
 676                                                 address, count);
 677                                         /* REVISIT return *actual* fault code */
 678                                         return ERROR_JTAG_DEVICE_ERROR;
 679                                 }
 680                         }
 681
 682                         buffer += nbytes >> 1;
 683                         writecount -= nbytes >> 1;
 684
 685                 } while (writecount);
 686                 wcount -= blocksize;
 687         }
 688
 689         return retval;
 690 }
 691
 692 int mem_ap_write_buf_u16(struct adiv5_dap *swjdp, uint8_t *buffer, int count, uint32_t address)
 693 {
 694         int retval = ERROR_OK;
 695
 696         if (count >= 4)
 697                 return mem_ap_write_buf_packed_u16(swjdp, buffer, count, address);
 698
 699         while (count > 0)
 700         {
 701                 dap_setup_accessport(swjdp, CSW_16BIT | CSW_ADDRINC_SINGLE, address);
 702                 uint16_t svalue;
 703                 memcpy(&svalue, buffer, sizeof(uint16_t));
 704                 uint32_t outvalue = (uint32_t)svalue << 8 * (address & 0x3);
 705                 retval = dap_queue_ap_write(swjdp, AP_REG_DRW, outvalue);
 706                 if (retval != ERROR_OK)
 707                         break;
 708
 709                 retval = dap_run(swjdp);
 710                 if (retval != ERROR_OK)
 711                         break;
 712
 713                 count -= 2;
 714                 address += 2;
 715                 buffer += 2;
 716         }
 717
 718         return retval;
 719 }
 720
 721 static int mem_ap_write_buf_packed_u8(struct adiv5_dap *swjdp,
 722                 uint8_t *buffer, int count, uint32_t address)
 723 {
 724         int retval = ERROR_OK;
 725         int wcount, blocksize, writecount, i;
 726
 727         wcount = count;
 728
 729         while (wcount > 0)
 730         {
 731                 int nbytes;
 732
 733                 /* Adjust to write blocks within boundaries aligned to the TAR autoincremnent size*/
 734                 blocksize = max_tar_block_size(swjdp->tar_autoincr_block, address);
 735
 736                 if (wcount < blocksize)
 737                         blocksize = wcount;
 738
 739                 dap_setup_accessport(swjdp, CSW_8BIT | CSW_ADDRINC_PACKED, address);
 740                 writecount = blocksize;
 741
 742                 do
 743                 {
 744                         nbytes = MIN(writecount, 4);
 745
 746                         if (nbytes < 4)
 747                         {
 748                                 if (mem_ap_write_buf_u8(swjdp, buffer, nbytes, address) != ERROR_OK)
 749                                 {
 750                                         LOG_WARNING("Block write error address "
 751                                                 "0x%" PRIx32 ", count 0x%x",
 752                                                 address, count);
 753                                         return ERROR_JTAG_DEVICE_ERROR;
 754                                 }
 755
 756                                 address += nbytes;
 757                         }
 758                         else
 759                         {
 760                                 uint32_t outvalue;
 761                                 memcpy(&outvalue, buffer, sizeof(uint32_t));
 762
 763                                 for (i = 0; i < nbytes; i++)
 764                                 {
 765                                         *((uint8_t*)buffer + (address & 0x3)) = outvalue;
 766                                         outvalue >>= 8;
 767                                         address++;
 768                                 }
 769
 770                                 memcpy(&outvalue, buffer, sizeof(uint32_t));
 771                                 retval = dap_queue_ap_write(swjdp,
 772                                                 AP_REG_DRW, outvalue);
 773                                 if (retval != ERROR_OK)
 774                                         break;
 775
 776                                 if (dap_run(swjdp) != ERROR_OK)
 777                                 {
 778                                         LOG_WARNING("Block write error address "
 779                                                 "0x%" PRIx32 ", count 0x%x",
 780                                                 address, count);
 781                                         /* REVISIT return *actual* fault code */
 782                                         return ERROR_JTAG_DEVICE_ERROR;
 783                                 }
 784                         }
 785
 786                         buffer += nbytes;
 787                         writecount -= nbytes;
 788
 789                 } while (writecount);
 790                 wcount -= blocksize;
 791         }
 792
 793         return retval;
 794 }
 795
 796 int mem_ap_write_buf_u8(struct adiv5_dap *swjdp, uint8_t *buffer, int count, uint32_t address)
 797 {
 798         int retval = ERROR_OK;
 799
 800         if (count >= 4)
 801                 return mem_ap_write_buf_packed_u8(swjdp, buffer, count, address);
 802
 803         while (count > 0)
 804         {
 805                 dap_setup_accessport(swjdp, CSW_8BIT | CSW_ADDRINC_SINGLE, address);
 806                 uint32_t outvalue = (uint32_t)*buffer << 8 * (address & 0x3);
 807                 retval = dap_queue_ap_write(swjdp, AP_REG_DRW, outvalue);
 808                 if (retval != ERROR_OK)
 809                         break;
 810
 811                 retval = dap_run(swjdp);
 812                 if (retval != ERROR_OK)
 813                         break;
 814
 815                 count--;
 816                 address++;
 817                 buffer++;
 818         }
 819
 820         return retval;
 821 }
 822
 823 /**
 824  * Synchronously read a block of 32-bit words into a buffer
 825  * @param swjdp The DAP connected to the MEM-AP.
 826  * @param buffer where the words will be stored (in host byte order).
 827  * @param count How many words to read.
 828  * @param address Memory address from which to read words; all the
 829  *      words must be readable by the currently selected MEM-AP.
 830  */
 831 int mem_ap_read_buf_u32(struct adiv5_dap *swjdp, uint8_t *buffer,
 832                 int count, uint32_t address)
 833 {
 834         int wcount, blocksize, readcount, errorcount = 0, retval = ERROR_OK;
 835         uint32_t adr = address;
 836         uint8_t* pBuffer = buffer;
 837
 838         count >>= 2;
 839         wcount = count;
 840
 841         while (wcount > 0)
 842         {
 843                 /* Adjust to read blocks within boundaries aligned to the
 844                  * TAR autoincrement size (at least 2^10).  Autoincrement
 845                  * mode avoids an extra per-word roundtrip to update TAR.
 846                  */
 847                 blocksize = max_tar_block_size(swjdp->tar_autoincr_block,
 848                                 address);
 849                 if (wcount < blocksize)
 850                         blocksize = wcount;
 851
 852                 /* handle unaligned data at 4k boundary */
 853                 if (blocksize == 0)
 854                         blocksize = 1;
 855
 856                 dap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_SINGLE,
 857                                 address);
 858
 859                 /* FIXME remove these three calls to adi_jtag_dp_scan(),
 860                  * so this routine becomes transport-neutral.  Be careful
 861                  * not to cause performance problems with JTAG; would it
 862                  * suffice to loop over dap_queue_ap_read(), or would that
 863                  * be slower when JTAG is the chosen transport?
 864                  */
 865
 866                 /* Scan out first read */
 867                 adi_jtag_dp_scan(swjdp, JTAG_DP_APACC, AP_REG_DRW,
 868                                 DPAP_READ, 0, NULL, NULL);
 869                 for (readcount = 0; readcount < blocksize - 1; readcount++)
 870                 {
 871                         /* Scan out next read; scan in posted value for the
 872                          * previous one.  Assumes read is acked "OK/FAULT",
 873                          * and CTRL_STAT says that meant "OK".
 874                          */
 875                         adi_jtag_dp_scan(swjdp, JTAG_DP_APACC, AP_REG_DRW,
 876                                         DPAP_READ, 0, buffer + 4 * readcount,
 877                                         &swjdp->ack);
 878                 }
 879
 880                 /* Scan in last posted value; RDBUFF has no other effect,
 881                  * assuming ack is OK/FAULT and CTRL_STAT says "OK".
 882                  */
 883                 adi_jtag_dp_scan(swjdp, JTAG_DP_DPACC, DP_RDBUFF,
 884                                 DPAP_READ, 0, buffer + 4 * readcount,
 885                                 &swjdp->ack);
 886                 if (dap_run(swjdp) == ERROR_OK)
 887                 {
 888                         wcount = wcount - blocksize;
 889                         address += 4 * blocksize;
 890                         buffer += 4 * blocksize;
 891                 }
 892                 else
 893                 {
 894                         errorcount++;
 895                 }
 896
 897                 if (errorcount > 1)
 898                 {
 899                         LOG_WARNING("Block read error address 0x%" PRIx32
 900                                 ", count 0x%x", address, count);
 901                         /* REVISIT return the *actual* fault code */
 902                         return ERROR_JTAG_DEVICE_ERROR;
 903                 }
 904         }
 905
 906         /* if we have an unaligned access - reorder data */
 907         if (adr & 0x3u)
 908         {
 909                 for (readcount = 0; readcount < count; readcount++)
 910                 {
 911                         int i;
 912                         uint32_t data;
 913                         memcpy(&data, pBuffer, sizeof(uint32_t));
 914
 915                         for (i = 0; i < 4; i++)
 916                         {
 917                                 *((uint8_t*)pBuffer) =
 918                                                 (data >> 8 * (adr & 0x3));
 919                                 pBuffer++;
 920                                 adr++;
 921                         }
 922                 }
 923         }
 924
 925         return retval;
 926 }
 927
 928 static int mem_ap_read_buf_packed_u16(struct adiv5_dap *swjdp,
 929                 uint8_t *buffer, int count, uint32_t address)
 930 {
 931         uint32_t invalue;
 932         int retval = ERROR_OK;
 933         int wcount, blocksize, readcount, i;
 934
 935         wcount = count >> 1;
 936
 937         while (wcount > 0)
 938         {
 939                 int nbytes;
 940
 941                 /* Adjust to read blocks within boundaries aligned to the TAR autoincremnent size*/
 942                 blocksize = max_tar_block_size(swjdp->tar_autoincr_block, address);
 943                 if (wcount < blocksize)
 944                         blocksize = wcount;
 945
 946                 dap_setup_accessport(swjdp, CSW_16BIT | CSW_ADDRINC_PACKED, address);
 947
 948                 /* handle unaligned data at 4k boundary */
 949                 if (blocksize == 0)
 950                         blocksize = 1;
 951                 readcount = blocksize;
 952
 953                 do
 954                 {
 955                         retval = dap_queue_ap_read(swjdp, AP_REG_DRW, &invalue);
 956                         if (dap_run(swjdp) != ERROR_OK)
 957                         {
 958                                 LOG_WARNING("Block read error address 0x%" PRIx32 ", count 0x%x", address, count);
 959                                 /* REVISIT return the *actual* fault code */
 960                                 return ERROR_JTAG_DEVICE_ERROR;
 961                         }
 962
 963                         nbytes = MIN((readcount << 1), 4);
 964
 965                         for (i = 0; i < nbytes; i++)
 966                         {
 967                                 *((uint8_t*)buffer) = (invalue >> 8 * (address & 0x3));
 968                                 buffer++;
 969                                 address++;
 970                         }
 971
 972                         readcount -= (nbytes >> 1);
 973                 } while (readcount);
 974                 wcount -= blocksize;
 975         }
 976
 977         return retval;
 978 }
 979
 980 /**
 981  * Synchronously read a block of 16-bit halfwords into a buffer
 982  * @param swjdp The DAP connected to the MEM-AP.
 983  * @param buffer where the halfwords will be stored (in host byte order).
 984  * @param count How many halfwords to read.
 985  * @param address Memory address from which to read words; all the
 986  *      words must be readable by the currently selected MEM-AP.
 987  */
 988 int mem_ap_read_buf_u16(struct adiv5_dap *swjdp, uint8_t *buffer,
 989                 int count, uint32_t address)
 990 {
 991         uint32_t invalue, i;
 992         int retval = ERROR_OK;
 993
 994         if (count >= 4)
 995                 return mem_ap_read_buf_packed_u16(swjdp, buffer, count, address);
 996
 997         while (count > 0)
 998         {
 999                 dap_setup_accessport(swjdp, CSW_16BIT | CSW_ADDRINC_SINGLE, address);
1000                 retval = dap_queue_ap_read(swjdp, AP_REG_DRW, &invalue);
1001                 if (retval != ERROR_OK)
1002                         break;
1003
1004                 retval = dap_run(swjdp);
1005                 if (retval != ERROR_OK)
1006                         break;
1007
1008                 if (address & 0x1)
1009                 {
1010                         for (i = 0; i < 2; i++)
1011                         {
1012                                 *((uint8_t*)buffer) = (invalue >> 8 * (address & 0x3));
1013                                 buffer++;
1014                                 address++;
1015                         }
1016                 }
1017                 else
1018                 {
1019                         uint16_t svalue = (invalue >> 8 * (address & 0x3));
1020                         memcpy(buffer, &svalue, sizeof(uint16_t));
1021                         address += 2;
1022                         buffer += 2;
1023                 }
1024                 count -= 2;
1025         }
1026
1027         return retval;
1028 }
1029
1030 /* FIX!!! is this a potential performance bottleneck w.r.t. requiring too many
1031  * roundtrips when jtag_execute_queue() has a large overhead(e.g. for USB)s?
1032  *
1033  * The solution is to arrange for a large out/in scan in this loop and
1034  * and convert data afterwards.
1035  */
1036 static int mem_ap_read_buf_packed_u8(struct adiv5_dap *swjdp,
1037                 uint8_t *buffer, int count, uint32_t address)
1038 {
1039         uint32_t invalue;
1040         int retval = ERROR_OK;
1041         int wcount, blocksize, readcount, i;
1042
1043         wcount = count;
1044
1045         while (wcount > 0)
1046         {
1047                 int nbytes;
1048
1049                 /* Adjust to read blocks within boundaries aligned to the TAR autoincremnent size*/
1050                 blocksize = max_tar_block_size(swjdp->tar_autoincr_block, address);
1051
1052                 if (wcount < blocksize)
1053                         blocksize = wcount;
1054
1055                 dap_setup_accessport(swjdp, CSW_8BIT | CSW_ADDRINC_PACKED, address);
1056                 readcount = blocksize;
1057
1058                 do
1059                 {
1060                         retval = dap_queue_ap_read(swjdp, AP_REG_DRW, &invalue);
1061                         if (dap_run(swjdp) != ERROR_OK)
1062                         {
1063                                 LOG_WARNING("Block read error address 0x%" PRIx32 ", count 0x%x", address, count);
1064                                 /* REVISIT return the *actual* fault code */
1065                                 return ERROR_JTAG_DEVICE_ERROR;
1066                         }
1067
1068                         nbytes = MIN(readcount, 4);
1069
1070                         for (i = 0; i < nbytes; i++)
1071                         {
1072                                 *((uint8_t*)buffer) = (invalue >> 8 * (address & 0x3));
1073                                 buffer++;
1074                                 address++;
1075                         }
1076
1077                         readcount -= nbytes;
1078                 } while (readcount);
1079                 wcount -= blocksize;
1080         }
1081
1082         return retval;
1083 }
1084
1085 /**
1086  * Synchronously read a block of bytes into a buffer
1087  * @param swjdp The DAP connected to the MEM-AP.
1088  * @param buffer where the bytes will be stored.
1089  * @param count How many bytes to read.
1090  * @param address Memory address from which to read data; all the
1091  *      data must be readable by the currently selected MEM-AP.
1092  */
1093 int mem_ap_read_buf_u8(struct adiv5_dap *swjdp, uint8_t *buffer,
1094                 int count, uint32_t address)
1095 {
1096         uint32_t invalue;
1097         int retval = ERROR_OK;
1098
1099         if (count >= 4)
1100                 return mem_ap_read_buf_packed_u8(swjdp, buffer, count, address);
1101
1102         while (count > 0)
1103         {
1104                 dap_setup_accessport(swjdp, CSW_8BIT | CSW_ADDRINC_SINGLE, address);
1105                 retval = dap_queue_ap_read(swjdp, AP_REG_DRW, &invalue);
1106                 retval = dap_run(swjdp);
1107                 if (retval != ERROR_OK)
1108                         break;
1109
1110                 *((uint8_t*)buffer) = (invalue >> 8 * (address & 0x3));
1111                 count--;
1112                 address++;
1113                 buffer++;
1114         }
1115
1116         return retval;
1117 }
1118
1119 /*--------------------------------------------------------------------------*/
1120
1121 static int jtag_idcode_q_read(struct adiv5_dap *dap,
1122                 uint8_t *ack, uint32_t *data)
1123 {
1124         struct arm_jtag *jtag_info = dap->jtag_info;
1125         int retval;
1126         struct scan_field fields[1];
1127
1128         jtag_set_end_state(TAP_IDLE);
1129
1130         /* This is a standard JTAG operation -- no DAP tweakage */
1131         retval = arm_jtag_set_instr(jtag_info, JTAG_DP_IDCODE, NULL);
1132         if (retval != ERROR_OK)
1133                 return retval;
1134
1135         fields[0].tap = jtag_info->tap;
1136         fields[0].num_bits = 32;
1137         fields[0].out_value = NULL;
1138         fields[0].in_value = (void *) data;
1139
1140         jtag_add_dr_scan(1, fields, jtag_get_end_state());
1141         retval = jtag_get_error();
1142         if (retval != ERROR_OK)
1143                 return retval;
1144
1145         jtag_add_callback(arm_le_to_h_u32,
1146                         (jtag_callback_data_t) data);
1147
1148         return retval;
1149 }
1150
1151 static int jtag_dp_q_read(struct adiv5_dap *dap, unsigned reg,
1152                 uint32_t *data)
1153 {
1154         return adi_jtag_scan_inout_check_u32(dap, JTAG_DP_DPACC,
1155                         reg, DPAP_READ, 0, data);
1156 }
1157
1158 static int jtag_dp_q_write(struct adiv5_dap *dap, unsigned reg,
1159                 uint32_t data)
1160 {
1161         return adi_jtag_scan_inout_check_u32(dap, JTAG_DP_DPACC,
1162                         reg, DPAP_WRITE, data, NULL);
1163 }
1164
1165 /** Select the AP register bank matching bits 7:4 of reg. */
1166 static int jtag_ap_q_bankselect(struct adiv5_dap *dap, unsigned reg)
1167 {
1168         uint32_t select = reg & 0x000000F0;
1169
1170         if (select == dap->ap_bank_value)
1171                 return ERROR_OK;
1172         dap->ap_bank_value = select;
1173
1174         select |= dap->apsel;
1175
1176         return jtag_dp_q_write(dap, DP_SELECT, select);
1177 }
1178
1179 static int jtag_ap_q_read(struct adiv5_dap *dap, unsigned reg,
1180                 uint32_t *data)
1181 {
1182         int retval = jtag_ap_q_bankselect(dap, reg);
1183
1184         if (retval != ERROR_OK)
1185                 return retval;
1186
1187         return adi_jtag_scan_inout_check_u32(dap, JTAG_DP_APACC, reg,
1188                         DPAP_READ, 0, data);
1189 }
1190
1191 static int jtag_ap_q_write(struct adiv5_dap *dap, unsigned reg,
1192                 uint32_t data)
1193 {
1194         uint8_t out_value_buf[4];
1195
1196         int retval = jtag_ap_q_bankselect(dap, reg);
1197         if (retval != ERROR_OK)
1198                 return retval;
1199
1200         buf_set_u32(out_value_buf, 0, 32, data);
1201
1202         return adi_jtag_ap_write_check(dap, reg, out_value_buf);
1203 }
1204
1205 static int jtag_ap_q_abort(struct adiv5_dap *dap, uint8_t *ack)
1206 {
1207         /* for JTAG, this is the only valid ABORT register operation */
1208         return adi_jtag_dp_scan_u32(dap, JTAG_DP_ABORT,
1209                         0, DPAP_WRITE, 1, NULL, ack);
1210 }
1211
1212 static int jtag_dp_run(struct adiv5_dap *dap)
1213 {
1214         return jtagdp_transaction_endcheck(dap);
1215 }
1216
1217 static const struct dap_ops jtag_dp_ops = {
1218         .queue_idcode_read =    jtag_idcode_q_read,
1219         .queue_dp_read =        jtag_dp_q_read,
1220         .queue_dp_write =       jtag_dp_q_write,
1221         .queue_ap_read =        jtag_ap_q_read,
1222         .queue_ap_write =       jtag_ap_q_write,
1223         .queue_ap_abort =       jtag_ap_q_abort,
1224         .run =                  jtag_dp_run,
1225 };
1226
1227 /*--------------------------------------------------------------------------*/
1228
1229 /**
1230  * Initialize a DAP.  This sets up the power domains, prepares the DP
1231  * for further use, and arranges to use AP #0 for all AP operations
1232  * until dap_ap-select() changes that policy.
1233  *
1234  * @param swjdp The DAP being initialized.
1235  *
1236  * @todo Rename this.  We also need an initialization scheme which account
1237  * for SWD transports not just JTAG; that will need to address differences
1238  * in layering.  (JTAG is useful without any debug target; but not SWD.)
1239  * And this may not even use an AHB-AP ... e.g. DAP-Lite uses an APB-AP.
1240  */
1241 int ahbap_debugport_init(struct adiv5_dap *swjdp)
1242 {
1243         uint32_t idreg, romaddr, dummy;
1244         uint32_t ctrlstat;
1245         int cnt = 0;
1246         int retval;
1247
1248         LOG_DEBUG(" ");
1249
1250         /* JTAG-DP or SWJ-DP, in JTAG mode */
1251         swjdp->ops = &jtag_dp_ops;
1252
1253         /* Default MEM-AP setup.
1254          *
1255          * REVISIT AP #0 may be an inappropriate default for this.
1256          * Should we probe, or take a hint from the caller?
1257          * Presumably we can ignore the possibility of multiple APs.
1258          */
1259         swjdp->apsel = !0;
1260         dap_ap_select(swjdp, 0);
1261
1262         /* DP initialization */
1263
1264         retval = dap_queue_dp_read(swjdp, DP_CTRL_STAT, &dummy);
1265         if (retval != ERROR_OK)
1266                 return retval;
1267
1268         retval = dap_queue_dp_write(swjdp, DP_CTRL_STAT, SSTICKYERR);
1269         if (retval != ERROR_OK)
1270                 return retval;
1271
1272         retval = dap_queue_dp_read(swjdp, DP_CTRL_STAT, &dummy);
1273         if (retval != ERROR_OK)
1274                 return retval;
1275
1276         swjdp->dp_ctrl_stat = CDBGPWRUPREQ | CSYSPWRUPREQ;
1277         retval = dap_queue_dp_write(swjdp, DP_CTRL_STAT, swjdp->dp_ctrl_stat);
1278         if (retval != ERROR_OK)
1279                 return retval;
1280
1281         retval = dap_queue_dp_read(swjdp, DP_CTRL_STAT, &ctrlstat);
1282         if (retval != ERROR_OK)
1283                 return retval;
1284         if ((retval = dap_run(swjdp)) != ERROR_OK)
1285                 return retval;
1286
1287         /* Check that we have debug power domains activated */
1288         while (!(ctrlstat & CDBGPWRUPACK) && (cnt++ < 10))
1289         {
1290                 LOG_DEBUG("DAP: wait CDBGPWRUPACK");
1291                 retval = dap_queue_dp_read(swjdp, DP_CTRL_STAT, &ctrlstat);
1292                 if (retval != ERROR_OK)
1293                         return retval;
1294                 if ((retval = dap_run(swjdp)) != ERROR_OK)
1295                         return retval;
1296                 alive_sleep(10);
1297         }
1298
1299         while (!(ctrlstat & CSYSPWRUPACK) && (cnt++ < 10))
1300         {
1301                 LOG_DEBUG("DAP: wait CSYSPWRUPACK");
1302                 retval = dap_queue_dp_read(swjdp, DP_CTRL_STAT, &ctrlstat);
1303                 if (retval != ERROR_OK)
1304                         return retval;
1305                 if ((retval = dap_run(swjdp)) != ERROR_OK)
1306                         return retval;
1307                 alive_sleep(10);
1308         }
1309
1310         retval = dap_queue_dp_read(swjdp, DP_CTRL_STAT, &dummy);
1311         if (retval != ERROR_OK)
1312                 return retval;
1313         /* With debug power on we can activate OVERRUN checking */
1314         swjdp->dp_ctrl_stat = CDBGPWRUPREQ | CSYSPWRUPREQ | CORUNDETECT;
1315         retval = dap_queue_dp_write(swjdp, DP_CTRL_STAT, swjdp->dp_ctrl_stat);
1316         if (retval != ERROR_OK)
1317                 return retval;
1318         retval = dap_queue_dp_read(swjdp, DP_CTRL_STAT, &dummy);
1319         if (retval != ERROR_OK)
1320                 return retval;
1321
1322         /*
1323          * REVISIT this isn't actually *initializing* anything in an AP,
1324          * and doesn't care if it's a MEM-AP at all (much less AHB-AP).
1325          * Should it?  If the ROM address is valid, is this the right
1326          * place to scan the table and do any topology detection?
1327          */
1328         retval = dap_queue_ap_read(swjdp, AP_REG_IDR, &idreg);
1329         retval = dap_queue_ap_read(swjdp, AP_REG_BASE, &romaddr);
1330
1331         LOG_DEBUG("MEM-AP #%d ID Register 0x%" PRIx32
1332                 ", Debug ROM Address 0x%" PRIx32,
1333                 swjdp->apsel, idreg, romaddr);
1334
1335         return ERROR_OK;
1336 }
1337
1338 /* CID interpretation -- see ARM IHI 0029B section 3
1339  * and ARM IHI 0031A table 13-3.
1340  */
1341 static const char *class_description[16] ={
1342         "Reserved", "ROM table", "Reserved", "Reserved",
1343         "Reserved", "Reserved", "Reserved", "Reserved",
1344         "Reserved", "CoreSight component", "Reserved", "Peripheral Test Block",
1345         "Reserved", "OptimoDE DESS",
1346                 "Generic IP component", "PrimeCell or System component"
1347 };
1348
1349 static bool
1350 is_dap_cid_ok(uint32_t cid3, uint32_t cid2, uint32_t cid1, uint32_t cid0)
1351 {
1352         return cid3 == 0xb1 && cid2 == 0x05
1353                         && ((cid1 & 0x0f) == 0) && cid0 == 0x0d;
1354 }
1355
1356 static int dap_info_command(struct command_context *cmd_ctx,
1357                 struct adiv5_dap *swjdp, int apsel)
1358 {
1359         int retval;
1360         uint32_t dbgbase, apid;
1361         int romtable_present = 0;
1362         uint8_t mem_ap;
1363         uint32_t apselold;
1364
1365         /* AP address is in bits 31:24 of DP_SELECT */
1366         if (apsel >= 256)
1367                 return ERROR_INVALID_ARGUMENTS;
1368
1369         apselold = swjdp->apsel;
1370         dap_ap_select(swjdp, apsel);
1371         retval = dap_queue_ap_read(swjdp, AP_REG_BASE, &dbgbase);
1372         retval = dap_queue_ap_read(swjdp, AP_REG_IDR, &apid);
1373         retval = dap_run(swjdp);
1374         if (retval != ERROR_OK)
1375                 return retval;
1376
1377         /* Now we read ROM table ID registers, ref. ARM IHI 0029B sec  */
1378         mem_ap = ((apid&0x10000) && ((apid&0x0F) != 0));
1379         command_print(cmd_ctx, "AP ID register 0x%8.8" PRIx32, apid);
1380         if (apid)
1381         {
1382                 switch (apid&0x0F)
1383                 {
1384                         case 0:
1385                                 command_print(cmd_ctx, "\tType is JTAG-AP");
1386                                 break;
1387                         case 1:
1388                                 command_print(cmd_ctx, "\tType is MEM-AP AHB");
1389                                 break;
1390                         case 2:
1391                                 command_print(cmd_ctx, "\tType is MEM-AP APB");
1392                                 break;
1393                         default:
1394                                 command_print(cmd_ctx, "\tUnknown AP type");
1395                                 break;
1396                 }
1397
1398                 /* NOTE: a MEM-AP may have a single CoreSight component that's
1399                  * not a ROM table ... or have no such components at all.
1400                  */
1401                 if (mem_ap)
1402                         command_print(cmd_ctx, "AP BASE 0x%8.8" PRIx32,
1403                                         dbgbase);
1404         }
1405         else
1406         {
1407                 command_print(cmd_ctx, "No AP found at this apsel 0x%x", apsel);
1408         }
1409
1410         romtable_present = ((mem_ap) && (dbgbase != 0xFFFFFFFF));
1411         if (romtable_present)
1412         {
1413                 uint32_t cid0,cid1,cid2,cid3,memtype,romentry;
1414                 uint16_t entry_offset;
1415
1416                 /* bit 16 of apid indicates a memory access port */
1417                 if (dbgbase & 0x02)
1418                         command_print(cmd_ctx, "\tValid ROM table present");
1419                 else
1420                         command_print(cmd_ctx, "\tROM table in legacy format");
1421
1422                 /* Now we read ROM table ID registers, ref. ARM IHI 0029B sec  */
1423                 mem_ap_read_u32(swjdp, (dbgbase&0xFFFFF000) | 0xFF0, &cid0);
1424                 mem_ap_read_u32(swjdp, (dbgbase&0xFFFFF000) | 0xFF4, &cid1);
1425                 mem_ap_read_u32(swjdp, (dbgbase&0xFFFFF000) | 0xFF8, &cid2);
1426                 mem_ap_read_u32(swjdp, (dbgbase&0xFFFFF000) | 0xFFC, &cid3);
1427                 mem_ap_read_u32(swjdp, (dbgbase&0xFFFFF000) | 0xFCC, &memtype);
1428                 retval = dap_run(swjdp);
1429                 if (retval != ERROR_OK)
1430                         return retval;
1431
1432                 if (!is_dap_cid_ok(cid3, cid2, cid1, cid0))
1433                         command_print(cmd_ctx, "\tCID3 0x%2.2" PRIx32
1434                                         ", CID2 0x%2.2" PRIx32
1435                                         ", CID1 0x%2.2" PRIx32
1436                                         ", CID0 0x%2.2" PRIx32,
1437                                         cid3, cid2, cid1, cid0);
1438                 if (memtype & 0x01)
1439                         command_print(cmd_ctx, "\tMEMTYPE system memory present on bus");
1440                 else
1441                         command_print(cmd_ctx, "\tMEMTYPE System memory not present. "
1442                                         "Dedicated debug bus.");
1443
1444                 /* Now we read ROM table entries from dbgbase&0xFFFFF000) | 0x000 until we get 0x00000000 */
1445                 entry_offset = 0;
1446                 do
1447                 {
1448                         mem_ap_read_atomic_u32(swjdp, (dbgbase&0xFFFFF000) | entry_offset, &romentry);
1449                         command_print(cmd_ctx, "\tROMTABLE[0x%x] = 0x%" PRIx32 "",entry_offset,romentry);
1450                         if (romentry&0x01)
1451                         {
1452                                 uint32_t c_cid0, c_cid1, c_cid2, c_cid3;
1453                                 uint32_t c_pid0, c_pid1, c_pid2, c_pid3, c_pid4;
1454                                 uint32_t component_start, component_base;
1455                                 unsigned part_num;
1456                                 char *type, *full;
1457
1458                                 component_base = (uint32_t)((dbgbase & 0xFFFFF000)
1459                                                 + (int)(romentry & 0xFFFFF000));
1460                                 mem_ap_read_atomic_u32(swjdp,
1461                                                 (component_base & 0xFFFFF000) | 0xFE0, &c_pid0);
1462                                 mem_ap_read_atomic_u32(swjdp,
1463                                                 (component_base & 0xFFFFF000) | 0xFE4, &c_pid1);
1464                                 mem_ap_read_atomic_u32(swjdp,
1465                                                 (component_base & 0xFFFFF000) | 0xFE8, &c_pid2);
1466                                 mem_ap_read_atomic_u32(swjdp,
1467                                                 (component_base & 0xFFFFF000) | 0xFEC, &c_pid3);
1468                                 mem_ap_read_atomic_u32(swjdp,
1469                                                 (component_base & 0xFFFFF000) | 0xFD0, &c_pid4);
1470                                 mem_ap_read_atomic_u32(swjdp,
1471                                                 (component_base & 0xFFFFF000) | 0xFF0, &c_cid0);
1472                                 mem_ap_read_atomic_u32(swjdp,
1473                                                 (component_base & 0xFFFFF000) | 0xFF4, &c_cid1);
1474                                 mem_ap_read_atomic_u32(swjdp,
1475                                                 (component_base & 0xFFFFF000) | 0xFF8, &c_cid2);
1476                                 mem_ap_read_atomic_u32(swjdp,
1477                                                 (component_base & 0xFFFFF000) | 0xFFC, &c_cid3);
1478                                 component_start = component_base - 0x1000*(c_pid4 >> 4);
1479
1480                                 command_print(cmd_ctx, "\t\tComponent base address 0x%" PRIx32
1481                                                 ", start address 0x%" PRIx32,
1482                                                 component_base, component_start);
1483                                 command_print(cmd_ctx, "\t\tComponent class is 0x%x, %s",
1484                                                 (int) (c_cid1 >> 4) & 0xf,
1485                                                 /* See ARM IHI 0029B Table 3-3 */
1486                                                 class_description[(c_cid1 >> 4) & 0xf]);
1487
1488                                 /* CoreSight component? */
1489                                 if (((c_cid1 >> 4) & 0x0f) == 9) {
1490                                         uint32_t devtype;
1491                                         unsigned minor;
1492                                         char *major = "Reserved", *subtype = "Reserved";
1493
1494                                         mem_ap_read_atomic_u32(swjdp,
1495                                                         (component_base & 0xfffff000) | 0xfcc,
1496                                                         &devtype);
1497                                         minor = (devtype >> 4) & 0x0f;
1498                                         switch (devtype & 0x0f) {
1499                                         case 0:
1500                                                 major = "Miscellaneous";
1501                                                 switch (minor) {
1502                                                 case 0:
1503                                                         subtype = "other";
1504                                                         break;
1505                                                 case 4:
1506                                                         subtype = "Validation component";
1507                                                         break;
1508                                                 }
1509                                                 break;
1510                                         case 1:
1511                                                 major = "Trace Sink";
1512                                                 switch (minor) {
1513                                                 case 0:
1514                                                         subtype = "other";
1515                                                         break;
1516                                                 case 1:
1517                                                         subtype = "Port";
1518                                                         break;
1519                                                 case 2:
1520                                                         subtype = "Buffer";
1521                                                         break;
1522                                                 }
1523                                                 break;
1524                                         case 2:
1525                                                 major = "Trace Link";
1526                                                 switch (minor) {
1527                                                 case 0:
1528                                                         subtype = "other";
1529                                                         break;
1530                                                 case 1:
1531                                                         subtype = "Funnel, router";
1532                                                         break;
1533                                                 case 2:
1534                                                         subtype = "Filter";
1535                                                         break;
1536                                                 case 3:
1537                                                         subtype = "FIFO, buffer";
1538                                                         break;
1539                                                 }
1540                                                 break;
1541                                         case 3:
1542                                                 major = "Trace Source";
1543                                                 switch (minor) {
1544                                                 case 0:
1545                                                         subtype = "other";
1546                                                         break;
1547                                                 case 1:
1548                                                         subtype = "Processor";
1549                                                         break;
1550                                                 case 2:
1551                                                         subtype = "DSP";
1552                                                         break;
1553                                                 case 3:
1554                                                         subtype = "Engine/Coprocessor";
1555                                                         break;
1556                                                 case 4:
1557                                                         subtype = "Bus";
1558                                                         break;
1559                                                 }
1560                                                 break;
1561                                         case 4:
1562                                                 major = "Debug Control";
1563                                                 switch (minor) {
1564                                                 case 0:
1565                                                         subtype = "other";
1566                                                         break;
1567                                                 case 1:
1568                                                         subtype = "Trigger Matrix";
1569                                                         break;
1570                                                 case 2:
1571                                                         subtype = "Debug Auth";
1572                                                         break;
1573                                                 }
1574                                                 break;
1575                                         case 5:
1576                                                 major = "Debug Logic";
1577                                                 switch (minor) {
1578                                                 case 0:
1579                                                         subtype = "other";
1580                                                         break;
1581                                                 case 1:
1582                                                         subtype = "Processor";
1583                                                         break;
1584                                                 case 2:
1585                                                         subtype = "DSP";
1586                                                         break;
1587                                                 case 3:
1588                                                         subtype = "Engine/Coprocessor";
1589                                                         break;
1590                                                 }
1591                                                 break;
1592                                         }
1593                                         command_print(cmd_ctx, "\t\tType is 0x%2.2x, %s, %s",
1594                                                         (unsigned) (devtype & 0xff),
1595                                                         major, subtype);
1596                                         /* REVISIT also show 0xfc8 DevId */
1597                                 }
1598
1599                                 if (!is_dap_cid_ok(cid3, cid2, cid1, cid0))
1600                                         command_print(cmd_ctx, "\t\tCID3 0x%2.2" PRIx32
1601                                                         ", CID2 0x%2.2" PRIx32
1602                                                         ", CID1 0x%2.2" PRIx32
1603                                                         ", CID0 0x%2.2" PRIx32,
1604                                                         c_cid3, c_cid2, c_cid1, c_cid0);
1605                                 command_print(cmd_ctx, "\t\tPeripheral ID[4..0] = hex "
1606                                                 "%2.2x %2.2x %2.2x %2.2x %2.2x",
1607                                                 (int) c_pid4,
1608                                                 (int) c_pid3, (int) c_pid2,
1609                                                 (int) c_pid1, (int) c_pid0);
1610
1611                                 /* Part number interpretations are from Cortex
1612                                  * core specs, the CoreSight components TRM
1613                                  * (ARM DDI 0314H), and ETM specs; also from
1614                                  * chip observation (e.g. TI SDTI).
1615                                  */
1616                                 part_num = c_pid0 & 0xff;
1617                                 part_num |= (c_pid1 & 0x0f) << 8;
1618                                 switch (part_num) {
1619                                 case 0x000:
1620                                         type = "Cortex-M3 NVIC";
1621                                         full = "(Interrupt Controller)";
1622                                         break;
1623                                 case 0x001:
1624                                         type = "Cortex-M3 ITM";
1625                                         full = "(Instrumentation Trace Module)";
1626                                         break;
1627                                 case 0x002:
1628                                         type = "Cortex-M3 DWT";
1629                                         full = "(Data Watchpoint and Trace)";
1630                                         break;
1631                                 case 0x003:
1632                                         type = "Cortex-M3 FBP";
1633                                         full = "(Flash Patch and Breakpoint)";
1634                                         break;
1635                                 case 0x00d:
1636                                         type = "CoreSight ETM11";
1637                                         full = "(Embedded Trace)";
1638                                         break;
1639                                 // case 0x113: what?
1640                                 case 0x120:             /* from OMAP3 memmap */
1641                                         type = "TI SDTI";
1642                                         full = "(System Debug Trace Interface)";
1643                                         break;
1644                                 case 0x343:             /* from OMAP3 memmap */
1645                                         type = "TI DAPCTL";
1646                                         full = "";
1647                                         break;
1648                                 case 0x906:
1649                                         type = "Coresight CTI";
1650                                         full = "(Cross Trigger)";
1651                                         break;
1652                                 case 0x907:
1653                                         type = "Coresight ETB";
1654                                         full = "(Trace Buffer)";
1655                                         break;
1656                                 case 0x908:
1657                                         type = "Coresight CSTF";
1658                                         full = "(Trace Funnel)";
1659                                         break;
1660                                 case 0x910:
1661                                         type = "CoreSight ETM9";
1662                                         full = "(Embedded Trace)";
1663                                         break;
1664                                 case 0x912:
1665                                         type = "Coresight TPIU";
1666                                         full = "(Trace Port Interface Unit)";
1667                                         break;
1668                                 case 0x921:
1669                                         type = "Cortex-A8 ETM";
1670                                         full = "(Embedded Trace)";
1671                                         break;
1672                                 case 0x922:
1673                                         type = "Cortex-A8 CTI";
1674                                         full = "(Cross Trigger)";
1675                                         break;
1676                                 case 0x923:
1677                                         type = "Cortex-M3 TPIU";
1678                                         full = "(Trace Port Interface Unit)";
1679                                         break;
1680                                 case 0x924:
1681                                         type = "Cortex-M3 ETM";
1682                                         full = "(Embedded Trace)";
1683                                         break;
1684                                 case 0xc08:
1685                                         type = "Cortex-A8 Debug";
1686                                         full = "(Debug Unit)";
1687                                         break;
1688                                 default:
1689                                         type = "-*- unrecognized -*-";
1690                                         full = "";
1691                                         break;
1692                                 }
1693                                 command_print(cmd_ctx, "\t\tPart is %s %s",
1694                                                 type, full);
1695                         }
1696                         else
1697                         {
1698                                 if (romentry)
1699                                         command_print(cmd_ctx, "\t\tComponent not present");
1700                                 else
1701                                         command_print(cmd_ctx, "\t\tEnd of ROM table");
1702                         }
1703                         entry_offset += 4;
1704                 } while (romentry > 0);
1705         }
1706         else
1707         {
1708                 command_print(cmd_ctx, "\tNo ROM table present");
1709         }
1710         dap_ap_select(swjdp, apselold);
1711
1712         return ERROR_OK;
1713 }
1714
1715 COMMAND_HANDLER(handle_dap_info_command)
1716 {
1717         struct target *target = get_current_target(CMD_CTX);
1718         struct arm *arm = target_to_arm(target);
1719         struct adiv5_dap *dap = arm->dap;
1720         uint32_t apsel;
1721
1722         switch (CMD_ARGC) {
1723         case 0:
1724                 apsel = dap->apsel;
1725                 break;
1726         case 1:
1727                 COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], apsel);
1728                 break;
1729         default:
1730                 return ERROR_COMMAND_SYNTAX_ERROR;
1731         }
1732
1733         return dap_info_command(CMD_CTX, dap, apsel);
1734 }
1735
1736 COMMAND_HANDLER(dap_baseaddr_command)
1737 {
1738         struct target *target = get_current_target(CMD_CTX);
1739         struct arm *arm = target_to_arm(target);
1740         struct adiv5_dap *dap = arm->dap;
1741
1742         uint32_t apsel, apselsave, baseaddr;
1743         int retval;
1744
1745         apselsave = dap->apsel;
1746         switch (CMD_ARGC) {
1747         case 0:
1748                 apsel = dap->apsel;
1749                 break;
1750         case 1:
1751                 COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], apsel);
1752                 /* AP address is in bits 31:24 of DP_SELECT */
1753                 if (apsel >= 256)
1754                         return ERROR_INVALID_ARGUMENTS;
1755                 break;
1756         default:
1757                 return ERROR_COMMAND_SYNTAX_ERROR;
1758         }
1759
1760         if (apselsave != apsel)
1761                 dap_ap_select(dap, apsel);
1762
1763         /* NOTE:  assumes we're talking to a MEM-AP, which
1764          * has a base address.  There are other kinds of AP,
1765          * though they're not common for now.  This should
1766          * use the ID register to verify it's a MEM-AP.
1767          */
1768         retval = dap_queue_ap_read(dap, AP_REG_BASE, &baseaddr);
1769         retval = dap_run(dap);
1770         if (retval != ERROR_OK)
1771                 return retval;
1772
1773         command_print(CMD_CTX, "0x%8.8" PRIx32, baseaddr);
1774
1775         if (apselsave != apsel)
1776                 dap_ap_select(dap, apselsave);
1777
1778         return retval;
1779 }
1780
1781 COMMAND_HANDLER(dap_memaccess_command)
1782 {
1783         struct target *target = get_current_target(CMD_CTX);
1784         struct arm *arm = target_to_arm(target);
1785         struct adiv5_dap *dap = arm->dap;
1786
1787         uint32_t memaccess_tck;
1788
1789         switch (CMD_ARGC) {
1790         case 0:
1791                 memaccess_tck = dap->memaccess_tck;
1792                 break;
1793         case 1:
1794                 COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], memaccess_tck);
1795                 break;
1796         default:
1797                 return ERROR_COMMAND_SYNTAX_ERROR;
1798         }
1799         dap->memaccess_tck = memaccess_tck;
1800
1801         command_print(CMD_CTX, "memory bus access delay set to %" PRIi32 " tck",
1802                         dap->memaccess_tck);
1803
1804         return ERROR_OK;
1805 }
1806
1807 COMMAND_HANDLER(dap_apsel_command)
1808 {
1809         struct target *target = get_current_target(CMD_CTX);
1810         struct arm *arm = target_to_arm(target);
1811         struct adiv5_dap *dap = arm->dap;
1812
1813         uint32_t apsel, apid;
1814         int retval;
1815
1816         switch (CMD_ARGC) {
1817         case 0:
1818                 apsel = 0;
1819                 break;
1820         case 1:
1821                 COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], apsel);
1822                 /* AP address is in bits 31:24 of DP_SELECT */
1823                 if (apsel >= 256)
1824                         return ERROR_INVALID_ARGUMENTS;
1825                 break;
1826         default:
1827                 return ERROR_COMMAND_SYNTAX_ERROR;
1828         }
1829
1830         dap_ap_select(dap, apsel);
1831         retval = dap_queue_ap_read(dap, AP_REG_IDR, &apid);
1832         retval = dap_run(dap);
1833         if (retval != ERROR_OK)
1834                 return retval;
1835
1836         command_print(CMD_CTX, "ap %" PRIi32 " selected, identification register 0x%8.8" PRIx32,
1837                         apsel, apid);
1838
1839         return retval;
1840 }
1841
1842 COMMAND_HANDLER(dap_apid_command)
1843 {
1844         struct target *target = get_current_target(CMD_CTX);
1845         struct arm *arm = target_to_arm(target);
1846         struct adiv5_dap *dap = arm->dap;
1847
1848         uint32_t apsel, apselsave, apid;
1849         int retval;
1850
1851         apselsave = dap->apsel;
1852         switch (CMD_ARGC) {
1853         case 0:
1854                 apsel = dap->apsel;
1855                 break;
1856         case 1:
1857                 COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], apsel);
1858                 /* AP address is in bits 31:24 of DP_SELECT */
1859                 if (apsel >= 256)
1860                         return ERROR_INVALID_ARGUMENTS;
1861                 break;
1862         default:
1863                 return ERROR_COMMAND_SYNTAX_ERROR;
1864         }
1865
1866         if (apselsave != apsel)
1867                 dap_ap_select(dap, apsel);
1868
1869         retval = dap_queue_ap_read(dap, AP_REG_IDR, &apid);
1870         retval = dap_run(dap);
1871         if (retval != ERROR_OK)
1872                 return retval;
1873
1874         command_print(CMD_CTX, "0x%8.8" PRIx32, apid);
1875         if (apselsave != apsel)
1876                 dap_ap_select(dap, apselsave);
1877
1878         return retval;
1879 }
1880
1881 static const struct command_registration dap_commands[] = {
1882         {
1883                 .name = "info",
1884                 .handler = handle_dap_info_command,
1885                 .mode = COMMAND_EXEC,
1886                 .help = "display ROM table for MEM-AP "
1887                         "(default currently selected AP)",
1888                 .usage = "[ap_num]",
1889         },
1890         {
1891                 .name = "apsel",
1892                 .handler = dap_apsel_command,
1893                 .mode = COMMAND_EXEC,
1894                 .help = "Set the currently selected AP (default 0) "
1895                         "and display the result",
1896                 .usage = "[ap_num]",
1897         },
1898         {
1899                 .name = "apid",
1900                 .handler = dap_apid_command,
1901                 .mode = COMMAND_EXEC,
1902                 .help = "return ID register from AP "
1903                         "(default currently selected AP)",
1904                 .usage = "[ap_num]",
1905         },
1906         {
1907                 .name = "baseaddr",
1908                 .handler = dap_baseaddr_command,
1909                 .mode = COMMAND_EXEC,
1910                 .help = "return debug base address from MEM-AP "
1911                         "(default currently selected AP)",
1912                 .usage = "[ap_num]",
1913         },
1914         {
1915                 .name = "memaccess",
1916                 .handler = dap_memaccess_command,
1917                 .mode = COMMAND_EXEC,
1918                 .help = "set/get number of extra tck for MEM-AP memory "
1919                         "bus access [0-255]",
1920                 .usage = "[cycles]",
1921         },
1922         COMMAND_REGISTRATION_DONE
1923 };
1924
1925 const struct command_registration dap_command_handlers[] = {
1926         {
1927                 .name = "dap",
1928                 .mode = COMMAND_EXEC,
1929                 .help = "DAP command group",
1930                 .chain = dap_commands,
1931         },
1932         COMMAND_REGISTRATION_DONE
1933 };
1934
1935
1936 /*
1937  * This represents the bits which must be sent out on TMS/SWDIO to
1938  * switch a DAP implemented using an SWJ-DP module into SWD mode.
1939  * These bits are stored (and transmitted) LSB-first.
1940  *
1941  * See the DAP-Lite specification, section 2.2.5 for information
1942  * about making the debug link select SWD or JTAG.  (Similar info
1943  * is in a few other ARM documents.)
1944  */
1945 static const uint8_t jtag2swd_bitseq[] = {
1946         /* More than 50 TCK/SWCLK cycles with TMS/SWDIO high,
1947          * putting both JTAG and SWD logic into reset state.
1948          */
1949         0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
1950         /* Switching sequence enables SWD and disables JTAG
1951          * NOTE: bits in the DP's IDCODE may expose the need for
1952          * an old/deprecated sequence (0xb6 0xed).
1953          */
1954         0x9e, 0xe7,
1955         /* More than 50 TCK/SWCLK cycles with TMS/SWDIO high,
1956          * putting both JTAG and SWD logic into reset state.
1957          */
1958         0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
1959 };
1960
1961 /**
1962  * Put the debug link into SWD mode, if the target supports it.
1963  * The link's initial mode may be either JTAG (for example,
1964  * with SWJ-DP after reset) or SWD.
1965  *
1966  * @param target Enters SWD mode (if possible).
1967  *
1968  * Note that targets using the JTAG-DP do not support SWD, and that
1969  * some targets which could otherwise support it may have have been
1970  * configured to disable SWD signaling
1971  *
1972  * @return ERROR_OK or else a fault code.
1973  */
1974 int dap_to_swd(struct target *target)
1975 {
1976         int retval;
1977
1978         LOG_DEBUG("Enter SWD mode");
1979
1980         /* REVISIT it's nasty to need to make calls to a "jtag"
1981          * subsystem if the link isn't in JTAG mode...
1982          */
1983
1984         retval =  jtag_add_tms_seq(8 * sizeof(jtag2swd_bitseq),
1985                         jtag2swd_bitseq, TAP_INVALID);
1986         if (retval == ERROR_OK)
1987                 retval = jtag_execute_queue();
1988
1989         /* REVISIT set up the DAP's ops vector for SWD mode. */
1990
1991         return retval;
1992 }
1993
1994 /**
1995  * This represents the bits which must be sent out on TMS/SWDIO to
1996  * switch a DAP implemented using an SWJ-DP module into JTAG mode.
1997  * These bits are stored (and transmitted) LSB-first.
1998  *
1999  * These bits are stored (and transmitted) LSB-first.
2000  */
2001 static const uint8_t swd2jtag_bitseq[] = {
2002         /* More than 50 TCK/SWCLK cycles with TMS/SWDIO high,
2003          * putting both JTAG and SWD logic into reset state.
2004          */
2005         0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
2006         /* Switching equence disables SWD and enables JTAG
2007          * NOTE: bits in the DP's IDCODE can expose the need for
2008          * the old/deprecated sequence (0xae 0xde).
2009          */
2010         0x3c, 0xe7,
2011         /* At least 50 TCK/SWCLK cycles with TMS/SWDIO high,
2012          * putting both JTAG and SWD logic into reset state.
2013          * NOTE:  some docs say "at least 5".
2014          */
2015         0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
2016 };
2017
2018 /** Put the debug link into JTAG mode, if the target supports it.
2019  * The link's initial mode may be either SWD or JTAG.
2020  *
2021  * @param target Enters JTAG mode (if possible).
2022  *
2023  * Note that targets implemented with SW-DP do not support JTAG, and
2024  * that some targets which could otherwise support it may have been
2025  * configured to disable JTAG signaling
2026  *
2027  * @return ERROR_OK or else a fault code.
2028  */
2029 int dap_to_jtag(struct target *target)
2030 {
2031         int retval;
2032
2033         LOG_DEBUG("Enter JTAG mode");
2034
2035         /* REVISIT it's nasty to need to make calls to a "jtag"
2036          * subsystem if the link isn't in JTAG mode...
2037          */
2038
2039         retval = jtag_add_tms_seq(8 * sizeof(swd2jtag_bitseq),
2040                         swd2jtag_bitseq, TAP_RESET);
2041         if (retval == ERROR_OK)
2042                 retval = jtag_execute_queue();
2043
2044         /* REVISIT set up the DAP's ops vector for JTAG mode. */
2045
2046         return retval;
2047 }