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, NULL);
  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, NULL);
 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 the 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         jtag_execute_queue();
 183
 184         swjdp->ack = swjdp->ack & 0x7;
 185
 186         while (swjdp->ack != 2)
 187         {
 188                 if (swjdp->ack == 1)
 189                 {
 190                         waitcount++;
 191                         if (waitcount > 100)
 192                         {
 193                                 WARNING("Timeout waiting for ACK = OK/FAULT in SWJDP transaction");
 194                                 return ERROR_JTAG_DEVICE_ERROR;
 195                         }
 196                 }
 197                 else
 198                 {
 199                         WARNING("Invalid ACK in SWJDP transaction");
 200                         return ERROR_JTAG_DEVICE_ERROR;
 201                 }
 202                 scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, DP_CTRL_STAT, DPAP_READ, 0, &ctrlstat);
 203                 jtag_execute_queue();
 204                 swjdp->ack = swjdp->ack & 0x7;
 205         }
 206
 207         /* Check for STICKYERR and STICKYORUN */
 208         if (ctrlstat & (SSTICKYORUN | SSTICKYERR))
 209         {
 210                 DEBUG("swjdp: CTRL/STAT error 0x%x", ctrlstat);
 211                 /* Check power to debug regions */
 212                 if ((ctrlstat & 0xf0000000) != 0xf0000000)
 213                 {
 214                          ahbap_debugport_init(swjdp);
 215                 }
 216                 else
 217                 {
 218                         u32 dcb_dhcsr,nvic_shcsr, nvic_bfar, nvic_cfsr;
 219
 220                         if (ctrlstat & SSTICKYORUN)
 221                                 ERROR("SWJ-DP OVERRUN - check clock or reduce jtag speed");
 222
 223                         if (ctrlstat & SSTICKYERR)
 224                                 ERROR("SWJ-DP STICKY ERROR");
 225
 226                         /* Clear Sticky Error Bits */
 227                         scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, DP_CTRL_STAT, DPAP_WRITE, swjdp->dp_ctrl_stat | SSTICKYORUN | SSTICKYERR, NULL);
 228                         scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, DP_CTRL_STAT, DPAP_READ, 0, &ctrlstat);
 229                         jtag_execute_queue();
 230
 231                         DEBUG("swjdp: status 0x%x", ctrlstat);
 232
 233                         /* Can we find out the reason for the error ?? */
 234                         ahbap_read_system_atomic_u32(swjdp, DCB_DHCSR, &dcb_dhcsr);
 235                         ahbap_read_system_atomic_u32(swjdp, NVIC_SHCSR, &nvic_shcsr);
 236                         ahbap_read_system_atomic_u32(swjdp, NVIC_CFSR, &nvic_cfsr);
 237                         ahbap_read_system_atomic_u32(swjdp, NVIC_BFAR, &nvic_bfar);
 238                         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);
 239                 }
 240                 jtag_execute_queue();
 241                 return ERROR_JTAG_DEVICE_ERROR;
 242         }
 243
 244         return ERROR_OK;
 245 }
 246
 247 /***************************************************************************
 248  *                                                                         *
 249  * DP and AHB-AP  register access  through APACC and DPACC                 *
 250  *                                                                         *
 251 ***************************************************************************/
 252
 253 int swjdp_write_dpacc(swjdp_common_t *swjdp, u32 value, u8 reg_addr)
 254 {
 255         u8 out_value_buf[4];
 256
 257         buf_set_u32(out_value_buf, 0, 32, value);
 258         return scan_inout_check(swjdp, SWJDP_IR_DPACC, reg_addr, DPAP_WRITE, out_value_buf, NULL);
 259 }
 260
 261 int swjdp_read_dpacc(swjdp_common_t *swjdp, u32 *value, u8 reg_addr)
 262 {
 263         scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, reg_addr, DPAP_READ, 0, value);
 264
 265     return ERROR_OK;
 266 }
 267
 268 int swjdp_bankselect_apacc(swjdp_common_t *swjdp,u32 reg_addr)
 269 {
 270         u32 select;
 271         select = (reg_addr & 0xFF0000F0);
 272
 273         if (select != swjdp->dp_select_value)
 274         {
 275                 swjdp_write_dpacc(swjdp, select, DP_SELECT);
 276                 swjdp->dp_select_value = select;
 277         }
 278
 279         return ERROR_OK;
 280 }
 281
 282 int ahbap_write_reg(swjdp_common_t *swjdp, u32 reg_addr, u8* out_value_buf)
 283 {
 284         swjdp_bankselect_apacc(swjdp, reg_addr);
 285         scan_inout_check(swjdp, SWJDP_IR_APACC, reg_addr, DPAP_WRITE, out_value_buf, NULL);
 286
 287         return ERROR_OK;
 288 }
 289
 290 int ahbap_read_reg(swjdp_common_t *swjdp, u32 reg_addr, u8 *in_value_buf)
 291 {
 292         swjdp_bankselect_apacc(swjdp, reg_addr);
 293         scan_inout_check(swjdp, SWJDP_IR_APACC, reg_addr, DPAP_READ, 0, in_value_buf);
 294
 295         return ERROR_OK;
 296 }
 297 int ahbap_write_reg_u32(swjdp_common_t *swjdp, u32 reg_addr, u32 value)
 298 {
 299         u8 out_value_buf[4];
 300
 301         buf_set_u32(out_value_buf, 0, 32, value);
 302         swjdp_bankselect_apacc(swjdp, reg_addr);
 303         scan_inout_check(swjdp, SWJDP_IR_APACC, reg_addr, DPAP_WRITE, out_value_buf, NULL);
 304
 305         return ERROR_OK;
 306 }
 307
 308 int ahbap_read_reg_u32(swjdp_common_t *swjdp, u32 reg_addr, u32 *value)
 309 {
 310         swjdp_bankselect_apacc(swjdp, reg_addr);
 311         scan_inout_check_u32(swjdp, SWJDP_IR_APACC, reg_addr, DPAP_READ, 0, value);
 312
 313         return ERROR_OK;
 314 }
 315
 316 /***************************************************************************
 317  *                                                                         *
 318  * AHB-AP access to memory and system registers on AHB bus                 *
 319  *                                                                         *
 320 ***************************************************************************/
 321
 322 int ahbap_setup_accessport(swjdp_common_t *swjdp, u32 csw, u32 tar)
 323 {
 324         csw = csw | CSW_DBGSWENABLE | CSW_MASTER_DEBUG | CSW_HPROT;
 325         if (csw != swjdp->ap_csw_value)
 326         {
 327                 //DEBUG("swjdp : Set CSW %x",csw);
 328                 ahbap_write_reg_u32(swjdp, AHBAP_CSW, csw );
 329                 swjdp->ap_csw_value = csw;
 330         }
 331         if (tar != swjdp->ap_tar_value)
 332         {
 333                 //DEBUG("swjdp : Set TAR %x",tar);
 334                 ahbap_write_reg_u32(swjdp, AHBAP_TAR, tar );
 335                 swjdp->ap_tar_value = tar;
 336         }
 337         if (csw & CSW_ADDRINC_MASK)
 338         {
 339                 /* Do not cache TAR value when autoincrementing */
 340                 swjdp->ap_tar_value = -1;
 341         }
 342         return ERROR_OK;
 343 }
 344
 345 /*****************************************************************************
 346 *                                                                            *
 347 * ahbap_read_system_u32(swjdp_common_t *swjdp, u32 address, u32 *value)      *
 348 *                                                                            *
 349 * Read a u32 value from memory or system register                            *
 350 * Functionally equivalent to target_read_u32(target, address, u32 *value),   *
 351 * but with less overhead                                                     *
 352 *****************************************************************************/
 353 int ahbap_read_system_u32(swjdp_common_t *swjdp, u32 address, u32 *value)
 354 {
 355         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 356
 357         ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, address & 0xFFFFFFF0);
 358         ahbap_read_reg_u32(swjdp, AHBAP_BD0 | (address & 0xC), value );
 359
 360         return ERROR_OK;
 361 }
 362
 363 int ahbap_read_system_atomic_u32(swjdp_common_t *swjdp, u32 address, u32 *value)
 364 {
 365         ahbap_read_system_u32(swjdp, address, value);
 366
 367         return swjdp_transaction_endcheck(swjdp);
 368 }
 369
 370 /*****************************************************************************
 371 *                                                                            *
 372 * ahbap_write_system_u32(swjdp_common_t *swjdp, u32 address, u32 value)      *
 373 *                                                                            *
 374 * Write a u32 value to memory or system register                             *
 375 *                                                                            *
 376 *****************************************************************************/
 377 int ahbap_write_system_u32(swjdp_common_t *swjdp, u32 address, u32 value)
 378 {
 379         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 380
 381         ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, address & 0xFFFFFFF0);
 382         ahbap_write_reg_u32(swjdp, AHBAP_BD0 | (address & 0xC), value );
 383
 384         return ERROR_OK;
 385 }
 386
 387 int ahbap_write_system_atomic_u32(swjdp_common_t *swjdp, u32 address, u32 value)
 388 {
 389         ahbap_write_system_u32(swjdp, address, value);
 390
 391         return swjdp_transaction_endcheck(swjdp);
 392 }
 393
 394 /*****************************************************************************
 395 *                                                                            *
 396 * ahbap_write_buf(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address) *
 397 *                                                                            *
 398 * Write a buffer in target order (little endian)                             *
 399 *                                                                            *
 400 *****************************************************************************/
 401 int ahbap_write_buf(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 402 {
 403         u32 outvalue;
 404         int wcount, blocksize, writecount, errorcount = 0, retval = ERROR_OK;
 405
 406         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 407
 408         while ((address & 0x3) && (count > 0))
 409         {
 410                 ahbap_setup_accessport(swjdp, CSW_8BIT | CSW_ADDRINC_SINGLE, address);
 411                 outvalue = (*buffer++) << 8 * (address & 0x3);
 412                 ahbap_write_reg_u32(swjdp, AHBAP_DRW, outvalue );
 413                 swjdp_transaction_endcheck(swjdp);
 414                 count--;
 415                 address++;
 416         }
 417         wcount = count >> 2;
 418         count = count - 4 * wcount;
 419         while (wcount > 0)
 420         {
 421                 /* Adjust to read within 4K block boundaries */
 422                 blocksize = (0x1000 - (0xFFF & address)) >> 2;
 423                 if (wcount < blocksize)
 424                         blocksize = wcount;
 425                 ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_SINGLE, address);
 426                 for (writecount=0; writecount<blocksize; writecount++)
 427                 {
 428                         ahbap_write_reg(swjdp, AHBAP_DRW, buffer + 4 * writecount );
 429                 }
 430                 if (swjdp_transaction_endcheck(swjdp) == ERROR_OK)
 431                 {
 432                         wcount = wcount - blocksize;
 433                         address = address + 4 * blocksize;
 434                         buffer = buffer + 4 * blocksize;
 435                 }
 436                 else
 437                 {
 438                         errorcount++;
 439                 }
 440                 if (errorcount > 1)
 441                 {
 442                         WARNING("Block read error address 0x%x, count 0x%x", address, count);
 443                         return ERROR_JTAG_DEVICE_ERROR;
 444                 }
 445         }
 446
 447         while (count > 0)
 448         {
 449                 ahbap_setup_accessport(swjdp, CSW_8BIT | CSW_ADDRINC_SINGLE, address);
 450                 outvalue = (*buffer++) << 8 * (address & 0x3);
 451                 ahbap_write_reg_u32(swjdp, AHBAP_DRW, outvalue );
 452                 retval = swjdp_transaction_endcheck(swjdp);
 453                 count--;
 454                 address++;
 455         }
 456
 457         return retval;
 458 }
 459
 460 int ahbap_write_buf_u16(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 461 {
 462         u32 outvalue;
 463         int retval = ERROR_OK;
 464
 465         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 466
 467         while (count > 0)
 468         {
 469                 ahbap_setup_accessport(swjdp, CSW_16BIT | CSW_ADDRINC_SINGLE, address);
 470                 outvalue = *((u16*)buffer) << 8 * (address & 0x3);
 471                 ahbap_write_reg_u32(swjdp, AHBAP_DRW, outvalue );
 472                 retval = swjdp_transaction_endcheck(swjdp);
 473                 count -= 2;
 474                 address += 2;
 475                 buffer += 2;
 476         }
 477
 478         return retval;
 479 }
 480
 481 /*****************************************************************************
 482 *                                                                            *
 483 * ahbap_read_buf(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)  *
 484 *                                                                            *
 485 * Read block fast in target order (little endian) into a buffer       *
 486 *                                                                            *
 487 *****************************************************************************/
 488 int ahbap_read_buf(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 489 {
 490         u32 invalue;
 491         int wcount, blocksize, readcount, errorcount = 0, retval = ERROR_OK;
 492
 493         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 494
 495         while ((address & 0x3) && (count > 0))
 496         {
 497                 ahbap_setup_accessport(swjdp, CSW_8BIT | CSW_ADDRINC_SINGLE, address);
 498                 ahbap_read_reg_u32(swjdp, AHBAP_DRW, &invalue);
 499                 swjdp_transaction_endcheck(swjdp);
 500                 *buffer++ = (invalue >> 8 * (address & 0x3)) & 0xFF;
 501                 count--;
 502                 address++;
 503         }
 504         wcount = count >> 2;
 505         count = count - 4 * wcount;
 506         while (wcount > 0)
 507         {
 508                 /* Adjust to read within 4K block boundaries */
 509                 blocksize = (0x1000 - (0xFFF & address)) >> 2;
 510                 if (wcount < blocksize)
 511                         blocksize = wcount;
 512                 ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_SINGLE, address);
 513                 /* Scan out first read */
 514                 swjdp_scan(swjdp->jtag_info, SWJDP_IR_APACC, AHBAP_DRW, DPAP_READ, 0, NULL, NULL);
 515                 for (readcount = 0; readcount < blocksize - 1; readcount++)
 516                 {
 517                         /* Scan out read instruction and scan in previous value */
 518                         swjdp_scan(swjdp->jtag_info, SWJDP_IR_APACC, AHBAP_DRW, DPAP_READ, 0, buffer + 4 * readcount, &swjdp->ack);
 519                 }
 520                 /* Scan in last value */
 521                 swjdp_scan(swjdp->jtag_info, SWJDP_IR_DPACC, 0xC, DPAP_READ, 0, buffer + 4 * readcount, &swjdp->ack);
 522                 if (swjdp_transaction_endcheck(swjdp) == ERROR_OK)
 523                 {
 524                         wcount = wcount - blocksize;
 525                         address += 4 * blocksize;
 526                         buffer += 4 * blocksize;
 527                 }
 528                 else
 529                 {
 530                         errorcount++;
 531                 }
 532                 if (errorcount > 1)
 533                 {
 534                         WARNING("Block read error address 0x%x, count 0x%x", address, count);
 535                         return ERROR_JTAG_DEVICE_ERROR;
 536                 }
 537         }
 538
 539         while (count > 0)
 540         {
 541                 ahbap_setup_accessport(swjdp, CSW_8BIT | CSW_ADDRINC_SINGLE, address);
 542                 ahbap_read_reg_u32(swjdp, AHBAP_DRW, &invalue );
 543                 retval = swjdp_transaction_endcheck(swjdp);
 544                 *buffer++ = (invalue >> 8 * (address & 0x3)) & 0xFF;
 545                 count--;
 546                 address++;
 547         }
 548
 549         return retval;
 550 }
 551
 552 int ahbap_read_buf_u16(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 553 {
 554         u32 invalue;
 555         int retval = ERROR_OK;
 556
 557         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 558
 559         while (count > 0)
 560         {
 561                 ahbap_setup_accessport(swjdp, CSW_16BIT | CSW_ADDRINC_SINGLE, address);
 562                 ahbap_read_reg_u32(swjdp, AHBAP_DRW, &invalue );
 563                 retval = swjdp_transaction_endcheck(swjdp);
 564                 *((u16*)buffer) = (invalue >> 8 * (address & 0x3));
 565                 count -= 2;
 566                 address += 2;
 567                 buffer += 2;
 568         }
 569
 570         return retval;
 571 }
 572
 573 int ahbap_block_read_u32(swjdp_common_t *swjdp, u32 *buffer, int count, u32 address)
 574 {
 575         int readcount, errorcount = 0;
 576         u32 blockmax, blocksize;
 577
 578         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 579
 580         while (count > 0)
 581         {
 582                 /* Adjust to read within 4K block boundaries */
 583                 blocksize = (0x1000 - (0xFFF & address)) >> 2;
 584                 if (count < blocksize)
 585                         blocksize = count;
 586                 ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_SINGLE, address);
 587                 for (readcount = 0; readcount < blocksize; readcount++)
 588                 {
 589                         ahbap_read_reg_u32(swjdp, AHBAP_DRW, buffer + readcount );
 590                 }
 591                 if (swjdp_transaction_endcheck(swjdp) == ERROR_OK)
 592                 {
 593                         count = count - blocksize;
 594                         address = address + 4 * blocksize;
 595                         buffer = buffer + blocksize;
 596                 }
 597                 else
 598                 {
 599                         errorcount++;
 600                 }
 601                 if (errorcount > 1)
 602                 {
 603                         WARNING("Block read error address 0x%x, count 0x%x", address, count);
 604                         return ERROR_JTAG_DEVICE_ERROR;
 605                 }
 606         }
 607
 608         return ERROR_OK;
 609 }
 610
 611 int ahbap_read_coreregister_u32(swjdp_common_t *swjdp, u32 *value, int regnum)
 612 {
 613         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 614
 615         /* ahbap_write_system_u32(swjdp, DCB_DCRSR, regnum); */
 616         ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, DCB_DCRSR & 0xFFFFFFF0);
 617         ahbap_write_reg_u32(swjdp, AHBAP_BD0 | (DCB_DCRSR & 0xC), regnum );
 618
 619         /* ahbap_read_system_u32(swjdp, DCB_DCRDR, value); */
 620         ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, DCB_DCRDR & 0xFFFFFFF0);
 621         ahbap_read_reg_u32(swjdp, AHBAP_BD0 | (DCB_DCRDR & 0xC), value );
 622
 623         return swjdp_transaction_endcheck(swjdp);
 624 }
 625
 626 int ahbap_write_coreregister_u32(swjdp_common_t *swjdp, u32 value, int regnum)
 627 {
 628         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 629
 630         /* ahbap_write_system_u32(swjdp, DCB_DCRDR, core_regs[i]); */
 631         ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, DCB_DCRDR & 0xFFFFFFF0);
 632         ahbap_write_reg_u32(swjdp, AHBAP_BD0 | (DCB_DCRDR & 0xC), value );
 633
 634         /* ahbap_write_system_u32(swjdp, DCB_DCRSR, i | DCRSR_WnR       ); */
 635         ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, DCB_DCRSR & 0xFFFFFFF0);
 636         ahbap_write_reg_u32(swjdp, AHBAP_BD0 | (DCB_DCRSR & 0xC), regnum | DCRSR_WnR );
 637
 638         return swjdp_transaction_endcheck(swjdp);
 639 }
 640
 641 int ahbap_debugport_init(swjdp_common_t *swjdp)
 642 {
 643         u32 idreg, romaddr, dummy;
 644         u32 ctrlstat;
 645         int cnt = 0;
 646
 647         DEBUG(" ");
 648
 649         swjdp->ap_csw_value = -1;
 650         swjdp->ap_tar_value = -1;
 651         swjdp->trans_mode = TRANS_MODE_ATOMIC;
 652         swjdp_read_dpacc(swjdp, &dummy, DP_CTRL_STAT);
 653         swjdp_write_dpacc(swjdp, SSTICKYERR, DP_CTRL_STAT);
 654         swjdp_read_dpacc(swjdp, &dummy, DP_CTRL_STAT);
 655
 656         swjdp->dp_ctrl_stat = CDBGPWRUPREQ | CSYSPWRUPREQ;
 657
 658         swjdp_write_dpacc(swjdp, swjdp->dp_ctrl_stat, DP_CTRL_STAT);
 659         swjdp_read_dpacc(swjdp, &ctrlstat, DP_CTRL_STAT);
 660         jtag_execute_queue();
 661
 662         /* Check that we have debug power domains activated */
 663         while (!(ctrlstat & CDBGPWRUPACK) && (cnt++ < 10))
 664         {
 665                 DEBUG("swjdp: wait CDBGPWRUPACK");
 666                 swjdp_read_dpacc(swjdp, &ctrlstat, DP_CTRL_STAT);
 667                 jtag_execute_queue();
 668                 usleep(10000);
 669         }
 670
 671         while (!(ctrlstat & CSYSPWRUPACK) && (cnt++ < 10))
 672         {
 673                 DEBUG("swjdp: wait CSYSPWRUPACK");
 674                 swjdp_read_dpacc(swjdp, &ctrlstat, DP_CTRL_STAT);
 675                 jtag_execute_queue();
 676                 usleep(10000);
 677         }
 678
 679         swjdp_read_dpacc(swjdp, &dummy, DP_CTRL_STAT);
 680         /* With debug power on we can activate OVERRUN checking */
 681         swjdp->dp_ctrl_stat = CDBGPWRUPREQ | CSYSPWRUPREQ | CORUNDETECT;
 682         swjdp_write_dpacc(swjdp, swjdp->dp_ctrl_stat , DP_CTRL_STAT);
 683         swjdp_read_dpacc(swjdp, &dummy, DP_CTRL_STAT);
 684
 685         ahbap_read_reg_u32(swjdp, 0xFC, &idreg);
 686         ahbap_read_reg_u32(swjdp, 0xF8, &romaddr);
 687
 688         DEBUG("AHB-AP ID Register 0x%x, Debug ROM Address 0x%x", idreg, romaddr);
 689
 690         return ERROR_OK;
 691 }