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