git.gag.com Git - fw/openocd/blob - src/target/arm_adi_v5.c

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