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