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