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

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