src/target/cortex_swjdp.c

   1 /***************************************************************************
   2  *   Copyright (C) 2006 by Magnus Lundin                                   *
   3  *   lundin@mlu.mine.nu                                                    *
   4  *                                                                         *
   5  *   This program is free software; you can redistribute it and/or modify  *
   6  *   it under the terms of the GNU General Public License as published by  *
   7  *   the Free Software Foundation; either version 2 of the License, or     *
   8  *   (at your option) any later version.                                   *
   9  *                                                                         *
  10  *   This program is distributed in the hope that it will be useful,       *
  11  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  12  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  13  *   GNU General Public License for more details.                          *
  14  *                                                                         *
  15  *   You should have received a copy of the GNU General Public License     *
  16  *   along with this program; if not, write to the                         *
  17  *   Free Software Foundation, Inc.,                                       *
  18  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  19  ***************************************************************************/
  20 /***************************************************************************
  21  *                                                                         *
  22  * CoreSight (Light?) SerialWireJtagDebugPort                              *
  23  *                                                                         *
  24  * CoreSight™ DAP-Lite TRM, ARM DDI 0316A                                  *
  25  * Cortex-M3™ TRM, ARM DDI 0337C                                            *
  26  *                                                                         *
  27 ***************************************************************************/
  28 #ifdef HAVE_CONFIG_H
  29 #include "config.h"
  30 #endif
  31
  32 #include "replacements.h"
  33
  34 #include "cortex_m3.h"
  35 #include "cortex_swjdp.h"
  36 #include "jtag.h"
  37 #include "log.h"
  38 #include <stdlib.h>
  39
  40 /*
  41
  42 Transaction Mode:
  43 swjdp->trans_mode = TRANS_MODE_COMPOSITE;
  44 Uses Overrun checking mode and does not do actual JTAG send/receive or transaction
  45 result checking until swjdp_end_transaction()
  46 This must be done before using or deallocating any return variables.
  47
  48 swjdp->trans_mode == TRANS_MODE_ATOMIC
  49 All reads and writes to the AHB bus are checked for valid completion, and return values
  50 are immediatley available.
  51
  52 */
  53
  54 /***************************************************************************
  55  *                                                                         *
  56  * DPACC and APACC scanchain access through JTAG-DR                        *
  57  *                                                                         *
  58 ***************************************************************************/
  59
  60 /* Scan out and in from target ordered u8 buffers */
  61 int swjdp_scan(arm_jtag_t *jtag_info, u8 instr, u8 reg_addr, u8 RnW, u8 *outvalue, u8 *invalue, u8 *ack)
  62 {
  63         scan_field_t fields[2];
  64         u8 out_addr_buf;
  65
  66         jtag_add_end_state(TAP_RTI);
  67         arm_jtag_set_instr(jtag_info, instr, NULL);
  68
  69         fields[0].device = jtag_info->chain_pos;
  70         fields[0].num_bits = 3;
  71         buf_set_u32(&out_addr_buf, 0, 3, ((reg_addr >> 1) & 0x6) | (RnW & 0x1));
  72         fields[0].out_value = &out_addr_buf;
  73         fields[0].out_mask = NULL;
  74         fields[0].in_value = ack;
  75         fields[0].in_check_value = NULL;
  76         fields[0].in_check_mask = NULL;
  77         fields[0].in_handler = NULL;
  78         fields[0].in_handler_priv = NULL;
  79
  80         fields[1].device = jtag_info->chain_pos;
  81         fields[1].num_bits = 32;
  82         fields[1].out_value = outvalue;
  83         fields[1].out_mask = NULL;
  84         fields[1].in_value = invalue;
  85         fields[1].in_handler = NULL;
  86         fields[1].in_handler_priv = NULL;
  87         fields[1].in_check_value = NULL;
  88         fields[1].in_check_mask = NULL;
  89
  90         jtag_add_dr_scan(2, fields, -1);
  91
  92         return ERROR_OK;
  93 }
  94
  95 /* Scan out and in from host ordered u32 variables */
  96 int swjdp_scan_u32(arm_jtag_t *jtag_info, u8 instr, u8 reg_addr, u8 RnW, u32 outvalue, u32 *invalue, u8 *ack)
  97 {
  98         scan_field_t fields[2];
  99         u8 out_value_buf[4];
 100         u8 out_addr_buf;
 101
 102         jtag_add_end_state(TAP_RTI);
 103         arm_jtag_set_instr(jtag_info, instr, NULL);
 104
 105         fields[0].device = jtag_info->chain_pos;
 106         fields[0].num_bits = 3;
 107         buf_set_u32(&out_addr_buf, 0, 3, ((reg_addr >> 1) & 0x6) | (RnW & 0x1));
 108         fields[0].out_value = &out_addr_buf;
 109         fields[0].out_mask = NULL;
 110         fields[0].in_value = ack;
 111         fields[0].in_check_value = NULL;
 112         fields[0].in_check_mask = NULL;
 113         fields[0].in_handler = NULL;
 114         fields[0].in_handler_priv = NULL;
 115
 116         fields[1].device = jtag_info->chain_pos;
 117         fields[1].num_bits = 32;
 118         buf_set_u32(out_value_buf, 0, 32, outvalue);
 119         fields[1].out_value = out_value_buf;
 120         fields[1].out_mask = NULL;
 121         fields[1].in_value = NULL;
 122         if (invalue)
 123         {
 124                 fields[1].in_handler = arm_jtag_buf_to_u32;
 125                 fields[1].in_handler_priv = invalue;
 126         }
 127         else
 128         {
 129                 fields[1].in_handler = NULL;
 130                 fields[1].in_handler_priv = NULL;
 131         }
 132         fields[1].in_check_value = NULL;
 133         fields[1].in_check_mask = NULL;
 134
 135         jtag_add_dr_scan(2, fields, -1);
 136
 137         return ERROR_OK;
 138 }
 139
 140 /* scan_inout_check adds one extra inscan for DPAP_READ commands to read variables */
 141 int scan_inout_check(swjdp_common_t *swjdp, u8 instr, u8 reg_addr, u8 RnW, u8 *outvalue, u8 *invalue)
 142 {
 143         swjdp_scan(swjdp->jtag_info, instr, reg_addr, RnW, outvalue, NULL, NULL);
 144         if ((RnW == DPAP_READ) && (invalue != NULL))
 145         {
 146                 swjdp_scan(swjdp->jtag_info, SWJDP_IR_DPACC, 0xC, DPAP_READ, 0, invalue, &swjdp->ack);
 147         }
 148
 149         /* In TRANS_MODE_ATOMIC all SWJDP_IR_APACC transactions wait for ack=OK/FAULT and the check CTRL_STAT */
 150         if ((instr == SWJDP_IR_APACC) && (swjdp->trans_mode == TRANS_MODE_ATOMIC))
 151         {
 152                 return swjdp_transaction_endcheck(swjdp);
 153         }
 154
 155         return ERROR_OK;
 156 }
 157
 158 int scan_inout_check_u32(swjdp_common_t *swjdp, u8 instr, u8 reg_addr, u8 RnW, u32 outvalue, u32 *invalue)
 159 {
 160
 161         swjdp_scan_u32(swjdp->jtag_info, instr, reg_addr, RnW, outvalue, NULL, NULL);
 162         if ((RnW==DPAP_READ) && (invalue != NULL))
 163         {
 164                 swjdp_scan_u32(swjdp->jtag_info, SWJDP_IR_DPACC, 0xC, DPAP_READ, 0, invalue, &swjdp->ack);
 165         }
 166
 167         /* In TRANS_MODE_ATOMIC all SWJDP_IR_APACC transactions wait for ack=OK/FAULT and then check CTRL_STAT */
 168         if ((instr == SWJDP_IR_APACC) && (swjdp->trans_mode == TRANS_MODE_ATOMIC))
 169         {
 170                 return swjdp_transaction_endcheck(swjdp);
 171         }
 172
 173         return ERROR_OK;
 174 }
 175
 176 int swjdp_transaction_endcheck(swjdp_common_t *swjdp)
 177 {
 178         int waitcount = 0;
 179         u32 ctrlstat;
 180
 181         scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, DP_CTRL_STAT, DPAP_READ, 0, &ctrlstat);
 182         scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, DP_CTRL_STAT, DPAP_READ, 0, &ctrlstat);
 183         jtag_execute_queue();
 184
 185         swjdp->ack = swjdp->ack & 0x7;
 186
 187         while (swjdp->ack != 2)
 188         {
 189                 if (swjdp->ack == 1)
 190                 {
 191                         waitcount++;
 192                         if (waitcount > 100)
 193                         {
 194                                 WARNING("Timeout waiting for ACK = OK/FAULT in SWJDP transaction");
 195
 196                                 return ERROR_JTAG_DEVICE_ERROR;
 197                         }
 198                 }
 199                 else
 200                 {
 201                         WARNING("Invalid ACK in SWJDP transaction");
 202                         return ERROR_JTAG_DEVICE_ERROR;
 203                 }
 204
 205                 scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, DP_CTRL_STAT, DPAP_READ, 0, &ctrlstat);
 206                 jtag_execute_queue();
 207                 swjdp->ack = swjdp->ack & 0x7;
 208         }
 209
 210         /* Check for STICKYERR and STICKYORUN */
 211         if (ctrlstat & (SSTICKYORUN | SSTICKYERR))
 212         {
 213                 DEBUG("swjdp: CTRL/STAT error 0x%x", ctrlstat);
 214                 /* Check power to debug regions */
 215                 if ((ctrlstat & 0xf0000000) != 0xf0000000)
 216                 {
 217                          ahbap_debugport_init(swjdp);
 218                 }
 219                 else
 220                 {
 221                         u32 dcb_dhcsr,nvic_shcsr, nvic_bfar, nvic_cfsr;
 222
 223                         if (ctrlstat & SSTICKYORUN)
 224                                 ERROR("SWJ-DP OVERRUN - check clock or reduce jtag speed");
 225
 226                         if (ctrlstat & SSTICKYERR)
 227                                 ERROR("SWJ-DP STICKY ERROR");
 228
 229                         /* Clear Sticky Error Bits */
 230                         scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, DP_CTRL_STAT, DPAP_WRITE, swjdp->dp_ctrl_stat | SSTICKYORUN | SSTICKYERR, NULL);
 231                         scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, DP_CTRL_STAT, DPAP_READ, 0, &ctrlstat);
 232                         jtag_execute_queue();
 233
 234                         DEBUG("swjdp: status 0x%x", ctrlstat);
 235
 236                         /* Can we find out the reason for the error ?? */
 237                         ahbap_read_system_atomic_u32(swjdp, DCB_DHCSR, &dcb_dhcsr);
 238                         ahbap_read_system_atomic_u32(swjdp, NVIC_SHCSR, &nvic_shcsr);
 239                         ahbap_read_system_atomic_u32(swjdp, NVIC_CFSR, &nvic_cfsr);
 240                         ahbap_read_system_atomic_u32(swjdp, NVIC_BFAR, &nvic_bfar);
 241                         ERROR("dcb_dhcsr 0x%x, nvic_shcsr 0x%x, nvic_cfsr 0x%x, nvic_bfar 0x%x", dcb_dhcsr, nvic_shcsr, nvic_cfsr, nvic_bfar);
 242                 }
 243                 jtag_execute_queue();
 244                 return ERROR_JTAG_DEVICE_ERROR;
 245         }
 246
 247         return ERROR_OK;
 248 }
 249
 250 /***************************************************************************
 251  *                                                                         *
 252  * DP and AHB-AP  register access  through APACC and DPACC                 *
 253  *                                                                         *
 254 ***************************************************************************/
 255
 256 int swjdp_write_dpacc(swjdp_common_t *swjdp, u32 value, u8 reg_addr)
 257 {
 258         u8 out_value_buf[4];
 259
 260         buf_set_u32(out_value_buf, 0, 32, value);
 261         return scan_inout_check(swjdp, SWJDP_IR_DPACC, reg_addr, DPAP_WRITE, out_value_buf, NULL);
 262 }
 263
 264 int swjdp_read_dpacc(swjdp_common_t *swjdp, u32 *value, u8 reg_addr)
 265 {
 266         scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, reg_addr, DPAP_READ, 0, value);
 267
 268     return ERROR_OK;
 269 }
 270
 271 int swjdp_bankselect_apacc(swjdp_common_t *swjdp,u32 reg_addr)
 272 {
 273         u32 select;
 274         select = (reg_addr & 0xFF0000F0);
 275
 276         if (select != swjdp->dp_select_value)
 277         {
 278                 swjdp_write_dpacc(swjdp, select, DP_SELECT);
 279                 swjdp->dp_select_value = select;
 280         }
 281
 282         return ERROR_OK;
 283 }
 284
 285 int ahbap_write_reg(swjdp_common_t *swjdp, u32 reg_addr, u8* out_value_buf)
 286 {
 287         swjdp_bankselect_apacc(swjdp, reg_addr);
 288         scan_inout_check(swjdp, SWJDP_IR_APACC, reg_addr, DPAP_WRITE, out_value_buf, NULL);
 289
 290         return ERROR_OK;
 291 }
 292
 293 int ahbap_read_reg(swjdp_common_t *swjdp, u32 reg_addr, u8 *in_value_buf)
 294 {
 295         swjdp_bankselect_apacc(swjdp, reg_addr);
 296         scan_inout_check(swjdp, SWJDP_IR_APACC, reg_addr, DPAP_READ, 0, in_value_buf);
 297
 298         return ERROR_OK;
 299 }
 300 int ahbap_write_reg_u32(swjdp_common_t *swjdp, u32 reg_addr, u32 value)
 301 {
 302         u8 out_value_buf[4];
 303
 304         buf_set_u32(out_value_buf, 0, 32, value);
 305         swjdp_bankselect_apacc(swjdp, reg_addr);
 306         scan_inout_check(swjdp, SWJDP_IR_APACC, reg_addr, DPAP_WRITE, out_value_buf, NULL);
 307
 308         return ERROR_OK;
 309 }
 310
 311 int ahbap_read_reg_u32(swjdp_common_t *swjdp, u32 reg_addr, u32 *value)
 312 {
 313         swjdp_bankselect_apacc(swjdp, reg_addr);
 314         scan_inout_check_u32(swjdp, SWJDP_IR_APACC, reg_addr, DPAP_READ, 0, value);
 315
 316         return ERROR_OK;
 317 }
 318
 319 /***************************************************************************
 320  *                                                                         *
 321  * AHB-AP access to memory and system registers on AHB bus                 *
 322  *                                                                         *
 323 ***************************************************************************/
 324
 325 int ahbap_setup_accessport(swjdp_common_t *swjdp, u32 csw, u32 tar)
 326 {
 327         csw = csw | CSW_DBGSWENABLE | CSW_MASTER_DEBUG | CSW_HPROT;
 328         if (csw != swjdp->ap_csw_value)
 329         {
 330                 //DEBUG("swjdp : Set CSW %x",csw);
 331                 ahbap_write_reg_u32(swjdp, AHBAP_CSW, csw );
 332                 swjdp->ap_csw_value = csw;
 333         }
 334         if (tar != swjdp->ap_tar_value)
 335         {
 336                 //DEBUG("swjdp : Set TAR %x",tar);
 337                 ahbap_write_reg_u32(swjdp, AHBAP_TAR, tar );
 338                 swjdp->ap_tar_value = tar;
 339         }
 340         if (csw & CSW_ADDRINC_MASK)
 341         {
 342                 /* Do not cache TAR value when autoincrementing */
 343                 swjdp->ap_tar_value = -1;
 344         }
 345         return ERROR_OK;
 346 }
 347
 348 /*****************************************************************************
 349 *                                                                            *
 350 * ahbap_read_system_u32(swjdp_common_t *swjdp, u32 address, u32 *value)      *
 351 *                                                                            *
 352 * Read a u32 value from memory or system register                            *
 353 * Functionally equivalent to target_read_u32(target, address, u32 *value),   *
 354 * but with less overhead                                                     *
 355 *****************************************************************************/
 356 int ahbap_read_system_u32(swjdp_common_t *swjdp, u32 address, u32 *value)
 357 {
 358         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 359
 360         ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, address & 0xFFFFFFF0);
 361         ahbap_read_reg_u32(swjdp, AHBAP_BD0 | (address & 0xC), value );
 362
 363         return ERROR_OK;
 364 }
 365
 366 int ahbap_read_system_atomic_u32(swjdp_common_t *swjdp, u32 address, u32 *value)
 367 {
 368         ahbap_read_system_u32(swjdp, address, value);
 369
 370         return swjdp_transaction_endcheck(swjdp);
 371 }
 372
 373 /*****************************************************************************
 374 *                                                                            *
 375 * ahbap_write_system_u32(swjdp_common_t *swjdp, u32 address, u32 value)      *
 376 *                                                                            *
 377 * Write a u32 value to memory or system register                             *
 378 *                                                                            *
 379 *****************************************************************************/
 380 int ahbap_write_system_u32(swjdp_common_t *swjdp, u32 address, u32 value)
 381 {
 382         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 383
 384         ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, address & 0xFFFFFFF0);
 385         ahbap_write_reg_u32(swjdp, AHBAP_BD0 | (address & 0xC), value );
 386
 387         return ERROR_OK;
 388 }
 389
 390 int ahbap_write_system_atomic_u32(swjdp_common_t *swjdp, u32 address, u32 value)
 391 {
 392         ahbap_write_system_u32(swjdp, address, value);
 393
 394         return swjdp_transaction_endcheck(swjdp);
 395 }
 396
 397 /*****************************************************************************
 398 *                                                                            *
 399 * ahbap_write_buf(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address) *
 400 *                                                                            *
 401 * Write a buffer in target order (little endian)                             *
 402 *                                                                            *
 403 *****************************************************************************/
 404 int ahbap_write_buf(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 405 {
 406         u32 outvalue;
 407         int wcount, blocksize, writecount, errorcount = 0, retval = ERROR_OK;
 408
 409         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 410
 411         while ((address & 0x3) && (count > 0))
 412         {
 413                 ahbap_setup_accessport(swjdp, CSW_8BIT | CSW_ADDRINC_SINGLE, address);
 414                 outvalue = (*buffer++) << 8 * (address & 0x3);
 415                 ahbap_write_reg_u32(swjdp, AHBAP_DRW, outvalue );
 416                 swjdp_transaction_endcheck(swjdp);
 417                 count--;
 418                 address++;
 419         }
 420         wcount = count >> 2;
 421         count = count - 4 * wcount;
 422         while (wcount > 0)
 423         {
 424                 /* Adjust to write blocks within 4K aligned boundaries */
 425                 blocksize = (0x1000 - (0xFFF & address)) >> 2;
 426                 if (wcount < blocksize)
 427                         blocksize = wcount;
 428                 ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_SINGLE, address);
 429                 for (writecount=0; writecount<blocksize; writecount++)
 430                 {
 431                         ahbap_write_reg(swjdp, AHBAP_DRW, buffer + 4 * writecount );
 432                 }
 433                 if (swjdp_transaction_endcheck(swjdp) == ERROR_OK)
 434                 {
 435                         wcount = wcount - blocksize;
 436                         address = address + 4 * blocksize;
 437                         buffer = buffer + 4 * blocksize;
 438                 }
 439                 else
 440                 {
 441                         errorcount++;
 442                 }
 443                 if (errorcount > 1)
 444                 {
 445                         WARNING("Block write error address 0x%x, wcount 0x%x", address, wcount);
 446                         return ERROR_JTAG_DEVICE_ERROR;
 447                 }
 448         }
 449
 450         while (count > 0)
 451         {
 452                 ahbap_setup_accessport(swjdp, CSW_8BIT | CSW_ADDRINC_SINGLE, address);
 453                 outvalue = (*buffer++) << 8 * (address & 0x3);
 454                 ahbap_write_reg_u32(swjdp, AHBAP_DRW, outvalue );
 455                 retval = swjdp_transaction_endcheck(swjdp);
 456                 count--;
 457                 address++;
 458         }
 459
 460         return retval;
 461 }
 462
 463 int ahbap_write_buf_u16(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 464 {
 465         u32 outvalue;
 466         int retval = ERROR_OK;
 467
 468         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 469
 470         while (count > 0)
 471         {
 472                 ahbap_setup_accessport(swjdp, CSW_16BIT | CSW_ADDRINC_SINGLE, address);
 473                 outvalue = *((u16*)buffer) << 8 * (address & 0x3);
 474                 ahbap_write_reg_u32(swjdp, AHBAP_DRW, outvalue );
 475                 retval = swjdp_transaction_endcheck(swjdp);
 476                 count -= 2;
 477                 address += 2;
 478                 buffer += 2;
 479         }
 480
 481         return retval;
 482 }
 483
 484 /*****************************************************************************
 485 *                                                                            *
 486 * ahbap_read_buf(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)  *
 487 *                                                                            *
 488 * Read block fast in target order (little endian) into a buffer       *
 489 *                                                                            *
 490 *****************************************************************************/
 491 int ahbap_read_buf(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 492 {
 493         u32 invalue;
 494         int wcount, blocksize, readcount, errorcount = 0, retval = ERROR_OK;
 495
 496         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 497
 498         while ((address & 0x3) && (count > 0))
 499         {
 500                 ahbap_setup_accessport(swjdp, CSW_8BIT | CSW_ADDRINC_SINGLE, address);
 501                 ahbap_read_reg_u32(swjdp, AHBAP_DRW, &invalue);
 502                 swjdp_transaction_endcheck(swjdp);
 503                 *buffer++ = (invalue >> 8 * (address & 0x3)) & 0xFF;
 504                 count--;
 505                 address++;
 506         }
 507         wcount = count >> 2;
 508         count = count - 4 * wcount;
 509         while (wcount > 0)
 510         {
 511                 /* Adjust to read within 4K block boundaries */
 512                 blocksize = (0x1000 - (0xFFF & address)) >> 2;
 513                 if (wcount < blocksize)
 514                         blocksize = wcount;
 515                 ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_SINGLE, address);
 516                 /* Scan out first read */
 517                 swjdp_scan(swjdp->jtag_info, SWJDP_IR_APACC, AHBAP_DRW, DPAP_READ, 0, NULL, NULL);
 518                 for (readcount = 0; readcount < blocksize - 1; readcount++)
 519                 {
 520                         /* Scan out read instruction and scan in previous value */
 521                         swjdp_scan(swjdp->jtag_info, SWJDP_IR_APACC, AHBAP_DRW, DPAP_READ, 0, buffer + 4 * readcount, &swjdp->ack);
 522                 }
 523                 /* Scan in last value */
 524                 swjdp_scan(swjdp->jtag_info, SWJDP_IR_DPACC, 0xC, DPAP_READ, 0, buffer + 4 * readcount, &swjdp->ack);
 525                 if (swjdp_transaction_endcheck(swjdp) == ERROR_OK)
 526                 {
 527                         wcount = wcount - blocksize;
 528                         address += 4 * blocksize;
 529                         buffer += 4 * blocksize;
 530                 }
 531                 else
 532                 {
 533                         errorcount++;
 534                 }
 535                 if (errorcount > 1)
 536                 {
 537                         WARNING("Block read error address 0x%x, count 0x%x", address, count);
 538                         return ERROR_JTAG_DEVICE_ERROR;
 539                 }
 540         }
 541
 542         while (count > 0)
 543         {
 544                 ahbap_setup_accessport(swjdp, CSW_8BIT | CSW_ADDRINC_SINGLE, address);
 545                 ahbap_read_reg_u32(swjdp, AHBAP_DRW, &invalue );
 546                 retval = swjdp_transaction_endcheck(swjdp);
 547                 *buffer++ = (invalue >> 8 * (address & 0x3)) & 0xFF;
 548                 count--;
 549                 address++;
 550         }
 551
 552         return retval;
 553 }
 554
 555 int ahbap_read_buf_u16(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 556 {
 557         u32 invalue;
 558         int retval = ERROR_OK;
 559
 560         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 561
 562         while (count > 0)
 563         {
 564                 ahbap_setup_accessport(swjdp, CSW_16BIT | CSW_ADDRINC_SINGLE, address);
 565                 ahbap_read_reg_u32(swjdp, AHBAP_DRW, &invalue );
 566                 retval = swjdp_transaction_endcheck(swjdp);
 567                 *((u16*)buffer) = (invalue >> 8 * (address & 0x3));
 568                 count -= 2;
 569                 address += 2;
 570                 buffer += 2;
 571         }
 572
 573         return retval;
 574 }
 575
 576 int ahbap_block_read_u32(swjdp_common_t *swjdp, u32 *buffer, int count, u32 address)
 577 {
 578         int readcount, errorcount = 0;
 579         u32 blocksize;
 580
 581         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 582
 583         while (count > 0)
 584         {
 585                 /* Adjust to read within 4K block boundaries */
 586                 blocksize = (0x1000 - (0xFFF & address)) >> 2;
 587                 if (count < blocksize)
 588                         blocksize = count;
 589                 ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_SINGLE, address);
 590                 for (readcount = 0; readcount < blocksize; readcount++)
 591                 {
 592                         ahbap_read_reg_u32(swjdp, AHBAP_DRW, buffer + readcount );
 593                 }
 594                 if (swjdp_transaction_endcheck(swjdp) == ERROR_OK)
 595                 {
 596                         count = count - blocksize;
 597                         address = address + 4 * blocksize;
 598                         buffer = buffer + blocksize;
 599                 }
 600                 else
 601                 {
 602                         errorcount++;
 603                 }
 604                 if (errorcount > 1)
 605                 {
 606                         WARNING("Block read error address 0x%x, count 0x%x", address, count);
 607                         return ERROR_JTAG_DEVICE_ERROR;
 608                 }
 609         }
 610
 611         return ERROR_OK;
 612 }
 613
 614 int ahbap_read_coreregister_u32(swjdp_common_t *swjdp, u32 *value, int regnum)
 615 {
 616         int retval;
 617         u32 dcrdr;
 618
 619         ahbap_read_system_atomic_u32(swjdp, DCB_DCRDR, &dcrdr);
 620
 621         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 622
 623         /* ahbap_write_system_u32(swjdp, DCB_DCRSR, regnum); */
 624         ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, DCB_DCRSR & 0xFFFFFFF0);
 625         ahbap_write_reg_u32(swjdp, AHBAP_BD0 | (DCB_DCRSR & 0xC), regnum );
 626
 627         /* ahbap_read_system_u32(swjdp, DCB_DCRDR, value); */
 628         ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, DCB_DCRDR & 0xFFFFFFF0);
 629         ahbap_read_reg_u32(swjdp, AHBAP_BD0 | (DCB_DCRDR & 0xC), value );
 630
 631         retval = swjdp_transaction_endcheck(swjdp);
 632         ahbap_write_system_atomic_u32(swjdp, DCB_DCRDR, dcrdr);
 633         return retval;
 634 }
 635
 636 int ahbap_write_coreregister_u32(swjdp_common_t *swjdp, u32 value, int regnum)
 637 {
 638         int retval;
 639         u32 dcrdr;
 640
 641         ahbap_read_system_atomic_u32(swjdp, DCB_DCRDR, &dcrdr);
 642
 643         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 644
 645         /* ahbap_write_system_u32(swjdp, DCB_DCRDR, core_regs[i]); */
 646         ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, DCB_DCRDR & 0xFFFFFFF0);
 647         ahbap_write_reg_u32(swjdp, AHBAP_BD0 | (DCB_DCRDR & 0xC), value );
 648
 649         /* ahbap_write_system_u32(swjdp, DCB_DCRSR, i | DCRSR_WnR       ); */
 650         ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, DCB_DCRSR & 0xFFFFFFF0);
 651         ahbap_write_reg_u32(swjdp, AHBAP_BD0 | (DCB_DCRSR & 0xC), regnum | DCRSR_WnR );
 652
 653         retval = swjdp_transaction_endcheck(swjdp);
 654         ahbap_write_system_atomic_u32(swjdp, DCB_DCRDR, dcrdr);
 655         return retval;
 656 }
 657
 658 int ahbap_debugport_init(swjdp_common_t *swjdp)
 659 {
 660         u32 idreg, romaddr, dummy;
 661         u32 ctrlstat;
 662         int cnt = 0;
 663
 664         DEBUG(" ");
 665
 666         swjdp->ap_csw_value = -1;
 667         swjdp->ap_tar_value = -1;
 668         swjdp->trans_mode = TRANS_MODE_ATOMIC;
 669         swjdp_read_dpacc(swjdp, &dummy, DP_CTRL_STAT);
 670         swjdp_write_dpacc(swjdp, SSTICKYERR, DP_CTRL_STAT);
 671         swjdp_read_dpacc(swjdp, &dummy, DP_CTRL_STAT);
 672
 673         swjdp->dp_ctrl_stat = CDBGPWRUPREQ | CSYSPWRUPREQ;
 674
 675         swjdp_write_dpacc(swjdp, swjdp->dp_ctrl_stat, DP_CTRL_STAT);
 676         swjdp_read_dpacc(swjdp, &ctrlstat, DP_CTRL_STAT);
 677         jtag_execute_queue();
 678
 679         /* Check that we have debug power domains activated */
 680         while (!(ctrlstat & CDBGPWRUPACK) && (cnt++ < 10))
 681         {
 682                 DEBUG("swjdp: wait CDBGPWRUPACK");
 683                 swjdp_read_dpacc(swjdp, &ctrlstat, DP_CTRL_STAT);
 684                 jtag_execute_queue();
 685                 usleep(10000);
 686         }
 687
 688         while (!(ctrlstat & CSYSPWRUPACK) && (cnt++ < 10))
 689         {
 690                 DEBUG("swjdp: wait CSYSPWRUPACK");
 691                 swjdp_read_dpacc(swjdp, &ctrlstat, DP_CTRL_STAT);
 692                 jtag_execute_queue();
 693                 usleep(10000);
 694         }
 695
 696         swjdp_read_dpacc(swjdp, &dummy, DP_CTRL_STAT);
 697         /* With debug power on we can activate OVERRUN checking */
 698         swjdp->dp_ctrl_stat = CDBGPWRUPREQ | CSYSPWRUPREQ | CORUNDETECT;
 699         swjdp_write_dpacc(swjdp, swjdp->dp_ctrl_stat , DP_CTRL_STAT);
 700         swjdp_read_dpacc(swjdp, &dummy, DP_CTRL_STAT);
 701
 702         ahbap_read_reg_u32(swjdp, 0xFC, &idreg);
 703         ahbap_read_reg_u32(swjdp, 0xF8, &romaddr);
 704
 705         DEBUG("AHB-AP ID Register 0x%x, Debug ROM Address 0x%x", idreg, romaddr);
 706
 707         return ERROR_OK;
 708 }