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