src/target/cortex_swjdp.c

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