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->dp_select_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;
 423         select = (apsel << 24) & 0xFF000000;
 424
 425         if (select != swjdp->apsel)
 426         {
 427                 swjdp->apsel = select;
 428                 /* Switching AP invalidates cached values */
 429                 swjdp->dp_select_value = -1;
 430                 swjdp->ap_csw_value = -1;
 431                 swjdp->ap_tar_value = -1;
 432         }
 433 }
 434
 435 static int dap_dp_bankselect(struct swjdp_common *swjdp, uint32_t ap_reg)
 436 {
 437         uint32_t select;
 438         select = (ap_reg & 0x000000F0);
 439
 440         if (select != swjdp->dp_select_value)
 441         {
 442                 dap_dp_write_reg(swjdp, select | swjdp->apsel, DP_SELECT);
 443                 swjdp->dp_select_value = select;
 444         }
 445
 446         /* FIXME return any fault code from write() call */
 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_dp_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_dp_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_dp_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 receve a hint from the caller?
1210          * Presumably we can ignore the possibility of multiple APs.
1211          */
1212         swjdp->apsel = 0;
1213         swjdp->ap_csw_value = -1;
1214         swjdp->ap_tar_value = -1;
1215
1216         /* DP initialization */
1217         swjdp->trans_mode = TRANS_MODE_ATOMIC;
1218         dap_dp_read_reg(swjdp, &dummy, DP_CTRL_STAT);
1219         dap_dp_write_reg(swjdp, SSTICKYERR, DP_CTRL_STAT);
1220         dap_dp_read_reg(swjdp, &dummy, DP_CTRL_STAT);
1221
1222         swjdp->dp_ctrl_stat = CDBGPWRUPREQ | CSYSPWRUPREQ;
1223
1224         dap_dp_write_reg(swjdp, swjdp->dp_ctrl_stat, DP_CTRL_STAT);
1225         dap_dp_read_reg(swjdp, &ctrlstat, DP_CTRL_STAT);
1226         if ((retval = jtag_execute_queue()) != ERROR_OK)
1227                 return retval;
1228
1229         /* Check that we have debug power domains activated */
1230         while (!(ctrlstat & CDBGPWRUPACK) && (cnt++ < 10))
1231         {
1232                 LOG_DEBUG("DAP: wait CDBGPWRUPACK");
1233                 dap_dp_read_reg(swjdp, &ctrlstat, DP_CTRL_STAT);
1234                 if ((retval = jtag_execute_queue()) != ERROR_OK)
1235                         return retval;
1236                 alive_sleep(10);
1237         }
1238
1239         while (!(ctrlstat & CSYSPWRUPACK) && (cnt++ < 10))
1240         {
1241                 LOG_DEBUG("DAP: wait CSYSPWRUPACK");
1242                 dap_dp_read_reg(swjdp, &ctrlstat, DP_CTRL_STAT);
1243                 if ((retval = jtag_execute_queue()) != ERROR_OK)
1244                         return retval;
1245                 alive_sleep(10);
1246         }
1247
1248         dap_dp_read_reg(swjdp, &dummy, DP_CTRL_STAT);
1249         /* With debug power on we can activate OVERRUN checking */
1250         swjdp->dp_ctrl_stat = CDBGPWRUPREQ | CSYSPWRUPREQ | CORUNDETECT;
1251         dap_dp_write_reg(swjdp, swjdp->dp_ctrl_stat, DP_CTRL_STAT);
1252         dap_dp_read_reg(swjdp, &dummy, DP_CTRL_STAT);
1253
1254         /*
1255          * REVISIT this isn't actually *initializing* anything in an AP,
1256          * and doesn't care if it's a MEM-AP at all (much less AHB-AP).
1257          * Should it?  If the ROM address is valid, is this the right
1258          * place to scan the table and do any topology detection?
1259          */
1260         dap_ap_read_reg_u32(swjdp, AP_REG_IDR, &idreg);
1261         dap_ap_read_reg_u32(swjdp, AP_REG_BASE, &romaddr);
1262
1263         LOG_DEBUG("MEM-AP #%d ID Register 0x%" PRIx32
1264                 ", Debug ROM Address 0x%" PRIx32,
1265                 swjdp->apsel, idreg, romaddr);
1266
1267         return ERROR_OK;
1268 }
1269
1270 /* CID interpretation -- see ARM IHI 0029B section 3
1271  * and ARM IHI 0031A table 13-3.
1272  */
1273 static const char *class_description[16] ={
1274         "Reserved", "ROM table", "Reserved", "Reserved",
1275         "Reserved", "Reserved", "Reserved", "Reserved",
1276         "Reserved", "CoreSight component", "Reserved", "Peripheral Test Block",
1277         "Reserved", "OptimoDE DESS",
1278                 "Generic IP component", "PrimeCell or System component"
1279 };
1280
1281 static bool
1282 is_dap_cid_ok(uint32_t cid3, uint32_t cid2, uint32_t cid1, uint32_t cid0)
1283 {
1284         return cid3 == 0xb1 && cid2 == 0x05
1285                         && ((cid1 & 0x0f) == 0) && cid0 == 0x0d;
1286 }
1287
1288 int dap_info_command(struct command_context *cmd_ctx,
1289                 struct swjdp_common *swjdp, int apsel)
1290 {
1291
1292         uint32_t dbgbase, apid;
1293         int romtable_present = 0;
1294         uint8_t mem_ap;
1295         uint32_t apselold;
1296
1297         /* AP address is in bits 31:24 of DP_SELECT */
1298         if (apsel >= 256)
1299                 return ERROR_INVALID_ARGUMENTS;
1300
1301         apselold = swjdp->apsel;
1302         dap_ap_select(swjdp, apsel);
1303         dap_ap_read_reg_u32(swjdp, AP_REG_BASE, &dbgbase);
1304         dap_ap_read_reg_u32(swjdp, AP_REG_IDR, &apid);
1305         jtagdp_transaction_endcheck(swjdp);
1306         /* Now we read ROM table ID registers, ref. ARM IHI 0029B sec  */
1307         mem_ap = ((apid&0x10000) && ((apid&0x0F) != 0));
1308         command_print(cmd_ctx, "AP ID register 0x%8.8" PRIx32, apid);
1309         if (apid)
1310         {
1311                 switch (apid&0x0F)
1312                 {
1313                         case 0:
1314                                 command_print(cmd_ctx, "\tType is JTAG-AP");
1315                                 break;
1316                         case 1:
1317                                 command_print(cmd_ctx, "\tType is MEM-AP AHB");
1318                                 break;
1319                         case 2:
1320                                 command_print(cmd_ctx, "\tType is MEM-AP APB");
1321                                 break;
1322                         default:
1323                                 command_print(cmd_ctx, "\tUnknown AP type");
1324                                 break;
1325                 }
1326
1327                 /* NOTE: a MEM-AP may have a single CoreSight component that's
1328                  * not a ROM table ... or have no such components at all.
1329                  */
1330                 if (mem_ap)
1331                         command_print(cmd_ctx, "AP BASE 0x%8.8" PRIx32,
1332                                         dbgbase);
1333         }
1334         else
1335         {
1336                 command_print(cmd_ctx, "No AP found at this apsel 0x%x", apsel);
1337         }
1338
1339         romtable_present = ((mem_ap) && (dbgbase != 0xFFFFFFFF));
1340         if (romtable_present)
1341         {
1342                 uint32_t cid0,cid1,cid2,cid3,memtype,romentry;
1343                 uint16_t entry_offset;
1344
1345                 /* bit 16 of apid indicates a memory access port */
1346                 if (dbgbase & 0x02)
1347                         command_print(cmd_ctx, "\tValid ROM table present");
1348                 else
1349                         command_print(cmd_ctx, "\tROM table in legacy format");
1350
1351                 /* Now we read ROM table ID registers, ref. ARM IHI 0029B sec  */
1352                 mem_ap_read_u32(swjdp, (dbgbase&0xFFFFF000) | 0xFF0, &cid0);
1353                 mem_ap_read_u32(swjdp, (dbgbase&0xFFFFF000) | 0xFF4, &cid1);
1354                 mem_ap_read_u32(swjdp, (dbgbase&0xFFFFF000) | 0xFF8, &cid2);
1355                 mem_ap_read_u32(swjdp, (dbgbase&0xFFFFF000) | 0xFFC, &cid3);
1356                 mem_ap_read_u32(swjdp, (dbgbase&0xFFFFF000) | 0xFCC, &memtype);
1357                 jtagdp_transaction_endcheck(swjdp);
1358                 if (!is_dap_cid_ok(cid3, cid2, cid1, cid0))
1359                         command_print(cmd_ctx, "\tCID3 0x%2.2" PRIx32
1360                                         ", CID2 0x%2.2" PRIx32
1361                                         ", CID1 0x%2.2" PRIx32
1362                                         ", CID0 0x%2.2" PRIx32,
1363                                         cid3, cid2, cid1, cid0);
1364                 if (memtype & 0x01)
1365                         command_print(cmd_ctx, "\tMEMTYPE system memory present on bus");
1366                 else
1367                         command_print(cmd_ctx, "\tMEMTYPE System memory not present. "
1368                                         "Dedicated debug bus.");
1369
1370                 /* Now we read ROM table entries from dbgbase&0xFFFFF000) | 0x000 until we get 0x00000000 */
1371                 entry_offset = 0;
1372                 do
1373                 {
1374                         mem_ap_read_atomic_u32(swjdp, (dbgbase&0xFFFFF000) | entry_offset, &romentry);
1375                         command_print(cmd_ctx, "\tROMTABLE[0x%x] = 0x%" PRIx32 "",entry_offset,romentry);
1376                         if (romentry&0x01)
1377                         {
1378                                 uint32_t c_cid0, c_cid1, c_cid2, c_cid3;
1379                                 uint32_t c_pid0, c_pid1, c_pid2, c_pid3, c_pid4;
1380                                 uint32_t component_start, component_base;
1381                                 unsigned part_num;
1382                                 char *type, *full;
1383
1384                                 component_base = (uint32_t)((dbgbase & 0xFFFFF000)
1385                                                 + (int)(romentry & 0xFFFFF000));
1386                                 mem_ap_read_atomic_u32(swjdp,
1387                                                 (component_base & 0xFFFFF000) | 0xFE0, &c_pid0);
1388                                 mem_ap_read_atomic_u32(swjdp,
1389                                                 (component_base & 0xFFFFF000) | 0xFE4, &c_pid1);
1390                                 mem_ap_read_atomic_u32(swjdp,
1391                                                 (component_base & 0xFFFFF000) | 0xFE8, &c_pid2);
1392                                 mem_ap_read_atomic_u32(swjdp,
1393                                                 (component_base & 0xFFFFF000) | 0xFEC, &c_pid3);
1394                                 mem_ap_read_atomic_u32(swjdp,
1395                                                 (component_base & 0xFFFFF000) | 0xFD0, &c_pid4);
1396                                 mem_ap_read_atomic_u32(swjdp,
1397                                                 (component_base & 0xFFFFF000) | 0xFF0, &c_cid0);
1398                                 mem_ap_read_atomic_u32(swjdp,
1399                                                 (component_base & 0xFFFFF000) | 0xFF4, &c_cid1);
1400                                 mem_ap_read_atomic_u32(swjdp,
1401                                                 (component_base & 0xFFFFF000) | 0xFF8, &c_cid2);
1402                                 mem_ap_read_atomic_u32(swjdp,
1403                                                 (component_base & 0xFFFFF000) | 0xFFC, &c_cid3);
1404                                 component_start = component_base - 0x1000*(c_pid4 >> 4);
1405
1406                                 command_print(cmd_ctx, "\t\tComponent base address 0x%" PRIx32
1407                                                 ", start address 0x%" PRIx32,
1408                                                 component_base, component_start);
1409                                 command_print(cmd_ctx, "\t\tComponent class is 0x%x, %s",
1410                                                 (int) (c_cid1 >> 4) & 0xf,
1411                                                 /* See ARM IHI 0029B Table 3-3 */
1412                                                 class_description[(c_cid1 >> 4) & 0xf]);
1413
1414                                 /* CoreSight component? */
1415                                 if (((c_cid1 >> 4) & 0x0f) == 9) {
1416                                         uint32_t devtype;
1417                                         unsigned minor;
1418                                         char *major = "Reserved", *subtype = "Reserved";
1419
1420                                         mem_ap_read_atomic_u32(swjdp,
1421                                                         (component_base & 0xfffff000) | 0xfcc,
1422                                                         &devtype);
1423                                         minor = (devtype >> 4) & 0x0f;
1424                                         switch (devtype & 0x0f) {
1425                                         case 0:
1426                                                 major = "Miscellaneous";
1427                                                 switch (minor) {
1428                                                 case 0:
1429                                                         subtype = "other";
1430                                                         break;
1431                                                 case 4:
1432                                                         subtype = "Validation component";
1433                                                         break;
1434                                                 }
1435                                                 break;
1436                                         case 1:
1437                                                 major = "Trace Sink";
1438                                                 switch (minor) {
1439                                                 case 0:
1440                                                         subtype = "other";
1441                                                         break;
1442                                                 case 1:
1443                                                         subtype = "Port";
1444                                                         break;
1445                                                 case 2:
1446                                                         subtype = "Buffer";
1447                                                         break;
1448                                                 }
1449                                                 break;
1450                                         case 2:
1451                                                 major = "Trace Link";
1452                                                 switch (minor) {
1453                                                 case 0:
1454                                                         subtype = "other";
1455                                                         break;
1456                                                 case 1:
1457                                                         subtype = "Funnel, router";
1458                                                         break;
1459                                                 case 2:
1460                                                         subtype = "Filter";
1461                                                         break;
1462                                                 case 3:
1463                                                         subtype = "FIFO, buffer";
1464                                                         break;
1465                                                 }
1466                                                 break;
1467                                         case 3:
1468                                                 major = "Trace Source";
1469                                                 switch (minor) {
1470                                                 case 0:
1471                                                         subtype = "other";
1472                                                         break;
1473                                                 case 1:
1474                                                         subtype = "Processor";
1475                                                         break;
1476                                                 case 2:
1477                                                         subtype = "DSP";
1478                                                         break;
1479                                                 case 3:
1480                                                         subtype = "Engine/Coprocessor";
1481                                                         break;
1482                                                 case 4:
1483                                                         subtype = "Bus";
1484                                                         break;
1485                                                 }
1486                                                 break;
1487                                         case 4:
1488                                                 major = "Debug Control";
1489                                                 switch (minor) {
1490                                                 case 0:
1491                                                         subtype = "other";
1492                                                         break;
1493                                                 case 1:
1494                                                         subtype = "Trigger Matrix";
1495                                                         break;
1496                                                 case 2:
1497                                                         subtype = "Debug Auth";
1498                                                         break;
1499                                                 }
1500                                                 break;
1501                                         case 5:
1502                                                 major = "Debug Logic";
1503                                                 switch (minor) {
1504                                                 case 0:
1505                                                         subtype = "other";
1506                                                         break;
1507                                                 case 1:
1508                                                         subtype = "Processor";
1509                                                         break;
1510                                                 case 2:
1511                                                         subtype = "DSP";
1512                                                         break;
1513                                                 case 3:
1514                                                         subtype = "Engine/Coprocessor";
1515                                                         break;
1516                                                 }
1517                                                 break;
1518                                         }
1519                                         command_print(cmd_ctx, "\t\tType is 0x%2.2x, %s, %s",
1520                                                         (unsigned) (devtype & 0xff),
1521                                                         major, subtype);
1522                                         /* REVISIT also show 0xfc8 DevId */
1523                                 }
1524
1525                                 if (!is_dap_cid_ok(cid3, cid2, cid1, cid0))
1526                                         command_print(cmd_ctx, "\t\tCID3 0x%2.2" PRIx32
1527                                                         ", CID2 0x%2.2" PRIx32
1528                                                         ", CID1 0x%2.2" PRIx32
1529                                                         ", CID0 0x%2.2" PRIx32,
1530                                                         c_cid3, c_cid2, c_cid1, c_cid0);
1531                                 command_print(cmd_ctx, "\t\tPeripheral ID[4..0] = hex "
1532                                                 "%2.2x %2.2x %2.2x %2.2x %2.2x",
1533                                                 (int) c_pid4,
1534                                                 (int) c_pid3, (int) c_pid2,
1535                                                 (int) c_pid1, (int) c_pid0);
1536
1537                                 /* Part number interpretations are from Cortex
1538                                  * core specs, the CoreSight components TRM
1539                                  * (ARM DDI 0314H), and ETM specs; also from
1540                                  * chip observation (e.g. TI SDTI).
1541                                  */
1542                                 part_num = c_pid0 & 0xff;
1543                                 part_num |= (c_pid1 & 0x0f) << 8;
1544                                 switch (part_num) {
1545                                 case 0x000:
1546                                         type = "Cortex-M3 NVIC";
1547                                         full = "(Interrupt Controller)";
1548                                         break;
1549                                 case 0x001:
1550                                         type = "Cortex-M3 ITM";
1551                                         full = "(Instrumentation Trace Module)";
1552                                         break;
1553                                 case 0x002:
1554                                         type = "Cortex-M3 DWT";
1555                                         full = "(Data Watchpoint and Trace)";
1556                                         break;
1557                                 case 0x003:
1558                                         type = "Cortex-M3 FBP";
1559                                         full = "(Flash Patch and Breakpoint)";
1560                                         break;
1561                                 case 0x00d:
1562                                         type = "CoreSight ETM11";
1563                                         full = "(Embedded Trace)";
1564                                         break;
1565                                 // case 0x113: what?
1566                                 case 0x120:             /* from OMAP3 memmap */
1567                                         type = "TI SDTI";
1568                                         full = "(System Debug Trace Interface)";
1569                                         break;
1570                                 case 0x343:             /* from OMAP3 memmap */
1571                                         type = "TI DAPCTL";
1572                                         full = "";
1573                                         break;
1574                                 case 0x4e0:
1575                                         type = "Cortex-M3 ETM";
1576                                         full = "(Embedded Trace)";
1577                                         break;
1578                                 case 0x906:
1579                                         type = "Coresight CTI";
1580                                         full = "(Cross Trigger)";
1581                                         break;
1582                                 case 0x907:
1583                                         type = "Coresight ETB";
1584                                         full = "(Trace Buffer)";
1585                                         break;
1586                                 case 0x908:
1587                                         type = "Coresight CSTF";
1588                                         full = "(Trace Funnel)";
1589                                         break;
1590                                 case 0x910:
1591                                         type = "CoreSight ETM9";
1592                                         full = "(Embedded Trace)";
1593                                         break;
1594                                 case 0x912:
1595                                         type = "Coresight TPIU";
1596                                         full = "(Trace Port Interface Unit)";
1597                                         break;
1598                                 case 0x921:
1599                                         type = "Cortex-A8 ETM";
1600                                         full = "(Embedded Trace)";
1601                                         break;
1602                                 case 0x922:
1603                                         type = "Cortex-A8 CTI";
1604                                         full = "(Cross Trigger)";
1605                                         break;
1606                                 case 0x923:
1607                                         type = "Cortex-M3 TPIU";
1608                                         full = "(Trace Port Interface Unit)";
1609                                         break;
1610                                 case 0xc08:
1611                                         type = "Cortex-A8 Debug";
1612                                         full = "(Debug Unit)";
1613                                         break;
1614                                 default:
1615                                         type = "-*- unrecognized -*-";
1616                                         full = "";
1617                                         break;
1618                                 }
1619                                 command_print(cmd_ctx, "\t\tPart is %s %s",
1620                                                 type, full);
1621                         }
1622                         else
1623                         {
1624                                 if (romentry)
1625                                         command_print(cmd_ctx, "\t\tComponent not present");
1626                                 else
1627                                         command_print(cmd_ctx, "\t\tEnd of ROM table");
1628                         }
1629                         entry_offset += 4;
1630                 } while (romentry > 0);
1631         }
1632         else
1633         {
1634                 command_print(cmd_ctx, "\tNo ROM table present");
1635         }
1636         dap_ap_select(swjdp, apselold);
1637
1638         return ERROR_OK;
1639 }
1640
1641 DAP_COMMAND_HANDLER(dap_baseaddr_command)
1642 {
1643         uint32_t apsel, apselsave, baseaddr;
1644         int retval;
1645
1646         apselsave = swjdp->apsel;
1647         switch (CMD_ARGC) {
1648         case 0:
1649                 apsel = swjdp->apsel;
1650                 break;
1651         case 1:
1652                 COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], apsel);
1653                 /* AP address is in bits 31:24 of DP_SELECT */
1654                 if (apsel >= 256)
1655                         return ERROR_INVALID_ARGUMENTS;
1656                 break;
1657         default:
1658                 return ERROR_COMMAND_SYNTAX_ERROR;
1659         }
1660
1661         if (apselsave != apsel)
1662                 dap_ap_select(swjdp, apsel);
1663
1664         /* NOTE:  assumes we're talking to a MEM-AP, which
1665          * has a base address.  There are other kinds of AP,
1666          * though they're not common for now.  This should
1667          * use the ID register to verify it's a MEM-AP.
1668          */
1669         dap_ap_read_reg_u32(swjdp, AP_REG_BASE, &baseaddr);
1670         retval = jtagdp_transaction_endcheck(swjdp);
1671         command_print(CMD_CTX, "0x%8.8" PRIx32, baseaddr);
1672
1673         if (apselsave != apsel)
1674                 dap_ap_select(swjdp, apselsave);
1675
1676         return retval;
1677 }
1678
1679 DAP_COMMAND_HANDLER(dap_memaccess_command)
1680 {
1681         uint32_t memaccess_tck;
1682
1683         switch (CMD_ARGC) {
1684         case 0:
1685                 memaccess_tck = swjdp->memaccess_tck;
1686                 break;
1687         case 1:
1688                 COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], memaccess_tck);
1689                 break;
1690         default:
1691                 return ERROR_COMMAND_SYNTAX_ERROR;
1692         }
1693         swjdp->memaccess_tck = memaccess_tck;
1694
1695         command_print(CMD_CTX, "memory bus access delay set to %" PRIi32 " tck",
1696                         swjdp->memaccess_tck);
1697
1698         return ERROR_OK;
1699 }
1700
1701 DAP_COMMAND_HANDLER(dap_apsel_command)
1702 {
1703         uint32_t apsel, apid;
1704         int retval;
1705
1706         switch (CMD_ARGC) {
1707         case 0:
1708                 apsel = 0;
1709                 break;
1710         case 1:
1711                 COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], apsel);
1712                 /* AP address is in bits 31:24 of DP_SELECT */
1713                 if (apsel >= 256)
1714                         return ERROR_INVALID_ARGUMENTS;
1715                 break;
1716         default:
1717                 return ERROR_COMMAND_SYNTAX_ERROR;
1718         }
1719
1720         dap_ap_select(swjdp, apsel);
1721         dap_ap_read_reg_u32(swjdp, AP_REG_IDR, &apid);
1722         retval = jtagdp_transaction_endcheck(swjdp);
1723         command_print(CMD_CTX, "ap %" PRIi32 " selected, identification register 0x%8.8" PRIx32,
1724                         apsel, apid);
1725
1726         return retval;
1727 }
1728
1729 DAP_COMMAND_HANDLER(dap_apid_command)
1730 {
1731         uint32_t apsel, apselsave, apid;
1732         int retval;
1733
1734         apselsave = swjdp->apsel;
1735         switch (CMD_ARGC) {
1736         case 0:
1737                 apsel = swjdp->apsel;
1738                 break;
1739         case 1:
1740                 COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], apsel);
1741                 /* AP address is in bits 31:24 of DP_SELECT */
1742                 if (apsel >= 256)
1743                         return ERROR_INVALID_ARGUMENTS;
1744                 break;
1745         default:
1746                 return ERROR_COMMAND_SYNTAX_ERROR;
1747         }
1748
1749         if (apselsave != apsel)
1750                 dap_ap_select(swjdp, apsel);
1751
1752         dap_ap_read_reg_u32(swjdp, AP_REG_IDR, &apid);
1753         retval = jtagdp_transaction_endcheck(swjdp);
1754         command_print(CMD_CTX, "0x%8.8" PRIx32, apid);
1755         if (apselsave != apsel)
1756                 dap_ap_select(swjdp, apselsave);
1757
1758         return retval;
1759 }