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