git.sur5r.net Git - openocd/blob - src/target/cortex_swjdp.c

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