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