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 waitcount = 0;
 175         u32 ctrlstat;
 176
 177         scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, DP_CTRL_STAT, DPAP_READ, 0, &ctrlstat);
 178         scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, DP_CTRL_STAT, DPAP_READ, 0, &ctrlstat);
 179         jtag_execute_queue();
 180
 181         swjdp->ack = swjdp->ack & 0x7;
 182
 183         while (swjdp->ack != 2)
 184         {
 185                 if (swjdp->ack == 1)
 186                 {
 187                         waitcount++;
 188                         if (waitcount > 100)
 189                         {
 190                                 LOG_WARNING("Timeout waiting for ACK = OK/FAULT in SWJDP transaction");
 191                                 return ERROR_JTAG_DEVICE_ERROR;
 192                         }
 193                 }
 194                 else
 195                 {
 196                         LOG_WARNING("Invalid ACK in SWJDP transaction");
 197                         return ERROR_JTAG_DEVICE_ERROR;
 198                 }
 199
 200                 scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, DP_CTRL_STAT, DPAP_READ, 0, &ctrlstat);
 201                 jtag_execute_queue();
 202                 swjdp->ack = swjdp->ack & 0x7;
 203         }
 204
 205         /* Check for STICKYERR and STICKYORUN */
 206         if (ctrlstat & (SSTICKYORUN | SSTICKYERR))
 207         {
 208                 LOG_DEBUG("swjdp: CTRL/STAT error 0x%x", ctrlstat);
 209                 /* Check power to debug regions */
 210                 if ((ctrlstat & 0xf0000000) != 0xf0000000)
 211                 {
 212                          ahbap_debugport_init(swjdp);
 213                 }
 214                 else
 215                 {
 216                         u32 dcb_dhcsr,nvic_shcsr, nvic_bfar, nvic_cfsr;
 217
 218                         if (ctrlstat & SSTICKYORUN)
 219                                 LOG_ERROR("SWJ-DP OVERRUN - check clock or reduce jtag speed");
 220
 221                         if (ctrlstat & SSTICKYERR)
 222                                 LOG_ERROR("SWJ-DP STICKY ERROR");
 223
 224                         /* Clear Sticky Error Bits */
 225                         scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, DP_CTRL_STAT, DPAP_WRITE, swjdp->dp_ctrl_stat | SSTICKYORUN | SSTICKYERR, NULL);
 226                         scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, DP_CTRL_STAT, DPAP_READ, 0, &ctrlstat);
 227                         jtag_execute_queue();
 228
 229                         LOG_DEBUG("swjdp: status 0x%x", ctrlstat);
 230
 231                         /* Can we find out the reason for the error ?? */
 232                         ahbap_read_system_atomic_u32(swjdp, DCB_DHCSR, &dcb_dhcsr);
 233                         ahbap_read_system_atomic_u32(swjdp, NVIC_SHCSR, &nvic_shcsr);
 234                         ahbap_read_system_atomic_u32(swjdp, NVIC_CFSR, &nvic_cfsr);
 235                         ahbap_read_system_atomic_u32(swjdp, NVIC_BFAR, &nvic_bfar);
 236                         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);
 237                 }
 238                 jtag_execute_queue();
 239                 return ERROR_JTAG_DEVICE_ERROR;
 240         }
 241
 242         return ERROR_OK;
 243 }
 244
 245 /***************************************************************************
 246  *                                                                         *
 247  * DP and AHB-AP  register access  through APACC and DPACC                 *
 248  *                                                                         *
 249 ***************************************************************************/
 250
 251 int swjdp_write_dpacc(swjdp_common_t *swjdp, u32 value, u8 reg_addr)
 252 {
 253         return scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, reg_addr, DPAP_WRITE, value, NULL);
 254 }
 255
 256 int swjdp_read_dpacc(swjdp_common_t *swjdp, u32 *value, u8 reg_addr)
 257 {
 258         return scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, reg_addr, DPAP_READ, 0, value);
 259 }
 260
 261 int swjdp_bankselect_apacc(swjdp_common_t *swjdp,u32 reg_addr)
 262 {
 263         u32 select;
 264         select = (reg_addr & 0xFF0000F0);
 265
 266         if (select != swjdp->dp_select_value)
 267         {
 268                 swjdp_write_dpacc(swjdp, select, DP_SELECT);
 269                 swjdp->dp_select_value = select;
 270         }
 271
 272         return ERROR_OK;
 273 }
 274
 275 int ahbap_write_reg(swjdp_common_t *swjdp, u32 reg_addr, u8* out_value_buf)
 276 {
 277         swjdp_bankselect_apacc(swjdp, reg_addr);
 278         scan_inout_check(swjdp, SWJDP_IR_APACC, reg_addr, DPAP_WRITE, out_value_buf, NULL);
 279
 280         return ERROR_OK;
 281 }
 282
 283 int ahbap_read_reg(swjdp_common_t *swjdp, u32 reg_addr, u8 *in_value_buf)
 284 {
 285         swjdp_bankselect_apacc(swjdp, reg_addr);
 286         scan_inout_check(swjdp, SWJDP_IR_APACC, reg_addr, DPAP_READ, 0, in_value_buf);
 287
 288         return ERROR_OK;
 289 }
 290 int ahbap_write_reg_u32(swjdp_common_t *swjdp, u32 reg_addr, u32 value)
 291 {
 292         u8 out_value_buf[4];
 293
 294         buf_set_u32(out_value_buf, 0, 32, value);
 295         swjdp_bankselect_apacc(swjdp, reg_addr);
 296         scan_inout_check(swjdp, SWJDP_IR_APACC, reg_addr, DPAP_WRITE, out_value_buf, NULL);
 297
 298         return ERROR_OK;
 299 }
 300
 301 int ahbap_read_reg_u32(swjdp_common_t *swjdp, u32 reg_addr, u32 *value)
 302 {
 303         swjdp_bankselect_apacc(swjdp, reg_addr);
 304         scan_inout_check_u32(swjdp, SWJDP_IR_APACC, reg_addr, DPAP_READ, 0, value);
 305
 306         return ERROR_OK;
 307 }
 308
 309 /***************************************************************************
 310  *                                                                         *
 311  * AHB-AP access to memory and system registers on AHB bus                 *
 312  *                                                                         *
 313 ***************************************************************************/
 314
 315 int ahbap_setup_accessport(swjdp_common_t *swjdp, u32 csw, u32 tar)
 316 {
 317         csw = csw | CSW_DBGSWENABLE | CSW_MASTER_DEBUG | CSW_HPROT;
 318         if (csw != swjdp->ap_csw_value)
 319         {
 320                 /* LOG_DEBUG("swjdp : Set CSW %x",csw); */
 321                 ahbap_write_reg_u32(swjdp, AHBAP_CSW, csw );
 322                 swjdp->ap_csw_value = csw;
 323         }
 324         if (tar != swjdp->ap_tar_value)
 325         {
 326                 /* LOG_DEBUG("swjdp : Set TAR %x",tar); */
 327                 ahbap_write_reg_u32(swjdp, AHBAP_TAR, tar );
 328                 swjdp->ap_tar_value = tar;
 329         }
 330         if (csw & CSW_ADDRINC_MASK)
 331         {
 332                 /* Do not cache TAR value when autoincrementing */
 333                 swjdp->ap_tar_value = -1;
 334         }
 335         return ERROR_OK;
 336 }
 337
 338 /*****************************************************************************
 339 *                                                                            *
 340 * ahbap_read_system_u32(swjdp_common_t *swjdp, u32 address, u32 *value)      *
 341 *                                                                            *
 342 * Read a u32 value from memory or system register                            *
 343 * Functionally equivalent to target_read_u32(target, address, u32 *value),   *
 344 * but with less overhead                                                     *
 345 *****************************************************************************/
 346 int ahbap_read_system_u32(swjdp_common_t *swjdp, u32 address, u32 *value)
 347 {
 348         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 349
 350         ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, address & 0xFFFFFFF0);
 351         ahbap_read_reg_u32(swjdp, AHBAP_BD0 | (address & 0xC), value );
 352
 353         return ERROR_OK;
 354 }
 355
 356 int ahbap_read_system_atomic_u32(swjdp_common_t *swjdp, u32 address, u32 *value)
 357 {
 358         ahbap_read_system_u32(swjdp, address, value);
 359
 360         return swjdp_transaction_endcheck(swjdp);
 361 }
 362
 363 /*****************************************************************************
 364 *                                                                            *
 365 * ahbap_write_system_u32(swjdp_common_t *swjdp, u32 address, u32 value)      *
 366 *                                                                            *
 367 * Write a u32 value to memory or system register                             *
 368 *                                                                            *
 369 *****************************************************************************/
 370 int ahbap_write_system_u32(swjdp_common_t *swjdp, u32 address, u32 value)
 371 {
 372         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 373
 374         ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, address & 0xFFFFFFF0);
 375         ahbap_write_reg_u32(swjdp, AHBAP_BD0 | (address & 0xC), value );
 376
 377         return ERROR_OK;
 378 }
 379
 380 int ahbap_write_system_atomic_u32(swjdp_common_t *swjdp, u32 address, u32 value)
 381 {
 382         ahbap_write_system_u32(swjdp, address, value);
 383
 384         return swjdp_transaction_endcheck(swjdp);
 385 }
 386
 387 /*****************************************************************************
 388 *                                                                            *
 389 * ahbap_write_buf(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address) *
 390 *                                                                            *
 391 * Write a buffer in target order (little endian)                             *
 392 *                                                                            *
 393 *****************************************************************************/
 394 int ahbap_write_buf_u32(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 395 {
 396         u32 outvalue;
 397         int wcount, blocksize, writecount, errorcount = 0, retval = ERROR_OK;
 398         u32 adr = address;
 399         u8* pBuffer = buffer;
 400
 401         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 402
 403         count >>= 2;
 404         wcount = count;
 405
 406         /* if we have an unaligned access - reorder data */
 407         if (adr & 0x3u)
 408         {
 409                 for (writecount = 0; writecount < count; writecount++)
 410                 {
 411                         int i;
 412                         outvalue = *((u32*)pBuffer);
 413
 414                         for (i = 0; i < 4; i++ )
 415                         {
 416                                 *((u8*)pBuffer + (adr & 0x3)) = outvalue;
 417                                 outvalue >>= 8;
 418                                 adr++;
 419                         }
 420                         pBuffer += 4;
 421                 }
 422         }
 423
 424         while (wcount > 0)
 425         {
 426                 /* Adjust to write blocks within 4K aligned boundaries */
 427                 blocksize = (0x1000 - (0xFFF & address)) >> 2;
 428                 if (wcount < blocksize)
 429                         blocksize = wcount;
 430
 431                 /* handle unaligned data at 4k boundary */
 432                 if (blocksize == 0)
 433                         blocksize = 1;
 434
 435                 ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_SINGLE, address);
 436
 437                 for (writecount = 0; writecount < blocksize; writecount++)
 438                 {
 439                         ahbap_write_reg(swjdp, AHBAP_DRW, buffer + 4 * writecount );
 440                 }
 441
 442                 if (swjdp_transaction_endcheck(swjdp) == ERROR_OK)
 443                 {
 444                         wcount = wcount - blocksize;
 445                         address = address + 4 * blocksize;
 446                         buffer = buffer + 4 * blocksize;
 447                 }
 448                 else
 449                 {
 450                         errorcount++;
 451                 }
 452
 453                 if (errorcount > 1)
 454                 {
 455                         LOG_WARNING("Block write error address 0x%x, wcount 0x%x", address, wcount);
 456                         return ERROR_JTAG_DEVICE_ERROR;
 457                 }
 458         }
 459
 460         return retval;
 461 }
 462
 463 int ahbap_write_buf_packed_u16(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 464 {
 465         u32 outvalue;
 466         int retval = ERROR_OK;
 467         int wcount, blocksize, writecount, i;
 468
 469         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 470
 471         wcount = count >> 1;
 472
 473         while (wcount > 0)
 474         {
 475                 int nbytes;
 476
 477                 /* Adjust to read within 4K block boundaries */
 478                 blocksize = (0x1000 - (0xFFF & address)) >> 1;
 479
 480                 if (wcount < blocksize)
 481                         blocksize = wcount;
 482
 483                 /* handle unaligned data at 4k boundary */
 484                 if (blocksize == 0)
 485                         blocksize = 1;
 486
 487                 ahbap_setup_accessport(swjdp, CSW_16BIT | CSW_ADDRINC_PACKED, address);
 488                 writecount = blocksize;
 489
 490                 do
 491                 {
 492                         nbytes = MIN((writecount << 1), 4);
 493
 494                         if (nbytes < 4 )
 495                         {
 496                                 if (ahbap_write_buf_u16(swjdp, buffer, nbytes, address) != ERROR_OK)
 497                                 {
 498                                         LOG_WARNING("Block read error address 0x%x, count 0x%x", address, count);
 499                                         return ERROR_JTAG_DEVICE_ERROR;
 500                                 }
 501
 502                                 address += nbytes >> 1;
 503                         }
 504                         else
 505                         {
 506                                 outvalue = *((u32*)buffer);
 507
 508                                 for (i = 0; i < nbytes; i++ )
 509                                 {
 510                                         *((u8*)buffer + (address & 0x3)) = outvalue;
 511                                         outvalue >>= 8;
 512                                         address++;
 513                                 }
 514
 515                                 outvalue = *((u32*)buffer);
 516                                 ahbap_write_reg_u32(swjdp, AHBAP_DRW, outvalue);
 517                                 if (swjdp_transaction_endcheck(swjdp) != 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
 524                         buffer += nbytes >> 1;
 525                         writecount -= nbytes >> 1;
 526
 527                 } while (writecount);
 528                 wcount -= blocksize;
 529         }
 530
 531         return retval;
 532 }
 533
 534 int ahbap_write_buf_u16(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 535 {
 536         u32 outvalue;
 537         int retval = ERROR_OK;
 538
 539         if (count >= 4)
 540                 return ahbap_write_buf_packed_u16(swjdp, buffer, count, address);
 541
 542         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 543
 544         while (count > 0)
 545         {
 546                 ahbap_setup_accessport(swjdp, CSW_16BIT | CSW_ADDRINC_SINGLE, address);
 547                 outvalue = *((u16*)buffer) << 8 * (address & 0x3);
 548                 ahbap_write_reg_u32(swjdp, AHBAP_DRW, outvalue );
 549                 retval = swjdp_transaction_endcheck(swjdp);
 550                 count -= 2;
 551                 address += 2;
 552                 buffer += 2;
 553         }
 554
 555         return retval;
 556 }
 557
 558 int ahbap_write_buf_packed_u8(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 559 {
 560         u32 outvalue;
 561         int retval = ERROR_OK;
 562         int wcount, blocksize, writecount, i;
 563
 564         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 565
 566         wcount = count;
 567
 568         while (wcount > 0)
 569         {
 570                 int nbytes;
 571
 572                 /* Adjust to read within 4K block boundaries */
 573                 blocksize = (0x1000 - (0xFFF & address));
 574
 575                 if (wcount < blocksize)
 576                         blocksize = wcount;
 577
 578                 ahbap_setup_accessport(swjdp, CSW_8BIT | CSW_ADDRINC_PACKED, address);
 579                 writecount = blocksize;
 580
 581                 do
 582                 {
 583                         nbytes = MIN(writecount, 4);
 584
 585                         if (nbytes < 4 )
 586                         {
 587                                 if (ahbap_write_buf_u8(swjdp, buffer, nbytes, address) != ERROR_OK)
 588                                 {
 589                                         LOG_WARNING("Block read error address 0x%x, count 0x%x", address, count);
 590                                         return ERROR_JTAG_DEVICE_ERROR;
 591                                 }
 592
 593                                 address += nbytes;
 594                         }
 595                         else
 596                         {
 597                                 outvalue = *((u32*)buffer);
 598
 599                                 for (i = 0; i < nbytes; i++ )
 600                                 {
 601                                         *((u8*)buffer + (address & 0x3)) = outvalue;
 602                                         outvalue >>= 8;
 603                                         address++;
 604                                 }
 605
 606                                 outvalue = *((u32*)buffer);
 607                                 ahbap_write_reg_u32(swjdp, AHBAP_DRW, outvalue);
 608                                 if (swjdp_transaction_endcheck(swjdp) != 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
 615                         buffer += nbytes;
 616                         writecount -= nbytes;
 617
 618                 } while (writecount);
 619                 wcount -= blocksize;
 620         }
 621
 622         return retval;
 623 }
 624
 625 int ahbap_write_buf_u8(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 626 {
 627         u32 outvalue;
 628         int retval = ERROR_OK;
 629
 630         if (count >= 4)
 631                 return ahbap_write_buf_packed_u8(swjdp, buffer, count, address);
 632
 633         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 634
 635         while (count > 0)
 636         {
 637                 ahbap_setup_accessport(swjdp, CSW_8BIT | CSW_ADDRINC_SINGLE, address);
 638                 outvalue = *((u8*)buffer) << 8 * (address & 0x3);
 639                 ahbap_write_reg_u32(swjdp, AHBAP_DRW, outvalue );
 640                 retval = swjdp_transaction_endcheck(swjdp);
 641                 count--;
 642                 address++;
 643                 buffer++;
 644         }
 645
 646         return retval;
 647 }
 648
 649 /*********************************************************************************
 650 *                                                                                *
 651 * ahbap_read_buf_u32(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)  *
 652 *                                                                                *
 653 * Read block fast in target order (little endian) into a buffer                  *
 654 *                                                                                *
 655 **********************************************************************************/
 656 int ahbap_read_buf_u32(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 657 {
 658         int wcount, blocksize, readcount, errorcount = 0, retval = ERROR_OK;
 659         u32 adr = address;
 660         u8* pBuffer = buffer;
 661
 662         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 663
 664         count >>= 2;
 665         wcount = count;
 666
 667         while (wcount > 0)
 668         {
 669                 /* Adjust to read within 4K block boundaries */
 670                 blocksize = (0x1000 - (0xFFF & address)) >> 2;
 671                 if (wcount < blocksize)
 672                         blocksize = wcount;
 673
 674                 /* handle unaligned data at 4k boundary */
 675                 if (blocksize == 0)
 676                         blocksize = 1;
 677
 678                 ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_SINGLE, address);
 679
 680                 /* Scan out first read */
 681                 swjdp_scan(swjdp->jtag_info, SWJDP_IR_APACC, AHBAP_DRW, DPAP_READ, 0, NULL, NULL);
 682                 for (readcount = 0; readcount < blocksize - 1; readcount++)
 683                 {
 684                         /* Scan out read instruction and scan in previous value */
 685                         swjdp_scan(swjdp->jtag_info, SWJDP_IR_APACC, AHBAP_DRW, DPAP_READ, 0, buffer + 4 * readcount, &swjdp->ack);
 686                 }
 687
 688                 /* Scan in last value */
 689                 swjdp_scan(swjdp->jtag_info, SWJDP_IR_DPACC, DP_RDBUFF, DPAP_READ, 0, buffer + 4 * readcount, &swjdp->ack);
 690                 if (swjdp_transaction_endcheck(swjdp) == ERROR_OK)
 691                 {
 692                         wcount = wcount - blocksize;
 693                         address += 4 * blocksize;
 694                         buffer += 4 * blocksize;
 695                 }
 696                 else
 697                 {
 698                         errorcount++;
 699                 }
 700
 701                 if (errorcount > 1)
 702                 {
 703                         LOG_WARNING("Block read error address 0x%x, count 0x%x", address, count);
 704                         return ERROR_JTAG_DEVICE_ERROR;
 705                 }
 706         }
 707
 708         /* if we have an unaligned access - reorder data */
 709         if (adr & 0x3u)
 710         {
 711                 for (readcount = 0; readcount < count; readcount++)
 712                 {
 713                         int i;
 714                         u32 data = *((u32*)pBuffer);
 715
 716                         for (i = 0; i < 4; i++ )
 717                         {
 718                                 *((u8*)pBuffer) = (data >> 8 * (adr & 0x3));
 719                                 pBuffer++;
 720                                 adr++;
 721                         }
 722                 }
 723         }
 724
 725         return retval;
 726 }
 727
 728 int ahbap_read_buf_packed_u16(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 729 {
 730         u32 invalue;
 731         int retval = ERROR_OK;
 732         int wcount, blocksize, readcount, i;
 733
 734         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 735
 736         wcount = count >> 1;
 737
 738         while (wcount > 0)
 739         {
 740                 int nbytes;
 741
 742                 /* Adjust to read within 4K block boundaries */
 743                 blocksize = (0x1000 - (0xFFF & address)) >> 1;
 744                 if (wcount < blocksize)
 745                         blocksize = wcount;
 746
 747                 ahbap_setup_accessport(swjdp, CSW_16BIT | CSW_ADDRINC_PACKED, address);
 748
 749                 /* handle unaligned data at 4k boundary */
 750                 if (blocksize == 0)
 751                         blocksize = 1;
 752                 readcount = blocksize;
 753
 754                 do
 755                 {
 756                         ahbap_read_reg_u32(swjdp, AHBAP_DRW, &invalue );
 757                         if (swjdp_transaction_endcheck(swjdp) != ERROR_OK)
 758                         {
 759                                 LOG_WARNING("Block read error address 0x%x, count 0x%x", address, count);
 760                                 return ERROR_JTAG_DEVICE_ERROR;
 761                         }
 762
 763                         nbytes = MIN((readcount << 1), 4);
 764
 765                         for (i = 0; i < nbytes; i++ )
 766                         {
 767                                 *((u8*)buffer) = (invalue >> 8 * (address & 0x3));
 768                                 buffer++;
 769                                 address++;
 770                         }
 771
 772                         readcount -= (nbytes >> 1);
 773                 } while (readcount);
 774                 wcount -= blocksize;
 775         }
 776
 777         return retval;
 778 }
 779
 780 int ahbap_read_buf_u16(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 781 {
 782         u32 invalue, i;
 783         int retval = ERROR_OK;
 784
 785         if (count >= 4)
 786                 return ahbap_read_buf_packed_u16(swjdp, buffer, count, address);
 787
 788         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 789
 790         while (count > 0)
 791         {
 792                 ahbap_setup_accessport(swjdp, CSW_16BIT | CSW_ADDRINC_SINGLE, address);
 793                 ahbap_read_reg_u32(swjdp, AHBAP_DRW, &invalue );
 794                 retval = swjdp_transaction_endcheck(swjdp);
 795                 if (address & 0x1)
 796                 {
 797                         for (i = 0; i < 2; i++ )
 798                         {
 799                                 *((u8*)buffer) = (invalue >> 8 * (address & 0x3));
 800                                 buffer++;
 801                                 address++;
 802                         }
 803                 }
 804                 else
 805                 {
 806                         *((u16*)buffer) = (invalue >> 8 * (address & 0x3));
 807                         address += 2;
 808                         buffer += 2;
 809                 }
 810                 count -= 2;
 811         }
 812
 813         return retval;
 814 }
 815
 816 int ahbap_read_buf_packed_u8(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 817 {
 818         u32 invalue;
 819         int retval = ERROR_OK;
 820         int wcount, blocksize, readcount, i;
 821
 822         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 823
 824         wcount = count;
 825
 826         while (wcount > 0)
 827         {
 828                 int nbytes;
 829
 830                 /* Adjust to read within 4K block boundaries */
 831                 blocksize = (0x1000 - (0xFFF & address));
 832
 833                 if (wcount < blocksize)
 834                         blocksize = wcount;
 835
 836                 ahbap_setup_accessport(swjdp, CSW_8BIT | CSW_ADDRINC_PACKED, address);
 837                 readcount = blocksize;
 838
 839                 do
 840                 {
 841                         ahbap_read_reg_u32(swjdp, AHBAP_DRW, &invalue );
 842                         if (swjdp_transaction_endcheck(swjdp) != ERROR_OK)
 843                         {
 844                                 LOG_WARNING("Block read error address 0x%x, count 0x%x", address, count);
 845                                 return ERROR_JTAG_DEVICE_ERROR;
 846                         }
 847
 848                         nbytes = MIN(readcount, 4);
 849
 850                         for (i = 0; i < nbytes; i++ )
 851                         {
 852                                 *((u8*)buffer) = (invalue >> 8 * (address & 0x3));
 853                                 buffer++;
 854                                 address++;
 855                         }
 856
 857                         readcount -= nbytes;
 858                 } while (readcount);
 859                 wcount -= blocksize;
 860         }
 861
 862         return retval;
 863 }
 864
 865 int ahbap_read_buf_u8(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 866 {
 867         u32 invalue;
 868         int retval = ERROR_OK;
 869
 870         if (count >= 4)
 871                 return ahbap_read_buf_packed_u8(swjdp, buffer, count, address);
 872
 873         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 874
 875         while (count > 0)
 876         {
 877                 ahbap_setup_accessport(swjdp, CSW_8BIT | CSW_ADDRINC_SINGLE, address);
 878                 ahbap_read_reg_u32(swjdp, AHBAP_DRW, &invalue );
 879                 retval = swjdp_transaction_endcheck(swjdp);
 880                 *((u8*)buffer) = (invalue >> 8 * (address & 0x3));
 881                 count--;
 882                 address++;
 883                 buffer++;
 884         }
 885
 886         return retval;
 887 }
 888
 889 int ahbap_read_coreregister_u32(swjdp_common_t *swjdp, u32 *value, int regnum)
 890 {
 891         int retval;
 892         u32 dcrdr;
 893
 894         /* because the DCB_DCRDR is used for the emulated dcc channel
 895          * we gave to save/restore the DCB_DCRDR when used */
 896
 897         ahbap_read_system_atomic_u32(swjdp, DCB_DCRDR, &dcrdr);
 898
 899         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 900
 901         /* ahbap_write_system_u32(swjdp, DCB_DCRSR, regnum); */
 902         ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, DCB_DCRSR & 0xFFFFFFF0);
 903         ahbap_write_reg_u32(swjdp, AHBAP_BD0 | (DCB_DCRSR & 0xC), regnum );
 904
 905         /* ahbap_read_system_u32(swjdp, DCB_DCRDR, value); */
 906         ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, DCB_DCRDR & 0xFFFFFFF0);
 907         ahbap_read_reg_u32(swjdp, AHBAP_BD0 | (DCB_DCRDR & 0xC), value );
 908
 909         retval = swjdp_transaction_endcheck(swjdp);
 910         ahbap_write_system_atomic_u32(swjdp, DCB_DCRDR, dcrdr);
 911         return retval;
 912 }
 913
 914 int ahbap_write_coreregister_u32(swjdp_common_t *swjdp, u32 value, int regnum)
 915 {
 916         int retval;
 917         u32 dcrdr;
 918
 919         /* because the DCB_DCRDR is used for the emulated dcc channel
 920          * we gave to save/restore the DCB_DCRDR when used */
 921
 922         ahbap_read_system_atomic_u32(swjdp, DCB_DCRDR, &dcrdr);
 923
 924         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 925
 926         /* ahbap_write_system_u32(swjdp, DCB_DCRDR, core_regs[i]); */
 927         ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, DCB_DCRDR & 0xFFFFFFF0);
 928         ahbap_write_reg_u32(swjdp, AHBAP_BD0 | (DCB_DCRDR & 0xC), value );
 929
 930         /* ahbap_write_system_u32(swjdp, DCB_DCRSR, i | DCRSR_WnR       ); */
 931         ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, DCB_DCRSR & 0xFFFFFFF0);
 932         ahbap_write_reg_u32(swjdp, AHBAP_BD0 | (DCB_DCRSR & 0xC), regnum | DCRSR_WnR );
 933
 934         retval = swjdp_transaction_endcheck(swjdp);
 935         ahbap_write_system_atomic_u32(swjdp, DCB_DCRDR, dcrdr);
 936         return retval;
 937 }
 938
 939 int ahbap_debugport_init(swjdp_common_t *swjdp)
 940 {
 941         u32 idreg, romaddr, dummy;
 942         u32 ctrlstat;
 943         int cnt = 0;
 944
 945         LOG_DEBUG(" ");
 946
 947         swjdp->ap_csw_value = -1;
 948         swjdp->ap_tar_value = -1;
 949         swjdp->trans_mode = TRANS_MODE_ATOMIC;
 950         swjdp_read_dpacc(swjdp, &dummy, DP_CTRL_STAT);
 951         swjdp_write_dpacc(swjdp, SSTICKYERR, DP_CTRL_STAT);
 952         swjdp_read_dpacc(swjdp, &dummy, DP_CTRL_STAT);
 953
 954         swjdp->dp_ctrl_stat = CDBGPWRUPREQ | CSYSPWRUPREQ;
 955
 956         swjdp_write_dpacc(swjdp, swjdp->dp_ctrl_stat, DP_CTRL_STAT);
 957         swjdp_read_dpacc(swjdp, &ctrlstat, DP_CTRL_STAT);
 958         jtag_execute_queue();
 959
 960         /* Check that we have debug power domains activated */
 961         while (!(ctrlstat & CDBGPWRUPACK) && (cnt++ < 10))
 962         {
 963                 LOG_DEBUG("swjdp: wait CDBGPWRUPACK");
 964                 swjdp_read_dpacc(swjdp, &ctrlstat, DP_CTRL_STAT);
 965                 jtag_execute_queue();
 966                 usleep(10000);
 967         }
 968
 969         while (!(ctrlstat & CSYSPWRUPACK) && (cnt++ < 10))
 970         {
 971                 LOG_DEBUG("swjdp: wait CSYSPWRUPACK");
 972                 swjdp_read_dpacc(swjdp, &ctrlstat, DP_CTRL_STAT);
 973                 jtag_execute_queue();
 974                 usleep(10000);
 975         }
 976
 977         swjdp_read_dpacc(swjdp, &dummy, DP_CTRL_STAT);
 978         /* With debug power on we can activate OVERRUN checking */
 979         swjdp->dp_ctrl_stat = CDBGPWRUPREQ | CSYSPWRUPREQ | CORUNDETECT;
 980         swjdp_write_dpacc(swjdp, swjdp->dp_ctrl_stat, DP_CTRL_STAT);
 981         swjdp_read_dpacc(swjdp, &dummy, DP_CTRL_STAT);
 982
 983         ahbap_read_reg_u32(swjdp, 0xFC, &idreg);
 984         ahbap_read_reg_u32(swjdp, 0xF8, &romaddr);
 985
 986         LOG_DEBUG("AHB-AP ID Register 0x%x, Debug ROM Address 0x%x", idreg, romaddr);
 987
 988         return ERROR_OK;
 989 }