git.gag.com Git - fw/openocd/blob - src/target/arm11_dbgtap.c

   1 /***************************************************************************
   2  *   Copyright (C) 2008 digenius technology GmbH.                          *
   3  *   Michael Bruck                                                         *
   4  *                                                                         *
   5  *   Copyright (C) 2008,2009 Oyvind Harboe oyvind.harboe@zylin.com         *
   6  *                                                                         *
   7  *   This program is free software; you can redistribute it and/or modify  *
   8  *   it under the terms of the GNU General Public License as published by  *
   9  *   the Free Software Foundation; either version 2 of the License, or     *
  10  *   (at your option) any later version.                                   *
  11  *                                                                         *
  12  *   This program is distributed in the hope that it will be useful,       *
  13  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  14  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  15  *   GNU General Public License for more details.                          *
  16  *                                                                         *
  17  *   You should have received a copy of the GNU General Public License     *
  18  *   along with this program; if not, write to the                         *
  19  *   Free Software Foundation, Inc.,                                       *
  20  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  21  ***************************************************************************/
  22
  23 #ifdef HAVE_CONFIG_H
  24 #include "config.h"
  25 #endif
  26
  27 #include "arm_jtag.h"
  28 #include "arm11_dbgtap.h"
  29
  30 #include <helper/time_support.h>
  31
  32 #if 0
  33 #define JTAG_DEBUG(expr ...)    do { if (1) \
  34                                              LOG_DEBUG(expr); } while (0)
  35 #else
  36 #define JTAG_DEBUG(expr ...)    do { if (0) \
  37                                              LOG_DEBUG(expr); } while (0)
  38 #endif
  39
  40 /*
  41 This pathmove goes from Pause-IR to Shift-IR while avoiding RTI. The
  42 behavior of the FTDI driver IIRC was to go via RTI.
  43
  44 Conversely there may be other places in this code where the ARM11 code relies
  45 on the driver to hit through RTI when coming from Update-?R.
  46 */
  47 static const tap_state_t arm11_move_pi_to_si_via_ci[] = {
  48         TAP_IREXIT2, TAP_IRUPDATE, TAP_DRSELECT, TAP_IRSELECT, TAP_IRCAPTURE, TAP_IRSHIFT
  49 };
  50
  51 /* REVISIT no error handling here! */
  52 static void arm11_add_ir_scan_vc(struct jtag_tap *tap, struct scan_field *fields,
  53         tap_state_t state)
  54 {
  55         if (cmd_queue_cur_state == TAP_IRPAUSE)
  56                 jtag_add_pathmove(ARRAY_SIZE(arm11_move_pi_to_si_via_ci),
  57                         arm11_move_pi_to_si_via_ci);
  58
  59         jtag_add_ir_scan(tap, fields, state);
  60 }
  61
  62 static const tap_state_t arm11_move_pd_to_sd_via_cd[] = {
  63         TAP_DREXIT2, TAP_DRUPDATE, TAP_DRSELECT, TAP_DRCAPTURE, TAP_DRSHIFT
  64 };
  65
  66 /* REVISIT no error handling here! */
  67 void arm11_add_dr_scan_vc(struct jtag_tap *tap, int num_fields, struct scan_field *fields,
  68         tap_state_t state)
  69 {
  70         if (cmd_queue_cur_state == TAP_DRPAUSE)
  71                 jtag_add_pathmove(ARRAY_SIZE(arm11_move_pd_to_sd_via_cd),
  72                         arm11_move_pd_to_sd_via_cd);
  73
  74         jtag_add_dr_scan(tap, num_fields, fields, state);
  75 }
  76
  77
  78 /** Code de-clutter: Construct struct scan_field to write out a value
  79  *
  80  * \param arm11                 Target state variable.
  81  * \param num_bits              Length of the data field
  82  * \param out_data              pointer to the data that will be sent out
  83  *                                              <em > (data is read when it is added to the JTAG queue)</em>
  84  * \param in_data               pointer to the memory that will receive data that was clocked in
  85  *                                              <em > (data is written when the JTAG queue is executed)</em>
  86  * \param field                 target data structure that will be initialized
  87  */
  88 void arm11_setup_field(struct arm11_common *arm11, int num_bits,
  89         void *out_data, void *in_data, struct scan_field *field)
  90 {
  91         field->num_bits                 = num_bits;
  92         field->out_value                = out_data;
  93         field->in_value                 = in_data;
  94 }
  95
  96 static const char *arm11_ir_to_string(uint8_t ir)
  97 {
  98         const char *s = "unknown";
  99
 100         switch (ir) {
 101                 case ARM11_EXTEST:
 102                         s = "EXTEST";
 103                         break;
 104                 case ARM11_SCAN_N:
 105                         s = "SCAN_N";
 106                         break;
 107                 case ARM11_RESTART:
 108                         s = "RESTART";
 109                         break;
 110                 case ARM11_HALT:
 111                         s = "HALT";
 112                         break;
 113                 case ARM11_INTEST:
 114                         s = "INTEST";
 115                         break;
 116                 case ARM11_ITRSEL:
 117                         s = "ITRSEL";
 118                         break;
 119                 case ARM11_IDCODE:
 120                         s = "IDCODE";
 121                         break;
 122                 case ARM11_BYPASS:
 123                         s = "BYPASS";
 124                         break;
 125         }
 126         return s;
 127 }
 128
 129 /** Write JTAG instruction register
 130  *
 131  * \param arm11         Target state variable.
 132  * \param instr         An ARM11 DBGTAP instruction. Use enum #arm11_instructions.
 133  * \param state         Pass the final TAP state or ARM11_TAP_DEFAULT for the default value (Pause-IR).
 134  *
 135  * \remarks                     This adds to the JTAG command queue but does \em not execute it.
 136  */
 137 void arm11_add_IR(struct arm11_common *arm11, uint8_t instr, tap_state_t state)
 138 {
 139         struct jtag_tap *tap = arm11->arm.target->tap;
 140
 141         if (buf_get_u32(tap->cur_instr, 0, 5) == instr) {
 142                 JTAG_DEBUG("IR <= 0x%02x SKIPPED", instr);
 143                 return;
 144         }
 145
 146         JTAG_DEBUG("IR <= %s (0x%02x)", arm11_ir_to_string(instr), instr);
 147
 148         struct scan_field field;
 149
 150         arm11_setup_field(arm11, 5, &instr, NULL, &field);
 151
 152         arm11_add_ir_scan_vc(arm11->arm.target->tap,
 153                 &field,
 154                 state == ARM11_TAP_DEFAULT ? TAP_IRPAUSE : state);
 155 }
 156
 157 /** Verify data shifted out from Scan Chain Register (SCREG). */
 158 static void arm11_in_handler_SCAN_N(uint8_t *in_value)
 159 {
 160         /* Don't expect JTAG layer to modify bits we didn't ask it to read */
 161         uint8_t v = *in_value & 0x1F;
 162
 163         if (v != 0x10) {
 164                 LOG_ERROR("'arm11 target' JTAG error SCREG OUT 0x%02x", v);
 165                 jtag_set_error(ERROR_FAIL);
 166         }
 167 }
 168
 169 /** Select and write to Scan Chain Register (SCREG)
 170  *
 171  * This function sets the instruction register to SCAN_N and writes
 172  * the data register with the selected chain number.
 173  *
 174  * http://infocenter.arm.com/help/topic/com.arm.doc.ddi0301f/Cacbjhfg.html
 175  *
 176  * \param arm11     Target state variable.
 177  * \param chain     Scan chain that will be selected.
 178  * \param state     Pass the final TAP state or ARM11_TAP_DEFAULT for the default
 179  *                                      value (Pause-DR).
 180  *
 181  * Changes the current scan chain if needed, transitions to the specified
 182  * TAP state, and leaves the IR undefined.
 183  *
 184  * The chain takes effect when Update-DR is passed (usually when subsequently
 185  * the INTEXT/EXTEST instructions are written).
 186  *
 187  * \warning                     (Obsolete) Using this twice in a row will \em fail. The first
 188  *                                      call will end in Pause-DR. The second call, due to the IR
 189  *                                      caching, will not go through Capture-DR when shifting in the
 190  *                                      new scan chain number. As a result the verification in
 191  *                                      arm11_in_handler_SCAN_N() must fail.
 192  *
 193  * \remarks                     This adds to the JTAG command queue but does \em not execute it.
 194  */
 195
 196 int arm11_add_debug_SCAN_N(struct arm11_common *arm11,
 197         uint8_t chain, tap_state_t state)
 198 {
 199         /* Don't needlessly switch the scan chain.
 200          * NOTE:  the ITRSEL instruction fakes SCREG changing;
 201          * but leaves its actual value unchanged.
 202          */
 203 #if 0
 204         /* FIX!!! the optimization below is broken because we do not */
 205         /* invalidate the cur_scan_chain upon a TRST/TMS. See arm_jtag.c */
 206         /* for example on how to invalidate cur_scan_chain. Tested patches gladly */
 207         /* accepted! */
 208         if (arm11->jtag_info.cur_scan_chain == chain) {
 209                 JTAG_DEBUG("SCREG <= %d SKIPPED", chain);
 210                 return jtag_add_statemove((state == ARM11_TAP_DEFAULT)
 211                         ? TAP_DRPAUSE : state);
 212         }
 213 #endif
 214         JTAG_DEBUG("SCREG <= %d", chain);
 215
 216         arm11_add_IR(arm11, ARM11_SCAN_N, ARM11_TAP_DEFAULT);
 217
 218         struct scan_field field;
 219
 220         uint8_t tmp[1];
 221         arm11_setup_field(arm11, 5, &chain, &tmp, &field);
 222
 223         arm11_add_dr_scan_vc(arm11->arm.target->tap,
 224                 1,
 225                 &field,
 226                 state == ARM11_TAP_DEFAULT ? TAP_DRPAUSE : state);
 227
 228         jtag_execute_queue_noclear();
 229
 230         arm11_in_handler_SCAN_N(tmp);
 231
 232         arm11->jtag_info.cur_scan_chain = chain;
 233
 234         return jtag_execute_queue();
 235 }
 236
 237 /**
 238  * Queue a DR scan of the ITR register.  Caller must have selected
 239  * scan chain 4 (ITR), possibly using ITRSEL.
 240  *
 241  * \param arm11         Target state variable.
 242  * \param inst          An ARM11 processor instruction/opcode.
 243  * \param flag          Optional parameter to retrieve the Ready flag;
 244  *      this address will be written when the JTAG chain is scanned.
 245  * \param state         The TAP state to enter after the DR scan.
 246  *
 247  * Going through the TAP_DRUPDATE state writes ITR only if Ready was
 248  * previously set.  Only the Ready flag is readable by the scan.
 249  *
 250  * An instruction loaded into ITR is executed when going through the
 251  * TAP_IDLE state only if Ready was previously set and the debug state
 252  * is properly set up.  Depending on the instruction, you may also need
 253  * to ensure that the rDTR is ready before that Run-Test/Idle state.
 254  */
 255 static void arm11_add_debug_INST(struct arm11_common *arm11,
 256         uint32_t inst, uint8_t *flag, tap_state_t state)
 257 {
 258         JTAG_DEBUG("INST <= 0x%08x", (unsigned) inst);
 259
 260         struct scan_field itr[2];
 261
 262         arm11_setup_field(arm11, 32, &inst, NULL, itr + 0);
 263         arm11_setup_field(arm11, 1, NULL, flag, itr + 1);
 264
 265         arm11_add_dr_scan_vc(arm11->arm.target->tap, ARRAY_SIZE(itr), itr, state);
 266 }
 267
 268 /**
 269  * Read and save the Debug Status and Control Register (DSCR).
 270  *
 271  * \param arm11         Target state variable.
 272  * \return Error status; arm11->dscr is updated on success.
 273  *
 274  * \remarks This is a stand-alone function that executes the JTAG
 275  * command queue.  It does not require the ARM11 debug TAP to be
 276  * in any particular state.
 277  */
 278 int arm11_read_DSCR(struct arm11_common *arm11)
 279 {
 280         int retval;
 281
 282         retval = arm11_add_debug_SCAN_N(arm11, 0x01, ARM11_TAP_DEFAULT);
 283         if (retval != ERROR_OK)
 284                 return retval;
 285
 286         arm11_add_IR(arm11, ARM11_INTEST, ARM11_TAP_DEFAULT);
 287
 288         uint32_t dscr;
 289         struct scan_field chain1_field;
 290
 291         arm11_setup_field(arm11, 32, NULL, &dscr, &chain1_field);
 292
 293         arm11_add_dr_scan_vc(arm11->arm.target->tap, 1, &chain1_field, TAP_DRPAUSE);
 294
 295         CHECK_RETVAL(jtag_execute_queue());
 296
 297         if (arm11->dscr != dscr)
 298                 JTAG_DEBUG("DSCR  = %08x (OLD %08x)",
 299                         (unsigned) dscr,
 300                         (unsigned) arm11->dscr);
 301
 302         arm11->dscr = dscr;
 303
 304         return ERROR_OK;
 305 }
 306
 307 /** Write the Debug Status and Control Register (DSCR)
 308  *
 309  * same as CP14 c1
 310  *
 311  * \param arm11         Target state variable.
 312  * \param dscr          DSCR content
 313  *
 314  * \remarks                     This is a stand-alone function that executes the JTAG command queue.
 315  */
 316 int arm11_write_DSCR(struct arm11_common *arm11, uint32_t dscr)
 317 {
 318         int retval;
 319         retval = arm11_add_debug_SCAN_N(arm11, 0x01, ARM11_TAP_DEFAULT);
 320         if (retval != ERROR_OK)
 321                 return retval;
 322
 323         arm11_add_IR(arm11, ARM11_EXTEST, ARM11_TAP_DEFAULT);
 324
 325         struct scan_field chain1_field;
 326
 327         arm11_setup_field(arm11, 32, &dscr, NULL, &chain1_field);
 328
 329         arm11_add_dr_scan_vc(arm11->arm.target->tap, 1, &chain1_field, TAP_DRPAUSE);
 330
 331         CHECK_RETVAL(jtag_execute_queue());
 332
 333         JTAG_DEBUG("DSCR <= %08x (OLD %08x)",
 334                 (unsigned) dscr,
 335                 (unsigned) arm11->dscr);
 336
 337         arm11->dscr = dscr;
 338
 339         return ERROR_OK;
 340 }
 341
 342 /** Prepare the stage for ITR/DTR operations
 343  * from the arm11_run_instr... group of functions.
 344  *
 345  * Put arm11_run_instr_data_prepare() and arm11_run_instr_data_finish()
 346  * around a block of arm11_run_instr_... calls.
 347  *
 348  * Select scan chain 5 to allow quick access to DTR. When scan
 349  * chain 4 is needed to put in a register the ITRSel instruction
 350  * shortcut is used instead of actually changing the Scan_N
 351  * register.
 352  *
 353  * \param arm11         Target state variable.
 354  *
 355  */
 356 int arm11_run_instr_data_prepare(struct arm11_common *arm11)
 357 {
 358         return arm11_add_debug_SCAN_N(arm11, 0x05, ARM11_TAP_DEFAULT);
 359 }
 360
 361 /** Cleanup after ITR/DTR operations
 362  * from the arm11_run_instr... group of functions
 363  *
 364  * Put arm11_run_instr_data_prepare() and arm11_run_instr_data_finish()
 365  * around a block of arm11_run_instr_... calls.
 366  *
 367  * Any IDLE can lead to an instruction execution when
 368  * scan chains 4 or 5 are selected and the IR holds
 369  * INTEST or EXTEST. So we must disable that before
 370  * any following activities lead to an IDLE.
 371  *
 372  * \param arm11         Target state variable.
 373  *
 374  */
 375 int arm11_run_instr_data_finish(struct arm11_common *arm11)
 376 {
 377         return arm11_add_debug_SCAN_N(arm11, 0x00, ARM11_TAP_DEFAULT);
 378 }
 379
 380 /**
 381  * Execute one or more instructions via ITR.
 382  * Caller guarantees that processor is in debug state, that DSCR_ITR_EN
 383  * is set, the ITR Ready flag is set (as seen on the previous entry to
 384  * TAP_DRCAPTURE), and the DSCR sticky abort flag is clear.
 385  *
 386  * \pre arm11_run_instr_data_prepare() /  arm11_run_instr_data_finish() block
 387  *
 388  * \param arm11         Target state variable.
 389  * \param opcode        Pointer to sequence of ARM opcodes
 390  * \param count         Number of opcodes to execute
 391  *
 392  */
 393 static
 394 int arm11_run_instr_no_data(struct arm11_common *arm11,
 395         uint32_t *opcode, size_t count)
 396 {
 397         arm11_add_IR(arm11, ARM11_ITRSEL, ARM11_TAP_DEFAULT);
 398
 399         while (count--) {
 400                 arm11_add_debug_INST(arm11, *opcode++, NULL, TAP_IDLE);
 401
 402                 int i = 0;
 403                 while (1) {
 404                         uint8_t flag;
 405
 406                         arm11_add_debug_INST(arm11, 0, &flag, count ? TAP_IDLE : TAP_DRPAUSE);
 407
 408                         CHECK_RETVAL(jtag_execute_queue());
 409
 410                         if (flag)
 411                                 break;
 412
 413                         long long then = 0;
 414
 415                         if (i == 1000)
 416                                 then = timeval_ms();
 417                         if (i >= 1000) {
 418                                 if ((timeval_ms()-then) > 1000) {
 419                                         LOG_WARNING(
 420                                                 "Timeout (1000ms) waiting for instructions to complete");
 421                                         return ERROR_FAIL;
 422                                 }
 423                         }
 424
 425                         i++;
 426                 }
 427         }
 428
 429         return ERROR_OK;
 430 }
 431
 432 /** Execute one instruction via ITR
 433  *
 434  * \pre arm11_run_instr_data_prepare() /  arm11_run_instr_data_finish() block
 435  *
 436  * \param arm11         Target state variable.
 437  * \param opcode        ARM opcode
 438  *
 439  */
 440 int arm11_run_instr_no_data1(struct arm11_common *arm11, uint32_t opcode)
 441 {
 442         return arm11_run_instr_no_data(arm11, &opcode, 1);
 443 }
 444
 445
 446 /** Execute one instruction via ITR repeatedly while
 447  *  passing data to the core via DTR on each execution.
 448  *
 449  * Caller guarantees that processor is in debug state, that DSCR_ITR_EN
 450  * is set, the ITR Ready flag is set (as seen on the previous entry to
 451  * TAP_DRCAPTURE), and the DSCR sticky abort flag is clear.
 452  *
 453  *  The executed instruction \em must read data from DTR.
 454  *
 455  * \pre arm11_run_instr_data_prepare() /  arm11_run_instr_data_finish() block
 456  *
 457  * \param arm11         Target state variable.
 458  * \param opcode        ARM opcode
 459  * \param data          Pointer to the data words to be passed to the core
 460  * \param count         Number of data words and instruction repetitions
 461  *
 462  */
 463 int arm11_run_instr_data_to_core(struct arm11_common *arm11,
 464         uint32_t opcode,
 465         uint32_t *data,
 466         size_t count)
 467 {
 468         arm11_add_IR(arm11, ARM11_ITRSEL, ARM11_TAP_DEFAULT);
 469
 470         arm11_add_debug_INST(arm11, opcode, NULL, TAP_DRPAUSE);
 471
 472         arm11_add_IR(arm11, ARM11_EXTEST, ARM11_TAP_DEFAULT);
 473
 474         struct scan_field chain5_fields[3];
 475
 476         uint32_t Data;
 477         uint8_t Ready;
 478         uint8_t nRetry;
 479
 480         arm11_setup_field(arm11, 32,    &Data,  NULL,           chain5_fields + 0);
 481         arm11_setup_field(arm11,  1,    NULL,   &Ready,         chain5_fields + 1);
 482         arm11_setup_field(arm11,  1,    NULL,   &nRetry,        chain5_fields + 2);
 483
 484         while (count--) {
 485                 int i = 0;
 486                 do {
 487                         Data        = *data;
 488
 489                         arm11_add_dr_scan_vc(arm11->arm.target->tap, ARRAY_SIZE(
 490                                         chain5_fields), chain5_fields, TAP_IDLE);
 491
 492                         CHECK_RETVAL(jtag_execute_queue());
 493
 494                         JTAG_DEBUG("DTR  Ready %d  nRetry %d", Ready, nRetry);
 495
 496                         long long then = 0;
 497
 498                         if (i == 1000)
 499                                 then = timeval_ms();
 500                         if (i >= 1000) {
 501                                 if ((timeval_ms()-then) > 1000) {
 502                                         LOG_WARNING(
 503                                                 "Timeout (1000ms) waiting for instructions to complete");
 504                                         return ERROR_FAIL;
 505                                 }
 506                         }
 507
 508                         i++;
 509                 } while (!Ready);
 510
 511                 data++;
 512         }
 513
 514         arm11_add_IR(arm11, ARM11_INTEST, ARM11_TAP_DEFAULT);
 515
 516         int i = 0;
 517         do {
 518                 Data        = 0;
 519
 520                 arm11_add_dr_scan_vc(arm11->arm.target->tap, ARRAY_SIZE(
 521                                 chain5_fields), chain5_fields, TAP_DRPAUSE);
 522
 523                 CHECK_RETVAL(jtag_execute_queue());
 524
 525                 JTAG_DEBUG("DTR  Data %08x  Ready %d  nRetry %d",
 526                         (unsigned) Data, Ready, nRetry);
 527
 528                 long long then = 0;
 529
 530                 if (i == 1000)
 531                         then = timeval_ms();
 532                 if (i >= 1000) {
 533                         if ((timeval_ms()-then) > 1000) {
 534                                 LOG_WARNING("Timeout (1000ms) waiting for instructions to complete");
 535                                 return ERROR_FAIL;
 536                         }
 537                 }
 538
 539                 i++;
 540         } while (!Ready);
 541
 542         return ERROR_OK;
 543 }
 544
 545 /** JTAG path for arm11_run_instr_data_to_core_noack
 546  *
 547  *  The repeated TAP_IDLE's do not cause a repeated execution
 548  *  if passed without leaving the state.
 549  *
 550  *  Since this is more than 7 bits (adjustable via adding more
 551  *  TAP_IDLE's) it produces an artificial delay in the lower
 552  *  layer (FT2232) that is long enough to finish execution on
 553  *  the core but still shorter than any manually inducible delays.
 554  *
 555  *  To disable this code, try "memwrite burst false"
 556  *
 557  *  FIX!!! should we use multiple TAP_IDLE here or not???
 558  *
 559  *  https://lists.berlios.de/pipermail/openocd-development/2009-July/009698.html
 560  *  https://lists.berlios.de/pipermail/openocd-development/2009-August/009865.html
 561  */
 562 static const tap_state_t arm11_MOVE_DRPAUSE_IDLE_DRPAUSE_with_delay[] = {
 563         TAP_DREXIT2, TAP_DRUPDATE, TAP_IDLE, TAP_IDLE, TAP_IDLE, TAP_DRSELECT, TAP_DRCAPTURE,
 564         TAP_DRSHIFT
 565 };
 566
 567 /* This inner loop can be implemented by the minidriver, oftentimes in hardware... The
 568  * minidriver can call the default implementation as a fallback or implement it
 569  * from scratch.
 570  */
 571 int arm11_run_instr_data_to_core_noack_inner_default(struct jtag_tap *tap,
 572         uint32_t opcode,
 573         uint32_t *data,
 574         size_t count)
 575 {
 576         struct scan_field chain5_fields[3];
 577
 578         chain5_fields[0].num_bits               = 32;
 579         chain5_fields[0].out_value              = NULL; /*&Data*/
 580         chain5_fields[0].in_value               = NULL;
 581
 582         chain5_fields[1].num_bits               = 1;
 583         chain5_fields[1].out_value              = NULL;
 584         chain5_fields[1].in_value               = NULL; /*&Ready*/
 585
 586         chain5_fields[2].num_bits               = 1;
 587         chain5_fields[2].out_value              = NULL;
 588         chain5_fields[2].in_value               = NULL;
 589
 590         uint8_t *Readies;
 591         unsigned readiesNum = count;
 592         unsigned bytes = sizeof(*Readies)*readiesNum;
 593
 594         Readies = (uint8_t *) malloc(bytes);
 595         if (Readies == NULL) {
 596                 LOG_ERROR("Out of memory allocating %u bytes", bytes);
 597                 return ERROR_FAIL;
 598         }
 599
 600         uint8_t *ReadyPos                        = Readies;
 601         while (count--) {
 602                 chain5_fields[0].out_value      = (void *)(data++);
 603                 chain5_fields[1].in_value       = ReadyPos++;
 604
 605                 if (count > 0) {
 606                         jtag_add_dr_scan(tap, ARRAY_SIZE(chain5_fields), chain5_fields,
 607                                 TAP_DRPAUSE);
 608                         jtag_add_pathmove(ARRAY_SIZE(arm11_MOVE_DRPAUSE_IDLE_DRPAUSE_with_delay),
 609                                 arm11_MOVE_DRPAUSE_IDLE_DRPAUSE_with_delay);
 610                 } else
 611                         jtag_add_dr_scan(tap, ARRAY_SIZE(chain5_fields), chain5_fields, TAP_IDLE);
 612         }
 613
 614         int retval = jtag_execute_queue();
 615         if (retval == ERROR_OK) {
 616                 unsigned error_count = 0;
 617
 618                 for (size_t i = 0; i < readiesNum; i++) {
 619                         if (Readies[i] != 1)
 620                                 error_count++;
 621                 }
 622
 623                 if (error_count > 0) {
 624                         LOG_ERROR("%u words out of %u not transferred",
 625                                 error_count, readiesNum);
 626                         retval = ERROR_FAIL;
 627                 }
 628         }
 629         free(Readies);
 630
 631         return retval;
 632 }
 633
 634 int arm11_run_instr_data_to_core_noack_inner(struct jtag_tap *tap,
 635                                              uint32_t opcode,
 636                                              uint32_t *data,
 637                                              size_t count);
 638
 639 #ifndef HAVE_JTAG_MINIDRIVER_H
 640 int arm11_run_instr_data_to_core_noack_inner(struct jtag_tap *tap,
 641         uint32_t opcode,
 642         uint32_t *data,
 643         size_t count)
 644 {
 645         return arm11_run_instr_data_to_core_noack_inner_default(tap, opcode, data, count);
 646 }
 647 #endif
 648
 649 /** Execute one instruction via ITR repeatedly while
 650  *  passing data to the core via DTR on each execution.
 651  *
 652  * Caller guarantees that processor is in debug state, that DSCR_ITR_EN
 653  * is set, the ITR Ready flag is set (as seen on the previous entry to
 654  * TAP_DRCAPTURE), and the DSCR sticky abort flag is clear.
 655  *
 656  *  No Ready check during transmission.
 657  *
 658  *  The executed instruction \em must read data from DTR.
 659  *
 660  * \pre arm11_run_instr_data_prepare() /  arm11_run_instr_data_finish() block
 661  *
 662  * \param arm11         Target state variable.
 663  * \param opcode        ARM opcode
 664  * \param data          Pointer to the data words to be passed to the core
 665  * \param count         Number of data words and instruction repetitions
 666  *
 667  */
 668 int arm11_run_instr_data_to_core_noack(struct arm11_common *arm11,
 669         uint32_t opcode,
 670         uint32_t *data,
 671         size_t count)
 672 {
 673         arm11_add_IR(arm11, ARM11_ITRSEL, ARM11_TAP_DEFAULT);
 674
 675         arm11_add_debug_INST(arm11, opcode, NULL, TAP_DRPAUSE);
 676
 677         arm11_add_IR(arm11, ARM11_EXTEST, ARM11_TAP_DEFAULT);
 678
 679         int retval = arm11_run_instr_data_to_core_noack_inner(arm11->arm.target->tap,
 680                         opcode,
 681                         data,
 682                         count);
 683
 684         if (retval != ERROR_OK)
 685                 return retval;
 686
 687         arm11_add_IR(arm11, ARM11_INTEST, ARM11_TAP_DEFAULT);
 688
 689         struct scan_field chain5_fields[3];
 690
 691         arm11_setup_field(arm11,
 692                 32,
 693                 NULL /*&Data*/,
 694                 NULL,
 695                 chain5_fields + 0);
 696         arm11_setup_field(arm11,
 697                 1,
 698                 NULL,
 699                 NULL /*&Ready*/,
 700                 chain5_fields + 1);
 701         arm11_setup_field(arm11,
 702                 1,
 703                 NULL,
 704                 NULL,
 705                 chain5_fields + 2);
 706
 707         uint8_t ready_flag;
 708         chain5_fields[1].in_value   = &ready_flag;
 709
 710         arm11_add_dr_scan_vc(arm11->arm.target->tap, ARRAY_SIZE(
 711                         chain5_fields), chain5_fields, TAP_DRPAUSE);
 712
 713         retval = jtag_execute_queue();
 714         if (retval == ERROR_OK) {
 715                 if (ready_flag != 1) {
 716                         LOG_ERROR("last word not transferred");
 717                         retval = ERROR_FAIL;
 718                 }
 719         }
 720
 721         return retval;
 722 }
 723
 724
 725 /** Execute an instruction via ITR while handing data into the core via DTR.
 726  *
 727  *  The executed instruction \em must read data from DTR.
 728  *
 729  * \pre arm11_run_instr_data_prepare() /  arm11_run_instr_data_finish() block
 730  *
 731  * \param arm11         Target state variable.
 732  * \param opcode        ARM opcode
 733  * \param data          Data word to be passed to the core via DTR
 734  *
 735  */
 736 int arm11_run_instr_data_to_core1(struct arm11_common *arm11, uint32_t opcode, uint32_t data)
 737 {
 738         return arm11_run_instr_data_to_core(arm11, opcode, &data, 1);
 739 }
 740
 741
 742 /** Execute one instruction via ITR repeatedly while
 743  *  reading data from the core via DTR on each execution.
 744  *
 745  * Caller guarantees that processor is in debug state, that DSCR_ITR_EN
 746  * is set, the ITR Ready flag is set (as seen on the previous entry to
 747  * TAP_DRCAPTURE), and the DSCR sticky abort flag is clear.
 748  *
 749  *  The executed instruction \em must write data to DTR.
 750  *
 751  * \pre arm11_run_instr_data_prepare() /  arm11_run_instr_data_finish() block
 752  *
 753  * \param arm11         Target state variable.
 754  * \param opcode        ARM opcode
 755  * \param data          Pointer to an array that receives the data words from the core
 756  * \param count         Number of data words and instruction repetitions
 757  *
 758  */
 759 int arm11_run_instr_data_from_core(struct arm11_common *arm11,
 760         uint32_t opcode,
 761         uint32_t *data,
 762         size_t count)
 763 {
 764         arm11_add_IR(arm11, ARM11_ITRSEL, ARM11_TAP_DEFAULT);
 765
 766         arm11_add_debug_INST(arm11, opcode, NULL, TAP_IDLE);
 767
 768         arm11_add_IR(arm11, ARM11_INTEST, ARM11_TAP_DEFAULT);
 769
 770         struct scan_field chain5_fields[3];
 771
 772         uint32_t Data;
 773         uint8_t Ready;
 774         uint8_t nRetry;
 775
 776         arm11_setup_field(arm11, 32,    NULL,   &Data,      chain5_fields + 0);
 777         arm11_setup_field(arm11,  1,    NULL,   &Ready,     chain5_fields + 1);
 778         arm11_setup_field(arm11,  1,    NULL,   &nRetry,    chain5_fields + 2);
 779
 780         while (count--) {
 781                 int i = 0;
 782                 do {
 783                         arm11_add_dr_scan_vc(arm11->arm.target->tap, ARRAY_SIZE(
 784                                         chain5_fields), chain5_fields,
 785                                 count ? TAP_IDLE : TAP_DRPAUSE);
 786
 787                         CHECK_RETVAL(jtag_execute_queue());
 788
 789                         JTAG_DEBUG("DTR  Data %08x  Ready %d  nRetry %d",
 790                                 (unsigned) Data, Ready, nRetry);
 791
 792                         long long then = 0;
 793
 794                         if (i == 1000)
 795                                 then = timeval_ms();
 796                         if (i >= 1000) {
 797                                 if ((timeval_ms()-then) > 1000) {
 798                                         LOG_WARNING(
 799                                                 "Timeout (1000ms) waiting for instructions to complete");
 800                                         return ERROR_FAIL;
 801                                 }
 802                         }
 803
 804                         i++;
 805                 } while (!Ready);
 806
 807                 *data++ = Data;
 808         }
 809
 810         return ERROR_OK;
 811 }
 812
 813 /** Execute one instruction via ITR
 814  *  then load r0 into DTR and read DTR from core.
 815  *
 816  *  The first executed instruction (\p opcode) should write data to r0.
 817  *
 818  * \pre arm11_run_instr_data_prepare() /  arm11_run_instr_data_finish() block
 819  *
 820  * \param arm11         Target state variable.
 821  * \param opcode        ARM opcode to write r0 with the value of interest
 822  * \param data          Pointer to a data word that receives the value from r0 after \p opcode was executed.
 823  *
 824  */
 825 int arm11_run_instr_data_from_core_via_r0(struct arm11_common *arm11,
 826         uint32_t opcode,
 827         uint32_t *data)
 828 {
 829         int retval;
 830         retval = arm11_run_instr_no_data1(arm11, opcode);
 831         if (retval != ERROR_OK)
 832                 return retval;
 833
 834         /* MCR p14,0,R0,c0,c5,0 (move r0 -> wDTR -> local var) */
 835         arm11_run_instr_data_from_core(arm11, 0xEE000E15, data, 1);
 836
 837         return ERROR_OK;
 838 }
 839
 840 /** Load data into core via DTR then move it to r0 then
 841  *  execute one instruction via ITR
 842  *
 843  *  The final executed instruction (\p opcode) should read data from r0.
 844  *
 845  * \pre arm11_run_instr_data_prepare() /  arm11_run_instr_data_finish() block
 846  *
 847  * \param arm11         Target state variable.
 848  * \param opcode        ARM opcode to read r0 act upon it
 849  * \param data          Data word that will be written to r0 before \p opcode is executed
 850  *
 851  */
 852 int arm11_run_instr_data_to_core_via_r0(struct arm11_common *arm11, uint32_t opcode, uint32_t data)
 853 {
 854         int retval;
 855         /* MRC p14,0,r0,c0,c5,0 */
 856         retval = arm11_run_instr_data_to_core1(arm11, 0xEE100E15, data);
 857         if (retval != ERROR_OK)
 858                 return retval;
 859
 860         retval = arm11_run_instr_no_data1(arm11, opcode);
 861         if (retval != ERROR_OK)
 862                 return retval;
 863
 864         return ERROR_OK;
 865 }
 866
 867 /** Apply reads and writes to scan chain 7
 868  *
 869  * \see struct arm11_sc7_action
 870  *
 871  * \param arm11         Target state variable.
 872  * \param actions       A list of read and/or write instructions
 873  * \param count         Number of instructions in the list.
 874  *
 875  */
 876 int arm11_sc7_run(struct arm11_common *arm11, struct arm11_sc7_action *actions, size_t count)
 877 {
 878         int retval;
 879
 880         retval = arm11_add_debug_SCAN_N(arm11, 0x07, ARM11_TAP_DEFAULT);
 881         if (retval != ERROR_OK)
 882                 return retval;
 883
 884         arm11_add_IR(arm11, ARM11_EXTEST, ARM11_TAP_DEFAULT);
 885
 886         struct scan_field chain7_fields[3];
 887
 888         uint8_t nRW;
 889         uint32_t DataOut;
 890         uint8_t AddressOut;
 891         uint8_t Ready;
 892         uint32_t DataIn;
 893         uint8_t AddressIn;
 894
 895         arm11_setup_field(arm11,  1, &nRW,                      &Ready,         chain7_fields + 0);
 896         arm11_setup_field(arm11, 32, &DataOut,          &DataIn,        chain7_fields + 1);
 897         arm11_setup_field(arm11,  7, &AddressOut,       &AddressIn,     chain7_fields + 2);
 898
 899         for (size_t i = 0; i < count + 1; i++) {
 900                 if (i < count) {
 901                         nRW                     = actions[i].write ? 1 : 0;
 902                         DataOut         = actions[i].value;
 903                         AddressOut      = actions[i].address;
 904                 } else {
 905                         nRW                     = 1;
 906                         DataOut         = 0;
 907                         AddressOut      = 0;
 908                 }
 909
 910                 /* Timeout here so we don't get stuck. */
 911                 int i_n = 0;
 912                 while (1) {
 913                         JTAG_DEBUG("SC7 <= c%-3d Data %08x %s",
 914                                 (unsigned) AddressOut,
 915                                 (unsigned) DataOut,
 916                                 nRW ? "write" : "read");
 917
 918                         arm11_add_dr_scan_vc(arm11->arm.target->tap, ARRAY_SIZE(chain7_fields),
 919                                 chain7_fields, TAP_DRPAUSE);
 920
 921                         CHECK_RETVAL(jtag_execute_queue());
 922
 923                         /* 'nRW' is 'Ready' on read out */
 924                         if (Ready)
 925                                 break;
 926
 927                         long long then = 0;
 928
 929                         if (i_n == 1000)
 930                                 then = timeval_ms();
 931                         if (i_n >= 1000) {
 932                                 if ((timeval_ms()-then) > 1000) {
 933                                         LOG_WARNING(
 934                                                 "Timeout (1000ms) waiting for instructions to complete");
 935                                         return ERROR_FAIL;
 936                                 }
 937                         }
 938
 939                         i_n++;
 940                 }
 941
 942                 if (!nRW)
 943                         JTAG_DEBUG("SC7 => Data %08x", (unsigned) DataIn);
 944
 945                 if (i > 0) {
 946                         if (actions[i - 1].address != AddressIn)
 947                                 LOG_WARNING("Scan chain 7 shifted out unexpected address");
 948
 949                         if (!actions[i - 1].write)
 950                                 actions[i - 1].value = DataIn;
 951                         else {
 952                                 if (actions[i - 1].value != DataIn)
 953                                         LOG_WARNING("Scan chain 7 shifted out unexpected data");
 954                         }
 955                 }
 956         }
 957         return ERROR_OK;
 958 }
 959
 960 /** Clear VCR and all breakpoints and watchpoints via scan chain 7
 961  *
 962  * \param arm11         Target state variable.
 963  *
 964  */
 965 int arm11_sc7_clear_vbw(struct arm11_common *arm11)
 966 {
 967         size_t clear_bw_size = arm11->brp + 1;
 968         struct arm11_sc7_action *clear_bw = malloc(sizeof(struct arm11_sc7_action) * clear_bw_size);
 969         struct arm11_sc7_action *pos = clear_bw;
 970
 971         for (size_t i = 0; i < clear_bw_size; i++) {
 972                 clear_bw[i].write       = true;
 973                 clear_bw[i].value       = 0;
 974         }
 975
 976         for (size_t i = 0; i < arm11->brp; i++)
 977                 (pos++)->address = ARM11_SC7_BCR0 + i;
 978
 979         (pos++)->address = ARM11_SC7_VCR;
 980
 981         int retval;
 982         retval = arm11_sc7_run(arm11, clear_bw, clear_bw_size);
 983
 984         free(clear_bw);
 985
 986         return retval;
 987 }
 988
 989 /** Write VCR register
 990  *
 991  * \param arm11         Target state variable.
 992  * \param value         Value to be written
 993  */
 994 int arm11_sc7_set_vcr(struct arm11_common *arm11, uint32_t value)
 995 {
 996         struct arm11_sc7_action set_vcr;
 997
 998         set_vcr.write           = true;
 999         set_vcr.address         = ARM11_SC7_VCR;
1000         set_vcr.value           = value;
1001
1002         return arm11_sc7_run(arm11, &set_vcr, 1);
1003 }
1004
1005 /** Read word from address
1006  *
1007  * \param arm11         Target state variable.
1008  * \param address       Memory address to be read
1009  * \param result        Pointer where to store result
1010  *
1011  */
1012 int arm11_read_memory_word(struct arm11_common *arm11, uint32_t address, uint32_t *result)
1013 {
1014         int retval;
1015         retval = arm11_run_instr_data_prepare(arm11);
1016         if (retval != ERROR_OK)
1017                 return retval;
1018
1019         /* MRC p14,0,r0,c0,c5,0 (r0 = address) */
1020         CHECK_RETVAL(arm11_run_instr_data_to_core1(arm11, 0xee100e15, address));
1021
1022         /* LDC p14,c5,[R0],#4 (DTR = [r0]) */
1023         CHECK_RETVAL(arm11_run_instr_data_from_core(arm11, 0xecb05e01, result, 1));
1024
1025         return arm11_run_instr_data_finish(arm11);
1026 }
1027
1028 /************************************************************************/
1029
1030 /*
1031  * ARM11 provider for the OpenOCD implementation of the standard
1032  * architectural ARM v6/v7 "Debug Programmer's Model" (DPM).
1033  */
1034
1035 static inline struct arm11_common *dpm_to_arm11(struct arm_dpm *dpm)
1036 {
1037         return container_of(dpm, struct arm11_common, dpm);
1038 }
1039
1040 static int arm11_dpm_prepare(struct arm_dpm *dpm)
1041 {
1042         return arm11_run_instr_data_prepare(dpm_to_arm11(dpm));
1043 }
1044
1045 static int arm11_dpm_finish(struct arm_dpm *dpm)
1046 {
1047         return arm11_run_instr_data_finish(dpm_to_arm11(dpm));
1048 }
1049
1050 static int arm11_dpm_instr_write_data_dcc(struct arm_dpm *dpm,
1051         uint32_t opcode, uint32_t data)
1052 {
1053         return arm11_run_instr_data_to_core(dpm_to_arm11(dpm),
1054                 opcode, &data, 1);
1055 }
1056
1057 static int arm11_dpm_instr_write_data_r0(struct arm_dpm *dpm,
1058         uint32_t opcode, uint32_t data)
1059 {
1060         return arm11_run_instr_data_to_core_via_r0(dpm_to_arm11(dpm),
1061                 opcode, data);
1062 }
1063
1064 static int arm11_dpm_instr_read_data_dcc(struct arm_dpm *dpm,
1065         uint32_t opcode, uint32_t *data)
1066 {
1067         return arm11_run_instr_data_from_core(dpm_to_arm11(dpm),
1068                 opcode, data, 1);
1069 }
1070
1071 static int arm11_dpm_instr_read_data_r0(struct arm_dpm *dpm,
1072         uint32_t opcode, uint32_t *data)
1073 {
1074         return arm11_run_instr_data_from_core_via_r0(dpm_to_arm11(dpm),
1075                 opcode, data);
1076 }
1077
1078 /* Because arm11_sc7_run() takes a vector of actions, we batch breakpoint
1079  * and watchpoint operations instead of running them right away.  Since we
1080  * pre-allocated our vector, we don't need to worry about space.
1081  */
1082 static int arm11_bpwp_enable(struct arm_dpm *dpm, unsigned index_t,
1083         uint32_t addr, uint32_t control)
1084 {
1085         struct arm11_common *arm11 = dpm_to_arm11(dpm);
1086         struct arm11_sc7_action *action;
1087
1088         action = arm11->bpwp_actions + arm11->bpwp_n;
1089
1090         /* Invariant:  this bp/wp is disabled.
1091          * It also happens that the core is halted here, but for
1092          * DPM-based cores we don't actually care about that.
1093          */
1094
1095         action[0].write = action[1].write = true;
1096
1097         action[0].value = addr;
1098         action[1].value = control;
1099
1100         switch (index_t) {
1101             case 0 ... 15:
1102                     action[0].address = ARM11_SC7_BVR0 + index_t;
1103                     action[1].address = ARM11_SC7_BCR0 + index_t;
1104                     break;
1105             case 16 ... 32:
1106                     index_t -= 16;
1107                     action[0].address = ARM11_SC7_WVR0 + index_t;
1108                     action[1].address = ARM11_SC7_WCR0 + index_t;
1109                     break;
1110             default:
1111                     return ERROR_FAIL;
1112         }
1113
1114         arm11->bpwp_n += 2;
1115
1116         return ERROR_OK;
1117 }
1118
1119 static int arm11_bpwp_disable(struct arm_dpm *dpm, unsigned index_t)
1120 {
1121         struct arm11_common *arm11 = dpm_to_arm11(dpm);
1122         struct arm11_sc7_action *action;
1123
1124         action = arm11->bpwp_actions + arm11->bpwp_n;
1125
1126         action[0].write = true;
1127         action[0].value = 0;
1128
1129         switch (index_t) {
1130             case 0 ... 15:
1131                     action[0].address = ARM11_SC7_BCR0 + index_t;
1132                     break;
1133             case 16 ... 32:
1134                     index_t -= 16;
1135                     action[0].address = ARM11_SC7_WCR0 + index_t;
1136                     break;
1137             default:
1138                     return ERROR_FAIL;
1139         }
1140
1141         arm11->bpwp_n += 1;
1142
1143         return ERROR_OK;
1144 }
1145
1146 /** Flush any pending breakpoint and watchpoint updates. */
1147 int arm11_bpwp_flush(struct arm11_common *arm11)
1148 {
1149         int retval;
1150
1151         if (!arm11->bpwp_n)
1152                 return ERROR_OK;
1153
1154         retval = arm11_sc7_run(arm11, arm11->bpwp_actions, arm11->bpwp_n);
1155         arm11->bpwp_n = 0;
1156
1157         return retval;
1158 }
1159
1160 /** Set up high-level debug module utilities */
1161 int arm11_dpm_init(struct arm11_common *arm11, uint32_t didr)
1162 {
1163         struct arm_dpm *dpm = &arm11->dpm;
1164         int retval;
1165
1166         dpm->arm = &arm11->arm;
1167
1168         dpm->didr = didr;
1169
1170         dpm->prepare = arm11_dpm_prepare;
1171         dpm->finish = arm11_dpm_finish;
1172
1173         dpm->instr_write_data_dcc = arm11_dpm_instr_write_data_dcc;
1174         dpm->instr_write_data_r0 = arm11_dpm_instr_write_data_r0;
1175
1176         dpm->instr_read_data_dcc = arm11_dpm_instr_read_data_dcc;
1177         dpm->instr_read_data_r0 = arm11_dpm_instr_read_data_r0;
1178
1179         dpm->bpwp_enable = arm11_bpwp_enable;
1180         dpm->bpwp_disable = arm11_bpwp_disable;
1181
1182         retval = arm_dpm_setup(dpm);
1183         if (retval != ERROR_OK)
1184                 return retval;
1185
1186         /* alloc enough to enable all breakpoints and watchpoints at once */
1187         arm11->bpwp_actions = calloc(2 * (dpm->nbp + dpm->nwp),
1188                         sizeof *arm11->bpwp_actions);
1189         if (!arm11->bpwp_actions)
1190                 return ERROR_FAIL;
1191
1192         retval = arm_dpm_initialize(dpm);
1193         if (retval != ERROR_OK)
1194                 return retval;
1195
1196         return arm11_bpwp_flush(arm11);
1197 }