git.sur5r.net Git - openocd/blob - 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 int swjdp_jtag_error_handler(u8 *in_value, void *priv)
  58 {
  59         char *caller = priv;
  60
  61         DEBUG("caller: %s", caller);
  62
  63         return ERROR_OK;
  64 }
  65
  66 /* Scan out and in from target ordered u8 buffers */
  67 int swjdp_scan(arm_jtag_t *jtag_info, u8 chain, u8 reg_addr, u8 RnW, u8 *outvalue, u8 *invalue, u8 *ack)
  68 {
  69         scan_field_t fields[2];
  70         u8 out_addr_buf;
  71         error_handler_t error_handler;
  72
  73         jtag_add_end_state(TAP_RTI);
  74         error_handler.error_handler = swjdp_jtag_error_handler;
  75         error_handler.error_handler_priv = "swjdp_scan";
  76         arm_jtag_set_instr(jtag_info, chain, &error_handler);
  77
  78         fields[0].device = jtag_info->chain_pos;
  79         fields[0].num_bits = 3;
  80         buf_set_u32(&out_addr_buf, 0, 3, ((reg_addr>>1)&0x6) | (RnW&0x1));
  81         fields[0].out_value = &out_addr_buf;
  82         fields[0].out_mask = NULL;
  83         fields[0].in_value = ack;
  84         fields[0].in_check_value = NULL;
  85         fields[0].in_check_mask = NULL;
  86         fields[0].in_handler = NULL;
  87         fields[0].in_handler_priv = NULL;
  88
  89         fields[1].device = jtag_info->chain_pos;
  90         fields[1].num_bits = 32;
  91         fields[1].out_value = outvalue;
  92         fields[1].out_mask = NULL;
  93         fields[1].in_value = invalue;
  94         fields[1].in_handler = NULL;
  95         fields[1].in_handler_priv = NULL;
  96         fields[1].in_check_value = NULL;
  97         fields[1].in_check_mask = NULL;
  98
  99         jtag_add_dr_scan(2, fields, -1, NULL);
 100
 101         return ERROR_OK;
 102
 103 }
 104
 105 /* Scan out and in from host ordered u32 variables */
 106 int swjdp_scan_u32(arm_jtag_t *jtag_info, u8 chain, u8 reg_addr, u8 RnW, u32 outvalue, u32 *invalue, u8 *ack)
 107 {
 108         scan_field_t fields[2];
 109         u8 out_value_buf[4];
 110         u8 out_addr_buf;
 111         error_handler_t error_handler;
 112
 113         jtag_add_end_state(TAP_RTI);
 114         error_handler.error_handler = swjdp_jtag_error_handler;
 115         error_handler.error_handler_priv = "swjdp_scan_u32";
 116         arm_jtag_set_instr(jtag_info, chain, &error_handler);
 117
 118         fields[0].device = jtag_info->chain_pos;
 119         fields[0].num_bits = 3;
 120         buf_set_u32(&out_addr_buf, 0, 3, ((reg_addr>>1)&0x6) | (RnW&0x1));
 121         fields[0].out_value = &out_addr_buf;
 122         fields[0].out_mask = NULL;
 123         fields[0].in_value = ack;
 124         fields[0].in_check_value = NULL;
 125         fields[0].in_check_mask = NULL;
 126         fields[0].in_handler = NULL;
 127         fields[0].in_handler_priv = NULL;
 128
 129         fields[1].device = jtag_info->chain_pos;
 130         fields[1].num_bits = 32;
 131         buf_set_u32(out_value_buf, 0, 32, outvalue);
 132         fields[1].out_value = out_value_buf;
 133         fields[1].out_mask = NULL;
 134         fields[1].in_value = NULL;
 135         if (invalue)
 136         {
 137                 fields[1].in_handler = arm_jtag_buf_to_u32;
 138                 fields[1].in_handler_priv = invalue;
 139         }
 140         else
 141         {
 142                 fields[1].in_handler = NULL;
 143                 fields[1].in_handler_priv = NULL;
 144         }
 145         fields[1].in_check_value = NULL;
 146         fields[1].in_check_mask = NULL;
 147
 148         jtag_add_dr_scan(2, fields, -1, NULL);
 149
 150         return ERROR_OK;
 151
 152 }
 153
 154 /* scan_inout_check adds one extra inscan for DPAP_READ commands to read variables */
 155 int scan_inout_check(swjdp_common_t *swjdp, u8 chain, u8 reg_addr, u8 RnW, u8 *outvalue, u8 *invalue)
 156 {
 157
 158         swjdp_scan(swjdp->jtag_info, chain, reg_addr, RnW, outvalue, NULL, NULL);
 159         if ((RnW==DPAP_READ) && (invalue != NULL))
 160         {
 161                 swjdp_scan(swjdp->jtag_info, SWJDP_IR_DPACC, 0xC, DPAP_READ, 0, invalue, &swjdp->ack);
 162         }
 163
 164         /* In TRANS_MODE_ATOMIC all SWJDP_IR_APACC transactions wait for ack=OK/FAULT and the check CTRL_STAT */
 165         if ((chain == SWJDP_IR_APACC)&&(swjdp->trans_mode == TRANS_MODE_ATOMIC))
 166         {
 167                 return swjdp_transaction_endcheck(swjdp);
 168         }
 169
 170         return ERROR_OK;
 171 }
 172
 173 int scan_inout_check_u32(swjdp_common_t *swjdp, u8 chain, u8 reg_addr, u8 RnW, u32 outvalue, u32 *invalue)
 174 {
 175
 176         swjdp_scan_u32(swjdp->jtag_info, chain, reg_addr, RnW, outvalue, NULL, NULL);
 177         if ((RnW==DPAP_READ) && (invalue != NULL))
 178         {
 179                 swjdp_scan_u32(swjdp->jtag_info, SWJDP_IR_DPACC, 0xC, DPAP_READ, 0, invalue, &swjdp->ack);
 180         }
 181
 182         /* In TRANS_MODE_ATOMIC all SWJDP_IR_APACC transactions wait for ack=OK/FAULT and the check CTRL_STAT */
 183         if ((chain == SWJDP_IR_APACC)&&(swjdp->trans_mode == TRANS_MODE_ATOMIC))
 184         {
 185                 return swjdp_transaction_endcheck(swjdp);
 186         }
 187
 188         return ERROR_OK;
 189 }
 190
 191 int swjdp_transaction_endcheck(swjdp_common_t *swjdp)
 192 {
 193         int waitcount = 0;
 194         u32 ctrlstat;
 195         u8 ack=0;
 196
 197         scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, DP_CTRL_STAT, DPAP_READ, 0, &ctrlstat);
 198         jtag_execute_queue();
 199         swjdp->ack = swjdp->ack&0x7;
 200         while (swjdp->ack != 2)
 201         {
 202                 if (swjdp->ack==1)
 203                 {
 204                         waitcount++;
 205                         if (waitcount>100)
 206                         {
 207                                 WARNING("Timeout waiting for ACK = OK/FAULT in SWJDP transaction");
 208                                 return ERROR_JTAG_DEVICE_ERROR;
 209                         }
 210                 }
 211                 else
 212                 {
 213                         WARNING("Invalid ACK in SWJDP transaction");
 214                         return ERROR_JTAG_DEVICE_ERROR;
 215                 }
 216                 scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, DP_CTRL_STAT, DPAP_READ, 0, &ctrlstat);
 217                 jtag_execute_queue();
 218                 swjdp->ack = swjdp->ack&0x7;
 219         }
 220
 221         /* Check for STICKYERR and STICKYORUN */
 222         if (ctrlstat & (SSTICKYORUN|SSTICKYERR))
 223         {
 224                 DEBUG("      swjdp: CTRL/STAT error 0x%x",ctrlstat);
 225                 /* Check power to debug regions */
 226                 if ((ctrlstat&0xf0000000)!=0xf0000000)
 227                 {
 228                          ahbap_debugport_init(swjdp);
 229                 }
 230                 else
 231                 {
 232                         u32 dcb_dhcsr,nvic_shcsr, nvic_bfar, nvic_cfsr;
 233                         if (ctrlstat&SSTICKYORUN) ERROR("SWJ-DP OVERRUN - check clock or reduce jtag speed");
 234                         if (ctrlstat&SSTICKYERR) ERROR("SWJ-DP STICKY ERROR");
 235                         /* Clear Sticky Error Bits */
 236                         scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, DP_CTRL_STAT, DPAP_WRITE, swjdp->dp_ctrl_stat|SSTICKYORUN|SSTICKYERR, NULL);
 237                         scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, DP_CTRL_STAT, DPAP_READ, 0, &ctrlstat);
 238                         jtag_execute_queue();
 239
 240                         /* Can we find out the reason for the error ?? */
 241                         ahbap_read_system_atomic_u32(swjdp, DCB_DHCSR, &dcb_dhcsr);
 242                         ahbap_read_system_atomic_u32(swjdp, NVIC_SHCSR, &nvic_shcsr);
 243                         ahbap_read_system_atomic_u32(swjdp, NVIC_CFSR, &nvic_cfsr);
 244                         ahbap_read_system_atomic_u32(swjdp, NVIC_BFAR, &nvic_bfar);
 245                         //DEBUG("dcb_dhcsr %x,  nvic_shcsr %x,  nvic_cfsr %x,  nvic_bfar %x",dcb_dhcsr,nvic_shcsr,nvic_cfsr,nvic_bfar);
 246                         ERROR("dcb_dhcsr %x,  nvic_shcsr %x,  nvic_cfsr %x,  nvic_bfar %x",dcb_dhcsr,nvic_shcsr,nvic_cfsr,nvic_bfar);
 247                 }
 248                 jtag_execute_queue();
 249                 return ERROR_JTAG_DEVICE_ERROR;
 250         }
 251
 252         return ERROR_OK;
 253
 254 }
 255
 256 /***************************************************************************
 257  *                                                                         *
 258  * DP and AHB-AP  register access  through APACC and DPACC                 *
 259  *                                                                         *
 260 ***************************************************************************/
 261
 262 int swjdp_write_dpacc(swjdp_common_t *swjdp, u32 value, u8 reg_addr)
 263 {
 264         u8 out_value_buf[4];
 265
 266         buf_set_u32(out_value_buf, 0, 32, value);
 267         return scan_inout_check(swjdp, SWJDP_IR_DPACC, reg_addr, DPAP_WRITE, out_value_buf, NULL);
 268
 269 }
 270
 271 int swjdp_read_dpacc(swjdp_common_t *swjdp, u32 *value, u8 reg_addr)
 272 {
 273
 274         scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, reg_addr, DPAP_READ, 0, value);
 275
 276     return ERROR_OK;
 277 }
 278
 279 int swjdp_bankselect_apacc(swjdp_common_t *swjdp,u32 reg_addr)
 280 {
 281         u32 select;
 282         select = (reg_addr&0xFF0000F0);
 283
 284         if ( select != swjdp->dp_select_value )
 285         {
 286                 swjdp_write_dpacc(swjdp, select, DP_SELECT);
 287                 swjdp->dp_select_value = select;
 288         }
 289
 290         return ERROR_OK;
 291 }
 292
 293 int ahbap_write_reg(swjdp_common_t *swjdp, u32 reg_addr, u8* out_value_buf)
 294 {
 295         swjdp_bankselect_apacc(swjdp, reg_addr);
 296         scan_inout_check(swjdp, SWJDP_IR_APACC, reg_addr, DPAP_WRITE, out_value_buf, NULL);
 297
 298         return ERROR_OK;
 299 }
 300
 301 int ahbap_read_reg(swjdp_common_t *swjdp, u32 reg_addr, u8 *in_value_buf)
 302 {
 303         swjdp_bankselect_apacc(swjdp, reg_addr);
 304         scan_inout_check(swjdp, SWJDP_IR_APACC, reg_addr, DPAP_READ, 0, in_value_buf);
 305
 306         return ERROR_OK;
 307 }
 308 int ahbap_write_reg_u32(swjdp_common_t *swjdp, u32 reg_addr, u32 value)
 309 {
 310         u8 out_value_buf[4];
 311
 312         buf_set_u32(out_value_buf, 0, 32, value);
 313         swjdp_bankselect_apacc(swjdp, reg_addr);
 314         scan_inout_check(swjdp, SWJDP_IR_APACC, reg_addr, DPAP_WRITE, out_value_buf, NULL);
 315
 316         return ERROR_OK;
 317 }
 318
 319 int ahbap_read_reg_u32(swjdp_common_t *swjdp, u32 reg_addr, u32 *value)
 320 {
 321         swjdp_bankselect_apacc(swjdp, reg_addr);
 322         scan_inout_check_u32(swjdp, SWJDP_IR_APACC, reg_addr, DPAP_READ, 0, value);
 323
 324         return ERROR_OK;
 325 }
 326
 327 /***************************************************************************
 328  *                                                                         *
 329  * AHB-AP access to memory and system registers on AHB bus                 *
 330  *                                                                         *
 331 ***************************************************************************/
 332
 333 int ahbap_setup_accessport(swjdp_common_t *swjdp, u32 csw, u32 tar)
 334 {
 335
 336         csw = csw | CSW_DBGSWENABLE | CSW_MASTER_DEBUG | CSW_HPROT;
 337         if ( csw != swjdp->ap_csw_value )
 338         {
 339         //DEBUG("swjdp : Set CSW %x",csw);
 340                 ahbap_write_reg_u32(swjdp, AHBAP_CSW, csw );
 341                 swjdp->ap_csw_value = csw;
 342         }
 343         if ( tar != swjdp->ap_tar_value )
 344         {
 345         //DEBUG("swjdp : Set TAR %x",tar);
 346                 ahbap_write_reg_u32(swjdp, AHBAP_TAR, tar );
 347                 swjdp->ap_tar_value = tar;
 348         }
 349         if (csw & CSW_ADDRINC_MASK)
 350         {
 351                 /* Do not cache TAR value when autoincrementing */
 352                 swjdp->ap_tar_value = -1;
 353         }
 354         return ERROR_OK;
 355 }
 356
 357 /*****************************************************************************
 358 *                                                                            *
 359 * ahbap_read_system_u32(swjdp_common_t *swjdp, u32 address, u32 *value)      *
 360 *                                                                            *
 361 * Read a u32 value from memory or system register                            *
 362 * Functionally equivalent to target_read_u32(target, address, u32 *value),   *
 363 * but with less overhead                                                     *
 364 *****************************************************************************/
 365 int ahbap_read_system_u32(swjdp_common_t *swjdp, u32 address, u32 *value)
 366 {
 367
 368         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 369
 370         ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, address&0xFFFFFFF0);
 371         ahbap_read_reg_u32(swjdp, AHBAP_BD0|address&0xC, value );
 372
 373         return ERROR_OK;
 374 }
 375
 376 int ahbap_read_system_atomic_u32(swjdp_common_t *swjdp, u32 address, u32 *value)
 377 {
 378         ahbap_read_system_u32(swjdp, address, value);
 379
 380         return swjdp_transaction_endcheck(swjdp);
 381 }
 382
 383 /*****************************************************************************
 384 *                                                                            *
 385 * ahbap_write_system_u32(swjdp_common_t *swjdp, u32 address, u32 value)      *
 386 *                                                                            *
 387 * Write a u32 value to memory or system register                             *
 388 *                                                                            *
 389 *****************************************************************************/
 390 int ahbap_write_system_u32(swjdp_common_t *swjdp, u32 address, u32 value)
 391 {
 392
 393         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 394
 395         ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, address&0xFFFFFFF0);
 396         ahbap_write_reg_u32(swjdp, AHBAP_BD0|address&0xC, value );
 397
 398         return ERROR_OK;
 399 }
 400
 401 int ahbap_write_system_atomic_u32(swjdp_common_t *swjdp, u32 address, u32 value)
 402 {
 403
 404         ahbap_write_system_u32(swjdp, address, value);
 405
 406         return swjdp_transaction_endcheck(swjdp);
 407 }
 408
 409 /*****************************************************************************
 410 *                                                                            *
 411 * ahbap_write_buf(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address) *
 412 *                                                                            *
 413 * Write a buffer in target order (little endian)                             *
 414 *                                                                            *
 415 *****************************************************************************/
 416 int ahbap_write_buf(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 417 {
 418         u32 outvalue;
 419         int wcount, blocksize, writecount, errorcount=0, retval=ERROR_OK;
 420
 421         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 422
 423         while ( (address&0x3)&&(count>0) )
 424         {
 425                 ahbap_setup_accessport(swjdp, CSW_8BIT | CSW_ADDRINC_SINGLE, address);
 426                 outvalue = (*buffer++)<<8*(address&0x3) ;
 427                 ahbap_write_reg_u32(swjdp, AHBAP_DRW, outvalue );
 428                 swjdp_transaction_endcheck(swjdp);
 429                 count--;
 430                 address++;
 431         }
 432         wcount = count>>2;
 433         count = count-4*wcount;
 434         while (wcount>0)
 435         {
 436                 /* Adjust to read within 4K block boundaries */
 437                 blocksize = (0x1000-(0xFFF&address))>>2;
 438                 if (wcount<blocksize)
 439                         blocksize = wcount;
 440                 ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_SINGLE, address);
 441                 for (writecount=0; writecount<blocksize; writecount++)
 442                 {
 443                         ahbap_write_reg(swjdp, AHBAP_DRW, buffer+4*writecount );
 444                 }
 445                 if (swjdp_transaction_endcheck(swjdp)==ERROR_OK)
 446                 {
 447                         wcount = wcount-blocksize;
 448                         address = address+4*blocksize;
 449                         buffer = buffer + 4*blocksize;
 450                 }
 451                 else
 452                 {
 453                         errorcount++;
 454                 }
 455                 if (errorcount>1)
 456                 {
 457                         WARNING("Block read error address %x, count %x", address, count);
 458                         return ERROR_JTAG_DEVICE_ERROR;
 459                 }
 460         }
 461
 462         while (count>0)
 463         {
 464                 ahbap_setup_accessport(swjdp, CSW_8BIT | CSW_ADDRINC_SINGLE, address);
 465                 outvalue = (*buffer++)<<8*(address&0x3) ;
 466                 ahbap_write_reg_u32(swjdp, AHBAP_DRW, outvalue );
 467                 retval = swjdp_transaction_endcheck(swjdp);
 468                 count--;
 469                 address++;
 470         }
 471
 472         return retval;
 473 }
 474
 475 /*****************************************************************************
 476 *                                                                            *
 477 * ahbap_read_buf(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)  *
 478 *                                                                            *
 479 * Read block fast in target order (little endian) into a buffer       *
 480 *                                                                            *
 481 *****************************************************************************/
 482 int ahbap_read_buf(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 483 {
 484         u32 invalue;
 485         int wcount, blocksize, readcount, errorcount=0, retval=ERROR_OK;
 486
 487         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 488
 489         while ( (address&0x3)&&(count>0) )
 490         {
 491                 ahbap_setup_accessport(swjdp, CSW_8BIT | CSW_ADDRINC_SINGLE, address);
 492                 ahbap_read_reg_u32(swjdp, AHBAP_DRW, &invalue );
 493                 swjdp_transaction_endcheck(swjdp);
 494                 *buffer++ = (invalue>>8*(address&0x3))&0xFF;
 495                 count--;
 496                 address++;
 497         }
 498         wcount = count>>2;
 499         count = count-4*wcount;
 500         while (wcount>0)
 501         {
 502                 /* Adjust to read within 4K block boundaries */
 503                 blocksize = (0x1000-(0xFFF&address))>>2;
 504                 if (wcount<blocksize)
 505                         blocksize = wcount;
 506                 ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_SINGLE, address);
 507                 /* Scan out first read */
 508                 swjdp_scan(swjdp->jtag_info, SWJDP_IR_APACC, AHBAP_DRW, DPAP_READ, 0, NULL, NULL);
 509                 for (readcount=0; readcount<blocksize-1; readcount++)
 510                 {
 511                         /* Scan out read instruction and scan in previous value */
 512                         swjdp_scan(swjdp->jtag_info, SWJDP_IR_APACC, AHBAP_DRW, DPAP_READ, 0, buffer+4*readcount, &swjdp->ack);
 513                 }
 514                 /* Scan in last value */
 515                 swjdp_scan(swjdp->jtag_info, SWJDP_IR_DPACC, 0xC, DPAP_READ, 0, buffer+4*readcount, &swjdp->ack);
 516                 if (swjdp_transaction_endcheck(swjdp)==ERROR_OK)
 517                 {
 518                         wcount = wcount-blocksize;
 519                         address += 4*blocksize;
 520                         buffer += 4*blocksize;
 521                 }
 522                 else
 523                 {
 524                         errorcount++;
 525                 }
 526                 if (errorcount>1)
 527                 {
 528                         WARNING("Block read error address %x, count %x", address, count);
 529                         return ERROR_JTAG_DEVICE_ERROR;
 530                 }
 531         }
 532
 533         while (count>0)
 534         {
 535                 ahbap_setup_accessport(swjdp, CSW_8BIT | CSW_ADDRINC_SINGLE, address);
 536                 ahbap_read_reg_u32(swjdp, AHBAP_DRW, &invalue );
 537                 retval = swjdp_transaction_endcheck(swjdp);
 538                 *buffer++ = (invalue>>8*(address&0x3))&0xFF;
 539                 count--;
 540                 address++;
 541         }
 542
 543         return retval;
 544 }
 545
 546 int ahbap_block_read_u32(swjdp_common_t *swjdp, u32 *buffer, int count, u32 address)
 547 {
 548         int readcount, errorcount=0;
 549         u32 blockmax, blocksize;
 550
 551         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 552
 553         while (count>0)
 554         {
 555                 /* Adjust to read within 4K block boundaries */
 556                 blocksize = (0x1000-(0xFFF&address))>>2;
 557                 if (count<blocksize)
 558                         blocksize = count;
 559                 ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_SINGLE, address);
 560                 for (readcount=0; readcount<blocksize; readcount++)
 561                 {
 562                         ahbap_read_reg_u32(swjdp, AHBAP_DRW, buffer+readcount );
 563                 }
 564                 if (swjdp_transaction_endcheck(swjdp)==ERROR_OK)
 565                 {
 566                         count = count-blocksize;
 567                         address = address+4*blocksize;
 568                         buffer = buffer + blocksize;
 569                 }
 570                 else
 571                 {
 572                         errorcount++;
 573                 }
 574                 if (errorcount>1)
 575                 {
 576                         WARNING("Block read error address %x, count %x", address, count);
 577                         return ERROR_JTAG_DEVICE_ERROR;
 578                 }
 579         }
 580
 581         return ERROR_OK;
 582 }
 583
 584 int ahbap_read_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_DCRSR, regnum); */
 589         ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, DCB_DCRSR&0xFFFFFFF0);
 590         ahbap_write_reg_u32(swjdp, AHBAP_BD0|DCB_DCRSR&0xC, regnum );
 591
 592         /* ahbap_read_system_u32(swjdp, DCB_DCRDR, value); */
 593         ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, DCB_DCRDR&0xFFFFFFF0);
 594         ahbap_read_reg_u32(swjdp, AHBAP_BD0|DCB_DCRDR&0xC, value );
 595
 596         return swjdp_transaction_endcheck(swjdp);
 597 }
 598
 599 int ahbap_write_coreregister_u32(swjdp_common_t *swjdp, u32 value, int regnum)
 600 {
 601         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 602
 603         /* ahbap_write_system_u32(swjdp, DCB_DCRDR, core_regs[i]); */
 604         ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, DCB_DCRDR&0xFFFFFFF0);
 605         ahbap_write_reg_u32(swjdp, AHBAP_BD0|DCB_DCRDR&0xC, value );
 606
 607         /* ahbap_write_system_u32(swjdp, DCB_DCRSR, i | DCRSR_WnR       ); */
 608         ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, DCB_DCRSR&0xFFFFFFF0);
 609         ahbap_write_reg_u32(swjdp, AHBAP_BD0|DCB_DCRSR&0xC, regnum | DCRSR_WnR );
 610
 611         return swjdp_transaction_endcheck(swjdp);
 612 }
 613
 614 int ahbap_debugport_init(swjdp_common_t *swjdp)
 615 {
 616
 617 u32 idreg, romaddr, dummy;
 618 u32 ctrlstat;
 619 int cnt=0;
 620 DEBUG("");
 621         swjdp->ap_csw_value = -1;
 622         swjdp->ap_tar_value = -1;
 623         swjdp->trans_mode = TRANS_MODE_ATOMIC;
 624         swjdp_read_dpacc(swjdp, &dummy, DP_CTRL_STAT);
 625         swjdp_write_dpacc(swjdp, SSTICKYERR, DP_CTRL_STAT);
 626         swjdp_read_dpacc(swjdp, &dummy, DP_CTRL_STAT);
 627
 628         swjdp->dp_ctrl_stat = CDBGPWRUPREQ|CSYSPWRUPREQ;
 629
 630         swjdp_write_dpacc(swjdp, swjdp->dp_ctrl_stat, DP_CTRL_STAT);
 631         swjdp_read_dpacc(swjdp, &ctrlstat, DP_CTRL_STAT);
 632         jtag_execute_queue();
 633
 634         /* Check that we have debug power domains activated */
 635         while (!(ctrlstat & CDBGPWRUPACK) && (cnt++<10))
 636         {
 637                 DEBUG("      swjdp: wait CDBGPWRUPACK");
 638                 swjdp_read_dpacc(swjdp, &ctrlstat, DP_CTRL_STAT);
 639                 jtag_execute_queue();
 640
 641                 usleep(10000);
 642         }
 643
 644         while (!(ctrlstat & CSYSPWRUPACK) && (cnt++<10))
 645         {
 646                 DEBUG("      swjdp: wait CSYSPWRUPACK");
 647                 swjdp_read_dpacc(swjdp, &ctrlstat, DP_CTRL_STAT);
 648                 jtag_execute_queue();
 649                 usleep(10000);
 650         }
 651
 652
 653         swjdp_read_dpacc(swjdp, &dummy, DP_CTRL_STAT);
 654         /* With debug power on we can activate OVERRUN checking */
 655         swjdp->dp_ctrl_stat = CDBGPWRUPREQ|CSYSPWRUPREQ|CORUNDETECT;
 656         swjdp_write_dpacc(swjdp, swjdp->dp_ctrl_stat , DP_CTRL_STAT);
 657         swjdp_read_dpacc(swjdp, &dummy, DP_CTRL_STAT);
 658
 659         ahbap_read_reg_u32(swjdp, 0xFC, &idreg );
 660         ahbap_read_reg_u32(swjdp, 0xF8, &romaddr );
 661
 662 DEBUG("AHB-AP ID Register 0x%x, Debug ROM Address 0x%x",idreg,romaddr);
 663
 664         return ERROR_OK;
 665 }