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
  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
  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
 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
 127         fields[1].in_value = NULL;
 128         if (invalue)
 129         {
 130                 fields[1].in_handler = arm_jtag_buf_to_u32; /* deprecated! invoke this from user code! */
 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, mem_ap_tar;
 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                         dap_ap_read_reg_u32(swjdp, AP_REG_TAR, &mem_ap_tar);
 270                         if ((retval=jtag_execute_queue())!=ERROR_OK)
 271                                 return retval;
 272                         LOG_ERROR("Read MEM_AP_CSW 0x%x, MEM_AP_TAR 0x%x", mem_ap_csw, mem_ap_tar);
 273
 274                 }
 275                 if ((retval=jtag_execute_queue())!=ERROR_OK)
 276                         return retval;
 277                 return ERROR_JTAG_DEVICE_ERROR;
 278         }
 279
 280         return ERROR_OK;
 281 }
 282
 283 /***************************************************************************
 284  *                                                                         *
 285  * DP and MEM-AP  register access  through APACC and DPACC                 *
 286  *                                                                         *
 287 ***************************************************************************/
 288
 289 int dap_dp_write_reg(swjdp_common_t *swjdp, u32 value, u8 reg_addr)
 290 {
 291         return scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, reg_addr, DPAP_WRITE, value, NULL);
 292 }
 293
 294 int dap_dp_read_reg(swjdp_common_t *swjdp, u32 *value, u8 reg_addr)
 295 {
 296         return scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, reg_addr, DPAP_READ, 0, value);
 297 }
 298
 299 int dap_ap_select(swjdp_common_t *swjdp,u8 apsel)
 300 {
 301         u32 select;
 302         select = (apsel<<24) & 0xFF000000;
 303
 304         if (select != swjdp->apsel)
 305         {
 306                 swjdp->apsel = select;
 307                 /* Switchin AP invalidates cached values */
 308                 swjdp->dp_select_value = -1;
 309                 swjdp->ap_csw_value = -1;
 310                 swjdp->ap_tar_value = -1;
 311         }
 312
 313         return ERROR_OK;
 314 }
 315
 316 int dap_dp_bankselect(swjdp_common_t *swjdp,u32 ap_reg)
 317 {
 318         u32 select;
 319         select = (ap_reg & 0x000000F0);
 320
 321         if (select != swjdp->dp_select_value)
 322         {
 323                 dap_dp_write_reg(swjdp, select | swjdp->apsel, DP_SELECT);
 324                 swjdp->dp_select_value = select;
 325         }
 326
 327         return ERROR_OK;
 328 }
 329
 330 int dap_ap_write_reg(swjdp_common_t *swjdp, u32 reg_addr, u8* out_value_buf)
 331 {
 332         dap_dp_bankselect(swjdp, reg_addr);
 333         scan_inout_check(swjdp, SWJDP_IR_APACC, reg_addr, DPAP_WRITE, out_value_buf, NULL);
 334
 335         return ERROR_OK;
 336 }
 337
 338 int dap_ap_read_reg(swjdp_common_t *swjdp, u32 reg_addr, u8 *in_value_buf)
 339 {
 340         dap_dp_bankselect(swjdp, reg_addr);
 341         scan_inout_check(swjdp, SWJDP_IR_APACC, reg_addr, DPAP_READ, 0, in_value_buf);
 342
 343         return ERROR_OK;
 344 }
 345 int dap_ap_write_reg_u32(swjdp_common_t *swjdp, u32 reg_addr, u32 value)
 346 {
 347         u8 out_value_buf[4];
 348
 349         buf_set_u32(out_value_buf, 0, 32, value);
 350         dap_dp_bankselect(swjdp, reg_addr);
 351         scan_inout_check(swjdp, SWJDP_IR_APACC, reg_addr, DPAP_WRITE, out_value_buf, NULL);
 352
 353         return ERROR_OK;
 354 }
 355
 356 int dap_ap_read_reg_u32(swjdp_common_t *swjdp, u32 reg_addr, u32 *value)
 357 {
 358         dap_dp_bankselect(swjdp, reg_addr);
 359         scan_inout_check_u32(swjdp, SWJDP_IR_APACC, reg_addr, DPAP_READ, 0, value);
 360
 361         return ERROR_OK;
 362 }
 363
 364 /***************************************************************************
 365  *                                                                         *
 366  * AHB-AP access to memory and system registers on AHB bus                 *
 367  *                                                                         *
 368 ***************************************************************************/
 369
 370 int dap_setup_accessport(swjdp_common_t *swjdp, u32 csw, u32 tar)
 371 {
 372         csw = csw | CSW_DBGSWENABLE | CSW_MASTER_DEBUG | CSW_HPROT;
 373         if (csw != swjdp->ap_csw_value)
 374         {
 375                 /* LOG_DEBUG("swjdp : Set CSW %x",csw); */
 376                 dap_ap_write_reg_u32(swjdp, AP_REG_CSW, csw );
 377                 swjdp->ap_csw_value = csw;
 378         }
 379         if (tar != swjdp->ap_tar_value)
 380         {
 381                 /* LOG_DEBUG("swjdp : Set TAR %x",tar); */
 382                 dap_ap_write_reg_u32(swjdp, AP_REG_TAR, tar );
 383                 swjdp->ap_tar_value = tar;
 384         }
 385         if (csw & CSW_ADDRINC_MASK)
 386         {
 387                 /* Do not cache TAR value when autoincrementing */
 388                 swjdp->ap_tar_value = -1;
 389         }
 390         return ERROR_OK;
 391 }
 392
 393 /*****************************************************************************
 394 *                                                                            *
 395 * mem_ap_read_u32(swjdp_common_t *swjdp, u32 address, u32 *value)      *
 396 *                                                                            *
 397 * Read a u32 value from memory or system register                            *
 398 * Functionally equivalent to target_read_u32(target, address, u32 *value),   *
 399 * but with less overhead                                                     *
 400 *****************************************************************************/
 401 int mem_ap_read_u32(swjdp_common_t *swjdp, u32 address, u32 *value)
 402 {
 403         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 404
 405         dap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, address & 0xFFFFFFF0);
 406         dap_ap_read_reg_u32(swjdp, AP_REG_BD0 | (address & 0xC), value );
 407
 408         return ERROR_OK;
 409 }
 410
 411 int mem_ap_read_atomic_u32(swjdp_common_t *swjdp, u32 address, u32 *value)
 412 {
 413         mem_ap_read_u32(swjdp, address, value);
 414
 415         return swjdp_transaction_endcheck(swjdp);
 416 }
 417
 418 /*****************************************************************************
 419 *                                                                            *
 420 * mem_ap_write_u32(swjdp_common_t *swjdp, u32 address, u32 value)      *
 421 *                                                                            *
 422 * Write a u32 value to memory or memory mapped register                              *
 423 *                                                                            *
 424 *****************************************************************************/
 425 int mem_ap_write_u32(swjdp_common_t *swjdp, u32 address, u32 value)
 426 {
 427         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 428
 429         dap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, address & 0xFFFFFFF0);
 430         dap_ap_write_reg_u32(swjdp, AP_REG_BD0 | (address & 0xC), value );
 431
 432         return ERROR_OK;
 433 }
 434
 435 int mem_ap_write_atomic_u32(swjdp_common_t *swjdp, u32 address, u32 value)
 436 {
 437         mem_ap_write_u32(swjdp, address, value);
 438
 439         return swjdp_transaction_endcheck(swjdp);
 440 }
 441
 442 /*****************************************************************************
 443 *                                                                            *
 444 * mem_ap_write_buf(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address) *
 445 *                                                                            *
 446 * Write a buffer in target order (little endian)                             *
 447 *                                                                            *
 448 *****************************************************************************/
 449 int mem_ap_write_buf_u32(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 450 {
 451         u32 outvalue;
 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                         outvalue = *((u32*)pBuffer);
 468
 469                         for (i = 0; i < 4; i++ )
 470                         {
 471                                 *((u8*)pBuffer + (adr & 0x3)) = outvalue;
 472                                 outvalue >>= 8;
 473                                 adr++;
 474                         }
 475                         pBuffer += 4;
 476                 }
 477         }
 478
 479         while (wcount > 0)
 480         {
 481                 /* Adjust to write blocks within 4K aligned boundaries */
 482                 blocksize = (0x1000 - (0xFFF & address)) >> 2;
 483                 if (wcount < blocksize)
 484                         blocksize = wcount;
 485
 486                 /* handle unaligned data at 4k boundary */
 487                 if (blocksize == 0)
 488                         blocksize = 1;
 489
 490                 dap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_SINGLE, address);
 491
 492                 for (writecount = 0; writecount < blocksize; writecount++)
 493                 {
 494                         dap_ap_write_reg(swjdp, AP_REG_DRW, buffer + 4 * writecount );
 495                 }
 496
 497                 if (swjdp_transaction_endcheck(swjdp) == ERROR_OK)
 498                 {
 499                         wcount = wcount - blocksize;
 500                         address = address + 4 * blocksize;
 501                         buffer = buffer + 4 * blocksize;
 502                 }
 503                 else
 504                 {
 505                         errorcount++;
 506                 }
 507
 508                 if (errorcount > 1)
 509                 {
 510                         LOG_WARNING("Block write error address 0x%x, wcount 0x%x", address, wcount);
 511                         return ERROR_JTAG_DEVICE_ERROR;
 512                 }
 513         }
 514
 515         return retval;
 516 }
 517
 518 int mem_ap_write_buf_packed_u16(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 519 {
 520         u32 outvalue;
 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                                 outvalue = *((u32*)buffer);
 562
 563                                 for (i = 0; i < nbytes; i++ )
 564                                 {
 565                                         *((u8*)buffer + (address & 0x3)) = outvalue;
 566                                         outvalue >>= 8;
 567                                         address++;
 568                                 }
 569
 570                                 outvalue = *((u32*)buffer);
 571                                 dap_ap_write_reg_u32(swjdp, AP_REG_DRW, outvalue);
 572                                 if (swjdp_transaction_endcheck(swjdp) != ERROR_OK)
 573                                 {
 574                                         LOG_WARNING("Block read error address 0x%x, count 0x%x", address, count);
 575                                         return ERROR_JTAG_DEVICE_ERROR;
 576                                 }
 577                         }
 578
 579                         buffer += nbytes >> 1;
 580                         writecount -= nbytes >> 1;
 581
 582                 } while (writecount);
 583                 wcount -= blocksize;
 584         }
 585
 586         return retval;
 587 }
 588
 589 int mem_ap_write_buf_u16(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 590 {
 591         u32 outvalue;
 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                 outvalue = *((u16*)buffer) << 8 * (address & 0x3);
 603                 dap_ap_write_reg_u32(swjdp, AP_REG_DRW, outvalue );
 604                 retval = swjdp_transaction_endcheck(swjdp);
 605                 count -= 2;
 606                 address += 2;
 607                 buffer += 2;
 608         }
 609
 610         return retval;
 611 }
 612
 613 int mem_ap_write_buf_packed_u8(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 614 {
 615         u32 outvalue;
 616         int retval = ERROR_OK;
 617         int wcount, blocksize, writecount, i;
 618
 619         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 620
 621         wcount = count;
 622
 623         while (wcount > 0)
 624         {
 625                 int nbytes;
 626
 627                 /* Adjust to read within 4K block boundaries */
 628                 blocksize = (0x1000 - (0xFFF & address));
 629
 630                 if (wcount < blocksize)
 631                         blocksize = wcount;
 632
 633                 dap_setup_accessport(swjdp, CSW_8BIT | CSW_ADDRINC_PACKED, address);
 634                 writecount = blocksize;
 635
 636                 do
 637                 {
 638                         nbytes = MIN(writecount, 4);
 639
 640                         if (nbytes < 4 )
 641                         {
 642                                 if (mem_ap_write_buf_u8(swjdp, buffer, nbytes, address) != ERROR_OK)
 643                                 {
 644                                         LOG_WARNING("Block read error address 0x%x, count 0x%x", address, count);
 645                                         return ERROR_JTAG_DEVICE_ERROR;
 646                                 }
 647
 648                                 address += nbytes;
 649                         }
 650                         else
 651                         {
 652                                 outvalue = *((u32*)buffer);
 653
 654                                 for (i = 0; i < nbytes; i++ )
 655                                 {
 656                                         *((u8*)buffer + (address & 0x3)) = outvalue;
 657                                         outvalue >>= 8;
 658                                         address++;
 659                                 }
 660
 661                                 outvalue = *((u32*)buffer);
 662                                 dap_ap_write_reg_u32(swjdp, AP_REG_DRW, outvalue);
 663                                 if (swjdp_transaction_endcheck(swjdp) != ERROR_OK)
 664                                 {
 665                                         LOG_WARNING("Block read error address 0x%x, count 0x%x", address, count);
 666                                         return ERROR_JTAG_DEVICE_ERROR;
 667                                 }
 668                         }
 669
 670                         buffer += nbytes;
 671                         writecount -= nbytes;
 672
 673                 } while (writecount);
 674                 wcount -= blocksize;
 675         }
 676
 677         return retval;
 678 }
 679
 680 int mem_ap_write_buf_u8(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 681 {
 682         u32 outvalue;
 683         int retval = ERROR_OK;
 684
 685         if (count >= 4)
 686                 return mem_ap_write_buf_packed_u8(swjdp, buffer, count, address);
 687
 688         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 689
 690         while (count > 0)
 691         {
 692                 dap_setup_accessport(swjdp, CSW_8BIT | CSW_ADDRINC_SINGLE, address);
 693                 outvalue = *((u8*)buffer) << 8 * (address & 0x3);
 694                 dap_ap_write_reg_u32(swjdp, AP_REG_DRW, outvalue );
 695                 retval = swjdp_transaction_endcheck(swjdp);
 696                 count--;
 697                 address++;
 698                 buffer++;
 699         }
 700
 701         return retval;
 702 }
 703
 704 /*********************************************************************************
 705 *                                                                                *
 706 * mem_ap_read_buf_u32(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)  *
 707 *                                                                                *
 708 * Read block fast in target order (little endian) into a buffer                  *
 709 *                                                                                *
 710 **********************************************************************************/
 711 int mem_ap_read_buf_u32(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 712 {
 713         int wcount, blocksize, readcount, errorcount = 0, retval = ERROR_OK;
 714         u32 adr = address;
 715         u8* pBuffer = buffer;
 716
 717         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 718
 719         count >>= 2;
 720         wcount = count;
 721
 722         while (wcount > 0)
 723         {
 724                 /* Adjust to read within 4K block boundaries */
 725                 blocksize = (0x1000 - (0xFFF & address)) >> 2;
 726                 if (wcount < blocksize)
 727                         blocksize = wcount;
 728
 729                 /* handle unaligned data at 4k boundary */
 730                 if (blocksize == 0)
 731                         blocksize = 1;
 732
 733                 dap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_SINGLE, address);
 734
 735                 /* Scan out first read */
 736                 adi_jtag_dp_scan(swjdp->jtag_info, SWJDP_IR_APACC, AP_REG_DRW, DPAP_READ, 0, NULL, NULL);
 737                 for (readcount = 0; readcount < blocksize - 1; readcount++)
 738                 {
 739                         /* Scan out read instruction and scan in previous value */
 740                         adi_jtag_dp_scan(swjdp->jtag_info, SWJDP_IR_APACC, AP_REG_DRW, DPAP_READ, 0, buffer + 4 * readcount, &swjdp->ack);
 741                 }
 742
 743                 /* Scan in last value */
 744                 adi_jtag_dp_scan(swjdp->jtag_info, SWJDP_IR_DPACC, DP_RDBUFF, DPAP_READ, 0, buffer + 4 * readcount, &swjdp->ack);
 745                 if (swjdp_transaction_endcheck(swjdp) == ERROR_OK)
 746                 {
 747                         wcount = wcount - blocksize;
 748                         address += 4 * blocksize;
 749                         buffer += 4 * blocksize;
 750                 }
 751                 else
 752                 {
 753                         errorcount++;
 754                 }
 755
 756                 if (errorcount > 1)
 757                 {
 758                         LOG_WARNING("Block read error address 0x%x, count 0x%x", address, count);
 759                         return ERROR_JTAG_DEVICE_ERROR;
 760                 }
 761         }
 762
 763         /* if we have an unaligned access - reorder data */
 764         if (adr & 0x3u)
 765         {
 766                 for (readcount = 0; readcount < count; readcount++)
 767                 {
 768                         int i;
 769                         u32 data = *((u32*)pBuffer);
 770
 771                         for (i = 0; i < 4; i++ )
 772                         {
 773                                 *((u8*)pBuffer) = (data >> 8 * (adr & 0x3));
 774                                 pBuffer++;
 775                                 adr++;
 776                         }
 777                 }
 778         }
 779
 780         return retval;
 781 }
 782
 783 int mem_ap_read_buf_packed_u16(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 784 {
 785         u32 invalue;
 786         int retval = ERROR_OK;
 787         int wcount, blocksize, readcount, i;
 788
 789         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 790
 791         wcount = count >> 1;
 792
 793         while (wcount > 0)
 794         {
 795                 int nbytes;
 796
 797                 /* Adjust to read within 4K block boundaries */
 798                 blocksize = (0x1000 - (0xFFF & address)) >> 1;
 799                 if (wcount < blocksize)
 800                         blocksize = wcount;
 801
 802                 dap_setup_accessport(swjdp, CSW_16BIT | CSW_ADDRINC_PACKED, address);
 803
 804                 /* handle unaligned data at 4k boundary */
 805                 if (blocksize == 0)
 806                         blocksize = 1;
 807                 readcount = blocksize;
 808
 809                 do
 810                 {
 811                         dap_ap_read_reg_u32(swjdp, AP_REG_DRW, &invalue );
 812                         if (swjdp_transaction_endcheck(swjdp) != ERROR_OK)
 813                         {
 814                                 LOG_WARNING("Block read error address 0x%x, count 0x%x", address, count);
 815                                 return ERROR_JTAG_DEVICE_ERROR;
 816                         }
 817
 818                         nbytes = MIN((readcount << 1), 4);
 819
 820                         for (i = 0; i < nbytes; i++ )
 821                         {
 822                                 *((u8*)buffer) = (invalue >> 8 * (address & 0x3));
 823                                 buffer++;
 824                                 address++;
 825                         }
 826
 827                         readcount -= (nbytes >> 1);
 828                 } while (readcount);
 829                 wcount -= blocksize;
 830         }
 831
 832         return retval;
 833 }
 834
 835 int mem_ap_read_buf_u16(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 836 {
 837         u32 invalue, i;
 838         int retval = ERROR_OK;
 839
 840         if (count >= 4)
 841                 return mem_ap_read_buf_packed_u16(swjdp, buffer, count, address);
 842
 843         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 844
 845         while (count > 0)
 846         {
 847                 dap_setup_accessport(swjdp, CSW_16BIT | CSW_ADDRINC_SINGLE, address);
 848                 dap_ap_read_reg_u32(swjdp, AP_REG_DRW, &invalue );
 849                 retval = swjdp_transaction_endcheck(swjdp);
 850                 if (address & 0x1)
 851                 {
 852                         for (i = 0; i < 2; i++ )
 853                         {
 854                                 *((u8*)buffer) = (invalue >> 8 * (address & 0x3));
 855                                 buffer++;
 856                                 address++;
 857                         }
 858                 }
 859                 else
 860                 {
 861                         *((u16*)buffer) = (invalue >> 8 * (address & 0x3));
 862                         address += 2;
 863                         buffer += 2;
 864                 }
 865                 count -= 2;
 866         }
 867
 868         return retval;
 869 }
 870
 871 int mem_ap_read_buf_packed_u8(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 872 {
 873         u32 invalue;
 874         int retval = ERROR_OK;
 875         int wcount, blocksize, readcount, i;
 876
 877         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 878
 879         wcount = count;
 880
 881         while (wcount > 0)
 882         {
 883                 int nbytes;
 884
 885                 /* Adjust to read within 4K block boundaries */
 886                 blocksize = (0x1000 - (0xFFF & address));
 887
 888                 if (wcount < blocksize)
 889                         blocksize = wcount;
 890
 891                 dap_setup_accessport(swjdp, CSW_8BIT | CSW_ADDRINC_PACKED, address);
 892                 readcount = blocksize;
 893
 894                 do
 895                 {
 896                         dap_ap_read_reg_u32(swjdp, AP_REG_DRW, &invalue );
 897                         if (swjdp_transaction_endcheck(swjdp) != ERROR_OK)
 898                         {
 899                                 LOG_WARNING("Block read error address 0x%x, count 0x%x", address, count);
 900                                 return ERROR_JTAG_DEVICE_ERROR;
 901                         }
 902
 903                         nbytes = MIN(readcount, 4);
 904
 905                         for (i = 0; i < nbytes; i++ )
 906                         {
 907                                 *((u8*)buffer) = (invalue >> 8 * (address & 0x3));
 908                                 buffer++;
 909                                 address++;
 910                         }
 911
 912                         readcount -= nbytes;
 913                 } while (readcount);
 914                 wcount -= blocksize;
 915         }
 916
 917         return retval;
 918 }
 919
 920 int mem_ap_read_buf_u8(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 921 {
 922         u32 invalue;
 923         int retval = ERROR_OK;
 924
 925         if (count >= 4)
 926                 return mem_ap_read_buf_packed_u8(swjdp, buffer, count, address);
 927
 928         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 929
 930         while (count > 0)
 931         {
 932                 dap_setup_accessport(swjdp, CSW_8BIT | CSW_ADDRINC_SINGLE, address);
 933                 dap_ap_read_reg_u32(swjdp, AP_REG_DRW, &invalue );
 934                 retval = swjdp_transaction_endcheck(swjdp);
 935                 *((u8*)buffer) = (invalue >> 8 * (address & 0x3));
 936                 count--;
 937                 address++;
 938                 buffer++;
 939         }
 940
 941         return retval;
 942 }
 943
 944 int ahbap_debugport_init(swjdp_common_t *swjdp)
 945 {
 946         u32 idreg, romaddr, dummy;
 947         u32 ctrlstat;
 948         int cnt = 0;
 949         int retval;
 950
 951         LOG_DEBUG(" ");
 952
 953         swjdp->apsel = 0;
 954         swjdp->ap_csw_value = -1;
 955         swjdp->ap_tar_value = -1;
 956         swjdp->trans_mode = TRANS_MODE_ATOMIC;
 957         dap_dp_read_reg(swjdp, &dummy, DP_CTRL_STAT);
 958         dap_dp_write_reg(swjdp, SSTICKYERR, DP_CTRL_STAT);
 959         dap_dp_read_reg(swjdp, &dummy, DP_CTRL_STAT);
 960
 961         swjdp->dp_ctrl_stat = CDBGPWRUPREQ | CSYSPWRUPREQ;
 962
 963         dap_dp_write_reg(swjdp, swjdp->dp_ctrl_stat, DP_CTRL_STAT);
 964         dap_dp_read_reg(swjdp, &ctrlstat, DP_CTRL_STAT);
 965         if ((retval=jtag_execute_queue())!=ERROR_OK)
 966                 return retval;
 967
 968         /* Check that we have debug power domains activated */
 969         while (!(ctrlstat & CDBGPWRUPACK) && (cnt++ < 10))
 970         {
 971                 LOG_DEBUG("swjdp: wait CDBGPWRUPACK");
 972                 dap_dp_read_reg(swjdp, &ctrlstat, DP_CTRL_STAT);
 973                 if ((retval=jtag_execute_queue())!=ERROR_OK)
 974                         return retval;
 975                 alive_sleep(10);
 976         }
 977
 978         while (!(ctrlstat & CSYSPWRUPACK) && (cnt++ < 10))
 979         {
 980                 LOG_DEBUG("swjdp: wait CSYSPWRUPACK");
 981                 dap_dp_read_reg(swjdp, &ctrlstat, DP_CTRL_STAT);
 982                 if ((retval=jtag_execute_queue())!=ERROR_OK)
 983                         return retval;
 984                 alive_sleep(10);
 985         }
 986
 987         dap_dp_read_reg(swjdp, &dummy, DP_CTRL_STAT);
 988         /* With debug power on we can activate OVERRUN checking */
 989         swjdp->dp_ctrl_stat = CDBGPWRUPREQ | CSYSPWRUPREQ | CORUNDETECT;
 990         dap_dp_write_reg(swjdp, swjdp->dp_ctrl_stat, DP_CTRL_STAT);
 991         dap_dp_read_reg(swjdp, &dummy, DP_CTRL_STAT);
 992
 993         dap_ap_read_reg_u32(swjdp, 0xFC, &idreg);
 994         dap_ap_read_reg_u32(swjdp, 0xF8, &romaddr);
 995
 996         LOG_DEBUG("AHB-AP ID Register 0x%x, Debug ROM Address 0x%x", idreg, romaddr);
 997
 998         return ERROR_OK;
 999 }
1000
1001
1002 char * class_description[16] ={
1003         "Reserved",
1004         "ROM table","Reserved","Reserved","Reserved","Reserved","Reserved","Reserved","Reserved",
1005         "CoreSight component","Reserved","Peripheral Test Block","Reserved","DESS","Generic IP component","Non standard layout"};
1006
1007 int dap_info_command(struct command_context_s *cmd_ctx, swjdp_common_t *swjdp, int apsel)
1008 {
1009
1010         u32 dbgbase,apid;
1011         int romtable_present = 0;
1012         u8 mem_ap;
1013         u32 apselold;
1014
1015         apselold = swjdp->apsel;
1016         dap_ap_select(swjdp, apsel);
1017         dap_ap_read_reg_u32(swjdp, 0xF8, &dbgbase);
1018         dap_ap_read_reg_u32(swjdp, 0xFC, &apid);
1019         swjdp_transaction_endcheck(swjdp);
1020         /* Now we read ROM table ID registers, ref. ARM IHI 0029B sec  */
1021         mem_ap = ((apid&0x10000)&&((apid&0x0F)!=0));
1022         command_print(cmd_ctx, "ap identification register 0x%8.8x", apid);
1023         if (apid)
1024         {
1025                 switch (apid&0x0F)
1026                 {
1027                         case 0:
1028                                 command_print(cmd_ctx, "\tType is jtag-ap");
1029                                 break;
1030                         case 1:
1031                                 command_print(cmd_ctx, "\tType is mem-ap AHB");
1032                                 break;
1033                         case 2:
1034                                 command_print(cmd_ctx, "\tType is mem-ap APB");
1035                                 break;
1036                         default:
1037                                 command_print(cmd_ctx, "\tUnknown AP-type");
1038                         break;
1039                 }
1040                 command_print(cmd_ctx, "ap debugbase 0x%8.8x", dbgbase);
1041         }
1042         else
1043         {
1044                 command_print(cmd_ctx, "No AP found at this apsel 0x%x", apsel);
1045         }
1046
1047         romtable_present = ((mem_ap)&&(dbgbase != 0xFFFFFFFF));
1048         if (romtable_present)
1049         {
1050                 u32 cid0,cid1,cid2,cid3,memtype,romentry;
1051                 u16 entry_offset;
1052                 /* bit 16 of apid indicates a memory access port */
1053                 if (dbgbase&0x02)
1054                 {
1055                         command_print(cmd_ctx, "\tValid ROM table present");
1056                 }
1057                 else
1058                 {
1059                         command_print(cmd_ctx, "\tROM table in legacy format" );
1060                 }
1061                 /* Now we read ROM table ID registers, ref. ARM IHI 0029B sec  */
1062                 mem_ap_read_u32(swjdp, (dbgbase&0xFFFFF000)|0xFF0, &cid0);
1063                 mem_ap_read_u32(swjdp, (dbgbase&0xFFFFF000)|0xFF4, &cid1);
1064                 mem_ap_read_u32(swjdp, (dbgbase&0xFFFFF000)|0xFF8, &cid2);
1065                 mem_ap_read_u32(swjdp, (dbgbase&0xFFFFF000)|0xFFC, &cid3);
1066                 mem_ap_read_u32(swjdp, (dbgbase&0xFFFFF000)|0xFCC, &memtype);
1067                 swjdp_transaction_endcheck(swjdp);
1068                 command_print(cmd_ctx, "\tCID3 0x%x, CID2 0x%x, CID1 0x%x, CID0, 0x%x",cid3,cid2,cid1,cid0);
1069                 if (memtype&0x01)
1070                 {
1071                         command_print(cmd_ctx, "\tMEMTYPE system memory present on bus");
1072                 }
1073                 else
1074                 {
1075                         command_print(cmd_ctx, "\tMEMTYPE system memory not present. Dedicated debug bus" );
1076                 }
1077
1078                 /* Now we read ROM table entries from dbgbase&0xFFFFF000)|0x000 until we get 0x00000000 */
1079                 entry_offset = 0;
1080                 do
1081                 {
1082                         mem_ap_read_atomic_u32(swjdp, (dbgbase&0xFFFFF000)|entry_offset, &romentry);
1083                         command_print(cmd_ctx, "\tROMTABLE[0x%x] = 0x%x",entry_offset,romentry);
1084                         if (romentry&0x01)
1085                         {
1086                                 u32 c_cid0,c_cid1,c_cid2,c_cid3,c_pid0,c_pid1,c_pid2,c_pid3,c_pid4,component_start;
1087                                 u32 component_base = (u32)((dbgbase&0xFFFFF000)+(int)(romentry&0xFFFFF000));
1088                                 mem_ap_read_atomic_u32(swjdp, (component_base&0xFFFFF000)|0xFE0, &c_pid0);
1089                                 mem_ap_read_atomic_u32(swjdp, (component_base&0xFFFFF000)|0xFE4, &c_pid1);
1090                                 mem_ap_read_atomic_u32(swjdp, (component_base&0xFFFFF000)|0xFE8, &c_pid2);
1091                                 mem_ap_read_atomic_u32(swjdp, (component_base&0xFFFFF000)|0xFEC, &c_pid3);
1092                                 mem_ap_read_atomic_u32(swjdp, (component_base&0xFFFFF000)|0xFD0, &c_pid4);
1093                                 mem_ap_read_atomic_u32(swjdp, (component_base&0xFFFFF000)|0xFF0, &c_cid0);
1094                                 mem_ap_read_atomic_u32(swjdp, (component_base&0xFFFFF000)|0xFF4, &c_cid1);
1095                                 mem_ap_read_atomic_u32(swjdp, (component_base&0xFFFFF000)|0xFF8, &c_cid2);
1096                                 mem_ap_read_atomic_u32(swjdp, (component_base&0xFFFFF000)|0xFFC, &c_cid3);
1097                                 component_start = component_base - 0x1000*(c_pid4>>4);
1098                                 command_print(cmd_ctx, "\t\tComponent base address 0x%x, pid4 0x%x, start address 0x%x",component_base,c_pid4,component_start);
1099                                 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 */
1100                                 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);
1101                                 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);
1102                                 /* For CoreSight components,  (c_cid1>>4)&0xF==9 , we also read 0xFC8 DevId and 0xFCC DevType */
1103                         }
1104                         else
1105                         {
1106                                 if (romentry)
1107                                         command_print(cmd_ctx, "\t\tComponent not present");
1108                                 else
1109                                         command_print(cmd_ctx, "\t\tEnd of ROM table");
1110                         }
1111                         entry_offset += 4;
1112                 } while (romentry>0);
1113         }
1114         else
1115         {
1116                 command_print(cmd_ctx, "\tNo ROM table present");
1117         }
1118         dap_ap_select(swjdp, apselold);
1119
1120         return ERROR_OK;
1121 }
1122