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
  29 #include "replacements.h"
  30
  31 #include "cortex_m3.h"
  32 #include "cortex_swjdp.h"
  33 #include "jtag.h"
  34 #include "log.h"
  35 #include <stdlib.h>
  36
  37 /*
  38
  39 Transaction Mode:
  40 swjdp->trans_mode = TRANS_MODE_COMPOSITE;
  41 Uses Overrun checking mode and does not do actual JTAG send/receive or transaction
  42 result checking until swjdp_end_transaction()
  43 This must be done before using or deallocating any return variables.
  44
  45 swjdp->trans_mode == TRANS_MODE_ATOMIC
  46 All reads and writes to the AHB bus are checked for valid completion, and return values
  47 are immediatley available.
  48
  49 */
  50
  51 /***************************************************************************
  52  *                                                                         *
  53  * DPACC and APACC scanchain access through JTAG-DR                        *
  54  *                                                                         *
  55 ***************************************************************************/
  56
  57 /* Scan out and in from target ordered u8 buffers */
  58 int swjdp_scan(arm_jtag_t *jtag_info, u8 chain, u8 reg_addr, u8 RnW, u8 *outvalue, u8 *invalue, u8 *ack)
  59 {
  60         scan_field_t fields[2];
  61         u8 out_addr_buf;
  62
  63         jtag_add_end_state(TAP_RTI);
  64         arm_jtag_set_instr(jtag_info, chain, NULL);
  65
  66         fields[0].device = jtag_info->chain_pos;
  67         fields[0].num_bits = 3;
  68         buf_set_u32(&out_addr_buf, 0, 3, ((reg_addr>>1)&0x6) | (RnW&0x1));
  69         fields[0].out_value = &out_addr_buf;
  70         fields[0].out_mask = NULL;
  71         fields[0].in_value = ack;
  72         fields[0].in_check_value = NULL;
  73         fields[0].in_check_mask = NULL;
  74         fields[0].in_handler = NULL;
  75         fields[0].in_handler_priv = NULL;
  76
  77         fields[1].device = jtag_info->chain_pos;
  78         fields[1].num_bits = 32;
  79         fields[1].out_value = outvalue;
  80         fields[1].out_mask = NULL;
  81         fields[1].in_value = invalue;
  82         fields[1].in_handler = NULL;
  83         fields[1].in_handler_priv = NULL;
  84         fields[1].in_check_value = NULL;
  85         fields[1].in_check_mask = NULL;
  86
  87         jtag_add_dr_scan(2, fields, -1, NULL);
  88
  89         return ERROR_OK;
  90
  91 }
  92
  93 /* Scan out and in from host ordered u32 variables */
  94 int swjdp_scan_u32(arm_jtag_t *jtag_info, u8 chain, u8 reg_addr, u8 RnW, u32 outvalue, u32 *invalue, u8 *ack)
  95 {
  96         scan_field_t fields[2];
  97         u8 out_value_buf[4];
  98         u8 out_addr_buf;
  99
 100         jtag_add_end_state(TAP_RTI);
 101         arm_jtag_set_instr(jtag_info, chain, NULL);
 102
 103         fields[0].device = jtag_info->chain_pos;
 104         fields[0].num_bits = 3;
 105         buf_set_u32(&out_addr_buf, 0, 3, ((reg_addr>>1)&0x6) | (RnW&0x1));
 106         fields[0].out_value = &out_addr_buf;
 107         fields[0].out_mask = NULL;
 108         fields[0].in_value = ack;
 109         fields[0].in_check_value = NULL;
 110         fields[0].in_check_mask = NULL;
 111         fields[0].in_handler = NULL;
 112         fields[0].in_handler_priv = NULL;
 113
 114         fields[1].device = jtag_info->chain_pos;
 115         fields[1].num_bits = 32;
 116         buf_set_u32(out_value_buf, 0, 32, outvalue);
 117         fields[1].out_value = out_value_buf;
 118         fields[1].out_mask = NULL;
 119         fields[1].in_value = NULL;
 120         if (invalue)
 121         {
 122                 fields[1].in_handler = arm_jtag_buf_to_u32;
 123                 fields[1].in_handler_priv = invalue;
 124         }
 125         else
 126         {
 127                 fields[1].in_handler = NULL;
 128                 fields[1].in_handler_priv = NULL;
 129         }
 130         fields[1].in_check_value = NULL;
 131         fields[1].in_check_mask = NULL;
 132
 133         jtag_add_dr_scan(2, fields, -1, NULL);
 134
 135         return ERROR_OK;
 136
 137 }
 138
 139 /* scan_inout_check adds one extra inscan for DPAP_READ commands to read variables */
 140 int scan_inout_check(swjdp_common_t *swjdp, u8 chain, u8 reg_addr, u8 RnW, u8 *outvalue, u8 *invalue)
 141 {
 142
 143         swjdp_scan(swjdp->jtag_info, chain, 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 ((chain == 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 chain, u8 reg_addr, u8 RnW, u32 outvalue, u32 *invalue)
 159 {
 160
 161         swjdp_scan_u32(swjdp->jtag_info, chain, 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 ((chain == 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         u8 ack=0;
 181
 182         scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, DP_CTRL_STAT, DPAP_READ, 0, &ctrlstat);
 183         jtag_execute_queue();
 184         swjdp->ack = swjdp->ack&0x7;
 185         while (swjdp->ack != 2)
 186         {
 187                 if (swjdp->ack==1)
 188                 {
 189                         waitcount++;
 190                         if (waitcount>100)
 191                         {
 192                                 WARNING("Timeout waiting for ACK = OK/FAULT in SWJDP transaction");
 193                                 return ERROR_JTAG_DEVICE_ERROR;
 194                         }
 195                 }
 196                 else
 197                 {
 198                         WARNING("Invalid ACK in SWJDP transaction");
 199                         return ERROR_JTAG_DEVICE_ERROR;
 200                 }
 201                 scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, DP_CTRL_STAT, DPAP_READ, 0, &ctrlstat);
 202                 jtag_execute_queue();
 203                 swjdp->ack = swjdp->ack&0x7;
 204         }
 205
 206         /* Check for STICKYERR and STICKYORUN */
 207         if (ctrlstat & (SSTICKYORUN|SSTICKYERR))
 208         {
 209                 DEBUG("      swjdp: CTRL/STAT error 0x%x",ctrlstat);
 210                 /* Check power to debug regions */
 211                 if ((ctrlstat&0xf0000000)!=0xf0000000)
 212                 {
 213                          ahbap_debugport_init(swjdp);
 214                 }
 215                 else
 216                 {
 217                         u32 dcb_dhcsr,nvic_shcsr, nvic_bfar, nvic_cfsr;
 218                         if (ctrlstat&SSTICKYORUN) ERROR("SWJ-DP OVERRUN - check clock or reduce jtag speed");
 219                         if (ctrlstat&SSTICKYERR) ERROR("SWJ-DP STICKY ERROR");
 220                         /* Clear Sticky Error Bits */
 221                         scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, DP_CTRL_STAT, DPAP_WRITE, swjdp->dp_ctrl_stat|SSTICKYORUN|SSTICKYERR, NULL);
 222                         scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, DP_CTRL_STAT, DPAP_READ, 0, &ctrlstat);
 223                         jtag_execute_queue();
 224
 225                         /* Can we find out the reason for the error ?? */
 226                         ahbap_read_system_atomic_u32(swjdp, DCB_DHCSR, &dcb_dhcsr);
 227                         ahbap_read_system_atomic_u32(swjdp, NVIC_SHCSR, &nvic_shcsr);
 228                         ahbap_read_system_atomic_u32(swjdp, NVIC_CFSR, &nvic_cfsr);
 229                         ahbap_read_system_atomic_u32(swjdp, NVIC_BFAR, &nvic_bfar);
 230                         //DEBUG("dcb_dhcsr %x,  nvic_shcsr %x,  nvic_cfsr %x,  nvic_bfar %x",dcb_dhcsr,nvic_shcsr,nvic_cfsr,nvic_bfar);
 231                         ERROR("dcb_dhcsr %x,  nvic_shcsr %x,  nvic_cfsr %x,  nvic_bfar %x",dcb_dhcsr,nvic_shcsr,nvic_cfsr,nvic_bfar);
 232                 }
 233                 jtag_execute_queue();
 234                 return ERROR_JTAG_DEVICE_ERROR;
 235         }
 236
 237         return ERROR_OK;
 238
 239 }
 240
 241 /***************************************************************************
 242  *                                                                         *
 243  * DP and AHB-AP  register access  through APACC and DPACC                 *
 244  *                                                                         *
 245 ***************************************************************************/
 246
 247 int swjdp_write_dpacc(swjdp_common_t *swjdp, u32 value, u8 reg_addr)
 248 {
 249         u8 out_value_buf[4];
 250
 251         buf_set_u32(out_value_buf, 0, 32, value);
 252         return scan_inout_check(swjdp, SWJDP_IR_DPACC, reg_addr, DPAP_WRITE, out_value_buf, NULL);
 253
 254 }
 255
 256 int swjdp_read_dpacc(swjdp_common_t *swjdp, u32 *value, u8 reg_addr)
 257 {
 258
 259         scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, reg_addr, DPAP_READ, 0, value);
 260
 261     return ERROR_OK;
 262 }
 263
 264 int swjdp_bankselect_apacc(swjdp_common_t *swjdp,u32 reg_addr)
 265 {
 266         u32 select;
 267         select = (reg_addr&0xFF0000F0);
 268
 269         if ( select != swjdp->dp_select_value )
 270         {
 271                 swjdp_write_dpacc(swjdp, select, DP_SELECT);
 272                 swjdp->dp_select_value = select;
 273         }
 274
 275         return ERROR_OK;
 276 }
 277
 278 int ahbap_write_reg(swjdp_common_t *swjdp, u32 reg_addr, u8* out_value_buf)
 279 {
 280         swjdp_bankselect_apacc(swjdp, reg_addr);
 281         scan_inout_check(swjdp, SWJDP_IR_APACC, reg_addr, DPAP_WRITE, out_value_buf, NULL);
 282
 283         return ERROR_OK;
 284 }
 285
 286 int ahbap_read_reg(swjdp_common_t *swjdp, u32 reg_addr, u8 *in_value_buf)
 287 {
 288         swjdp_bankselect_apacc(swjdp, reg_addr);
 289         scan_inout_check(swjdp, SWJDP_IR_APACC, reg_addr, DPAP_READ, 0, in_value_buf);
 290
 291         return ERROR_OK;
 292 }
 293 int ahbap_write_reg_u32(swjdp_common_t *swjdp, u32 reg_addr, u32 value)
 294 {
 295         u8 out_value_buf[4];
 296
 297         buf_set_u32(out_value_buf, 0, 32, value);
 298         swjdp_bankselect_apacc(swjdp, reg_addr);
 299         scan_inout_check(swjdp, SWJDP_IR_APACC, reg_addr, DPAP_WRITE, out_value_buf, NULL);
 300
 301         return ERROR_OK;
 302 }
 303
 304 int ahbap_read_reg_u32(swjdp_common_t *swjdp, u32 reg_addr, u32 *value)
 305 {
 306         swjdp_bankselect_apacc(swjdp, reg_addr);
 307         scan_inout_check_u32(swjdp, SWJDP_IR_APACC, reg_addr, DPAP_READ, 0, value);
 308
 309         return ERROR_OK;
 310 }
 311
 312 /***************************************************************************
 313  *                                                                         *
 314  * AHB-AP access to memory and system registers on AHB bus                 *
 315  *                                                                         *
 316 ***************************************************************************/
 317
 318 int ahbap_setup_accessport(swjdp_common_t *swjdp, u32 csw, u32 tar)
 319 {
 320
 321         csw = csw | CSW_DBGSWENABLE | CSW_MASTER_DEBUG | CSW_HPROT;
 322         if ( csw != swjdp->ap_csw_value )
 323         {
 324         //DEBUG("swjdp : Set CSW %x",csw);
 325                 ahbap_write_reg_u32(swjdp, AHBAP_CSW, csw );
 326                 swjdp->ap_csw_value = csw;
 327         }
 328         if ( tar != swjdp->ap_tar_value )
 329         {
 330         //DEBUG("swjdp : Set TAR %x",tar);
 331                 ahbap_write_reg_u32(swjdp, AHBAP_TAR, tar );
 332                 swjdp->ap_tar_value = tar;
 333         }
 334         if (csw & CSW_ADDRINC_MASK)
 335         {
 336                 /* Do not cache TAR value when autoincrementing */
 337                 swjdp->ap_tar_value = -1;
 338         }
 339         return ERROR_OK;
 340 }
 341
 342 /*****************************************************************************
 343 *                                                                            *
 344 * ahbap_read_system_u32(swjdp_common_t *swjdp, u32 address, u32 *value)      *
 345 *                                                                            *
 346 * Read a u32 value from memory or system register                            *
 347 * Functionally equivalent to target_read_u32(target, address, u32 *value),   *
 348 * but with less overhead                                                     *
 349 *****************************************************************************/
 350 int ahbap_read_system_u32(swjdp_common_t *swjdp, u32 address, u32 *value)
 351 {
 352
 353         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 354
 355         ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, address&0xFFFFFFF0);
 356         ahbap_read_reg_u32(swjdp, AHBAP_BD0|address&0xC, value );
 357
 358         return ERROR_OK;
 359 }
 360
 361 int ahbap_read_system_atomic_u32(swjdp_common_t *swjdp, u32 address, u32 *value)
 362 {
 363         ahbap_read_system_u32(swjdp, address, value);
 364
 365         return swjdp_transaction_endcheck(swjdp);
 366 }
 367
 368 /*****************************************************************************
 369 *                                                                            *
 370 * ahbap_write_system_u32(swjdp_common_t *swjdp, u32 address, u32 value)      *
 371 *                                                                            *
 372 * Write a u32 value to memory or system register                             *
 373 *                                                                            *
 374 *****************************************************************************/
 375 int ahbap_write_system_u32(swjdp_common_t *swjdp, u32 address, u32 value)
 376 {
 377
 378         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 379
 380         ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, address&0xFFFFFFF0);
 381         ahbap_write_reg_u32(swjdp, AHBAP_BD0|address&0xC, value );
 382
 383         return ERROR_OK;
 384 }
 385
 386 int ahbap_write_system_atomic_u32(swjdp_common_t *swjdp, u32 address, u32 value)
 387 {
 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 %x, count %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 /*****************************************************************************
 461 *                                                                            *
 462 * ahbap_read_buf(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)  *
 463 *                                                                            *
 464 * Read block fast in target order (little endian) into a buffer       *
 465 *                                                                            *
 466 *****************************************************************************/
 467 int ahbap_read_buf(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 468 {
 469         u32 invalue;
 470         int wcount, blocksize, readcount, errorcount=0, retval=ERROR_OK;
 471
 472         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 473
 474         while ( (address&0x3)&&(count>0) )
 475         {
 476                 ahbap_setup_accessport(swjdp, CSW_8BIT | CSW_ADDRINC_SINGLE, address);
 477                 ahbap_read_reg_u32(swjdp, AHBAP_DRW, &invalue );
 478                 swjdp_transaction_endcheck(swjdp);
 479                 *buffer++ = (invalue>>8*(address&0x3))&0xFF;
 480                 count--;
 481                 address++;
 482         }
 483         wcount = count>>2;
 484         count = count-4*wcount;
 485         while (wcount>0)
 486         {
 487                 /* Adjust to read within 4K block boundaries */
 488                 blocksize = (0x1000-(0xFFF&address))>>2;
 489                 if (wcount<blocksize)
 490                         blocksize = wcount;
 491                 ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_SINGLE, address);
 492                 /* Scan out first read */
 493                 swjdp_scan(swjdp->jtag_info, SWJDP_IR_APACC, AHBAP_DRW, DPAP_READ, 0, NULL, NULL);
 494                 for (readcount=0; readcount<blocksize-1; readcount++)
 495                 {
 496                         /* Scan out read instruction and scan in previous value */
 497                         swjdp_scan(swjdp->jtag_info, SWJDP_IR_APACC, AHBAP_DRW, DPAP_READ, 0, buffer+4*readcount, &swjdp->ack);
 498                 }
 499                 /* Scan in last value */
 500                 swjdp_scan(swjdp->jtag_info, SWJDP_IR_DPACC, 0xC, DPAP_READ, 0, buffer+4*readcount, &swjdp->ack);
 501                 if (swjdp_transaction_endcheck(swjdp)==ERROR_OK)
 502                 {
 503                         wcount = wcount-blocksize;
 504                         address += 4*blocksize;
 505                         buffer += 4*blocksize;
 506                 }
 507                 else
 508                 {
 509                         errorcount++;
 510                 }
 511                 if (errorcount>1)
 512                 {
 513                         WARNING("Block read error address %x, count %x", address, count);
 514                         return ERROR_JTAG_DEVICE_ERROR;
 515                 }
 516         }
 517
 518         while (count>0)
 519         {
 520                 ahbap_setup_accessport(swjdp, CSW_8BIT | CSW_ADDRINC_SINGLE, address);
 521                 ahbap_read_reg_u32(swjdp, AHBAP_DRW, &invalue );
 522                 retval = swjdp_transaction_endcheck(swjdp);
 523                 *buffer++ = (invalue>>8*(address&0x3))&0xFF;
 524                 count--;
 525                 address++;
 526         }
 527
 528         return retval;
 529 }
 530
 531 int ahbap_block_read_u32(swjdp_common_t *swjdp, u32 *buffer, int count, u32 address)
 532 {
 533         int readcount, errorcount=0;
 534         u32 blockmax, blocksize;
 535
 536         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 537
 538         while (count>0)
 539         {
 540                 /* Adjust to read within 4K block boundaries */
 541                 blocksize = (0x1000-(0xFFF&address))>>2;
 542                 if (count<blocksize)
 543                         blocksize = count;
 544                 ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_SINGLE, address);
 545                 for (readcount=0; readcount<blocksize; readcount++)
 546                 {
 547                         ahbap_read_reg_u32(swjdp, AHBAP_DRW, buffer+readcount );
 548                 }
 549                 if (swjdp_transaction_endcheck(swjdp)==ERROR_OK)
 550                 {
 551                         count = count-blocksize;
 552                         address = address+4*blocksize;
 553                         buffer = buffer + blocksize;
 554                 }
 555                 else
 556                 {
 557                         errorcount++;
 558                 }
 559                 if (errorcount>1)
 560                 {
 561                         WARNING("Block read error address %x, count %x", address, count);
 562                         return ERROR_JTAG_DEVICE_ERROR;
 563                 }
 564         }
 565
 566         return ERROR_OK;
 567 }
 568
 569 int ahbap_read_coreregister_u32(swjdp_common_t *swjdp, u32 *value, int regnum)
 570 {
 571         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 572
 573         /* ahbap_write_system_u32(swjdp, DCB_DCRSR, regnum); */
 574         ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, DCB_DCRSR&0xFFFFFFF0);
 575         ahbap_write_reg_u32(swjdp, AHBAP_BD0|DCB_DCRSR&0xC, regnum );
 576
 577         /* ahbap_read_system_u32(swjdp, DCB_DCRDR, value); */
 578         ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, DCB_DCRDR&0xFFFFFFF0);
 579         ahbap_read_reg_u32(swjdp, AHBAP_BD0|DCB_DCRDR&0xC, value );
 580
 581         return swjdp_transaction_endcheck(swjdp);
 582 }
 583
 584 int ahbap_write_coreregister_u32(swjdp_common_t *swjdp, u32 value, int regnum)
 585 {
 586         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 587
 588         /* ahbap_write_system_u32(swjdp, DCB_DCRDR, core_regs[i]); */
 589         ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, DCB_DCRDR&0xFFFFFFF0);
 590         ahbap_write_reg_u32(swjdp, AHBAP_BD0|DCB_DCRDR&0xC, value );
 591
 592         /* ahbap_write_system_u32(swjdp, DCB_DCRSR, i | DCRSR_WnR       ); */
 593         ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, DCB_DCRSR&0xFFFFFFF0);
 594         ahbap_write_reg_u32(swjdp, AHBAP_BD0|DCB_DCRSR&0xC, regnum | DCRSR_WnR );
 595
 596         return swjdp_transaction_endcheck(swjdp);
 597 }
 598
 599 int ahbap_debugport_init(swjdp_common_t *swjdp)
 600 {
 601
 602 u32 idreg, romaddr, dummy;
 603 u32 ctrlstat;
 604 int cnt=0;
 605 DEBUG("");
 606         swjdp->ap_csw_value = -1;
 607         swjdp->ap_tar_value = -1;
 608         swjdp->trans_mode = TRANS_MODE_ATOMIC;
 609         swjdp_read_dpacc(swjdp, &dummy, DP_CTRL_STAT);
 610         swjdp_write_dpacc(swjdp, SSTICKYERR, DP_CTRL_STAT);
 611         swjdp_read_dpacc(swjdp, &dummy, DP_CTRL_STAT);
 612
 613         swjdp->dp_ctrl_stat = CDBGPWRUPREQ|CSYSPWRUPREQ;
 614
 615         swjdp_write_dpacc(swjdp, swjdp->dp_ctrl_stat, DP_CTRL_STAT);
 616         swjdp_read_dpacc(swjdp, &ctrlstat, DP_CTRL_STAT);
 617         jtag_execute_queue();
 618
 619         /* Check that we have debug power domains activated */
 620         while (!(ctrlstat & CDBGPWRUPACK) && (cnt++<10))
 621         {
 622                 DEBUG("      swjdp: wait CDBGPWRUPACK");
 623                 swjdp_read_dpacc(swjdp, &ctrlstat, DP_CTRL_STAT);
 624                 jtag_execute_queue();
 625
 626                 usleep(10000);
 627         }
 628
 629         while (!(ctrlstat & CSYSPWRUPACK) && (cnt++<10))
 630         {
 631                 DEBUG("      swjdp: wait CSYSPWRUPACK");
 632                 swjdp_read_dpacc(swjdp, &ctrlstat, DP_CTRL_STAT);
 633                 jtag_execute_queue();
 634                 usleep(10000);
 635         }
 636
 637
 638         swjdp_read_dpacc(swjdp, &dummy, DP_CTRL_STAT);
 639         /* With debug power on we can activate OVERRUN checking */
 640         swjdp->dp_ctrl_stat = CDBGPWRUPREQ|CSYSPWRUPREQ|CORUNDETECT;
 641         swjdp_write_dpacc(swjdp, swjdp->dp_ctrl_stat , DP_CTRL_STAT);
 642         swjdp_read_dpacc(swjdp, &dummy, DP_CTRL_STAT);
 643
 644         ahbap_read_reg_u32(swjdp, 0xFC, &idreg );
 645         ahbap_read_reg_u32(swjdp, 0xF8, &romaddr );
 646
 647 DEBUG("AHB-AP ID Register 0x%x, Debug ROM Address 0x%x",idreg,romaddr);
 648
 649         return ERROR_OK;
 650 }