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 0316A                               *
  33  * Cortex-M3(tm) TRM, ARM DDI 0337C                                        *
  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  *                                                                         *
  57  * DPACC and APACC scanchain access through JTAG-DP                        *
  58  *                                                                         *
  59 ***************************************************************************/
  60
  61 /* Scan out and in from target ordered u8 buffers */
  62 int adi_jtag_dp_scan(arm_jtag_t *jtag_info, u8 instr, u8 reg_addr, u8 RnW, u8 *outvalue, u8 *invalue, u8 *ack)
  63 {
  64         scan_field_t fields[2];
  65         u8 out_addr_buf;
  66
  67         jtag_add_end_state(TAP_IDLE);
  68         arm_jtag_set_instr(jtag_info, instr, NULL);
  69
  70         fields[0].tap = jtag_info->tap;
  71         fields[0].num_bits = 3;
  72         buf_set_u32(&out_addr_buf, 0, 3, ((reg_addr >> 1) & 0x6) | (RnW & 0x1));
  73         fields[0].out_value = &out_addr_buf;
  74
  75         fields[0].in_value = ack;
  76
  77
  78
  79
  80
  81         fields[1].tap = jtag_info->tap;
  82         fields[1].num_bits = 32;
  83         fields[1].out_value = outvalue;
  84
  85         fields[1].in_value = invalue;
  86
  87
  88
  89
  90
  91         jtag_add_dr_scan(2, fields, TAP_INVALID);
  92
  93         return ERROR_OK;
  94 }
  95
  96 /* Scan out and in from host ordered u32 variables */
  97 int adi_jtag_dp_scan_u32(arm_jtag_t *jtag_info, u8 instr, u8 reg_addr, u8 RnW, u32 outvalue, u32 *invalue, u8 *ack)
  98 {
  99         scan_field_t fields[2];
 100         u8 out_value_buf[4];
 101         u8 out_addr_buf;
 102
 103         jtag_add_end_state(TAP_IDLE);
 104         arm_jtag_set_instr(jtag_info, instr, NULL);
 105
 106         fields[0].tap = jtag_info->tap;
 107         fields[0].num_bits = 3;
 108         buf_set_u32(&out_addr_buf, 0, 3, ((reg_addr >> 1) & 0x6) | (RnW & 0x1));
 109         fields[0].out_value = &out_addr_buf;
 110         fields[0].in_value = ack;
 111
 112
 113
 114         fields[1].tap = jtag_info->tap;
 115         fields[1].num_bits = 32;
 116         buf_set_u32(out_value_buf, 0, 32, outvalue);
 117         fields[1].out_value = out_value_buf;
 118         fields[1].in_value = NULL;
 119
 120
 121         if (invalue)
 122         {
 123                 fields[1].in_value = (u8 *)invalue;
 124                 jtag_add_dr_scan(2, fields, TAP_INVALID);
 125
 126                 jtag_add_callback(arm_le_to_h_u32, (u8 *)invalue);
 127         } else
 128         {
 129
 130                 jtag_add_dr_scan(2, fields, TAP_INVALID);
 131         }
 132
 133         return ERROR_OK;
 134 }
 135
 136 /* scan_inout_check adds one extra inscan for DPAP_READ commands to read variables */
 137 int scan_inout_check(swjdp_common_t *swjdp, u8 instr, u8 reg_addr, u8 RnW, u8 *outvalue, u8 *invalue)
 138 {
 139         adi_jtag_dp_scan(swjdp->jtag_info, instr, reg_addr, RnW, outvalue, NULL, NULL);
 140         if ((RnW == DPAP_READ) && (invalue != NULL))
 141         {
 142                 adi_jtag_dp_scan(swjdp->jtag_info, SWJDP_IR_DPACC, DP_RDBUFF, DPAP_READ, 0, invalue, &swjdp->ack);
 143         }
 144
 145         /* In TRANS_MODE_ATOMIC all SWJDP_IR_APACC transactions wait for ack=OK/FAULT and the check CTRL_STAT */
 146         if ((instr == SWJDP_IR_APACC) && (swjdp->trans_mode == TRANS_MODE_ATOMIC))
 147         {
 148                 return swjdp_transaction_endcheck(swjdp);
 149         }
 150
 151         return ERROR_OK;
 152 }
 153
 154 int scan_inout_check_u32(swjdp_common_t *swjdp, u8 instr, u8 reg_addr, u8 RnW, u32 outvalue, u32 *invalue)
 155 {
 156         adi_jtag_dp_scan_u32(swjdp->jtag_info, instr, reg_addr, RnW, outvalue, NULL, NULL);
 157         if ((RnW==DPAP_READ) && (invalue != NULL))
 158         {
 159                 adi_jtag_dp_scan_u32(swjdp->jtag_info, SWJDP_IR_DPACC, DP_RDBUFF, DPAP_READ, 0, invalue, &swjdp->ack);
 160         }
 161
 162         /* In TRANS_MODE_ATOMIC all SWJDP_IR_APACC transactions wait for ack=OK/FAULT and then check CTRL_STAT */
 163         if ((instr == SWJDP_IR_APACC) && (swjdp->trans_mode == TRANS_MODE_ATOMIC))
 164         {
 165                 return swjdp_transaction_endcheck(swjdp);
 166         }
 167
 168         return ERROR_OK;
 169 }
 170
 171 int swjdp_transaction_endcheck(swjdp_common_t *swjdp)
 172 {
 173         int retval;
 174         u32 ctrlstat;
 175
 176         /* too expensive to call keep_alive() here */
 177
 178 #if 0
 179         /* Danger!!!! BROKEN!!!! */
 180         scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, DP_CTRL_STAT, DPAP_READ, 0, &ctrlstat);
 181         /* Danger!!!! BROKEN!!!! Why will jtag_execute_queue() fail here????
 182         R956 introduced the check on return value here and now Michael Schwingen reports
 183         that this code no longer works....
 184
 185         https://lists.berlios.de/pipermail/openocd-development/2008-September/003107.html
 186         */
 187         if ((retval=jtag_execute_queue())!=ERROR_OK)
 188         {
 189                 LOG_ERROR("BUG: Why does this fail the first time????");
 190         }
 191         /* Why??? second time it works??? */
 192 #endif
 193
 194         scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, DP_CTRL_STAT, DPAP_READ, 0, &ctrlstat);
 195         if ((retval=jtag_execute_queue())!=ERROR_OK)
 196                 return retval;
 197
 198         swjdp->ack = swjdp->ack & 0x7;
 199
 200         if (swjdp->ack != 2)
 201         {
 202                 long long then=timeval_ms();
 203                 while (swjdp->ack != 2)
 204                 {
 205                         if (swjdp->ack == 1)
 206                         {
 207                                 if ((timeval_ms()-then) > 1000)
 208                                 {
 209                                         LOG_WARNING("Timeout (1000ms) waiting for ACK = OK/FAULT in SWJDP transaction");
 210                                         return ERROR_JTAG_DEVICE_ERROR;
 211                                 }
 212                         }
 213                         else
 214                         {
 215                                 LOG_WARNING("Invalid ACK in SWJDP transaction");
 216                                 return ERROR_JTAG_DEVICE_ERROR;
 217                         }
 218
 219                         scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, DP_CTRL_STAT, DPAP_READ, 0, &ctrlstat);
 220                         if ((retval=jtag_execute_queue())!=ERROR_OK)
 221                                 return retval;
 222                         swjdp->ack = swjdp->ack & 0x7;
 223                 }
 224         } else
 225         {
 226                 /* common code path avoids fn to timeval_ms() */
 227         }
 228
 229         /* Check for STICKYERR and STICKYORUN */
 230         if (ctrlstat & (SSTICKYORUN | SSTICKYERR))
 231         {
 232                 LOG_DEBUG("swjdp: CTRL/STAT error 0x%x", ctrlstat);
 233                 /* Check power to debug regions */
 234                 if ((ctrlstat & 0xf0000000) != 0xf0000000)
 235                 {
 236                          ahbap_debugport_init(swjdp);
 237                 }
 238                 else
 239                 {
 240                         u32 mem_ap_csw, mem_ap_tar;
 241
 242                         /* Print information about last AHBAP access */
 243                         LOG_ERROR("AHBAP Cached values: dp_select 0x%x, ap_csw 0x%x, ap_tar 0x%x", swjdp->dp_select_value, swjdp->ap_csw_value, swjdp->ap_tar_value);
 244                         if (ctrlstat & SSTICKYORUN)
 245                                 LOG_ERROR("SWJ-DP OVERRUN - check clock or reduce jtag speed");
 246
 247                         if (ctrlstat & SSTICKYERR)
 248                                 LOG_ERROR("SWJ-DP STICKY ERROR");
 249
 250                         /* Clear Sticky Error Bits */
 251                         scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, DP_CTRL_STAT, DPAP_WRITE, swjdp->dp_ctrl_stat | SSTICKYORUN | SSTICKYERR, NULL);
 252                         scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, DP_CTRL_STAT, DPAP_READ, 0, &ctrlstat);
 253                         if ((retval=jtag_execute_queue())!=ERROR_OK)
 254                                 return retval;
 255
 256                         LOG_DEBUG("swjdp: status 0x%x", ctrlstat);
 257
 258                         dap_ap_read_reg_u32(swjdp, AP_REG_CSW, &mem_ap_csw);
 259                         dap_ap_read_reg_u32(swjdp, AP_REG_TAR, &mem_ap_tar);
 260                         if ((retval=jtag_execute_queue())!=ERROR_OK)
 261                                 return retval;
 262                         LOG_ERROR("Read MEM_AP_CSW 0x%x, MEM_AP_TAR 0x%x", mem_ap_csw, mem_ap_tar);
 263
 264                 }
 265                 if ((retval=jtag_execute_queue())!=ERROR_OK)
 266                         return retval;
 267                 return ERROR_JTAG_DEVICE_ERROR;
 268         }
 269
 270         return ERROR_OK;
 271 }
 272
 273 /***************************************************************************
 274  *                                                                         *
 275  * DP and MEM-AP  register access  through APACC and DPACC                 *
 276  *                                                                         *
 277 ***************************************************************************/
 278
 279 int dap_dp_write_reg(swjdp_common_t *swjdp, u32 value, u8 reg_addr)
 280 {
 281         return scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, reg_addr, DPAP_WRITE, value, NULL);
 282 }
 283
 284 int dap_dp_read_reg(swjdp_common_t *swjdp, u32 *value, u8 reg_addr)
 285 {
 286         return scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, reg_addr, DPAP_READ, 0, value);
 287 }
 288
 289 int dap_ap_select(swjdp_common_t *swjdp,u8 apsel)
 290 {
 291         u32 select;
 292         select = (apsel<<24) & 0xFF000000;
 293
 294         if (select != swjdp->apsel)
 295         {
 296                 swjdp->apsel = select;
 297                 /* Switchin AP invalidates cached values */
 298                 swjdp->dp_select_value = -1;
 299                 swjdp->ap_csw_value = -1;
 300                 swjdp->ap_tar_value = -1;
 301         }
 302
 303         return ERROR_OK;
 304 }
 305
 306 int dap_dp_bankselect(swjdp_common_t *swjdp,u32 ap_reg)
 307 {
 308         u32 select;
 309         select = (ap_reg & 0x000000F0);
 310
 311         if (select != swjdp->dp_select_value)
 312         {
 313                 dap_dp_write_reg(swjdp, select | swjdp->apsel, DP_SELECT);
 314                 swjdp->dp_select_value = select;
 315         }
 316
 317         return ERROR_OK;
 318 }
 319
 320 int dap_ap_write_reg(swjdp_common_t *swjdp, u32 reg_addr, u8* out_value_buf)
 321 {
 322         dap_dp_bankselect(swjdp, reg_addr);
 323         scan_inout_check(swjdp, SWJDP_IR_APACC, reg_addr, DPAP_WRITE, out_value_buf, NULL);
 324
 325         return ERROR_OK;
 326 }
 327
 328 int dap_ap_read_reg(swjdp_common_t *swjdp, u32 reg_addr, u8 *in_value_buf)
 329 {
 330         dap_dp_bankselect(swjdp, reg_addr);
 331         scan_inout_check(swjdp, SWJDP_IR_APACC, reg_addr, DPAP_READ, 0, in_value_buf);
 332
 333         return ERROR_OK;
 334 }
 335 int dap_ap_write_reg_u32(swjdp_common_t *swjdp, u32 reg_addr, u32 value)
 336 {
 337         u8 out_value_buf[4];
 338
 339         buf_set_u32(out_value_buf, 0, 32, value);
 340         dap_dp_bankselect(swjdp, reg_addr);
 341         scan_inout_check(swjdp, SWJDP_IR_APACC, reg_addr, DPAP_WRITE, out_value_buf, NULL);
 342
 343         return ERROR_OK;
 344 }
 345
 346 int dap_ap_read_reg_u32(swjdp_common_t *swjdp, u32 reg_addr, u32 *value)
 347 {
 348         dap_dp_bankselect(swjdp, reg_addr);
 349         scan_inout_check_u32(swjdp, SWJDP_IR_APACC, reg_addr, DPAP_READ, 0, value);
 350
 351         return ERROR_OK;
 352 }
 353
 354 /***************************************************************************
 355  *                                                                         *
 356  * AHB-AP access to memory and system registers on AHB bus                 *
 357  *                                                                         *
 358 ***************************************************************************/
 359
 360 int dap_setup_accessport(swjdp_common_t *swjdp, u32 csw, u32 tar)
 361 {
 362         csw = csw | CSW_DBGSWENABLE | CSW_MASTER_DEBUG | CSW_HPROT;
 363         if (csw != swjdp->ap_csw_value)
 364         {
 365                 /* LOG_DEBUG("swjdp : Set CSW %x",csw); */
 366                 dap_ap_write_reg_u32(swjdp, AP_REG_CSW, csw );
 367                 swjdp->ap_csw_value = csw;
 368         }
 369         if (tar != swjdp->ap_tar_value)
 370         {
 371                 /* LOG_DEBUG("swjdp : Set TAR %x",tar); */
 372                 dap_ap_write_reg_u32(swjdp, AP_REG_TAR, tar );
 373                 swjdp->ap_tar_value = tar;
 374         }
 375         if (csw & CSW_ADDRINC_MASK)
 376         {
 377                 /* Do not cache TAR value when autoincrementing */
 378                 swjdp->ap_tar_value = -1;
 379         }
 380         return ERROR_OK;
 381 }
 382
 383 /*****************************************************************************
 384 *                                                                            *
 385 * mem_ap_read_u32(swjdp_common_t *swjdp, u32 address, u32 *value)      *
 386 *                                                                            *
 387 * Read a u32 value from memory or system register                            *
 388 * Functionally equivalent to target_read_u32(target, address, u32 *value),   *
 389 * but with less overhead                                                     *
 390 *****************************************************************************/
 391 int mem_ap_read_u32(swjdp_common_t *swjdp, u32 address, u32 *value)
 392 {
 393         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 394
 395         dap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, address & 0xFFFFFFF0);
 396         dap_ap_read_reg_u32(swjdp, AP_REG_BD0 | (address & 0xC), value );
 397
 398         return ERROR_OK;
 399 }
 400
 401 int mem_ap_read_atomic_u32(swjdp_common_t *swjdp, u32 address, u32 *value)
 402 {
 403         mem_ap_read_u32(swjdp, address, value);
 404
 405         return swjdp_transaction_endcheck(swjdp);
 406 }
 407
 408 /*****************************************************************************
 409 *                                                                            *
 410 * mem_ap_write_u32(swjdp_common_t *swjdp, u32 address, u32 value)      *
 411 *                                                                            *
 412 * Write a u32 value to memory or memory mapped register                              *
 413 *                                                                            *
 414 *****************************************************************************/
 415 int mem_ap_write_u32(swjdp_common_t *swjdp, u32 address, u32 value)
 416 {
 417         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 418
 419         dap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, address & 0xFFFFFFF0);
 420         dap_ap_write_reg_u32(swjdp, AP_REG_BD0 | (address & 0xC), value );
 421
 422         return ERROR_OK;
 423 }
 424
 425 int mem_ap_write_atomic_u32(swjdp_common_t *swjdp, u32 address, u32 value)
 426 {
 427         mem_ap_write_u32(swjdp, address, value);
 428
 429         return swjdp_transaction_endcheck(swjdp);
 430 }
 431
 432 /*****************************************************************************
 433 *                                                                            *
 434 * mem_ap_write_buf(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address) *
 435 *                                                                            *
 436 * Write a buffer in target order (little endian)                             *
 437 *                                                                            *
 438 *****************************************************************************/
 439 int mem_ap_write_buf_u32(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 440 {
 441         int wcount, blocksize, writecount, errorcount = 0, retval = ERROR_OK;
 442         u32 adr = address;
 443         u8* pBuffer = buffer;
 444
 445         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 446
 447         count >>= 2;
 448         wcount = count;
 449
 450         /* if we have an unaligned access - reorder data */
 451         if (adr & 0x3u)
 452         {
 453                 for (writecount = 0; writecount < count; writecount++)
 454                 {
 455                         int i;
 456                         u32 outvalue;
 457                         memcpy(&outvalue, pBuffer, sizeof(u32));
 458
 459                         for (i = 0; i < 4; i++ )
 460                         {
 461                                 *((u8*)pBuffer + (adr & 0x3)) = outvalue;
 462                                 outvalue >>= 8;
 463                                 adr++;
 464                         }
 465                         pBuffer += sizeof(u32);
 466                 }
 467         }
 468
 469         while (wcount > 0)
 470         {
 471                 /* Adjust to write blocks within 4K aligned boundaries */
 472                 blocksize = (0x1000 - (0xFFF & address)) >> 2;
 473                 if (wcount < blocksize)
 474                         blocksize = wcount;
 475
 476                 /* handle unaligned data at 4k boundary */
 477                 if (blocksize == 0)
 478                         blocksize = 1;
 479
 480                 dap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_SINGLE, address);
 481
 482                 for (writecount = 0; writecount < blocksize; writecount++)
 483                 {
 484                         dap_ap_write_reg(swjdp, AP_REG_DRW, buffer + 4 * writecount );
 485                 }
 486
 487                 if (swjdp_transaction_endcheck(swjdp) == ERROR_OK)
 488                 {
 489                         wcount = wcount - blocksize;
 490                         address = address + 4 * blocksize;
 491                         buffer = buffer + 4 * blocksize;
 492                 }
 493                 else
 494                 {
 495                         errorcount++;
 496                 }
 497
 498                 if (errorcount > 1)
 499                 {
 500                         LOG_WARNING("Block write error address 0x%x, wcount 0x%x", address, wcount);
 501                         return ERROR_JTAG_DEVICE_ERROR;
 502                 }
 503         }
 504
 505         return retval;
 506 }
 507
 508 int mem_ap_write_buf_packed_u16(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 509 {
 510         int retval = ERROR_OK;
 511         int wcount, blocksize, writecount, i;
 512
 513         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 514
 515         wcount = count >> 1;
 516
 517         while (wcount > 0)
 518         {
 519                 int nbytes;
 520
 521                 /* Adjust to read within 4K block boundaries */
 522                 blocksize = (0x1000 - (0xFFF & address)) >> 1;
 523
 524                 if (wcount < blocksize)
 525                         blocksize = wcount;
 526
 527                 /* handle unaligned data at 4k boundary */
 528                 if (blocksize == 0)
 529                         blocksize = 1;
 530
 531                 dap_setup_accessport(swjdp, CSW_16BIT | CSW_ADDRINC_PACKED, address);
 532                 writecount = blocksize;
 533
 534                 do
 535                 {
 536                         nbytes = MIN((writecount << 1), 4);
 537
 538                         if (nbytes < 4 )
 539                         {
 540                                 if (mem_ap_write_buf_u16(swjdp, buffer, nbytes, address) != ERROR_OK)
 541                                 {
 542                                         LOG_WARNING("Block read error address 0x%x, count 0x%x", address, count);
 543                                         return ERROR_JTAG_DEVICE_ERROR;
 544                                 }
 545
 546                                 address += nbytes >> 1;
 547                         }
 548                         else
 549                         {
 550                                 u32 outvalue;
 551                                 memcpy(&outvalue, buffer, sizeof(u32));
 552
 553                                 for (i = 0; i < nbytes; i++ )
 554                                 {
 555                                         *((u8*)buffer + (address & 0x3)) = outvalue;
 556                                         outvalue >>= 8;
 557                                         address++;
 558                                 }
 559
 560                                 memcpy(&outvalue, buffer, sizeof(u32));
 561                                 dap_ap_write_reg_u32(swjdp, AP_REG_DRW, outvalue);
 562                                 if (swjdp_transaction_endcheck(swjdp) != ERROR_OK)
 563                                 {
 564                                         LOG_WARNING("Block read error address 0x%x, count 0x%x", address, count);
 565                                         return ERROR_JTAG_DEVICE_ERROR;
 566                                 }
 567                         }
 568
 569                         buffer += nbytes >> 1;
 570                         writecount -= nbytes >> 1;
 571
 572                 } while (writecount);
 573                 wcount -= blocksize;
 574         }
 575
 576         return retval;
 577 }
 578
 579 int mem_ap_write_buf_u16(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 580 {
 581         int retval = ERROR_OK;
 582
 583         if (count >= 4)
 584                 return mem_ap_write_buf_packed_u16(swjdp, buffer, count, address);
 585
 586         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 587
 588         while (count > 0)
 589         {
 590                 dap_setup_accessport(swjdp, CSW_16BIT | CSW_ADDRINC_SINGLE, address);
 591                 u16 svalue;
 592                 memcpy(&svalue, buffer, sizeof(u16));
 593                 u32 outvalue = (u32)svalue << 8 * (address & 0x3);
 594                 dap_ap_write_reg_u32(swjdp, AP_REG_DRW, outvalue );
 595                 retval = swjdp_transaction_endcheck(swjdp);
 596                 count -= 2;
 597                 address += 2;
 598                 buffer += 2;
 599         }
 600
 601         return retval;
 602 }
 603
 604 int mem_ap_write_buf_packed_u8(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 605 {
 606         int retval = ERROR_OK;
 607         int wcount, blocksize, writecount, i;
 608
 609         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 610
 611         wcount = count;
 612
 613         while (wcount > 0)
 614         {
 615                 int nbytes;
 616
 617                 /* Adjust to read within 4K block boundaries */
 618                 blocksize = (0x1000 - (0xFFF & address));
 619
 620                 if (wcount < blocksize)
 621                         blocksize = wcount;
 622
 623                 dap_setup_accessport(swjdp, CSW_8BIT | CSW_ADDRINC_PACKED, address);
 624                 writecount = blocksize;
 625
 626                 do
 627                 {
 628                         nbytes = MIN(writecount, 4);
 629
 630                         if (nbytes < 4 )
 631                         {
 632                                 if (mem_ap_write_buf_u8(swjdp, buffer, nbytes, address) != ERROR_OK)
 633                                 {
 634                                         LOG_WARNING("Block read error address 0x%x, count 0x%x", address, count);
 635                                         return ERROR_JTAG_DEVICE_ERROR;
 636                                 }
 637
 638                                 address += nbytes;
 639                         }
 640                         else
 641                         {
 642                                 u32 outvalue;
 643                                 memcpy(&outvalue, buffer, sizeof(u32));
 644
 645                                 for (i = 0; i < nbytes; i++ )
 646                                 {
 647                                         *((u8*)buffer + (address & 0x3)) = outvalue;
 648                                         outvalue >>= 8;
 649                                         address++;
 650                                 }
 651
 652                                 memcpy(&outvalue, buffer, sizeof(u32));
 653                                 dap_ap_write_reg_u32(swjdp, AP_REG_DRW, outvalue);
 654                                 if (swjdp_transaction_endcheck(swjdp) != ERROR_OK)
 655                                 {
 656                                         LOG_WARNING("Block read error address 0x%x, count 0x%x", address, count);
 657                                         return ERROR_JTAG_DEVICE_ERROR;
 658                                 }
 659                         }
 660
 661                         buffer += nbytes;
 662                         writecount -= nbytes;
 663
 664                 } while (writecount);
 665                 wcount -= blocksize;
 666         }
 667
 668         return retval;
 669 }
 670
 671 int mem_ap_write_buf_u8(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 672 {
 673         int retval = ERROR_OK;
 674
 675         if (count >= 4)
 676                 return mem_ap_write_buf_packed_u8(swjdp, buffer, count, address);
 677
 678         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 679
 680         while (count > 0)
 681         {
 682                 dap_setup_accessport(swjdp, CSW_8BIT | CSW_ADDRINC_SINGLE, address);
 683                 u32 outvalue = (u32)*buffer << 8 * (address & 0x3);
 684                 dap_ap_write_reg_u32(swjdp, AP_REG_DRW, outvalue );
 685                 retval = swjdp_transaction_endcheck(swjdp);
 686                 count--;
 687                 address++;
 688                 buffer++;
 689         }
 690
 691         return retval;
 692 }
 693
 694 /*********************************************************************************
 695 *                                                                                *
 696 * mem_ap_read_buf_u32(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)  *
 697 *                                                                                *
 698 * Read block fast in target order (little endian) into a buffer                  *
 699 *                                                                                *
 700 **********************************************************************************/
 701 int mem_ap_read_buf_u32(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 702 {
 703         int wcount, blocksize, readcount, errorcount = 0, retval = ERROR_OK;
 704         u32 adr = address;
 705         u8* pBuffer = buffer;
 706
 707         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 708
 709         count >>= 2;
 710         wcount = count;
 711
 712         while (wcount > 0)
 713         {
 714                 /* Adjust to read within 4K block boundaries */
 715                 blocksize = (0x1000 - (0xFFF & address)) >> 2;
 716                 if (wcount < blocksize)
 717                         blocksize = wcount;
 718
 719                 /* handle unaligned data at 4k boundary */
 720                 if (blocksize == 0)
 721                         blocksize = 1;
 722
 723                 dap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_SINGLE, address);
 724
 725                 /* Scan out first read */
 726                 adi_jtag_dp_scan(swjdp->jtag_info, SWJDP_IR_APACC, AP_REG_DRW, DPAP_READ, 0, NULL, NULL);
 727                 for (readcount = 0; readcount < blocksize - 1; readcount++)
 728                 {
 729                         /* Scan out read instruction and scan in previous value */
 730                         adi_jtag_dp_scan(swjdp->jtag_info, SWJDP_IR_APACC, AP_REG_DRW, DPAP_READ, 0, buffer + 4 * readcount, &swjdp->ack);
 731                 }
 732
 733                 /* Scan in last value */
 734                 adi_jtag_dp_scan(swjdp->jtag_info, SWJDP_IR_DPACC, DP_RDBUFF, DPAP_READ, 0, buffer + 4 * readcount, &swjdp->ack);
 735                 if (swjdp_transaction_endcheck(swjdp) == ERROR_OK)
 736                 {
 737                         wcount = wcount - blocksize;
 738                         address += 4 * blocksize;
 739                         buffer += 4 * blocksize;
 740                 }
 741                 else
 742                 {
 743                         errorcount++;
 744                 }
 745
 746                 if (errorcount > 1)
 747                 {
 748                         LOG_WARNING("Block read error address 0x%x, count 0x%x", address, count);
 749                         return ERROR_JTAG_DEVICE_ERROR;
 750                 }
 751         }
 752
 753         /* if we have an unaligned access - reorder data */
 754         if (adr & 0x3u)
 755         {
 756                 for (readcount = 0; readcount < count; readcount++)
 757                 {
 758                         int i;
 759                         u32 data;
 760                         memcpy(&data, pBuffer, sizeof(u32));
 761
 762                         for (i = 0; i < 4; i++ )
 763                         {
 764                                 *((u8*)pBuffer) = (data >> 8 * (adr & 0x3));
 765                                 pBuffer++;
 766                                 adr++;
 767                         }
 768                 }
 769         }
 770
 771         return retval;
 772 }
 773
 774 int mem_ap_read_buf_packed_u16(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 775 {
 776         u32 invalue;
 777         int retval = ERROR_OK;
 778         int wcount, blocksize, readcount, i;
 779
 780         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 781
 782         wcount = count >> 1;
 783
 784         while (wcount > 0)
 785         {
 786                 int nbytes;
 787
 788                 /* Adjust to read within 4K block boundaries */
 789                 blocksize = (0x1000 - (0xFFF & address)) >> 1;
 790                 if (wcount < blocksize)
 791                         blocksize = wcount;
 792
 793                 dap_setup_accessport(swjdp, CSW_16BIT | CSW_ADDRINC_PACKED, address);
 794
 795                 /* handle unaligned data at 4k boundary */
 796                 if (blocksize == 0)
 797                         blocksize = 1;
 798                 readcount = blocksize;
 799
 800                 do
 801                 {
 802                         dap_ap_read_reg_u32(swjdp, AP_REG_DRW, &invalue );
 803                         if (swjdp_transaction_endcheck(swjdp) != ERROR_OK)
 804                         {
 805                                 LOG_WARNING("Block read error address 0x%x, count 0x%x", address, count);
 806                                 return ERROR_JTAG_DEVICE_ERROR;
 807                         }
 808
 809                         nbytes = MIN((readcount << 1), 4);
 810
 811                         for (i = 0; i < nbytes; i++ )
 812                         {
 813                                 *((u8*)buffer) = (invalue >> 8 * (address & 0x3));
 814                                 buffer++;
 815                                 address++;
 816                         }
 817
 818                         readcount -= (nbytes >> 1);
 819                 } while (readcount);
 820                 wcount -= blocksize;
 821         }
 822
 823         return retval;
 824 }
 825
 826 int mem_ap_read_buf_u16(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 827 {
 828         u32 invalue, i;
 829         int retval = ERROR_OK;
 830
 831         if (count >= 4)
 832                 return mem_ap_read_buf_packed_u16(swjdp, buffer, count, address);
 833
 834         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 835
 836         while (count > 0)
 837         {
 838                 dap_setup_accessport(swjdp, CSW_16BIT | CSW_ADDRINC_SINGLE, address);
 839                 dap_ap_read_reg_u32(swjdp, AP_REG_DRW, &invalue );
 840                 retval = swjdp_transaction_endcheck(swjdp);
 841                 if (address & 0x1)
 842                 {
 843                         for (i = 0; i < 2; i++ )
 844                         {
 845                                 *((u8*)buffer) = (invalue >> 8 * (address & 0x3));
 846                                 buffer++;
 847                                 address++;
 848                         }
 849                 }
 850                 else
 851                 {
 852                         u16 svalue = (invalue >> 8 * (address & 0x3));
 853                         memcpy(buffer, &svalue, sizeof(u16));
 854                         address += 2;
 855                         buffer += 2;
 856                 }
 857                 count -= 2;
 858         }
 859
 860         return retval;
 861 }
 862
 863 /* FIX!!! is this a potential performance bottleneck w.r.t. requiring too many
 864  * roundtrips when jtag_execute_queue() has a large overhead(e.g. for USB)s?
 865  *
 866  * The solution is to arrange for a large out/in scan in this loop and
 867  * and convert data afterwards.
 868  */
 869 int mem_ap_read_buf_packed_u8(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 870 {
 871         u32 invalue;
 872         int retval = ERROR_OK;
 873         int wcount, blocksize, readcount, i;
 874
 875         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 876
 877         wcount = count;
 878
 879         while (wcount > 0)
 880         {
 881                 int nbytes;
 882
 883                 /* Adjust to read within 4K block boundaries */
 884                 blocksize = (0x1000 - (0xFFF & address));
 885
 886                 if (wcount < blocksize)
 887                         blocksize = wcount;
 888
 889                 dap_setup_accessport(swjdp, CSW_8BIT | CSW_ADDRINC_PACKED, address);
 890                 readcount = blocksize;
 891
 892                 do
 893                 {
 894                         dap_ap_read_reg_u32(swjdp, AP_REG_DRW, &invalue );
 895                         if (swjdp_transaction_endcheck(swjdp) != ERROR_OK)
 896                         {
 897                                 LOG_WARNING("Block read error address 0x%x, count 0x%x", address, count);
 898                                 return ERROR_JTAG_DEVICE_ERROR;
 899                         }
 900
 901                         nbytes = MIN(readcount, 4);
 902
 903                         for (i = 0; i < nbytes; i++ )
 904                         {
 905                                 *((u8*)buffer) = (invalue >> 8 * (address & 0x3));
 906                                 buffer++;
 907                                 address++;
 908                         }
 909
 910                         readcount -= nbytes;
 911                 } while (readcount);
 912                 wcount -= blocksize;
 913         }
 914
 915         return retval;
 916 }
 917
 918 int mem_ap_read_buf_u8(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 919 {
 920         u32 invalue;
 921         int retval = ERROR_OK;
 922
 923         if (count >= 4)
 924                 return mem_ap_read_buf_packed_u8(swjdp, buffer, count, address);
 925
 926         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 927
 928         while (count > 0)
 929         {
 930                 dap_setup_accessport(swjdp, CSW_8BIT | CSW_ADDRINC_SINGLE, address);
 931                 dap_ap_read_reg_u32(swjdp, AP_REG_DRW, &invalue );
 932                 retval = swjdp_transaction_endcheck(swjdp);
 933                 *((u8*)buffer) = (invalue >> 8 * (address & 0x3));
 934                 count--;
 935                 address++;
 936                 buffer++;
 937         }
 938
 939         return retval;
 940 }
 941
 942 int ahbap_debugport_init(swjdp_common_t *swjdp)
 943 {
 944         u32 idreg, romaddr, dummy;
 945         u32 ctrlstat;
 946         int cnt = 0;
 947         int retval;
 948
 949         LOG_DEBUG(" ");
 950
 951         swjdp->apsel = 0;
 952         swjdp->ap_csw_value = -1;
 953         swjdp->ap_tar_value = -1;
 954         swjdp->trans_mode = TRANS_MODE_ATOMIC;
 955         dap_dp_read_reg(swjdp, &dummy, DP_CTRL_STAT);
 956         dap_dp_write_reg(swjdp, SSTICKYERR, DP_CTRL_STAT);
 957         dap_dp_read_reg(swjdp, &dummy, DP_CTRL_STAT);
 958
 959         swjdp->dp_ctrl_stat = CDBGPWRUPREQ | CSYSPWRUPREQ;
 960
 961         dap_dp_write_reg(swjdp, swjdp->dp_ctrl_stat, DP_CTRL_STAT);
 962         dap_dp_read_reg(swjdp, &ctrlstat, DP_CTRL_STAT);
 963         if ((retval=jtag_execute_queue())!=ERROR_OK)
 964                 return retval;
 965
 966         /* Check that we have debug power domains activated */
 967         while (!(ctrlstat & CDBGPWRUPACK) && (cnt++ < 10))
 968         {
 969                 LOG_DEBUG("swjdp: wait CDBGPWRUPACK");
 970                 dap_dp_read_reg(swjdp, &ctrlstat, DP_CTRL_STAT);
 971                 if ((retval=jtag_execute_queue())!=ERROR_OK)
 972                         return retval;
 973                 alive_sleep(10);
 974         }
 975
 976         while (!(ctrlstat & CSYSPWRUPACK) && (cnt++ < 10))
 977         {
 978                 LOG_DEBUG("swjdp: wait CSYSPWRUPACK");
 979                 dap_dp_read_reg(swjdp, &ctrlstat, DP_CTRL_STAT);
 980                 if ((retval=jtag_execute_queue())!=ERROR_OK)
 981                         return retval;
 982                 alive_sleep(10);
 983         }
 984
 985         dap_dp_read_reg(swjdp, &dummy, DP_CTRL_STAT);
 986         /* With debug power on we can activate OVERRUN checking */
 987         swjdp->dp_ctrl_stat = CDBGPWRUPREQ | CSYSPWRUPREQ | CORUNDETECT;
 988         dap_dp_write_reg(swjdp, swjdp->dp_ctrl_stat, DP_CTRL_STAT);
 989         dap_dp_read_reg(swjdp, &dummy, DP_CTRL_STAT);
 990
 991         dap_ap_read_reg_u32(swjdp, 0xFC, &idreg);
 992         dap_ap_read_reg_u32(swjdp, 0xF8, &romaddr);
 993
 994         LOG_DEBUG("AHB-AP ID Register 0x%x, Debug ROM Address 0x%x", idreg, romaddr);
 995
 996         return ERROR_OK;
 997 }
 998
 999
1000 char * class_description[16] ={
1001         "Reserved",
1002         "ROM table","Reserved","Reserved","Reserved","Reserved","Reserved","Reserved","Reserved",
1003         "CoreSight component","Reserved","Peripheral Test Block","Reserved","DESS","Generic IP component","Non standard layout"};
1004
1005 int dap_info_command(struct command_context_s *cmd_ctx, swjdp_common_t *swjdp, int apsel)
1006 {
1007
1008         u32 dbgbase,apid;
1009         int romtable_present = 0;
1010         u8 mem_ap;
1011         u32 apselold;
1012
1013         apselold = swjdp->apsel;
1014         dap_ap_select(swjdp, apsel);
1015         dap_ap_read_reg_u32(swjdp, 0xF8, &dbgbase);
1016         dap_ap_read_reg_u32(swjdp, 0xFC, &apid);
1017         swjdp_transaction_endcheck(swjdp);
1018         /* Now we read ROM table ID registers, ref. ARM IHI 0029B sec  */
1019         mem_ap = ((apid&0x10000)&&((apid&0x0F)!=0));
1020         command_print(cmd_ctx, "ap identification register 0x%8.8x", apid);
1021         if (apid)
1022         {
1023                 switch (apid&0x0F)
1024                 {
1025                         case 0:
1026                                 command_print(cmd_ctx, "\tType is jtag-ap");
1027                                 break;
1028                         case 1:
1029                                 command_print(cmd_ctx, "\tType is mem-ap AHB");
1030                                 break;
1031                         case 2:
1032                                 command_print(cmd_ctx, "\tType is mem-ap APB");
1033                                 break;
1034                         default:
1035                                 command_print(cmd_ctx, "\tUnknown AP-type");
1036                         break;
1037                 }
1038                 command_print(cmd_ctx, "ap debugbase 0x%8.8x", dbgbase);
1039         }
1040         else
1041         {
1042                 command_print(cmd_ctx, "No AP found at this apsel 0x%x", apsel);
1043         }
1044
1045         romtable_present = ((mem_ap)&&(dbgbase != 0xFFFFFFFF));
1046         if (romtable_present)
1047         {
1048                 u32 cid0,cid1,cid2,cid3,memtype,romentry;
1049                 u16 entry_offset;
1050                 /* bit 16 of apid indicates a memory access port */
1051                 if (dbgbase&0x02)
1052                 {
1053                         command_print(cmd_ctx, "\tValid ROM table present");
1054                 }
1055                 else
1056                 {
1057                         command_print(cmd_ctx, "\tROM table in legacy format" );
1058                 }
1059                 /* Now we read ROM table ID registers, ref. ARM IHI 0029B sec  */
1060                 mem_ap_read_u32(swjdp, (dbgbase&0xFFFFF000)|0xFF0, &cid0);
1061                 mem_ap_read_u32(swjdp, (dbgbase&0xFFFFF000)|0xFF4, &cid1);
1062                 mem_ap_read_u32(swjdp, (dbgbase&0xFFFFF000)|0xFF8, &cid2);
1063                 mem_ap_read_u32(swjdp, (dbgbase&0xFFFFF000)|0xFFC, &cid3);
1064                 mem_ap_read_u32(swjdp, (dbgbase&0xFFFFF000)|0xFCC, &memtype);
1065                 swjdp_transaction_endcheck(swjdp);
1066                 command_print(cmd_ctx, "\tCID3 0x%x, CID2 0x%x, CID1 0x%x, CID0, 0x%x",cid3,cid2,cid1,cid0);
1067                 if (memtype&0x01)
1068                 {
1069                         command_print(cmd_ctx, "\tMEMTYPE system memory present on bus");
1070                 }
1071                 else
1072                 {
1073                         command_print(cmd_ctx, "\tMEMTYPE system memory not present. Dedicated debug bus" );
1074                 }
1075
1076                 /* Now we read ROM table entries from dbgbase&0xFFFFF000)|0x000 until we get 0x00000000 */
1077                 entry_offset = 0;
1078                 do
1079                 {
1080                         mem_ap_read_atomic_u32(swjdp, (dbgbase&0xFFFFF000)|entry_offset, &romentry);
1081                         command_print(cmd_ctx, "\tROMTABLE[0x%x] = 0x%x",entry_offset,romentry);
1082                         if (romentry&0x01)
1083                         {
1084                                 u32 c_cid0,c_cid1,c_cid2,c_cid3,c_pid0,c_pid1,c_pid2,c_pid3,c_pid4,component_start;
1085                                 u32 component_base = (u32)((dbgbase&0xFFFFF000)+(int)(romentry&0xFFFFF000));
1086                                 mem_ap_read_atomic_u32(swjdp, (component_base&0xFFFFF000)|0xFE0, &c_pid0);
1087                                 mem_ap_read_atomic_u32(swjdp, (component_base&0xFFFFF000)|0xFE4, &c_pid1);
1088                                 mem_ap_read_atomic_u32(swjdp, (component_base&0xFFFFF000)|0xFE8, &c_pid2);
1089                                 mem_ap_read_atomic_u32(swjdp, (component_base&0xFFFFF000)|0xFEC, &c_pid3);
1090                                 mem_ap_read_atomic_u32(swjdp, (component_base&0xFFFFF000)|0xFD0, &c_pid4);
1091                                 mem_ap_read_atomic_u32(swjdp, (component_base&0xFFFFF000)|0xFF0, &c_cid0);
1092                                 mem_ap_read_atomic_u32(swjdp, (component_base&0xFFFFF000)|0xFF4, &c_cid1);
1093                                 mem_ap_read_atomic_u32(swjdp, (component_base&0xFFFFF000)|0xFF8, &c_cid2);
1094                                 mem_ap_read_atomic_u32(swjdp, (component_base&0xFFFFF000)|0xFFC, &c_cid3);
1095                                 component_start = component_base - 0x1000*(c_pid4>>4);
1096                                 command_print(cmd_ctx, "\t\tComponent base address 0x%x, pid4 0x%x, start address 0x%x",component_base,c_pid4,component_start);
1097                                 command_print(cmd_ctx, "\t\tComponent cid1 0x%x, class is %s",c_cid1,class_description[(c_cid1>>4)&0xF]); /* Se ARM DDI 0314 C Table 2.2 */
1098                                 command_print(cmd_ctx, "\t\tCID3 0x%x, CID2 0x%x, CID1 0x%x, CID0, 0x%x",c_cid3,c_cid2,c_cid1,c_cid0);
1099                                 command_print(cmd_ctx, "\t\tPID3 0x%x, PID2 0x%x, PID1 0x%x, PID0, 0x%x",c_pid3,c_pid2,c_pid1,c_pid0);
1100                                 /* For CoreSight components,  (c_cid1>>4)&0xF==9 , we also read 0xFC8 DevId and 0xFCC DevType */
1101                         }
1102                         else
1103                         {
1104                                 if (romentry)
1105                                         command_print(cmd_ctx, "\t\tComponent not present");
1106                                 else
1107                                         command_print(cmd_ctx, "\t\tEnd of ROM table");
1108                         }
1109                         entry_offset += 4;
1110                 } while (romentry>0);
1111         }
1112         else
1113         {
1114                 command_print(cmd_ctx, "\tNo ROM table present");
1115         }
1116         dap_ap_select(swjdp, apselold);
1117
1118         return ERROR_OK;
1119 }
1120