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