git.gag.com Git - fw/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 #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  * Transaction Mode:
  42  * swjdp->trans_mode = TRANS_MODE_COMPOSITE;
  43  * Uses Overrun checking mode and does not do actual JTAG send/receive or transaction
  44  * result checking until swjdp_end_transaction()
  45  * This must be done before using or deallocating any return variables.
  46  * swjdp->trans_mode == TRANS_MODE_ATOMIC
  47  * All reads and writes to the AHB bus are checked for valid completion, and return values
  48  * are immediatley available.
  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 instr, 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, instr, 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);
  88
  89         return ERROR_OK;
  90 }
  91
  92 /* Scan out and in from host ordered u32 variables */
  93 int swjdp_scan_u32(arm_jtag_t *jtag_info, u8 instr, u8 reg_addr, u8 RnW, u32 outvalue, u32 *invalue, u8 *ack)
  94 {
  95         scan_field_t fields[2];
  96         u8 out_value_buf[4];
  97         u8 out_addr_buf;
  98
  99         jtag_add_end_state(TAP_RTI);
 100         arm_jtag_set_instr(jtag_info, instr, NULL);
 101
 102         fields[0].device = jtag_info->chain_pos;
 103         fields[0].num_bits = 3;
 104         buf_set_u32(&out_addr_buf, 0, 3, ((reg_addr >> 1) & 0x6) | (RnW & 0x1));
 105         fields[0].out_value = &out_addr_buf;
 106         fields[0].out_mask = NULL;
 107         fields[0].in_value = ack;
 108         fields[0].in_check_value = NULL;
 109         fields[0].in_check_mask = NULL;
 110         fields[0].in_handler = NULL;
 111         fields[0].in_handler_priv = NULL;
 112
 113         fields[1].device = jtag_info->chain_pos;
 114         fields[1].num_bits = 32;
 115         buf_set_u32(out_value_buf, 0, 32, outvalue);
 116         fields[1].out_value = out_value_buf;
 117         fields[1].out_mask = NULL;
 118         fields[1].in_value = NULL;
 119         if (invalue)
 120         {
 121                 fields[1].in_handler = arm_jtag_buf_to_u32;
 122                 fields[1].in_handler_priv = invalue;
 123         }
 124         else
 125         {
 126                 fields[1].in_handler = NULL;
 127                 fields[1].in_handler_priv = NULL;
 128         }
 129         fields[1].in_check_value = NULL;
 130         fields[1].in_check_mask = NULL;
 131
 132         jtag_add_dr_scan(2, fields, -1);
 133
 134         return ERROR_OK;
 135 }
 136
 137 /* scan_inout_check adds one extra inscan for DPAP_READ commands to read variables */
 138 int scan_inout_check(swjdp_common_t *swjdp, u8 instr, u8 reg_addr, u8 RnW, u8 *outvalue, u8 *invalue)
 139 {
 140         swjdp_scan(swjdp->jtag_info, instr, reg_addr, RnW, outvalue, NULL, NULL);
 141         if ((RnW == DPAP_READ) && (invalue != NULL))
 142         {
 143                 swjdp_scan(swjdp->jtag_info, SWJDP_IR_DPACC, DP_RDBUFF, DPAP_READ, 0, invalue, &swjdp->ack);
 144         }
 145
 146         /* In TRANS_MODE_ATOMIC all SWJDP_IR_APACC transactions wait for ack=OK/FAULT and the check CTRL_STAT */
 147         if ((instr == SWJDP_IR_APACC) && (swjdp->trans_mode == TRANS_MODE_ATOMIC))
 148         {
 149                 return swjdp_transaction_endcheck(swjdp);
 150         }
 151
 152         return ERROR_OK;
 153 }
 154
 155 int scan_inout_check_u32(swjdp_common_t *swjdp, u8 instr, u8 reg_addr, u8 RnW, u32 outvalue, u32 *invalue)
 156 {
 157         swjdp_scan_u32(swjdp->jtag_info, instr, reg_addr, RnW, outvalue, NULL, NULL);
 158         if ((RnW==DPAP_READ) && (invalue != NULL))
 159         {
 160                 swjdp_scan_u32(swjdp->jtag_info, SWJDP_IR_DPACC, DP_RDBUFF, DPAP_READ, 0, invalue, &swjdp->ack);
 161         }
 162
 163         /* In TRANS_MODE_ATOMIC all SWJDP_IR_APACC transactions wait for ack=OK/FAULT and then check CTRL_STAT */
 164         if ((instr == SWJDP_IR_APACC) && (swjdp->trans_mode == TRANS_MODE_ATOMIC))
 165         {
 166                 return swjdp_transaction_endcheck(swjdp);
 167         }
 168
 169         return ERROR_OK;
 170 }
 171
 172 int swjdp_transaction_endcheck(swjdp_common_t *swjdp)
 173 {
 174         int retval;
 175         int waitcount = 0;
 176         u32 ctrlstat;
 177
 178         keep_alive();
 179
 180         /* Danger!!!! BROKEN!!!! */
 181         scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, DP_CTRL_STAT, DPAP_READ, 0, &ctrlstat);
 182         /* Danger!!!! BROKEN!!!! Why will jtag_execute_queue() fail here????
 183         R956 introduced the check on return value here and now Michael Schwingen reports
 184         that this code no longer works....
 185
 186         https://lists.berlios.de/pipermail/openocd-development/2008-September/003107.html
 187         */
 188         if ((retval=jtag_execute_queue())!=ERROR_OK)
 189         {
 190                 LOG_ERROR("BUG: Why does this fail the first time????");
 191         }
 192         /* Why??? second time it works??? */
 193         scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, DP_CTRL_STAT, DPAP_READ, 0, &ctrlstat);
 194         if ((retval=jtag_execute_queue())!=ERROR_OK)
 195                 return retval;
 196
 197         swjdp->ack = swjdp->ack & 0x7;
 198
 199         while (swjdp->ack != 2)
 200         {
 201                 if (swjdp->ack == 1)
 202                 {
 203                         waitcount++;
 204                         if (waitcount > 100)
 205                         {
 206                                 LOG_WARNING("Timeout waiting for ACK = OK/FAULT in SWJDP transaction");
 207                                 return ERROR_JTAG_DEVICE_ERROR;
 208                         }
 209                 }
 210                 else
 211                 {
 212                         LOG_WARNING("Invalid ACK in SWJDP transaction");
 213                         return ERROR_JTAG_DEVICE_ERROR;
 214                 }
 215
 216                 scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, DP_CTRL_STAT, DPAP_READ, 0, &ctrlstat);
 217                 if ((retval=jtag_execute_queue())!=ERROR_OK)
 218                         return retval;
 219                 swjdp->ack = swjdp->ack & 0x7;
 220         }
 221
 222         /* Check for STICKYERR and STICKYORUN */
 223         if (ctrlstat & (SSTICKYORUN | SSTICKYERR))
 224         {
 225                 LOG_DEBUG("swjdp: CTRL/STAT error 0x%x", ctrlstat);
 226                 /* Check power to debug regions */
 227                 if ((ctrlstat & 0xf0000000) != 0xf0000000)
 228                 {
 229                          ahbap_debugport_init(swjdp);
 230                 }
 231                 else
 232                 {
 233                         u32 dcb_dhcsr,nvic_shcsr, nvic_bfar, nvic_cfsr;
 234
 235                         if (ctrlstat & SSTICKYORUN)
 236                                 LOG_ERROR("SWJ-DP OVERRUN - check clock or reduce jtag speed");
 237
 238                         if (ctrlstat & SSTICKYERR)
 239                                 LOG_ERROR("SWJ-DP STICKY ERROR");
 240
 241                         /* Clear Sticky Error Bits */
 242                         scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, DP_CTRL_STAT, DPAP_WRITE, swjdp->dp_ctrl_stat | SSTICKYORUN | SSTICKYERR, NULL);
 243                         scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, DP_CTRL_STAT, DPAP_READ, 0, &ctrlstat);
 244                         if ((retval=jtag_execute_queue())!=ERROR_OK)
 245                                 return retval;
 246
 247                         LOG_DEBUG("swjdp: status 0x%x", ctrlstat);
 248
 249                         /* Can we find out the reason for the error ?? */
 250                         ahbap_read_system_atomic_u32(swjdp, DCB_DHCSR, &dcb_dhcsr);
 251                         ahbap_read_system_atomic_u32(swjdp, NVIC_SHCSR, &nvic_shcsr);
 252                         ahbap_read_system_atomic_u32(swjdp, NVIC_CFSR, &nvic_cfsr);
 253                         ahbap_read_system_atomic_u32(swjdp, NVIC_BFAR, &nvic_bfar);
 254                         LOG_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);
 255                 }
 256                 if ((retval=jtag_execute_queue())!=ERROR_OK)
 257                         return retval;
 258                 return ERROR_JTAG_DEVICE_ERROR;
 259         }
 260
 261         return ERROR_OK;
 262 }
 263
 264 /***************************************************************************
 265  *                                                                         *
 266  * DP and AHB-AP  register access  through APACC and DPACC                 *
 267  *                                                                         *
 268 ***************************************************************************/
 269
 270 int swjdp_write_dpacc(swjdp_common_t *swjdp, u32 value, u8 reg_addr)
 271 {
 272         return scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, reg_addr, DPAP_WRITE, value, NULL);
 273 }
 274
 275 int swjdp_read_dpacc(swjdp_common_t *swjdp, u32 *value, u8 reg_addr)
 276 {
 277         return scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, reg_addr, DPAP_READ, 0, value);
 278 }
 279
 280 int swjdp_bankselect_apacc(swjdp_common_t *swjdp,u32 reg_addr)
 281 {
 282         u32 select;
 283         select = (reg_addr & 0xFF0000F0);
 284
 285         if (select != swjdp->dp_select_value)
 286         {
 287                 swjdp_write_dpacc(swjdp, select, DP_SELECT);
 288                 swjdp->dp_select_value = select;
 289         }
 290
 291         return ERROR_OK;
 292 }
 293
 294 int ahbap_write_reg(swjdp_common_t *swjdp, u32 reg_addr, u8* out_value_buf)
 295 {
 296         swjdp_bankselect_apacc(swjdp, reg_addr);
 297         scan_inout_check(swjdp, SWJDP_IR_APACC, reg_addr, DPAP_WRITE, out_value_buf, NULL);
 298
 299         return ERROR_OK;
 300 }
 301
 302 int ahbap_read_reg(swjdp_common_t *swjdp, u32 reg_addr, u8 *in_value_buf)
 303 {
 304         swjdp_bankselect_apacc(swjdp, reg_addr);
 305         scan_inout_check(swjdp, SWJDP_IR_APACC, reg_addr, DPAP_READ, 0, in_value_buf);
 306
 307         return ERROR_OK;
 308 }
 309 int ahbap_write_reg_u32(swjdp_common_t *swjdp, u32 reg_addr, u32 value)
 310 {
 311         u8 out_value_buf[4];
 312
 313         buf_set_u32(out_value_buf, 0, 32, value);
 314         swjdp_bankselect_apacc(swjdp, reg_addr);
 315         scan_inout_check(swjdp, SWJDP_IR_APACC, reg_addr, DPAP_WRITE, out_value_buf, NULL);
 316
 317         return ERROR_OK;
 318 }
 319
 320 int ahbap_read_reg_u32(swjdp_common_t *swjdp, u32 reg_addr, u32 *value)
 321 {
 322         swjdp_bankselect_apacc(swjdp, reg_addr);
 323         scan_inout_check_u32(swjdp, SWJDP_IR_APACC, reg_addr, DPAP_READ, 0, value);
 324
 325         return ERROR_OK;
 326 }
 327
 328 /***************************************************************************
 329  *                                                                         *
 330  * AHB-AP access to memory and system registers on AHB bus                 *
 331  *                                                                         *
 332 ***************************************************************************/
 333
 334 int ahbap_setup_accessport(swjdp_common_t *swjdp, u32 csw, u32 tar)
 335 {
 336         csw = csw | CSW_DBGSWENABLE | CSW_MASTER_DEBUG | CSW_HPROT;
 337         if (csw != swjdp->ap_csw_value)
 338         {
 339                 /* LOG_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                 /* LOG_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         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 368
 369         ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, address & 0xFFFFFFF0);
 370         ahbap_read_reg_u32(swjdp, AHBAP_BD0 | (address & 0xC), value );
 371
 372         return ERROR_OK;
 373 }
 374
 375 int ahbap_read_system_atomic_u32(swjdp_common_t *swjdp, u32 address, u32 *value)
 376 {
 377         ahbap_read_system_u32(swjdp, address, value);
 378
 379         return swjdp_transaction_endcheck(swjdp);
 380 }
 381
 382 /*****************************************************************************
 383 *                                                                            *
 384 * ahbap_write_system_u32(swjdp_common_t *swjdp, u32 address, u32 value)      *
 385 *                                                                            *
 386 * Write a u32 value to memory or system register                             *
 387 *                                                                            *
 388 *****************************************************************************/
 389 int ahbap_write_system_u32(swjdp_common_t *swjdp, u32 address, u32 value)
 390 {
 391         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 392
 393         ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, address & 0xFFFFFFF0);
 394         ahbap_write_reg_u32(swjdp, AHBAP_BD0 | (address & 0xC), value );
 395
 396         return ERROR_OK;
 397 }
 398
 399 int ahbap_write_system_atomic_u32(swjdp_common_t *swjdp, u32 address, u32 value)
 400 {
 401         ahbap_write_system_u32(swjdp, address, value);
 402
 403         return swjdp_transaction_endcheck(swjdp);
 404 }
 405
 406 /*****************************************************************************
 407 *                                                                            *
 408 * ahbap_write_buf(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address) *
 409 *                                                                            *
 410 * Write a buffer in target order (little endian)                             *
 411 *                                                                            *
 412 *****************************************************************************/
 413 int ahbap_write_buf_u32(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 414 {
 415         u32 outvalue;
 416         int wcount, blocksize, writecount, errorcount = 0, retval = ERROR_OK;
 417         u32 adr = address;
 418         u8* pBuffer = buffer;
 419
 420         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 421
 422         count >>= 2;
 423         wcount = count;
 424
 425         /* if we have an unaligned access - reorder data */
 426         if (adr & 0x3u)
 427         {
 428                 for (writecount = 0; writecount < count; writecount++)
 429                 {
 430                         int i;
 431                         outvalue = *((u32*)pBuffer);
 432
 433                         for (i = 0; i < 4; i++ )
 434                         {
 435                                 *((u8*)pBuffer + (adr & 0x3)) = outvalue;
 436                                 outvalue >>= 8;
 437                                 adr++;
 438                         }
 439                         pBuffer += 4;
 440                 }
 441         }
 442
 443         while (wcount > 0)
 444         {
 445                 /* Adjust to write blocks within 4K aligned boundaries */
 446                 blocksize = (0x1000 - (0xFFF & address)) >> 2;
 447                 if (wcount < blocksize)
 448                         blocksize = wcount;
 449
 450                 /* handle unaligned data at 4k boundary */
 451                 if (blocksize == 0)
 452                         blocksize = 1;
 453
 454                 ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_SINGLE, address);
 455
 456                 for (writecount = 0; writecount < blocksize; writecount++)
 457                 {
 458                         ahbap_write_reg(swjdp, AHBAP_DRW, buffer + 4 * writecount );
 459                 }
 460
 461                 if (swjdp_transaction_endcheck(swjdp) == ERROR_OK)
 462                 {
 463                         wcount = wcount - blocksize;
 464                         address = address + 4 * blocksize;
 465                         buffer = buffer + 4 * blocksize;
 466                 }
 467                 else
 468                 {
 469                         errorcount++;
 470                 }
 471
 472                 if (errorcount > 1)
 473                 {
 474                         LOG_WARNING("Block write error address 0x%x, wcount 0x%x", address, wcount);
 475                         return ERROR_JTAG_DEVICE_ERROR;
 476                 }
 477         }
 478
 479         return retval;
 480 }
 481
 482 int ahbap_write_buf_packed_u16(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 483 {
 484         u32 outvalue;
 485         int retval = ERROR_OK;
 486         int wcount, blocksize, writecount, i;
 487
 488         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 489
 490         wcount = count >> 1;
 491
 492         while (wcount > 0)
 493         {
 494                 int nbytes;
 495
 496                 /* Adjust to read within 4K block boundaries */
 497                 blocksize = (0x1000 - (0xFFF & address)) >> 1;
 498
 499                 if (wcount < blocksize)
 500                         blocksize = wcount;
 501
 502                 /* handle unaligned data at 4k boundary */
 503                 if (blocksize == 0)
 504                         blocksize = 1;
 505
 506                 ahbap_setup_accessport(swjdp, CSW_16BIT | CSW_ADDRINC_PACKED, address);
 507                 writecount = blocksize;
 508
 509                 do
 510                 {
 511                         nbytes = MIN((writecount << 1), 4);
 512
 513                         if (nbytes < 4 )
 514                         {
 515                                 if (ahbap_write_buf_u16(swjdp, buffer, nbytes, address) != ERROR_OK)
 516                                 {
 517                                         LOG_WARNING("Block read error address 0x%x, count 0x%x", address, count);
 518                                         return ERROR_JTAG_DEVICE_ERROR;
 519                                 }
 520
 521                                 address += nbytes >> 1;
 522                         }
 523                         else
 524                         {
 525                                 outvalue = *((u32*)buffer);
 526
 527                                 for (i = 0; i < nbytes; i++ )
 528                                 {
 529                                         *((u8*)buffer + (address & 0x3)) = outvalue;
 530                                         outvalue >>= 8;
 531                                         address++;
 532                                 }
 533
 534                                 outvalue = *((u32*)buffer);
 535                                 ahbap_write_reg_u32(swjdp, AHBAP_DRW, outvalue);
 536                                 if (swjdp_transaction_endcheck(swjdp) != ERROR_OK)
 537                                 {
 538                                         LOG_WARNING("Block read error address 0x%x, count 0x%x", address, count);
 539                                         return ERROR_JTAG_DEVICE_ERROR;
 540                                 }
 541                         }
 542
 543                         buffer += nbytes >> 1;
 544                         writecount -= nbytes >> 1;
 545
 546                 } while (writecount);
 547                 wcount -= blocksize;
 548         }
 549
 550         return retval;
 551 }
 552
 553 int ahbap_write_buf_u16(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 554 {
 555         u32 outvalue;
 556         int retval = ERROR_OK;
 557
 558         if (count >= 4)
 559                 return ahbap_write_buf_packed_u16(swjdp, buffer, count, address);
 560
 561         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 562
 563         while (count > 0)
 564         {
 565                 ahbap_setup_accessport(swjdp, CSW_16BIT | CSW_ADDRINC_SINGLE, address);
 566                 outvalue = *((u16*)buffer) << 8 * (address & 0x3);
 567                 ahbap_write_reg_u32(swjdp, AHBAP_DRW, outvalue );
 568                 retval = swjdp_transaction_endcheck(swjdp);
 569                 count -= 2;
 570                 address += 2;
 571                 buffer += 2;
 572         }
 573
 574         return retval;
 575 }
 576
 577 int ahbap_write_buf_packed_u8(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 578 {
 579         u32 outvalue;
 580         int retval = ERROR_OK;
 581         int wcount, blocksize, writecount, i;
 582
 583         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 584
 585         wcount = count;
 586
 587         while (wcount > 0)
 588         {
 589                 int nbytes;
 590
 591                 /* Adjust to read within 4K block boundaries */
 592                 blocksize = (0x1000 - (0xFFF & address));
 593
 594                 if (wcount < blocksize)
 595                         blocksize = wcount;
 596
 597                 ahbap_setup_accessport(swjdp, CSW_8BIT | CSW_ADDRINC_PACKED, address);
 598                 writecount = blocksize;
 599
 600                 do
 601                 {
 602                         nbytes = MIN(writecount, 4);
 603
 604                         if (nbytes < 4 )
 605                         {
 606                                 if (ahbap_write_buf_u8(swjdp, buffer, nbytes, address) != ERROR_OK)
 607                                 {
 608                                         LOG_WARNING("Block read error address 0x%x, count 0x%x", address, count);
 609                                         return ERROR_JTAG_DEVICE_ERROR;
 610                                 }
 611
 612                                 address += nbytes;
 613                         }
 614                         else
 615                         {
 616                                 outvalue = *((u32*)buffer);
 617
 618                                 for (i = 0; i < nbytes; i++ )
 619                                 {
 620                                         *((u8*)buffer + (address & 0x3)) = outvalue;
 621                                         outvalue >>= 8;
 622                                         address++;
 623                                 }
 624
 625                                 outvalue = *((u32*)buffer);
 626                                 ahbap_write_reg_u32(swjdp, AHBAP_DRW, outvalue);
 627                                 if (swjdp_transaction_endcheck(swjdp) != ERROR_OK)
 628                                 {
 629                                         LOG_WARNING("Block read error address 0x%x, count 0x%x", address, count);
 630                                         return ERROR_JTAG_DEVICE_ERROR;
 631                                 }
 632                         }
 633
 634                         buffer += nbytes;
 635                         writecount -= nbytes;
 636
 637                 } while (writecount);
 638                 wcount -= blocksize;
 639         }
 640
 641         return retval;
 642 }
 643
 644 int ahbap_write_buf_u8(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 645 {
 646         u32 outvalue;
 647         int retval = ERROR_OK;
 648
 649         if (count >= 4)
 650                 return ahbap_write_buf_packed_u8(swjdp, buffer, count, address);
 651
 652         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 653
 654         while (count > 0)
 655         {
 656                 ahbap_setup_accessport(swjdp, CSW_8BIT | CSW_ADDRINC_SINGLE, address);
 657                 outvalue = *((u8*)buffer) << 8 * (address & 0x3);
 658                 ahbap_write_reg_u32(swjdp, AHBAP_DRW, outvalue );
 659                 retval = swjdp_transaction_endcheck(swjdp);
 660                 count--;
 661                 address++;
 662                 buffer++;
 663         }
 664
 665         return retval;
 666 }
 667
 668 /*********************************************************************************
 669 *                                                                                *
 670 * ahbap_read_buf_u32(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)  *
 671 *                                                                                *
 672 * Read block fast in target order (little endian) into a buffer                  *
 673 *                                                                                *
 674 **********************************************************************************/
 675 int ahbap_read_buf_u32(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 676 {
 677         int wcount, blocksize, readcount, errorcount = 0, retval = ERROR_OK;
 678         u32 adr = address;
 679         u8* pBuffer = buffer;
 680
 681         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 682
 683         count >>= 2;
 684         wcount = count;
 685
 686         while (wcount > 0)
 687         {
 688                 /* Adjust to read within 4K block boundaries */
 689                 blocksize = (0x1000 - (0xFFF & address)) >> 2;
 690                 if (wcount < blocksize)
 691                         blocksize = wcount;
 692
 693                 /* handle unaligned data at 4k boundary */
 694                 if (blocksize == 0)
 695                         blocksize = 1;
 696
 697                 ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_SINGLE, address);
 698
 699                 /* Scan out first read */
 700                 swjdp_scan(swjdp->jtag_info, SWJDP_IR_APACC, AHBAP_DRW, DPAP_READ, 0, NULL, NULL);
 701                 for (readcount = 0; readcount < blocksize - 1; readcount++)
 702                 {
 703                         /* Scan out read instruction and scan in previous value */
 704                         swjdp_scan(swjdp->jtag_info, SWJDP_IR_APACC, AHBAP_DRW, DPAP_READ, 0, buffer + 4 * readcount, &swjdp->ack);
 705                 }
 706
 707                 /* Scan in last value */
 708                 swjdp_scan(swjdp->jtag_info, SWJDP_IR_DPACC, DP_RDBUFF, DPAP_READ, 0, buffer + 4 * readcount, &swjdp->ack);
 709                 if (swjdp_transaction_endcheck(swjdp) == ERROR_OK)
 710                 {
 711                         wcount = wcount - blocksize;
 712                         address += 4 * blocksize;
 713                         buffer += 4 * blocksize;
 714                 }
 715                 else
 716                 {
 717                         errorcount++;
 718                 }
 719
 720                 if (errorcount > 1)
 721                 {
 722                         LOG_WARNING("Block read error address 0x%x, count 0x%x", address, count);
 723                         return ERROR_JTAG_DEVICE_ERROR;
 724                 }
 725         }
 726
 727         /* if we have an unaligned access - reorder data */
 728         if (adr & 0x3u)
 729         {
 730                 for (readcount = 0; readcount < count; readcount++)
 731                 {
 732                         int i;
 733                         u32 data = *((u32*)pBuffer);
 734
 735                         for (i = 0; i < 4; i++ )
 736                         {
 737                                 *((u8*)pBuffer) = (data >> 8 * (adr & 0x3));
 738                                 pBuffer++;
 739                                 adr++;
 740                         }
 741                 }
 742         }
 743
 744         return retval;
 745 }
 746
 747 int ahbap_read_buf_packed_u16(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 748 {
 749         u32 invalue;
 750         int retval = ERROR_OK;
 751         int wcount, blocksize, readcount, i;
 752
 753         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 754
 755         wcount = count >> 1;
 756
 757         while (wcount > 0)
 758         {
 759                 int nbytes;
 760
 761                 /* Adjust to read within 4K block boundaries */
 762                 blocksize = (0x1000 - (0xFFF & address)) >> 1;
 763                 if (wcount < blocksize)
 764                         blocksize = wcount;
 765
 766                 ahbap_setup_accessport(swjdp, CSW_16BIT | CSW_ADDRINC_PACKED, address);
 767
 768                 /* handle unaligned data at 4k boundary */
 769                 if (blocksize == 0)
 770                         blocksize = 1;
 771                 readcount = blocksize;
 772
 773                 do
 774                 {
 775                         ahbap_read_reg_u32(swjdp, AHBAP_DRW, &invalue );
 776                         if (swjdp_transaction_endcheck(swjdp) != ERROR_OK)
 777                         {
 778                                 LOG_WARNING("Block read error address 0x%x, count 0x%x", address, count);
 779                                 return ERROR_JTAG_DEVICE_ERROR;
 780                         }
 781
 782                         nbytes = MIN((readcount << 1), 4);
 783
 784                         for (i = 0; i < nbytes; i++ )
 785                         {
 786                                 *((u8*)buffer) = (invalue >> 8 * (address & 0x3));
 787                                 buffer++;
 788                                 address++;
 789                         }
 790
 791                         readcount -= (nbytes >> 1);
 792                 } while (readcount);
 793                 wcount -= blocksize;
 794         }
 795
 796         return retval;
 797 }
 798
 799 int ahbap_read_buf_u16(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 800 {
 801         u32 invalue, i;
 802         int retval = ERROR_OK;
 803
 804         if (count >= 4)
 805                 return ahbap_read_buf_packed_u16(swjdp, buffer, count, address);
 806
 807         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 808
 809         while (count > 0)
 810         {
 811                 ahbap_setup_accessport(swjdp, CSW_16BIT | CSW_ADDRINC_SINGLE, address);
 812                 ahbap_read_reg_u32(swjdp, AHBAP_DRW, &invalue );
 813                 retval = swjdp_transaction_endcheck(swjdp);
 814                 if (address & 0x1)
 815                 {
 816                         for (i = 0; i < 2; i++ )
 817                         {
 818                                 *((u8*)buffer) = (invalue >> 8 * (address & 0x3));
 819                                 buffer++;
 820                                 address++;
 821                         }
 822                 }
 823                 else
 824                 {
 825                         *((u16*)buffer) = (invalue >> 8 * (address & 0x3));
 826                         address += 2;
 827                         buffer += 2;
 828                 }
 829                 count -= 2;
 830         }
 831
 832         return retval;
 833 }
 834
 835 int ahbap_read_buf_packed_u8(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 836 {
 837         u32 invalue;
 838         int retval = ERROR_OK;
 839         int wcount, blocksize, readcount, i;
 840
 841         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 842
 843         wcount = count;
 844
 845         while (wcount > 0)
 846         {
 847                 int nbytes;
 848
 849                 /* Adjust to read within 4K block boundaries */
 850                 blocksize = (0x1000 - (0xFFF & address));
 851
 852                 if (wcount < blocksize)
 853                         blocksize = wcount;
 854
 855                 ahbap_setup_accessport(swjdp, CSW_8BIT | CSW_ADDRINC_PACKED, address);
 856                 readcount = blocksize;
 857
 858                 do
 859                 {
 860                         ahbap_read_reg_u32(swjdp, AHBAP_DRW, &invalue );
 861                         if (swjdp_transaction_endcheck(swjdp) != ERROR_OK)
 862                         {
 863                                 LOG_WARNING("Block read error address 0x%x, count 0x%x", address, count);
 864                                 return ERROR_JTAG_DEVICE_ERROR;
 865                         }
 866
 867                         nbytes = MIN(readcount, 4);
 868
 869                         for (i = 0; i < nbytes; i++ )
 870                         {
 871                                 *((u8*)buffer) = (invalue >> 8 * (address & 0x3));
 872                                 buffer++;
 873                                 address++;
 874                         }
 875
 876                         readcount -= nbytes;
 877                 } while (readcount);
 878                 wcount -= blocksize;
 879         }
 880
 881         return retval;
 882 }
 883
 884 int ahbap_read_buf_u8(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 885 {
 886         u32 invalue;
 887         int retval = ERROR_OK;
 888
 889         if (count >= 4)
 890                 return ahbap_read_buf_packed_u8(swjdp, buffer, count, address);
 891
 892         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 893
 894         while (count > 0)
 895         {
 896                 ahbap_setup_accessport(swjdp, CSW_8BIT | CSW_ADDRINC_SINGLE, address);
 897                 ahbap_read_reg_u32(swjdp, AHBAP_DRW, &invalue );
 898                 retval = swjdp_transaction_endcheck(swjdp);
 899                 *((u8*)buffer) = (invalue >> 8 * (address & 0x3));
 900                 count--;
 901                 address++;
 902                 buffer++;
 903         }
 904
 905         return retval;
 906 }
 907
 908 int ahbap_read_coreregister_u32(swjdp_common_t *swjdp, u32 *value, int regnum)
 909 {
 910         int retval;
 911         u32 dcrdr;
 912
 913         /* because the DCB_DCRDR is used for the emulated dcc channel
 914          * we gave to save/restore the DCB_DCRDR when used */
 915
 916         ahbap_read_system_atomic_u32(swjdp, DCB_DCRDR, &dcrdr);
 917
 918         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 919
 920         /* ahbap_write_system_u32(swjdp, DCB_DCRSR, regnum); */
 921         ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, DCB_DCRSR & 0xFFFFFFF0);
 922         ahbap_write_reg_u32(swjdp, AHBAP_BD0 | (DCB_DCRSR & 0xC), regnum );
 923
 924         /* ahbap_read_system_u32(swjdp, DCB_DCRDR, value); */
 925         ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, DCB_DCRDR & 0xFFFFFFF0);
 926         ahbap_read_reg_u32(swjdp, AHBAP_BD0 | (DCB_DCRDR & 0xC), value );
 927
 928         retval = swjdp_transaction_endcheck(swjdp);
 929         ahbap_write_system_atomic_u32(swjdp, DCB_DCRDR, dcrdr);
 930         return retval;
 931 }
 932
 933 int ahbap_write_coreregister_u32(swjdp_common_t *swjdp, u32 value, int regnum)
 934 {
 935         int retval;
 936         u32 dcrdr;
 937
 938         /* because the DCB_DCRDR is used for the emulated dcc channel
 939          * we gave to save/restore the DCB_DCRDR when used */
 940
 941         ahbap_read_system_atomic_u32(swjdp, DCB_DCRDR, &dcrdr);
 942
 943         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 944
 945         /* ahbap_write_system_u32(swjdp, DCB_DCRDR, core_regs[i]); */
 946         ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, DCB_DCRDR & 0xFFFFFFF0);
 947         ahbap_write_reg_u32(swjdp, AHBAP_BD0 | (DCB_DCRDR & 0xC), value );
 948
 949         /* ahbap_write_system_u32(swjdp, DCB_DCRSR, i | DCRSR_WnR       ); */
 950         ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, DCB_DCRSR & 0xFFFFFFF0);
 951         ahbap_write_reg_u32(swjdp, AHBAP_BD0 | (DCB_DCRSR & 0xC), regnum | DCRSR_WnR );
 952
 953         retval = swjdp_transaction_endcheck(swjdp);
 954         ahbap_write_system_atomic_u32(swjdp, DCB_DCRDR, dcrdr);
 955         return retval;
 956 }
 957
 958 int ahbap_debugport_init(swjdp_common_t *swjdp)
 959 {
 960         u32 idreg, romaddr, dummy;
 961         u32 ctrlstat;
 962         int cnt = 0;
 963         int retval;
 964
 965         LOG_DEBUG(" ");
 966
 967         swjdp->ap_csw_value = -1;
 968         swjdp->ap_tar_value = -1;
 969         swjdp->trans_mode = TRANS_MODE_ATOMIC;
 970         swjdp_read_dpacc(swjdp, &dummy, DP_CTRL_STAT);
 971         swjdp_write_dpacc(swjdp, SSTICKYERR, DP_CTRL_STAT);
 972         swjdp_read_dpacc(swjdp, &dummy, DP_CTRL_STAT);
 973
 974         swjdp->dp_ctrl_stat = CDBGPWRUPREQ | CSYSPWRUPREQ;
 975
 976         swjdp_write_dpacc(swjdp, swjdp->dp_ctrl_stat, DP_CTRL_STAT);
 977         swjdp_read_dpacc(swjdp, &ctrlstat, DP_CTRL_STAT);
 978         if ((retval=jtag_execute_queue())!=ERROR_OK)
 979                 return retval;
 980
 981         /* Check that we have debug power domains activated */
 982         while (!(ctrlstat & CDBGPWRUPACK) && (cnt++ < 10))
 983         {
 984                 LOG_DEBUG("swjdp: wait CDBGPWRUPACK");
 985                 swjdp_read_dpacc(swjdp, &ctrlstat, DP_CTRL_STAT);
 986                 if ((retval=jtag_execute_queue())!=ERROR_OK)
 987                         return retval;
 988                 alive_sleep(10);
 989         }
 990
 991         while (!(ctrlstat & CSYSPWRUPACK) && (cnt++ < 10))
 992         {
 993                 LOG_DEBUG("swjdp: wait CSYSPWRUPACK");
 994                 swjdp_read_dpacc(swjdp, &ctrlstat, DP_CTRL_STAT);
 995                 if ((retval=jtag_execute_queue())!=ERROR_OK)
 996                         return retval;
 997                 alive_sleep(10);
 998         }
 999
1000         swjdp_read_dpacc(swjdp, &dummy, DP_CTRL_STAT);
1001         /* With debug power on we can activate OVERRUN checking */
1002         swjdp->dp_ctrl_stat = CDBGPWRUPREQ | CSYSPWRUPREQ | CORUNDETECT;
1003         swjdp_write_dpacc(swjdp, swjdp->dp_ctrl_stat, DP_CTRL_STAT);
1004         swjdp_read_dpacc(swjdp, &dummy, DP_CTRL_STAT);
1005
1006         ahbap_read_reg_u32(swjdp, 0xFC, &idreg);
1007         ahbap_read_reg_u32(swjdp, 0xF8, &romaddr);
1008
1009         LOG_DEBUG("AHB-AP ID Register 0x%x, Debug ROM Address 0x%x", idreg, romaddr);
1010
1011         return ERROR_OK;
1012 }