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