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