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