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