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