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