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