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  *   This program is free software; you can redistribute it and/or modify  *
  12  *   it under the terms of the GNU General Public License as published by  *
  13  *   the Free Software Foundation; either version 2 of the License, or     *
  14  *   (at your option) any later version.                                   *
  15  *                                                                         *
  16  *   This program is distributed in the hope that it will be useful,       *
  17  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  18  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  19  *   GNU General Public License for more details.                          *
  20  *                                                                         *
  21  *   You should have received a copy of the GNU General Public License     *
  22  *   along with this program; if not, write to the                         *
  23  *   Free Software Foundation, Inc.,                                       *
  24  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  25  ***************************************************************************/
  26 /***************************************************************************
  27  *                                                                         *
  28  * This file implements support for the ARM Debug Interface v5  (ADI_V5)   *
  29  *                                                                         *
  30  * ARM(tm) Debug Interface v5 Architecture Specification    ARM IHI 0031A  *
  31  *                                                                         *
  32  * CoreSight(tm) DAP-Lite TRM, ARM DDI 0316D                               *
  33  * Cortex-M3(tm) TRM, ARM DDI 0337G                                        *
  34  *                                                                         *
  35 ***************************************************************************/
  36
  37 #ifdef HAVE_CONFIG_H
  38 #include "config.h"
  39 #endif
  40
  41 #include "arm_adi_v5.h"
  42 #include "time_support.h"
  43
  44 /*
  45  * Transaction Mode:
  46  * swjdp->trans_mode = TRANS_MODE_COMPOSITE;
  47  * Uses Overrun checking mode and does not do actual JTAG send/receive or transaction
  48  * result checking until swjdp_end_transaction()
  49  * This must be done before using or deallocating any return variables.
  50  * swjdp->trans_mode == TRANS_MODE_ATOMIC
  51  * All reads and writes to the AHB bus are checked for valid completion, and return values
  52  * are immediatley available.
  53 */
  54
  55
  56 /* ARM ADI Specification requires at least 10 bits used for TAR autoincrement  */
  57
  58 /*
  59         uint32_t tar_block_size(uint32_t address)
  60         Return the largest block starting at address that does not cross a tar block size alignment boundary
  61 */
  62 static uint32_t max_tar_block_size(uint32_t tar_autoincr_block, uint32_t address)
  63 {
  64         return (tar_autoincr_block - ((tar_autoincr_block - 1) & address)) >> 2;
  65 }
  66
  67 /***************************************************************************
  68  *                                                                         *
  69  * DPACC and APACC scanchain access through JTAG-DP                        *
  70  *                                                                         *
  71 ***************************************************************************/
  72
  73 /* Scan out and in from target ordered uint8_t buffers */
  74 int adi_jtag_dp_scan(struct swjdp_common *swjdp, uint8_t instr, uint8_t reg_addr, uint8_t RnW, uint8_t *outvalue, uint8_t *invalue, uint8_t *ack)
  75 {
  76         struct arm_jtag *jtag_info = swjdp->jtag_info;
  77         struct scan_field fields[2];
  78         uint8_t out_addr_buf;
  79
  80         jtag_set_end_state(TAP_IDLE);
  81         arm_jtag_set_instr(jtag_info, instr, NULL);
  82
  83         /* Add specified number of tck clocks before accessing memory bus */
  84         if ((instr == DAP_IR_APACC) && ((reg_addr == AP_REG_DRW)||((reg_addr&0xF0) == AP_REG_BD0))&& (swjdp->memaccess_tck != 0))
  85                 jtag_add_runtest(swjdp->memaccess_tck, jtag_set_end_state(TAP_IDLE));
  86
  87         fields[0].tap = jtag_info->tap;
  88         fields[0].num_bits = 3;
  89         buf_set_u32(&out_addr_buf, 0, 3, ((reg_addr >> 1) & 0x6) | (RnW & 0x1));
  90         fields[0].out_value = &out_addr_buf;
  91         fields[0].in_value = ack;
  92
  93         fields[1].tap = jtag_info->tap;
  94         fields[1].num_bits = 32;
  95         fields[1].out_value = outvalue;
  96         fields[1].in_value = invalue;
  97
  98         jtag_add_dr_scan(2, fields, jtag_get_end_state());
  99
 100         return ERROR_OK;
 101 }
 102
 103 /* Scan out and in from host ordered uint32_t variables */
 104 int adi_jtag_dp_scan_u32(struct swjdp_common *swjdp, uint8_t instr, uint8_t reg_addr, uint8_t RnW, uint32_t outvalue, uint32_t *invalue, uint8_t *ack)
 105 {
 106         struct arm_jtag *jtag_info = swjdp->jtag_info;
 107         struct scan_field fields[2];
 108         uint8_t out_value_buf[4];
 109         uint8_t out_addr_buf;
 110
 111         jtag_set_end_state(TAP_IDLE);
 112         arm_jtag_set_instr(jtag_info, instr, NULL);
 113
 114         /* Add specified number of tck clocks before accessing memory bus */
 115         if ((instr == DAP_IR_APACC) && ((reg_addr == AP_REG_DRW)||((reg_addr&0xF0) == AP_REG_BD0))&& (swjdp->memaccess_tck != 0))
 116                 jtag_add_runtest(swjdp->memaccess_tck, jtag_set_end_state(TAP_IDLE));
 117
 118         fields[0].tap = jtag_info->tap;
 119         fields[0].num_bits = 3;
 120         buf_set_u32(&out_addr_buf, 0, 3, ((reg_addr >> 1) & 0x6) | (RnW & 0x1));
 121         fields[0].out_value = &out_addr_buf;
 122         fields[0].in_value = ack;
 123
 124         fields[1].tap = jtag_info->tap;
 125         fields[1].num_bits = 32;
 126         buf_set_u32(out_value_buf, 0, 32, outvalue);
 127         fields[1].out_value = out_value_buf;
 128         fields[1].in_value = NULL;
 129
 130         if (invalue)
 131         {
 132                 fields[1].in_value = (uint8_t *)invalue;
 133                 jtag_add_dr_scan(2, fields, jtag_get_end_state());
 134
 135                 jtag_add_callback(arm_le_to_h_u32, (jtag_callback_data_t) invalue);
 136         } else
 137         {
 138
 139                 jtag_add_dr_scan(2, fields, jtag_get_end_state());
 140         }
 141
 142         return ERROR_OK;
 143 }
 144
 145 /* scan_inout_check adds one extra inscan for DPAP_READ commands to read variables */
 146 int scan_inout_check(struct swjdp_common *swjdp, uint8_t instr, uint8_t reg_addr, uint8_t RnW, uint8_t *outvalue, uint8_t *invalue)
 147 {
 148         adi_jtag_dp_scan(swjdp, instr, reg_addr, RnW, outvalue, NULL, NULL);
 149
 150         if ((RnW == DPAP_READ) && (invalue != NULL))
 151         {
 152                 adi_jtag_dp_scan(swjdp, DAP_IR_DPACC, DP_RDBUFF, DPAP_READ, 0, invalue, &swjdp->ack);
 153         }
 154
 155         /* In TRANS_MODE_ATOMIC all DAP_IR_APACC transactions wait for ack = OK/FAULT and the check CTRL_STAT */
 156         if ((instr == DAP_IR_APACC) && (swjdp->trans_mode == TRANS_MODE_ATOMIC))
 157         {
 158                 return swjdp_transaction_endcheck(swjdp);
 159         }
 160
 161         return ERROR_OK;
 162 }
 163
 164 int scan_inout_check_u32(struct swjdp_common *swjdp, uint8_t instr, uint8_t reg_addr, uint8_t RnW, uint32_t outvalue, uint32_t *invalue)
 165 {
 166         adi_jtag_dp_scan_u32(swjdp, instr, reg_addr, RnW, outvalue, NULL, NULL);
 167
 168         if ((RnW == DPAP_READ) && (invalue != NULL))
 169         {
 170                 adi_jtag_dp_scan_u32(swjdp, DAP_IR_DPACC, DP_RDBUFF, DPAP_READ, 0, invalue, &swjdp->ack);
 171         }
 172
 173         /* In TRANS_MODE_ATOMIC all DAP_IR_APACC transactions wait for ack = OK/FAULT and then check CTRL_STAT */
 174         if ((instr == DAP_IR_APACC) && (swjdp->trans_mode == TRANS_MODE_ATOMIC))
 175         {
 176                 return swjdp_transaction_endcheck(swjdp);
 177         }
 178
 179         return ERROR_OK;
 180 }
 181
 182 int swjdp_transaction_endcheck(struct swjdp_common *swjdp)
 183 {
 184         int retval;
 185         uint32_t ctrlstat;
 186
 187         /* too expensive to call keep_alive() here */
 188
 189 #if 0
 190         /* Danger!!!! BROKEN!!!! */
 191         scan_inout_check_u32(swjdp, DAP_IR_DPACC, DP_CTRL_STAT, DPAP_READ, 0, &ctrlstat);
 192         /* Danger!!!! BROKEN!!!! Why will jtag_execute_queue() fail here????
 193         R956 introduced the check on return value here and now Michael Schwingen reports
 194         that this code no longer works....
 195
 196         https://lists.berlios.de/pipermail/openocd-development/2008-September/003107.html
 197         */
 198         if ((retval = jtag_execute_queue()) != ERROR_OK)
 199         {
 200                 LOG_ERROR("BUG: Why does this fail the first time????");
 201         }
 202         /* Why??? second time it works??? */
 203 #endif
 204
 205         scan_inout_check_u32(swjdp, DAP_IR_DPACC, DP_CTRL_STAT, DPAP_READ, 0, &ctrlstat);
 206         if ((retval = jtag_execute_queue()) != ERROR_OK)
 207                 return retval;
 208
 209         swjdp->ack = swjdp->ack & 0x7;
 210
 211         if (swjdp->ack != 2)
 212         {
 213                 long long then = timeval_ms();
 214                 while (swjdp->ack != 2)
 215                 {
 216                         if (swjdp->ack == 1)
 217                         {
 218                                 if ((timeval_ms()-then) > 1000)
 219                                 {
 220                                         LOG_WARNING("Timeout (1000ms) waiting for ACK = OK/FAULT in SWJDP transaction");
 221                                         return ERROR_JTAG_DEVICE_ERROR;
 222                                 }
 223                         }
 224                         else
 225                         {
 226                                 LOG_WARNING("Invalid ACK in SWJDP transaction");
 227                                 return ERROR_JTAG_DEVICE_ERROR;
 228                         }
 229
 230                         scan_inout_check_u32(swjdp, DAP_IR_DPACC, DP_CTRL_STAT, DPAP_READ, 0, &ctrlstat);
 231                         if ((retval = jtag_execute_queue()) != ERROR_OK)
 232                                 return retval;
 233                         swjdp->ack = swjdp->ack & 0x7;
 234                 }
 235         } else
 236         {
 237                 /* common code path avoids fn to timeval_ms() */
 238         }
 239
 240         /* Check for STICKYERR and STICKYORUN */
 241         if (ctrlstat & (SSTICKYORUN | SSTICKYERR))
 242         {
 243                 LOG_DEBUG("swjdp: CTRL/STAT error 0x%" PRIx32 "", ctrlstat);
 244                 /* Check power to debug regions */
 245                 if ((ctrlstat & 0xf0000000) != 0xf0000000)
 246                 {
 247                          ahbap_debugport_init(swjdp);
 248                 }
 249                 else
 250                 {
 251                         uint32_t mem_ap_csw, mem_ap_tar;
 252
 253                         /* Print information about last AHBAP access */
 254                         LOG_ERROR("AHBAP Cached values: dp_select 0x%" PRIx32 ", ap_csw 0x%" PRIx32 ", ap_tar 0x%" PRIx32 "", swjdp->dp_select_value, swjdp->ap_csw_value, swjdp->ap_tar_value);
 255                         if (ctrlstat & SSTICKYORUN)
 256                                 LOG_ERROR("SWJ-DP OVERRUN - check clock or reduce jtag speed");
 257
 258                         if (ctrlstat & SSTICKYERR)
 259                                 LOG_ERROR("SWJ-DP STICKY ERROR");
 260
 261                         /* Clear Sticky Error Bits */
 262                         scan_inout_check_u32(swjdp, DAP_IR_DPACC, DP_CTRL_STAT, DPAP_WRITE, swjdp->dp_ctrl_stat | SSTICKYORUN | SSTICKYERR, NULL);
 263                         scan_inout_check_u32(swjdp, DAP_IR_DPACC, DP_CTRL_STAT, DPAP_READ, 0, &ctrlstat);
 264                         if ((retval = jtag_execute_queue()) != ERROR_OK)
 265                                 return retval;
 266
 267                         LOG_DEBUG("swjdp: status 0x%" PRIx32 "", ctrlstat);
 268
 269                         dap_ap_read_reg_u32(swjdp, AP_REG_CSW, &mem_ap_csw);
 270                         dap_ap_read_reg_u32(swjdp, AP_REG_TAR, &mem_ap_tar);
 271                         if ((retval = jtag_execute_queue()) != ERROR_OK)
 272                                 return retval;
 273                         LOG_ERROR("Read MEM_AP_CSW 0x%" PRIx32 ", MEM_AP_TAR 0x%" PRIx32 "", mem_ap_csw, mem_ap_tar);
 274
 275                 }
 276                 if ((retval = jtag_execute_queue()) != ERROR_OK)
 277                         return retval;
 278                 return ERROR_JTAG_DEVICE_ERROR;
 279         }
 280
 281         return ERROR_OK;
 282 }
 283
 284 /***************************************************************************
 285  *                                                                         *
 286  * DP and MEM-AP  register access  through APACC and DPACC                 *
 287  *                                                                         *
 288 ***************************************************************************/
 289
 290 int dap_dp_write_reg(struct swjdp_common *swjdp, uint32_t value, uint8_t reg_addr)
 291 {
 292         return scan_inout_check_u32(swjdp, DAP_IR_DPACC, reg_addr, DPAP_WRITE, value, NULL);
 293 }
 294
 295 int dap_dp_read_reg(struct swjdp_common *swjdp, uint32_t *value, uint8_t reg_addr)
 296 {
 297         return scan_inout_check_u32(swjdp, DAP_IR_DPACC, reg_addr, DPAP_READ, 0, value);
 298 }
 299
 300 int dap_ap_select(struct swjdp_common *swjdp,uint8_t apsel)
 301 {
 302         uint32_t select;
 303         select = (apsel << 24) & 0xFF000000;
 304
 305         if (select != swjdp->apsel)
 306         {
 307                 swjdp->apsel = select;
 308                 /* Switching AP invalidates cached values */
 309                 swjdp->dp_select_value = -1;
 310                 swjdp->ap_csw_value = -1;
 311                 swjdp->ap_tar_value = -1;
 312         }
 313
 314         return ERROR_OK;
 315 }
 316
 317 int dap_dp_bankselect(struct swjdp_common *swjdp,uint32_t ap_reg)
 318 {
 319         uint32_t select;
 320         select = (ap_reg & 0x000000F0);
 321
 322         if (select != swjdp->dp_select_value)
 323         {
 324                 dap_dp_write_reg(swjdp, select | swjdp->apsel, DP_SELECT);
 325                 swjdp->dp_select_value = select;
 326         }
 327
 328         return ERROR_OK;
 329 }
 330
 331 int dap_ap_write_reg(struct swjdp_common *swjdp, uint32_t reg_addr, uint8_t* out_value_buf)
 332 {
 333         dap_dp_bankselect(swjdp, reg_addr);
 334         scan_inout_check(swjdp, DAP_IR_APACC, reg_addr, DPAP_WRITE, out_value_buf, NULL);
 335
 336         return ERROR_OK;
 337 }
 338
 339 int dap_ap_read_reg(struct swjdp_common *swjdp, uint32_t reg_addr, uint8_t *in_value_buf)
 340 {
 341         dap_dp_bankselect(swjdp, reg_addr);
 342         scan_inout_check(swjdp, DAP_IR_APACC, reg_addr, DPAP_READ, 0, in_value_buf);
 343
 344         return ERROR_OK;
 345 }
 346 int dap_ap_write_reg_u32(struct swjdp_common *swjdp, uint32_t reg_addr, uint32_t value)
 347 {
 348         uint8_t out_value_buf[4];
 349
 350         buf_set_u32(out_value_buf, 0, 32, value);
 351         dap_dp_bankselect(swjdp, reg_addr);
 352         scan_inout_check(swjdp, DAP_IR_APACC, reg_addr, DPAP_WRITE, out_value_buf, NULL);
 353
 354         return ERROR_OK;
 355 }
 356
 357 int dap_ap_read_reg_u32(struct swjdp_common *swjdp, uint32_t reg_addr, uint32_t *value)
 358 {
 359         dap_dp_bankselect(swjdp, reg_addr);
 360         scan_inout_check_u32(swjdp, DAP_IR_APACC, reg_addr, DPAP_READ, 0, value);
 361
 362         return ERROR_OK;
 363 }
 364
 365 /***************************************************************************
 366  *                                                                         *
 367  * AHB-AP access to memory and system registers on AHB bus                 *
 368  *                                                                         *
 369 ***************************************************************************/
 370
 371 int dap_setup_accessport(struct swjdp_common *swjdp, uint32_t csw, uint32_t tar)
 372 {
 373         csw = csw | CSW_DBGSWENABLE | CSW_MASTER_DEBUG | CSW_HPROT;
 374         if (csw != swjdp->ap_csw_value)
 375         {
 376                 /* LOG_DEBUG("swjdp : Set CSW %x",csw); */
 377                 dap_ap_write_reg_u32(swjdp, AP_REG_CSW, csw);
 378                 swjdp->ap_csw_value = csw;
 379         }
 380         if (tar != swjdp->ap_tar_value)
 381         {
 382                 /* LOG_DEBUG("swjdp : Set TAR %x",tar); */
 383                 dap_ap_write_reg_u32(swjdp, AP_REG_TAR, tar);
 384                 swjdp->ap_tar_value = tar;
 385         }
 386         if (csw & CSW_ADDRINC_MASK)
 387         {
 388                 /* Do not cache TAR value when autoincrementing */
 389                 swjdp->ap_tar_value = -1;
 390         }
 391         return ERROR_OK;
 392 }
 393
 394 /*****************************************************************************
 395 *                                                                            *
 396 * mem_ap_read_u32(struct swjdp_common *swjdp, uint32_t address, uint32_t *value)      *
 397 *                                                                            *
 398 * Read a uint32_t value from memory or system register                            *
 399 * Functionally equivalent to target_read_u32(target, address, uint32_t *value),   *
 400 * but with less overhead                                                     *
 401 *****************************************************************************/
 402 int mem_ap_read_u32(struct swjdp_common *swjdp, uint32_t address, uint32_t *value)
 403 {
 404         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 405
 406         dap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, address & 0xFFFFFFF0);
 407         dap_ap_read_reg_u32(swjdp, AP_REG_BD0 | (address & 0xC), value);
 408
 409         return ERROR_OK;
 410 }
 411
 412 int mem_ap_read_atomic_u32(struct swjdp_common *swjdp, uint32_t address, uint32_t *value)
 413 {
 414         mem_ap_read_u32(swjdp, address, value);
 415
 416         return swjdp_transaction_endcheck(swjdp);
 417 }
 418
 419 /*****************************************************************************
 420 *                                                                            *
 421 * mem_ap_write_u32(struct swjdp_common *swjdp, uint32_t address, uint32_t value)      *
 422 *                                                                            *
 423 * Write a uint32_t value to memory or memory mapped register                              *
 424 *                                                                            *
 425 *****************************************************************************/
 426 int mem_ap_write_u32(struct swjdp_common *swjdp, uint32_t address, uint32_t value)
 427 {
 428         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 429
 430         dap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, address & 0xFFFFFFF0);
 431         dap_ap_write_reg_u32(swjdp, AP_REG_BD0 | (address & 0xC), value);
 432
 433         return ERROR_OK;
 434 }
 435
 436 int mem_ap_write_atomic_u32(struct swjdp_common *swjdp, uint32_t address, uint32_t value)
 437 {
 438         mem_ap_write_u32(swjdp, address, value);
 439
 440         return swjdp_transaction_endcheck(swjdp);
 441 }
 442
 443 /*****************************************************************************
 444 *                                                                            *
 445 * mem_ap_write_buf(struct swjdp_common *swjdp, uint8_t *buffer, int count, uint32_t address) *
 446 *                                                                            *
 447 * Write a buffer in target order (little endian)                             *
 448 *                                                                            *
 449 *****************************************************************************/
 450 int mem_ap_write_buf_u32(struct swjdp_common *swjdp, uint8_t *buffer, int count, uint32_t address)
 451 {
 452         int wcount, blocksize, writecount, errorcount = 0, retval = ERROR_OK;
 453         uint32_t adr = address;
 454         uint8_t* pBuffer = buffer;
 455
 456         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 457
 458         count >>= 2;
 459         wcount = count;
 460
 461         /* if we have an unaligned access - reorder data */
 462         if (adr & 0x3u)
 463         {
 464                 for (writecount = 0; writecount < count; writecount++)
 465                 {
 466                         int i;
 467                         uint32_t outvalue;
 468                         memcpy(&outvalue, pBuffer, sizeof(uint32_t));
 469
 470                         for (i = 0; i < 4; i++)
 471                         {
 472                                 *((uint8_t*)pBuffer + (adr & 0x3)) = outvalue;
 473                                 outvalue >>= 8;
 474                                 adr++;
 475                         }
 476                         pBuffer += sizeof(uint32_t);
 477                 }
 478         }
 479
 480         while (wcount > 0)
 481         {
 482                 /* Adjust to write blocks within boundaries aligned to the TAR autoincremnent size*/
 483                 blocksize = max_tar_block_size(swjdp->tar_autoincr_block, address);
 484                 if (wcount < blocksize)
 485                         blocksize = wcount;
 486
 487                 /* handle unaligned data at 4k boundary */
 488                 if (blocksize == 0)
 489                         blocksize = 1;
 490
 491                 dap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_SINGLE, address);
 492
 493                 for (writecount = 0; writecount < blocksize; writecount++)
 494                 {
 495                         dap_ap_write_reg(swjdp, AP_REG_DRW, buffer + 4 * writecount);
 496                 }
 497
 498                 if (swjdp_transaction_endcheck(swjdp) == ERROR_OK)
 499                 {
 500                         wcount = wcount - blocksize;
 501                         address = address + 4 * blocksize;
 502                         buffer = buffer + 4 * blocksize;
 503                 }
 504                 else
 505                 {
 506                         errorcount++;
 507                 }
 508
 509                 if (errorcount > 1)
 510                 {
 511                         LOG_WARNING("Block write error address 0x%" PRIx32 ", wcount 0x%x", address, wcount);
 512                         return ERROR_JTAG_DEVICE_ERROR;
 513                 }
 514         }
 515
 516         return retval;
 517 }
 518
 519 int mem_ap_write_buf_packed_u16(struct swjdp_common *swjdp, uint8_t *buffer, int count, uint32_t address)
 520 {
 521         int retval = ERROR_OK;
 522         int wcount, blocksize, writecount, i;
 523
 524         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 525
 526         wcount = count >> 1;
 527
 528         while (wcount > 0)
 529         {
 530                 int nbytes;
 531
 532                 /* Adjust to write blocks within boundaries aligned to the TAR autoincremnent size*/
 533                 blocksize = max_tar_block_size(swjdp->tar_autoincr_block, address);
 534
 535                 if (wcount < blocksize)
 536                         blocksize = wcount;
 537
 538                 /* handle unaligned data at 4k boundary */
 539                 if (blocksize == 0)
 540                         blocksize = 1;
 541
 542                 dap_setup_accessport(swjdp, CSW_16BIT | CSW_ADDRINC_PACKED, address);
 543                 writecount = blocksize;
 544
 545                 do
 546                 {
 547                         nbytes = MIN((writecount << 1), 4);
 548
 549                         if (nbytes < 4)
 550                         {
 551                                 if (mem_ap_write_buf_u16(swjdp, buffer, nbytes, address) != ERROR_OK)
 552                                 {
 553                                         LOG_WARNING("Block read error address 0x%" PRIx32 ", count 0x%x", address, count);
 554                                         return ERROR_JTAG_DEVICE_ERROR;
 555                                 }
 556
 557                                 address += nbytes >> 1;
 558                         }
 559                         else
 560                         {
 561                                 uint32_t outvalue;
 562                                 memcpy(&outvalue, buffer, sizeof(uint32_t));
 563
 564                                 for (i = 0; i < nbytes; i++)
 565                                 {
 566                                         *((uint8_t*)buffer + (address & 0x3)) = outvalue;
 567                                         outvalue >>= 8;
 568                                         address++;
 569                                 }
 570
 571                                 memcpy(&outvalue, buffer, sizeof(uint32_t));
 572                                 dap_ap_write_reg_u32(swjdp, AP_REG_DRW, outvalue);
 573                                 if (swjdp_transaction_endcheck(swjdp) != ERROR_OK)
 574                                 {
 575                                         LOG_WARNING("Block read error address 0x%" PRIx32 ", count 0x%x", address, count);
 576                                         return ERROR_JTAG_DEVICE_ERROR;
 577                                 }
 578                         }
 579
 580                         buffer += nbytes >> 1;
 581                         writecount -= nbytes >> 1;
 582
 583                 } while (writecount);
 584                 wcount -= blocksize;
 585         }
 586
 587         return retval;
 588 }
 589
 590 int mem_ap_write_buf_u16(struct swjdp_common *swjdp, uint8_t *buffer, int count, uint32_t address)
 591 {
 592         int retval = ERROR_OK;
 593
 594         if (count >= 4)
 595                 return mem_ap_write_buf_packed_u16(swjdp, buffer, count, address);
 596
 597         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 598
 599         while (count > 0)
 600         {
 601                 dap_setup_accessport(swjdp, CSW_16BIT | CSW_ADDRINC_SINGLE, address);
 602                 uint16_t svalue;
 603                 memcpy(&svalue, buffer, sizeof(uint16_t));
 604                 uint32_t outvalue = (uint32_t)svalue << 8 * (address & 0x3);
 605                 dap_ap_write_reg_u32(swjdp, AP_REG_DRW, outvalue);
 606                 retval = swjdp_transaction_endcheck(swjdp);
 607                 count -= 2;
 608                 address += 2;
 609                 buffer += 2;
 610         }
 611
 612         return retval;
 613 }
 614
 615 int mem_ap_write_buf_packed_u8(struct swjdp_common *swjdp, uint8_t *buffer, int count, uint32_t address)
 616 {
 617         int retval = ERROR_OK;
 618         int wcount, blocksize, writecount, i;
 619
 620         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 621
 622         wcount = count;
 623
 624         while (wcount > 0)
 625         {
 626                 int nbytes;
 627
 628                 /* Adjust to write blocks within boundaries aligned to the TAR autoincremnent size*/
 629                 blocksize = max_tar_block_size(swjdp->tar_autoincr_block, address);
 630
 631                 if (wcount < blocksize)
 632                         blocksize = wcount;
 633
 634                 dap_setup_accessport(swjdp, CSW_8BIT | CSW_ADDRINC_PACKED, address);
 635                 writecount = blocksize;
 636
 637                 do
 638                 {
 639                         nbytes = MIN(writecount, 4);
 640
 641                         if (nbytes < 4)
 642                         {
 643                                 if (mem_ap_write_buf_u8(swjdp, buffer, nbytes, address) != ERROR_OK)
 644                                 {
 645                                         LOG_WARNING("Block read error address 0x%" PRIx32 ", count 0x%x", address, count);
 646                                         return ERROR_JTAG_DEVICE_ERROR;
 647                                 }
 648
 649                                 address += nbytes;
 650                         }
 651                         else
 652                         {
 653                                 uint32_t outvalue;
 654                                 memcpy(&outvalue, buffer, sizeof(uint32_t));
 655
 656                                 for (i = 0; i < nbytes; i++)
 657                                 {
 658                                         *((uint8_t*)buffer + (address & 0x3)) = outvalue;
 659                                         outvalue >>= 8;
 660                                         address++;
 661                                 }
 662
 663                                 memcpy(&outvalue, buffer, sizeof(uint32_t));
 664                                 dap_ap_write_reg_u32(swjdp, AP_REG_DRW, outvalue);
 665                                 if (swjdp_transaction_endcheck(swjdp) != ERROR_OK)
 666                                 {
 667                                         LOG_WARNING("Block read error address 0x%" PRIx32 ", count 0x%x", address, count);
 668                                         return ERROR_JTAG_DEVICE_ERROR;
 669                                 }
 670                         }
 671
 672                         buffer += nbytes;
 673                         writecount -= nbytes;
 674
 675                 } while (writecount);
 676                 wcount -= blocksize;
 677         }
 678
 679         return retval;
 680 }
 681
 682 int mem_ap_write_buf_u8(struct swjdp_common *swjdp, uint8_t *buffer, int count, uint32_t address)
 683 {
 684         int retval = ERROR_OK;
 685
 686         if (count >= 4)
 687                 return mem_ap_write_buf_packed_u8(swjdp, buffer, count, address);
 688
 689         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 690
 691         while (count > 0)
 692         {
 693                 dap_setup_accessport(swjdp, CSW_8BIT | CSW_ADDRINC_SINGLE, address);
 694                 uint32_t outvalue = (uint32_t)*buffer << 8 * (address & 0x3);
 695                 dap_ap_write_reg_u32(swjdp, AP_REG_DRW, outvalue);
 696                 retval = swjdp_transaction_endcheck(swjdp);
 697                 count--;
 698                 address++;
 699                 buffer++;
 700         }
 701
 702         return retval;
 703 }
 704
 705 /*********************************************************************************
 706 *                                                                                *
 707 * mem_ap_read_buf_u32(struct swjdp_common *swjdp, uint8_t *buffer, int count, uint32_t address)  *
 708 *                                                                                *
 709 * Read block fast in target order (little endian) into a buffer                  *
 710 *                                                                                *
 711 **********************************************************************************/
 712 int mem_ap_read_buf_u32(struct swjdp_common *swjdp, uint8_t *buffer, int count, uint32_t address)
 713 {
 714         int wcount, blocksize, readcount, errorcount = 0, retval = ERROR_OK;
 715         uint32_t adr = address;
 716         uint8_t* pBuffer = buffer;
 717
 718         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 719
 720         count >>= 2;
 721         wcount = count;
 722
 723         while (wcount > 0)
 724         {
 725                 /* Adjust to read blocks within boundaries aligned to the TAR autoincremnent size*/
 726                 blocksize = max_tar_block_size(swjdp->tar_autoincr_block, address);
 727                 if (wcount < blocksize)
 728                         blocksize = wcount;
 729
 730                 /* handle unaligned data at 4k boundary */
 731                 if (blocksize == 0)
 732                         blocksize = 1;
 733
 734                 dap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_SINGLE, address);
 735
 736                 /* Scan out first read */
 737                 adi_jtag_dp_scan(swjdp, DAP_IR_APACC, AP_REG_DRW, DPAP_READ, 0, NULL, NULL);
 738                 for (readcount = 0; readcount < blocksize - 1; readcount++)
 739                 {
 740                         /* Scan out read instruction and scan in previous value */
 741                         adi_jtag_dp_scan(swjdp, DAP_IR_APACC, AP_REG_DRW, DPAP_READ, 0, buffer + 4 * readcount, &swjdp->ack);
 742                 }
 743
 744                 /* Scan in last value */
 745                 adi_jtag_dp_scan(swjdp, DAP_IR_DPACC, DP_RDBUFF, DPAP_READ, 0, buffer + 4 * readcount, &swjdp->ack);
 746                 if (swjdp_transaction_endcheck(swjdp) == ERROR_OK)
 747                 {
 748                         wcount = wcount - blocksize;
 749                         address += 4 * blocksize;
 750                         buffer += 4 * blocksize;
 751                 }
 752                 else
 753                 {
 754                         errorcount++;
 755                 }
 756
 757                 if (errorcount > 1)
 758                 {
 759                         LOG_WARNING("Block read error address 0x%" PRIx32 ", count 0x%x", address, count);
 760                         return ERROR_JTAG_DEVICE_ERROR;
 761                 }
 762         }
 763
 764         /* if we have an unaligned access - reorder data */
 765         if (adr & 0x3u)
 766         {
 767                 for (readcount = 0; readcount < count; readcount++)
 768                 {
 769                         int i;
 770                         uint32_t data;
 771                         memcpy(&data, pBuffer, sizeof(uint32_t));
 772
 773                         for (i = 0; i < 4; i++)
 774                         {
 775                                 *((uint8_t*)pBuffer) = (data >> 8 * (adr & 0x3));
 776                                 pBuffer++;
 777                                 adr++;
 778                         }
 779                 }
 780         }
 781
 782         return retval;
 783 }
 784
 785 int mem_ap_read_buf_packed_u16(struct swjdp_common *swjdp, uint8_t *buffer, int count, uint32_t address)
 786 {
 787         uint32_t invalue;
 788         int retval = ERROR_OK;
 789         int wcount, blocksize, readcount, i;
 790
 791         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 792
 793         wcount = count >> 1;
 794
 795         while (wcount > 0)
 796         {
 797                 int nbytes;
 798
 799                 /* Adjust to read blocks within boundaries aligned to the TAR autoincremnent size*/
 800                 blocksize = max_tar_block_size(swjdp->tar_autoincr_block, address);
 801                 if (wcount < blocksize)
 802                         blocksize = wcount;
 803
 804                 dap_setup_accessport(swjdp, CSW_16BIT | CSW_ADDRINC_PACKED, address);
 805
 806                 /* handle unaligned data at 4k boundary */
 807                 if (blocksize == 0)
 808                         blocksize = 1;
 809                 readcount = blocksize;
 810
 811                 do
 812                 {
 813                         dap_ap_read_reg_u32(swjdp, AP_REG_DRW, &invalue);
 814                         if (swjdp_transaction_endcheck(swjdp) != ERROR_OK)
 815                         {
 816                                 LOG_WARNING("Block read error address 0x%" PRIx32 ", count 0x%x", address, count);
 817                                 return ERROR_JTAG_DEVICE_ERROR;
 818                         }
 819
 820                         nbytes = MIN((readcount << 1), 4);
 821
 822                         for (i = 0; i < nbytes; i++)
 823                         {
 824                                 *((uint8_t*)buffer) = (invalue >> 8 * (address & 0x3));
 825                                 buffer++;
 826                                 address++;
 827                         }
 828
 829                         readcount -= (nbytes >> 1);
 830                 } while (readcount);
 831                 wcount -= blocksize;
 832         }
 833
 834         return retval;
 835 }
 836
 837 int mem_ap_read_buf_u16(struct swjdp_common *swjdp, uint8_t *buffer, int count, uint32_t address)
 838 {
 839         uint32_t invalue, i;
 840         int retval = ERROR_OK;
 841
 842         if (count >= 4)
 843                 return mem_ap_read_buf_packed_u16(swjdp, buffer, count, address);
 844
 845         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 846
 847         while (count > 0)
 848         {
 849                 dap_setup_accessport(swjdp, CSW_16BIT | CSW_ADDRINC_SINGLE, address);
 850                 dap_ap_read_reg_u32(swjdp, AP_REG_DRW, &invalue);
 851                 retval = swjdp_transaction_endcheck(swjdp);
 852                 if (address & 0x1)
 853                 {
 854                         for (i = 0; i < 2; i++)
 855                         {
 856                                 *((uint8_t*)buffer) = (invalue >> 8 * (address & 0x3));
 857                                 buffer++;
 858                                 address++;
 859                         }
 860                 }
 861                 else
 862                 {
 863                         uint16_t svalue = (invalue >> 8 * (address & 0x3));
 864                         memcpy(buffer, &svalue, sizeof(uint16_t));
 865                         address += 2;
 866                         buffer += 2;
 867                 }
 868                 count -= 2;
 869         }
 870
 871         return retval;
 872 }
 873
 874 /* FIX!!! is this a potential performance bottleneck w.r.t. requiring too many
 875  * roundtrips when jtag_execute_queue() has a large overhead(e.g. for USB)s?
 876  *
 877  * The solution is to arrange for a large out/in scan in this loop and
 878  * and convert data afterwards.
 879  */
 880 int mem_ap_read_buf_packed_u8(struct swjdp_common *swjdp, uint8_t *buffer, int count, uint32_t address)
 881 {
 882         uint32_t invalue;
 883         int retval = ERROR_OK;
 884         int wcount, blocksize, readcount, i;
 885
 886         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 887
 888         wcount = count;
 889
 890         while (wcount > 0)
 891         {
 892                 int nbytes;
 893
 894                 /* Adjust to read blocks within boundaries aligned to the TAR autoincremnent size*/
 895                 blocksize = max_tar_block_size(swjdp->tar_autoincr_block, address);
 896
 897                 if (wcount < blocksize)
 898                         blocksize = wcount;
 899
 900                 dap_setup_accessport(swjdp, CSW_8BIT | CSW_ADDRINC_PACKED, address);
 901                 readcount = blocksize;
 902
 903                 do
 904                 {
 905                         dap_ap_read_reg_u32(swjdp, AP_REG_DRW, &invalue);
 906                         if (swjdp_transaction_endcheck(swjdp) != ERROR_OK)
 907                         {
 908                                 LOG_WARNING("Block read error address 0x%" PRIx32 ", count 0x%x", address, count);
 909                                 return ERROR_JTAG_DEVICE_ERROR;
 910                         }
 911
 912                         nbytes = MIN(readcount, 4);
 913
 914                         for (i = 0; i < nbytes; i++)
 915                         {
 916                                 *((uint8_t*)buffer) = (invalue >> 8 * (address & 0x3));
 917                                 buffer++;
 918                                 address++;
 919                         }
 920
 921                         readcount -= nbytes;
 922                 } while (readcount);
 923                 wcount -= blocksize;
 924         }
 925
 926         return retval;
 927 }
 928
 929 int mem_ap_read_buf_u8(struct swjdp_common *swjdp, uint8_t *buffer, int count, uint32_t address)
 930 {
 931         uint32_t invalue;
 932         int retval = ERROR_OK;
 933
 934         if (count >= 4)
 935                 return mem_ap_read_buf_packed_u8(swjdp, buffer, count, address);
 936
 937         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 938
 939         while (count > 0)
 940         {
 941                 dap_setup_accessport(swjdp, CSW_8BIT | CSW_ADDRINC_SINGLE, address);
 942                 dap_ap_read_reg_u32(swjdp, AP_REG_DRW, &invalue);
 943                 retval = swjdp_transaction_endcheck(swjdp);
 944                 *((uint8_t*)buffer) = (invalue >> 8 * (address & 0x3));
 945                 count--;
 946                 address++;
 947                 buffer++;
 948         }
 949
 950         return retval;
 951 }
 952
 953 int ahbap_debugport_init(struct swjdp_common *swjdp)
 954 {
 955         uint32_t idreg, romaddr, dummy;
 956         uint32_t ctrlstat;
 957         int cnt = 0;
 958         int retval;
 959
 960         LOG_DEBUG(" ");
 961
 962         swjdp->apsel = 0;
 963         swjdp->ap_csw_value = -1;
 964         swjdp->ap_tar_value = -1;
 965         swjdp->trans_mode = TRANS_MODE_ATOMIC;
 966         dap_dp_read_reg(swjdp, &dummy, DP_CTRL_STAT);
 967         dap_dp_write_reg(swjdp, SSTICKYERR, DP_CTRL_STAT);
 968         dap_dp_read_reg(swjdp, &dummy, DP_CTRL_STAT);
 969
 970         swjdp->dp_ctrl_stat = CDBGPWRUPREQ | CSYSPWRUPREQ;
 971
 972         dap_dp_write_reg(swjdp, swjdp->dp_ctrl_stat, DP_CTRL_STAT);
 973         dap_dp_read_reg(swjdp, &ctrlstat, DP_CTRL_STAT);
 974         if ((retval = jtag_execute_queue()) != ERROR_OK)
 975                 return retval;
 976
 977         /* Check that we have debug power domains activated */
 978         while (!(ctrlstat & CDBGPWRUPACK) && (cnt++ < 10))
 979         {
 980                 LOG_DEBUG("swjdp: wait CDBGPWRUPACK");
 981                 dap_dp_read_reg(swjdp, &ctrlstat, DP_CTRL_STAT);
 982                 if ((retval = jtag_execute_queue()) != ERROR_OK)
 983                         return retval;
 984                 alive_sleep(10);
 985         }
 986
 987         while (!(ctrlstat & CSYSPWRUPACK) && (cnt++ < 10))
 988         {
 989                 LOG_DEBUG("swjdp: wait CSYSPWRUPACK");
 990                 dap_dp_read_reg(swjdp, &ctrlstat, DP_CTRL_STAT);
 991                 if ((retval = jtag_execute_queue()) != ERROR_OK)
 992                         return retval;
 993                 alive_sleep(10);
 994         }
 995
 996         dap_dp_read_reg(swjdp, &dummy, DP_CTRL_STAT);
 997         /* With debug power on we can activate OVERRUN checking */
 998         swjdp->dp_ctrl_stat = CDBGPWRUPREQ | CSYSPWRUPREQ | CORUNDETECT;
 999         dap_dp_write_reg(swjdp, swjdp->dp_ctrl_stat, DP_CTRL_STAT);
1000         dap_dp_read_reg(swjdp, &dummy, DP_CTRL_STAT);
1001
1002         dap_ap_read_reg_u32(swjdp, 0xFC, &idreg);
1003         dap_ap_read_reg_u32(swjdp, 0xF8, &romaddr);
1004
1005         LOG_DEBUG("AHB-AP ID Register 0x%" PRIx32 ", Debug ROM Address 0x%" PRIx32 "", idreg, romaddr);
1006
1007         return ERROR_OK;
1008 }
1009
1010 /* CID interpretation -- see ARM IHI 0029B section 3 */
1011 static const char *class_description[16] ={
1012         "Reserved", "ROM table", "Reserved", "Reserved",
1013         "Reserved", "Reserved", "Reserved", "Reserved",
1014         "Reserved", "CoreSight component", "Reserved", "Peripheral Test Block",
1015         "Reserved", "DESS", "Generic IP component", "PrimeCell or System component"
1016 };
1017
1018 static bool
1019 is_dap_cid_ok(uint32_t cid3, uint32_t cid2, uint32_t cid1, uint32_t cid0)
1020 {
1021         return cid3 == 0xb1 && cid2 == 0x05
1022                         && ((cid1 & 0x0f) == 0) && cid0 == 0x0d;
1023 }
1024
1025 int dap_info_command(struct command_context *cmd_ctx, struct swjdp_common *swjdp, int apsel)
1026 {
1027
1028         uint32_t dbgbase,apid;
1029         int romtable_present = 0;
1030         uint8_t mem_ap;
1031         uint32_t apselold;
1032
1033         apselold = swjdp->apsel;
1034         dap_ap_select(swjdp, apsel);
1035         dap_ap_read_reg_u32(swjdp, 0xF8, &dbgbase);
1036         dap_ap_read_reg_u32(swjdp, 0xFC, &apid);
1037         swjdp_transaction_endcheck(swjdp);
1038         /* Now we read ROM table ID registers, ref. ARM IHI 0029B sec  */
1039         mem_ap = ((apid&0x10000) && ((apid&0x0F) != 0));
1040         command_print(cmd_ctx, "ap identification register 0x%8.8" PRIx32 "", apid);
1041         if (apid)
1042         {
1043                 switch (apid&0x0F)
1044                 {
1045                         case 0:
1046                                 command_print(cmd_ctx, "\tType is jtag-ap");
1047                                 break;
1048                         case 1:
1049                                 command_print(cmd_ctx, "\tType is mem-ap AHB");
1050                                 break;
1051                         case 2:
1052                                 command_print(cmd_ctx, "\tType is mem-ap APB");
1053                                 break;
1054                         default:
1055                                 command_print(cmd_ctx, "\tUnknown AP-type");
1056                         break;
1057                 }
1058                 command_print(cmd_ctx, "ap debugbase 0x%8.8" PRIx32 "", dbgbase);
1059         }
1060         else
1061         {
1062                 command_print(cmd_ctx, "No AP found at this apsel 0x%x", apsel);
1063         }
1064
1065         romtable_present = ((mem_ap) && (dbgbase != 0xFFFFFFFF));
1066         if (romtable_present)
1067         {
1068                 uint32_t cid0,cid1,cid2,cid3,memtype,romentry;
1069                 uint16_t entry_offset;
1070
1071                 /* bit 16 of apid indicates a memory access port */
1072                 if (dbgbase & 0x02)
1073                         command_print(cmd_ctx, "\tValid ROM table present");
1074                 else
1075                         command_print(cmd_ctx, "\tROM table in legacy format");
1076
1077                 /* Now we read ROM table ID registers, ref. ARM IHI 0029B sec  */
1078                 mem_ap_read_u32(swjdp, (dbgbase&0xFFFFF000) | 0xFF0, &cid0);
1079                 mem_ap_read_u32(swjdp, (dbgbase&0xFFFFF000) | 0xFF4, &cid1);
1080                 mem_ap_read_u32(swjdp, (dbgbase&0xFFFFF000) | 0xFF8, &cid2);
1081                 mem_ap_read_u32(swjdp, (dbgbase&0xFFFFF000) | 0xFFC, &cid3);
1082                 mem_ap_read_u32(swjdp, (dbgbase&0xFFFFF000) | 0xFCC, &memtype);
1083                 swjdp_transaction_endcheck(swjdp);
1084                 if (!is_dap_cid_ok(cid3, cid2, cid1, cid0))
1085                         command_print(cmd_ctx, "\tCID3 0x%2.2" PRIx32
1086                                         ", CID2 0x%2.2" PRIx32
1087                                         ", CID1 0x%2.2" PRIx32
1088                                         ", CID0 0x%2.2" PRIx32,
1089                                         cid3, cid2, cid1, cid0);
1090                 if (memtype & 0x01)
1091                         command_print(cmd_ctx, "\tMEMTYPE system memory present on bus");
1092                 else
1093                         command_print(cmd_ctx, "\tMEMTYPE System memory not present. "
1094                                         "Dedicated debug bus.");
1095
1096                 /* Now we read ROM table entries from dbgbase&0xFFFFF000) | 0x000 until we get 0x00000000 */
1097                 entry_offset = 0;
1098                 do
1099                 {
1100                         mem_ap_read_atomic_u32(swjdp, (dbgbase&0xFFFFF000) | entry_offset, &romentry);
1101                         command_print(cmd_ctx, "\tROMTABLE[0x%x] = 0x%" PRIx32 "",entry_offset,romentry);
1102                         if (romentry&0x01)
1103                         {
1104                                 uint32_t c_cid0, c_cid1, c_cid2, c_cid3;
1105                                 uint32_t c_pid0, c_pid1, c_pid2, c_pid3, c_pid4;
1106                                 uint32_t component_start, component_base;
1107                                 unsigned part_num;
1108                                 char *type, *full;
1109
1110                                 component_base = (uint32_t)((dbgbase & 0xFFFFF000)
1111                                                 + (int)(romentry & 0xFFFFF000));
1112                                 mem_ap_read_atomic_u32(swjdp,
1113                                                 (component_base & 0xFFFFF000) | 0xFE0, &c_pid0);
1114                                 mem_ap_read_atomic_u32(swjdp,
1115                                                 (component_base & 0xFFFFF000) | 0xFE4, &c_pid1);
1116                                 mem_ap_read_atomic_u32(swjdp,
1117                                                 (component_base & 0xFFFFF000) | 0xFE8, &c_pid2);
1118                                 mem_ap_read_atomic_u32(swjdp,
1119                                                 (component_base & 0xFFFFF000) | 0xFEC, &c_pid3);
1120                                 mem_ap_read_atomic_u32(swjdp,
1121                                                 (component_base & 0xFFFFF000) | 0xFD0, &c_pid4);
1122                                 mem_ap_read_atomic_u32(swjdp,
1123                                                 (component_base & 0xFFFFF000) | 0xFF0, &c_cid0);
1124                                 mem_ap_read_atomic_u32(swjdp,
1125                                                 (component_base & 0xFFFFF000) | 0xFF4, &c_cid1);
1126                                 mem_ap_read_atomic_u32(swjdp,
1127                                                 (component_base & 0xFFFFF000) | 0xFF8, &c_cid2);
1128                                 mem_ap_read_atomic_u32(swjdp,
1129                                                 (component_base & 0xFFFFF000) | 0xFFC, &c_cid3);
1130                                 component_start = component_base - 0x1000*(c_pid4 >> 4);
1131
1132                                 command_print(cmd_ctx, "\t\tComponent base address 0x%" PRIx32
1133                                                 ", start address 0x%" PRIx32,
1134                                                 component_base, component_start);
1135                                 command_print(cmd_ctx, "\t\tComponent class is 0x%x, %s",
1136                                                 (int) (c_cid1 >> 4) & 0xf,
1137                                                 /* See ARM IHI 0029B Table 3-3 */
1138                                                 class_description[(c_cid1 >> 4) & 0xf]);
1139
1140                                 /* CoreSight component? */
1141                                 if (((c_cid1 >> 4) & 0x0f) == 9) {
1142                                         uint32_t devtype;
1143                                         unsigned minor;
1144                                         char *major = "Reserved", *subtype = "Reserved";
1145
1146                                         mem_ap_read_atomic_u32(swjdp,
1147                                                         (component_base & 0xfffff000) | 0xfcc,
1148                                                         &devtype);
1149                                         minor = (devtype >> 4) & 0x0f;
1150                                         switch (devtype & 0x0f) {
1151                                         case 0:
1152                                                 major = "Miscellaneous";
1153                                                 switch (minor) {
1154                                                 case 0:
1155                                                         subtype = "other";
1156                                                         break;
1157                                                 case 4:
1158                                                         subtype = "Validation component";
1159                                                         break;
1160                                                 }
1161                                                 break;
1162                                         case 1:
1163                                                 major = "Trace Sink";
1164                                                 switch (minor) {
1165                                                 case 0:
1166                                                         subtype = "other";
1167                                                         break;
1168                                                 case 1:
1169                                                         subtype = "Port";
1170                                                         break;
1171                                                 case 2:
1172                                                         subtype = "Buffer";
1173                                                         break;
1174                                                 }
1175                                                 break;
1176                                         case 2:
1177                                                 major = "Trace Link";
1178                                                 switch (minor) {
1179                                                 case 0:
1180                                                         subtype = "other";
1181                                                         break;
1182                                                 case 1:
1183                                                         subtype = "Funnel, router";
1184                                                         break;
1185                                                 case 2:
1186                                                         subtype = "Filter";
1187                                                         break;
1188                                                 case 3:
1189                                                         subtype = "FIFO, buffer";
1190                                                         break;
1191                                                 }
1192                                                 break;
1193                                         case 3:
1194                                                 major = "Trace Source";
1195                                                 switch (minor) {
1196                                                 case 0:
1197                                                         subtype = "other";
1198                                                         break;
1199                                                 case 1:
1200                                                         subtype = "Processor";
1201                                                         break;
1202                                                 case 2:
1203                                                         subtype = "DSP";
1204                                                         break;
1205                                                 case 3:
1206                                                         subtype = "Engine/Coprocessor";
1207                                                         break;
1208                                                 case 4:
1209                                                         subtype = "Bus";
1210                                                         break;
1211                                                 }
1212                                                 break;
1213                                         case 4:
1214                                                 major = "Debug Control";
1215                                                 switch (minor) {
1216                                                 case 0:
1217                                                         subtype = "other";
1218                                                         break;
1219                                                 case 1:
1220                                                         subtype = "Trigger Matrix";
1221                                                         break;
1222                                                 case 2:
1223                                                         subtype = "Debug Auth";
1224                                                         break;
1225                                                 }
1226                                                 break;
1227                                         case 5:
1228                                                 major = "Debug Logic";
1229                                                 switch (minor) {
1230                                                 case 0:
1231                                                         subtype = "other";
1232                                                         break;
1233                                                 case 1:
1234                                                         subtype = "Processor";
1235                                                         break;
1236                                                 case 2:
1237                                                         subtype = "DSP";
1238                                                         break;
1239                                                 case 3:
1240                                                         subtype = "Engine/Coprocessor";
1241                                                         break;
1242                                                 }
1243                                                 break;
1244                                         }
1245                                         command_print(cmd_ctx, "\t\tType is 0x%2.2x, %s, %s",
1246                                                         (unsigned) (devtype & 0xff),
1247                                                         major, subtype);
1248                                         /* REVISIT also show 0xfc8 DevId */
1249                                 }
1250
1251                                 if (!is_dap_cid_ok(cid3, cid2, cid1, cid0))
1252                                         command_print(cmd_ctx, "\t\tCID3 0x%2.2" PRIx32
1253                                                         ", CID2 0x%2.2" PRIx32
1254                                                         ", CID1 0x%2.2" PRIx32
1255                                                         ", CID0 0x%2.2" PRIx32,
1256                                                         c_cid3, c_cid2, c_cid1, c_cid0);
1257                                 command_print(cmd_ctx, "\t\tPeripheral ID[4..0] = hex "
1258                                                 "%2.2x %2.2x %2.2x %2.2x %2.2x",
1259                                                 (int) c_pid4,
1260                                                 (int) c_pid3, (int) c_pid2,
1261                                                 (int) c_pid1, (int) c_pid0);
1262
1263                                 /* Part number interpretations are from Cortex
1264                                  * core specs, the CoreSight components TRM
1265                                  * (ARM DDI 0314H), and ETM specs; also from
1266                                  * chip observation (e.g. TI SDTI).
1267                                  */
1268                                 part_num = c_pid0 & 0xff;
1269                                 part_num |= (c_pid1 & 0x0f) << 8;
1270                                 switch (part_num) {
1271                                 case 0x000:
1272                                         type = "Cortex-M3 NVIC";
1273                                         full = "(Interrupt Controller)";
1274                                         break;
1275                                 case 0x001:
1276                                         type = "Cortex-M3 ITM";
1277                                         full = "(Instrumentation Trace Module)";
1278                                         break;
1279                                 case 0x002:
1280                                         type = "Cortex-M3 DWT";
1281                                         full = "(Data Watchpoint and Trace)";
1282                                         break;
1283                                 case 0x003:
1284                                         type = "Cortex-M3 FBP";
1285                                         full = "(Flash Patch and Breakpoint)";
1286                                         break;
1287                                 case 0x00d:
1288                                         type = "CoreSight ETM11";
1289                                         full = "(Embedded Trace)";
1290                                         break;
1291                                 // case 0x113: what?
1292                                 case 0x120:             /* from OMAP3 memmap */
1293                                         type = "TI SDTI";
1294                                         full = "(System Debug Trace Interface)";
1295                                         break;
1296                                 case 0x343:             /* from OMAP3 memmap */
1297                                         type = "TI DAPCTL";
1298                                         full = "";
1299                                         break;
1300                                 case 0x4e0:
1301                                         type = "Cortex-M3 ETM";
1302                                         full = "(Embedded Trace)";
1303                                         break;
1304                                 case 0x906:
1305                                         type = "Coresight CTI";
1306                                         full = "(Cross Trigger)";
1307                                         break;
1308                                 case 0x907:
1309                                         type = "Coresight ETB";
1310                                         full = "(Trace Buffer)";
1311                                         break;
1312                                 case 0x908:
1313                                         type = "Coresight CSTF";
1314                                         full = "(Trace Funnel)";
1315                                         break;
1316                                 case 0x910:
1317                                         type = "CoreSight ETM9";
1318                                         full = "(Embedded Trace)";
1319                                         break;
1320                                 case 0x912:
1321                                         type = "Coresight TPIU";
1322                                         full = "(Trace Port Interface Unit)";
1323                                         break;
1324                                 case 0x921:
1325                                         type = "Cortex-A8 ETM";
1326                                         full = "(Embedded Trace)";
1327                                         break;
1328                                 case 0x922:
1329                                         type = "Cortex-A8 CTI";
1330                                         full = "(Cross Trigger)";
1331                                         break;
1332                                 case 0x923:
1333                                         type = "Cortex-M3 TPIU";
1334                                         full = "(Trace Port Interface Unit)";
1335                                         break;
1336                                 case 0xc08:
1337                                         type = "Cortex-A8 Debug";
1338                                         full = "(Debug Unit)";
1339                                         break;
1340                                 default:
1341                                         type = "-*- unrecognized -*-";
1342                                         full = "";
1343                                         break;
1344                                 }
1345                                 command_print(cmd_ctx, "\t\tPart is %s %s",
1346                                                 type, full);
1347                         }
1348                         else
1349                         {
1350                                 if (romentry)
1351                                         command_print(cmd_ctx, "\t\tComponent not present");
1352                                 else
1353                                         command_print(cmd_ctx, "\t\tEnd of ROM table");
1354                         }
1355                         entry_offset += 4;
1356                 } while (romentry > 0);
1357         }
1358         else
1359         {
1360                 command_print(cmd_ctx, "\tNo ROM table present");
1361         }
1362         dap_ap_select(swjdp, apselold);
1363
1364         return ERROR_OK;
1365 }
1366
1367 DAP_COMMAND_HANDLER(dap_baseaddr_command)
1368 {
1369         uint32_t apsel, apselsave, baseaddr;
1370         int retval;
1371
1372         apselsave = swjdp->apsel;
1373         switch (argc) {
1374         case 0:
1375                 apsel = swjdp->apsel;
1376                 break;
1377         case 1:
1378                 COMMAND_PARSE_NUMBER(u32, args[0], apsel);
1379                 break;
1380         default:
1381                 return ERROR_COMMAND_SYNTAX_ERROR;
1382         }
1383
1384         if (apselsave != apsel)
1385                 dap_ap_select(swjdp, apsel);
1386
1387         dap_ap_read_reg_u32(swjdp, 0xF8, &baseaddr);
1388         retval = swjdp_transaction_endcheck(swjdp);
1389         command_print(cmd_ctx, "0x%8.8" PRIx32, baseaddr);
1390
1391         if (apselsave != apsel)
1392                 dap_ap_select(swjdp, apselsave);
1393
1394         return retval;
1395 }
1396
1397 DAP_COMMAND_HANDLER(dap_memaccess_command)
1398 {
1399         uint32_t memaccess_tck;
1400
1401         switch (argc) {
1402         case 0:
1403                 memaccess_tck = swjdp->memaccess_tck;
1404                 break;
1405         case 1:
1406                 COMMAND_PARSE_NUMBER(u32, args[0], memaccess_tck);
1407                 break;
1408         default:
1409                 return ERROR_COMMAND_SYNTAX_ERROR;
1410         }
1411         swjdp->memaccess_tck = memaccess_tck;
1412
1413         command_print(cmd_ctx, "memory bus access delay set to %" PRIi32 " tck",
1414                         swjdp->memaccess_tck);
1415
1416         return ERROR_OK;
1417 }
1418
1419 DAP_COMMAND_HANDLER(dap_apsel_command)
1420 {
1421         uint32_t apsel, apid;
1422         int retval;
1423
1424         switch (argc) {
1425         case 0:
1426                 apsel = 0;
1427                 break;
1428         case 1:
1429                 COMMAND_PARSE_NUMBER(u32, args[0], apsel);
1430                 break;
1431         default:
1432                 return ERROR_COMMAND_SYNTAX_ERROR;
1433         }
1434
1435         dap_ap_select(swjdp, apsel);
1436         dap_ap_read_reg_u32(swjdp, 0xFC, &apid);
1437         retval = swjdp_transaction_endcheck(swjdp);
1438         command_print(cmd_ctx, "ap %" PRIi32 " selected, identification register 0x%8.8" PRIx32,
1439                         apsel, apid);
1440
1441         return retval;
1442 }
1443
1444 DAP_COMMAND_HANDLER(dap_apid_command)
1445 {
1446         uint32_t apsel, apselsave, apid;
1447         int retval;
1448
1449         apselsave = swjdp->apsel;
1450         switch (argc) {
1451         case 0:
1452                 apsel = swjdp->apsel;
1453                 break;
1454         case 1:
1455                 COMMAND_PARSE_NUMBER(u32, args[0], apsel);
1456                 break;
1457         default:
1458                 return ERROR_COMMAND_SYNTAX_ERROR;
1459         }
1460
1461         if (apselsave != apsel)
1462                 dap_ap_select(swjdp, apsel);
1463
1464         dap_ap_read_reg_u32(swjdp, 0xFC, &apid);
1465         retval = swjdp_transaction_endcheck(swjdp);
1466         command_print(cmd_ctx, "0x%8.8" PRIx32, apid);
1467         if (apselsave != apsel)
1468                 dap_ap_select(swjdp, apselsave);
1469
1470         return retval;
1471 }