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