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