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