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

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