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

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