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 n_retry;
 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,   &n_retry,       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  n_retry %d", ready, n_retry);
 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  n_retry %d",
 524                         (unsigned) _data, ready, n_retry);
 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 static int arm11_run_instr_data_to_core_noack_inner(struct jtag_tap *tap,
 566         uint32_t opcode,
 567         uint32_t *data,
 568         size_t count)
 569 {
 570         struct scan_field chain5_fields[3];
 571
 572         chain5_fields[0].num_bits               = 32;
 573         chain5_fields[0].out_value              = NULL; /*&Data*/
 574         chain5_fields[0].in_value               = NULL;
 575
 576         chain5_fields[1].num_bits               = 1;
 577         chain5_fields[1].out_value              = NULL;
 578         chain5_fields[1].in_value               = NULL; /*&Ready*/
 579
 580         chain5_fields[2].num_bits               = 1;
 581         chain5_fields[2].out_value              = NULL;
 582         chain5_fields[2].in_value               = NULL;
 583
 584         uint8_t *readies;
 585         unsigned readies_num = count;
 586         unsigned bytes = sizeof(*readies)*readies_num;
 587
 588         readies = malloc(bytes);
 589         if (!readies) {
 590                 LOG_ERROR("Out of memory allocating %u bytes", bytes);
 591                 return ERROR_FAIL;
 592         }
 593
 594         uint8_t *ready_pos                      = readies;
 595         while (count--) {
 596                 chain5_fields[0].out_value      = (uint8_t *)(data++);
 597                 chain5_fields[1].in_value       = ready_pos++;
 598
 599                 if (count > 0) {
 600                         jtag_add_dr_scan(tap, ARRAY_SIZE(chain5_fields), chain5_fields,
 601                                 TAP_DRPAUSE);
 602                         jtag_add_pathmove(ARRAY_SIZE(arm11_move_drpause_idle_drpause_with_delay),
 603                                 arm11_move_drpause_idle_drpause_with_delay);
 604                 } else
 605                         jtag_add_dr_scan(tap, ARRAY_SIZE(chain5_fields), chain5_fields, TAP_IDLE);
 606         }
 607
 608         int retval = jtag_execute_queue();
 609         if (retval == ERROR_OK) {
 610                 unsigned error_count = 0;
 611
 612                 for (size_t i = 0; i < readies_num; i++) {
 613                         if (readies[i] != 1)
 614                                 error_count++;
 615                 }
 616
 617                 if (error_count > 0) {
 618                         LOG_ERROR("%u words out of %u not transferred",
 619                                 error_count, readies_num);
 620                         retval = ERROR_FAIL;
 621                 }
 622         }
 623         free(readies);
 624
 625         return retval;
 626 }
 627
 628 /** Execute one instruction via ITR repeatedly while
 629  *  passing data to the core via DTR on each execution.
 630  *
 631  * Caller guarantees that processor is in debug state, that DSCR_ITR_EN
 632  * is set, the ITR Ready flag is set (as seen on the previous entry to
 633  * TAP_DRCAPTURE), and the DSCR sticky abort flag is clear.
 634  *
 635  *  No Ready check during transmission.
 636  *
 637  *  The executed instruction \em must read data from DTR.
 638  *
 639  * \pre arm11_run_instr_data_prepare() /  arm11_run_instr_data_finish() block
 640  *
 641  * \param arm11         Target state variable.
 642  * \param opcode        ARM opcode
 643  * \param data          Pointer to the data words to be passed to the core
 644  * \param count         Number of data words and instruction repetitions
 645  *
 646  */
 647 int arm11_run_instr_data_to_core_noack(struct arm11_common *arm11,
 648         uint32_t opcode,
 649         uint32_t *data,
 650         size_t count)
 651 {
 652         arm11_add_ir(arm11, ARM11_ITRSEL, ARM11_TAP_DEFAULT);
 653
 654         arm11_add_debug_inst(arm11, opcode, NULL, TAP_DRPAUSE);
 655
 656         arm11_add_ir(arm11, ARM11_EXTEST, ARM11_TAP_DEFAULT);
 657
 658         int retval = arm11_run_instr_data_to_core_noack_inner(arm11->arm.target->tap,
 659                         opcode,
 660                         data,
 661                         count);
 662
 663         if (retval != ERROR_OK)
 664                 return retval;
 665
 666         arm11_add_ir(arm11, ARM11_INTEST, ARM11_TAP_DEFAULT);
 667
 668         struct scan_field chain5_fields[3];
 669
 670         arm11_setup_field(arm11,
 671                 32,
 672                 NULL /*&Data*/,
 673                 NULL,
 674                 chain5_fields + 0);
 675         arm11_setup_field(arm11,
 676                 1,
 677                 NULL,
 678                 NULL /*&Ready*/,
 679                 chain5_fields + 1);
 680         arm11_setup_field(arm11,
 681                 1,
 682                 NULL,
 683                 NULL,
 684                 chain5_fields + 2);
 685
 686         uint8_t ready_flag;
 687         chain5_fields[1].in_value   = &ready_flag;
 688
 689         arm11_add_dr_scan_vc(arm11->arm.target->tap, ARRAY_SIZE(
 690                         chain5_fields), chain5_fields, TAP_DRPAUSE);
 691
 692         retval = jtag_execute_queue();
 693         if (retval == ERROR_OK) {
 694                 if (ready_flag != 1) {
 695                         LOG_ERROR("last word not transferred");
 696                         retval = ERROR_FAIL;
 697                 }
 698         }
 699
 700         return retval;
 701 }
 702
 703
 704 /** Execute an instruction via ITR while handing data into the core via DTR.
 705  *
 706  *  The executed instruction \em must read data from DTR.
 707  *
 708  * \pre arm11_run_instr_data_prepare() /  arm11_run_instr_data_finish() block
 709  *
 710  * \param arm11         Target state variable.
 711  * \param opcode        ARM opcode
 712  * \param data          Data word to be passed to the core via DTR
 713  *
 714  */
 715 int arm11_run_instr_data_to_core1(struct arm11_common *arm11, uint32_t opcode, uint32_t data)
 716 {
 717         return arm11_run_instr_data_to_core(arm11, opcode, &data, 1);
 718 }
 719
 720
 721 /** Execute one instruction via ITR repeatedly while
 722  *  reading data from the core via DTR on each execution.
 723  *
 724  * Caller guarantees that processor is in debug state, that DSCR_ITR_EN
 725  * is set, the ITR Ready flag is set (as seen on the previous entry to
 726  * TAP_DRCAPTURE), and the DSCR sticky abort flag is clear.
 727  *
 728  *  The executed instruction \em must write data to DTR.
 729  *
 730  * \pre arm11_run_instr_data_prepare() /  arm11_run_instr_data_finish() block
 731  *
 732  * \param arm11         Target state variable.
 733  * \param opcode        ARM opcode
 734  * \param data          Pointer to an array that receives the data words from the core
 735  * \param count         Number of data words and instruction repetitions
 736  *
 737  */
 738 int arm11_run_instr_data_from_core(struct arm11_common *arm11,
 739         uint32_t opcode,
 740         uint32_t *data,
 741         size_t count)
 742 {
 743         arm11_add_ir(arm11, ARM11_ITRSEL, ARM11_TAP_DEFAULT);
 744
 745         arm11_add_debug_inst(arm11, opcode, NULL, TAP_IDLE);
 746
 747         arm11_add_ir(arm11, ARM11_INTEST, ARM11_TAP_DEFAULT);
 748
 749         struct scan_field chain5_fields[3];
 750
 751         uint32_t _data;
 752         uint8_t ready;
 753         uint8_t n_retry;
 754
 755         arm11_setup_field(arm11, 32,    NULL,   &_data,     chain5_fields + 0);
 756         arm11_setup_field(arm11,  1,    NULL,   &ready,     chain5_fields + 1);
 757         arm11_setup_field(arm11,  1,    NULL,   &n_retry,   chain5_fields + 2);
 758
 759         while (count--) {
 760                 int i = 0;
 761                 do {
 762                         arm11_add_dr_scan_vc(arm11->arm.target->tap, ARRAY_SIZE(
 763                                         chain5_fields), chain5_fields,
 764                                 count ? TAP_IDLE : TAP_DRPAUSE);
 765
 766                         CHECK_RETVAL(jtag_execute_queue());
 767
 768                         JTAG_DEBUG("DTR  _data %08x  ready %d  n_retry %d",
 769                                 (unsigned) _data, ready, n_retry);
 770
 771                         int64_t then = 0;
 772
 773                         if (i == 1000)
 774                                 then = timeval_ms();
 775                         if (i >= 1000) {
 776                                 if ((timeval_ms()-then) > 1000) {
 777                                         LOG_WARNING(
 778                                                 "Timeout (1000ms) waiting for instructions to complete");
 779                                         return ERROR_FAIL;
 780                                 }
 781                         }
 782
 783                         i++;
 784                 } while (!ready);
 785
 786                 *data++ = _data;
 787         }
 788
 789         return ERROR_OK;
 790 }
 791
 792 /** Execute one instruction via ITR
 793  *  then load r0 into DTR and read DTR from core.
 794  *
 795  *  The first executed instruction (\p opcode) should write data to r0.
 796  *
 797  * \pre arm11_run_instr_data_prepare() /  arm11_run_instr_data_finish() block
 798  *
 799  * \param arm11         Target state variable.
 800  * \param opcode        ARM opcode to write r0 with the value of interest
 801  * \param data          Pointer to a data word that receives the value from r0 after \p opcode was executed.
 802  *
 803  */
 804 int arm11_run_instr_data_from_core_via_r0(struct arm11_common *arm11,
 805         uint32_t opcode,
 806         uint32_t *data)
 807 {
 808         int retval;
 809         retval = arm11_run_instr_no_data1(arm11, opcode);
 810         if (retval != ERROR_OK)
 811                 return retval;
 812
 813         /* MCR p14,0,R0,c0,c5,0 (move r0 -> wDTR -> local var) */
 814         arm11_run_instr_data_from_core(arm11, 0xEE000E15, data, 1);
 815
 816         return ERROR_OK;
 817 }
 818
 819 /** Load data into core via DTR then move it to r0 then
 820  *  execute one instruction via ITR
 821  *
 822  *  The final executed instruction (\p opcode) should read data from r0.
 823  *
 824  * \pre arm11_run_instr_data_prepare() /  arm11_run_instr_data_finish() block
 825  *
 826  * \param arm11         Target state variable.
 827  * \param opcode        ARM opcode to read r0 act upon it
 828  * \param data          Data word that will be written to r0 before \p opcode is executed
 829  *
 830  */
 831 int arm11_run_instr_data_to_core_via_r0(struct arm11_common *arm11, uint32_t opcode, uint32_t data)
 832 {
 833         int retval;
 834         /* MRC p14,0,r0,c0,c5,0 */
 835         retval = arm11_run_instr_data_to_core1(arm11, 0xEE100E15, data);
 836         if (retval != ERROR_OK)
 837                 return retval;
 838
 839         retval = arm11_run_instr_no_data1(arm11, opcode);
 840         if (retval != ERROR_OK)
 841                 return retval;
 842
 843         return ERROR_OK;
 844 }
 845
 846 /** Apply reads and writes to scan chain 7
 847  *
 848  * \see struct arm11_sc7_action
 849  *
 850  * \param arm11         Target state variable.
 851  * \param actions       A list of read and/or write instructions
 852  * \param count         Number of instructions in the list.
 853  *
 854  */
 855 int arm11_sc7_run(struct arm11_common *arm11, struct arm11_sc7_action *actions, size_t count)
 856 {
 857         int retval;
 858
 859         retval = arm11_add_debug_scan_n(arm11, 0x07, ARM11_TAP_DEFAULT);
 860         if (retval != ERROR_OK)
 861                 return retval;
 862
 863         arm11_add_ir(arm11, ARM11_EXTEST, ARM11_TAP_DEFAULT);
 864
 865         struct scan_field chain7_fields[3];
 866
 867         uint8_t n_rw;
 868         uint32_t data_out;
 869         uint8_t address_out;
 870         uint8_t ready;
 871         uint32_t data_in;
 872         uint8_t address_in;
 873
 874         arm11_setup_field(arm11,  1, &n_rw,              &ready,          chain7_fields + 0);
 875         arm11_setup_field(arm11, 32, &data_out,          &data_in,        chain7_fields + 1);
 876         arm11_setup_field(arm11,  7, &address_out,       &address_in,     chain7_fields + 2);
 877
 878         for (size_t i = 0; i < count + 1; i++) {
 879                 if (i < count) {
 880                         n_rw             = actions[i].write ? 1 : 0;
 881                         data_out         = actions[i].value;
 882                         address_out      = actions[i].address;
 883                 } else {
 884                         n_rw             = 1;
 885                         data_out         = 0;
 886                         address_out      = 0;
 887                 }
 888
 889                 /* Timeout here so we don't get stuck. */
 890                 int i_n = 0;
 891                 while (1) {
 892                         JTAG_DEBUG("SC7 <= c%-3d Data %08x %s",
 893                                 (unsigned) address_out,
 894                                 (unsigned) data_out,
 895                                 n_rw ? "write" : "read");
 896
 897                         arm11_add_dr_scan_vc(arm11->arm.target->tap, ARRAY_SIZE(chain7_fields),
 898                                 chain7_fields, TAP_DRPAUSE);
 899
 900                         CHECK_RETVAL(jtag_execute_queue());
 901
 902                         /* 'n_rw' is 'ready' on read out */
 903                         if (ready)
 904                                 break;
 905
 906                         int64_t then = 0;
 907
 908                         if (i_n == 1000)
 909                                 then = timeval_ms();
 910                         if (i_n >= 1000) {
 911                                 if ((timeval_ms()-then) > 1000) {
 912                                         LOG_WARNING(
 913                                                 "Timeout (1000ms) waiting for instructions to complete");
 914                                         return ERROR_FAIL;
 915                                 }
 916                         }
 917
 918                         i_n++;
 919                 }
 920
 921                 if (!n_rw)
 922                         JTAG_DEBUG("SC7 => Data %08x", (unsigned) data_in);
 923
 924                 if (i > 0) {
 925                         if (actions[i - 1].address != address_in)
 926                                 LOG_WARNING("Scan chain 7 shifted out unexpected address");
 927
 928                         if (!actions[i - 1].write)
 929                                 actions[i - 1].value = data_in;
 930                         else {
 931                                 if (actions[i - 1].value != data_in)
 932                                         LOG_WARNING("Scan chain 7 shifted out unexpected data");
 933                         }
 934                 }
 935         }
 936         return ERROR_OK;
 937 }
 938
 939 /** Clear VCR and all breakpoints and watchpoints via scan chain 7
 940  *
 941  * \param arm11         Target state variable.
 942  *
 943  */
 944 int arm11_sc7_clear_vbw(struct arm11_common *arm11)
 945 {
 946         size_t clear_bw_size = arm11->brp + 1;
 947         struct arm11_sc7_action *clear_bw = malloc(sizeof(struct arm11_sc7_action) * clear_bw_size);
 948         struct arm11_sc7_action *pos = clear_bw;
 949
 950         for (size_t i = 0; i < clear_bw_size; i++) {
 951                 clear_bw[i].write       = true;
 952                 clear_bw[i].value       = 0;
 953         }
 954
 955         for (size_t i = 0; i < arm11->brp; i++)
 956                 (pos++)->address = ARM11_SC7_BCR0 + i;
 957
 958         (pos++)->address = ARM11_SC7_VCR;
 959
 960         int retval;
 961         retval = arm11_sc7_run(arm11, clear_bw, clear_bw_size);
 962
 963         free(clear_bw);
 964
 965         return retval;
 966 }
 967
 968 /** Write VCR register
 969  *
 970  * \param arm11         Target state variable.
 971  * \param value         Value to be written
 972  */
 973 int arm11_sc7_set_vcr(struct arm11_common *arm11, uint32_t value)
 974 {
 975         struct arm11_sc7_action set_vcr;
 976
 977         set_vcr.write           = true;
 978         set_vcr.address         = ARM11_SC7_VCR;
 979         set_vcr.value           = value;
 980
 981         return arm11_sc7_run(arm11, &set_vcr, 1);
 982 }
 983
 984 /** Read word from address
 985  *
 986  * \param arm11         Target state variable.
 987  * \param address       Memory address to be read
 988  * \param result        Pointer where to store result
 989  *
 990  */
 991 int arm11_read_memory_word(struct arm11_common *arm11, uint32_t address, uint32_t *result)
 992 {
 993         int retval;
 994         retval = arm11_run_instr_data_prepare(arm11);
 995         if (retval != ERROR_OK)
 996                 return retval;
 997
 998         /* MRC p14,0,r0,c0,c5,0 (r0 = address) */
 999         CHECK_RETVAL(arm11_run_instr_data_to_core1(arm11, 0xee100e15, address));
1000
1001         /* LDC p14,c5,[R0],#4 (DTR = [r0]) */
1002         CHECK_RETVAL(arm11_run_instr_data_from_core(arm11, 0xecb05e01, result, 1));
1003
1004         return arm11_run_instr_data_finish(arm11);
1005 }
1006
1007 /************************************************************************/
1008
1009 /*
1010  * ARM11 provider for the OpenOCD implementation of the standard
1011  * architectural ARM v6/v7 "Debug Programmer's Model" (DPM).
1012  */
1013
1014 static inline struct arm11_common *dpm_to_arm11(struct arm_dpm *dpm)
1015 {
1016         return container_of(dpm, struct arm11_common, dpm);
1017 }
1018
1019 static int arm11_dpm_prepare(struct arm_dpm *dpm)
1020 {
1021         return arm11_run_instr_data_prepare(dpm_to_arm11(dpm));
1022 }
1023
1024 static int arm11_dpm_finish(struct arm_dpm *dpm)
1025 {
1026         return arm11_run_instr_data_finish(dpm_to_arm11(dpm));
1027 }
1028
1029 static int arm11_dpm_instr_write_data_dcc(struct arm_dpm *dpm,
1030         uint32_t opcode, uint32_t data)
1031 {
1032         return arm11_run_instr_data_to_core(dpm_to_arm11(dpm),
1033                 opcode, &data, 1);
1034 }
1035
1036 static int arm11_dpm_instr_write_data_r0(struct arm_dpm *dpm,
1037         uint32_t opcode, uint32_t data)
1038 {
1039         return arm11_run_instr_data_to_core_via_r0(dpm_to_arm11(dpm),
1040                 opcode, data);
1041 }
1042
1043 static int arm11_dpm_instr_read_data_dcc(struct arm_dpm *dpm,
1044         uint32_t opcode, uint32_t *data)
1045 {
1046         return arm11_run_instr_data_from_core(dpm_to_arm11(dpm),
1047                 opcode, data, 1);
1048 }
1049
1050 static int arm11_dpm_instr_read_data_r0(struct arm_dpm *dpm,
1051         uint32_t opcode, uint32_t *data)
1052 {
1053         return arm11_run_instr_data_from_core_via_r0(dpm_to_arm11(dpm),
1054                 opcode, data);
1055 }
1056
1057 /* Because arm11_sc7_run() takes a vector of actions, we batch breakpoint
1058  * and watchpoint operations instead of running them right away.  Since we
1059  * pre-allocated our vector, we don't need to worry about space.
1060  */
1061 static int arm11_bpwp_enable(struct arm_dpm *dpm, unsigned index_t,
1062         uint32_t addr, uint32_t control)
1063 {
1064         struct arm11_common *arm11 = dpm_to_arm11(dpm);
1065         struct arm11_sc7_action *action;
1066
1067         action = arm11->bpwp_actions + arm11->bpwp_n;
1068
1069         /* Invariant:  this bp/wp is disabled.
1070          * It also happens that the core is halted here, but for
1071          * DPM-based cores we don't actually care about that.
1072          */
1073
1074         action[0].write = action[1].write = true;
1075
1076         action[0].value = addr;
1077         action[1].value = control;
1078
1079         switch (index_t) {
1080             case 0 ... 15:
1081                     action[0].address = ARM11_SC7_BVR0 + index_t;
1082                     action[1].address = ARM11_SC7_BCR0 + index_t;
1083                     break;
1084             case 16 ... 32:
1085                     index_t -= 16;
1086                     action[0].address = ARM11_SC7_WVR0 + index_t;
1087                     action[1].address = ARM11_SC7_WCR0 + index_t;
1088                     break;
1089             default:
1090                     return ERROR_FAIL;
1091         }
1092
1093         arm11->bpwp_n += 2;
1094
1095         return ERROR_OK;
1096 }
1097
1098 static int arm11_bpwp_disable(struct arm_dpm *dpm, unsigned index_t)
1099 {
1100         struct arm11_common *arm11 = dpm_to_arm11(dpm);
1101         struct arm11_sc7_action *action;
1102
1103         action = arm11->bpwp_actions + arm11->bpwp_n;
1104
1105         action[0].write = true;
1106         action[0].value = 0;
1107
1108         switch (index_t) {
1109             case 0 ... 15:
1110                     action[0].address = ARM11_SC7_BCR0 + index_t;
1111                     break;
1112             case 16 ... 32:
1113                     index_t -= 16;
1114                     action[0].address = ARM11_SC7_WCR0 + index_t;
1115                     break;
1116             default:
1117                     return ERROR_FAIL;
1118         }
1119
1120         arm11->bpwp_n += 1;
1121
1122         return ERROR_OK;
1123 }
1124
1125 /** Flush any pending breakpoint and watchpoint updates. */
1126 int arm11_bpwp_flush(struct arm11_common *arm11)
1127 {
1128         int retval;
1129
1130         if (!arm11->bpwp_n)
1131                 return ERROR_OK;
1132
1133         retval = arm11_sc7_run(arm11, arm11->bpwp_actions, arm11->bpwp_n);
1134         arm11->bpwp_n = 0;
1135
1136         return retval;
1137 }
1138
1139 /** Set up high-level debug module utilities */
1140 int arm11_dpm_init(struct arm11_common *arm11, uint32_t didr)
1141 {
1142         struct arm_dpm *dpm = &arm11->dpm;
1143         int retval;
1144
1145         dpm->arm = &arm11->arm;
1146
1147         dpm->didr = didr;
1148
1149         dpm->prepare = arm11_dpm_prepare;
1150         dpm->finish = arm11_dpm_finish;
1151
1152         dpm->instr_write_data_dcc = arm11_dpm_instr_write_data_dcc;
1153         dpm->instr_write_data_r0 = arm11_dpm_instr_write_data_r0;
1154
1155         dpm->instr_read_data_dcc = arm11_dpm_instr_read_data_dcc;
1156         dpm->instr_read_data_r0 = arm11_dpm_instr_read_data_r0;
1157
1158         dpm->bpwp_enable = arm11_bpwp_enable;
1159         dpm->bpwp_disable = arm11_bpwp_disable;
1160
1161         retval = arm_dpm_setup(dpm);
1162         if (retval != ERROR_OK)
1163                 return retval;
1164
1165         /* alloc enough to enable all breakpoints and watchpoints at once */
1166         arm11->bpwp_actions = calloc(2 * (dpm->nbp + dpm->nwp),
1167                         sizeof(*arm11->bpwp_actions));
1168         if (!arm11->bpwp_actions)
1169                 return ERROR_FAIL;
1170
1171         retval = arm_dpm_initialize(dpm);
1172         if (retval != ERROR_OK)
1173                 return retval;
1174
1175         return arm11_bpwp_flush(arm11);
1176 }
1177
1178 void arm11_dpm_deinit(struct arm11_common *arm11)
1179 {
1180         struct arm_dpm *dpm = &arm11->dpm;
1181
1182         free(arm11->bpwp_actions);
1183         arm_free_reg_cache(dpm->arm);
1184         free(dpm->dbp);
1185         free(dpm->dwp);
1186 }