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 "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 static int arm11_add_ir_scan_vc(int num_fields, struct scan_field *fields,
  52                 tap_state_t state)
  53 {
  54         if (cmd_queue_cur_state == TAP_IRPAUSE)
  55                 jtag_add_pathmove(ARRAY_SIZE(arm11_move_pi_to_si_via_ci), arm11_move_pi_to_si_via_ci);
  56
  57         jtag_add_ir_scan(num_fields, fields, state);
  58         return ERROR_OK;
  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 int arm11_add_dr_scan_vc(int num_fields, struct scan_field *fields, 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), arm11_move_pd_to_sd_via_cd);
  70
  71         jtag_add_dr_scan(num_fields, fields, state);
  72         return ERROR_OK;
  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, void * out_data, void * in_data, struct scan_field * field)
  87 {
  88         field->tap                      = arm11->target->tap;
  89         field->num_bits                 = num_bits;
  90         field->out_value                = out_data;
  91         field->in_value                 = in_data;
  92 }
  93
  94
  95 /** Write JTAG instruction register
  96  *
  97  * \param arm11         Target state variable.
  98  * \param instr         An ARM11 DBGTAP instruction. Use enum #arm11_instructions.
  99  * \param state         Pass the final TAP state or ARM11_TAP_DEFAULT for the default value (Pause-IR).
 100  *
 101  * \remarks                     This adds to the JTAG command queue but does \em not execute it.
 102  */
 103 void arm11_add_IR(struct arm11_common * arm11, uint8_t instr, tap_state_t state)
 104 {
 105         struct jtag_tap *tap;
 106         tap = arm11->target->tap;
 107
 108         if (buf_get_u32(tap->cur_instr, 0, 5) == instr)
 109         {
 110                 JTAG_DEBUG("IR <= 0x%02x SKIPPED", instr);
 111                 return;
 112         }
 113
 114         JTAG_DEBUG("IR <= 0x%02x", instr);
 115
 116         struct scan_field field;
 117
 118         arm11_setup_field(arm11, 5, &instr, NULL, &field);
 119
 120         arm11_add_ir_scan_vc(1, &field, state == ARM11_TAP_DEFAULT ? TAP_IRPAUSE : state);
 121 }
 122
 123 /** Verify shifted out data from Scan Chain Register (SCREG)
 124  *  Used as parameter to struct scan_field::in_handler in
 125  *  arm11_add_debug_SCAN_N().
 126  *
 127  */
 128 static void arm11_in_handler_SCAN_N(uint8_t *in_value)
 129 {
 130         /** \todo TODO: clarify why this isnt properly masked in core.c jtag_read_buffer() */
 131         uint8_t v = *in_value & 0x1F;
 132
 133         if (v != 0x10)
 134         {
 135                 LOG_ERROR("'arm11 target' JTAG communication error SCREG SCAN OUT 0x%02x (expected 0x10)", v);
 136                 jtag_set_error(ERROR_FAIL);
 137         }
 138
 139         JTAG_DEBUG("SCREG SCAN OUT 0x%02x", v);
 140 }
 141
 142 /** Select and write to Scan Chain Register (SCREG)
 143  *
 144  * This function sets the instruction register to SCAN_N and writes
 145  * the data register with the selected chain number.
 146  *
 147  * http://infocenter.arm.com/help/topic/com.arm.doc.ddi0301f/Cacbjhfg.html
 148  *
 149  * \param arm11     Target state variable.
 150  * \param chain     Scan chain that will be selected.
 151  * \param state     Pass the final TAP state or ARM11_TAP_DEFAULT for the default
 152  *                                      value (Pause-DR).
 153  *
 154  * The chain takes effect when Update-DR is passed (usually when subsequently
 155  * the INTEXT/EXTEST instructions are written).
 156  *
 157  * \warning                     (Obsolete) Using this twice in a row will \em fail. The first
 158  *                                      call will end in Pause-DR. The second call, due to the IR
 159  *                                      caching, will not go through Capture-DR when shifting in the
 160  *                                      new scan chain number. As a result the verification in
 161  *                                      arm11_in_handler_SCAN_N() must fail.
 162  *
 163  * \remarks                     This adds to the JTAG command queue but does \em not execute it.
 164  */
 165
 166 int arm11_add_debug_SCAN_N(struct arm11_common * arm11, uint8_t chain, tap_state_t state)
 167 {
 168         JTAG_DEBUG("SCREG <= 0x%02x", chain);
 169
 170         arm11_add_IR(arm11, ARM11_SCAN_N, ARM11_TAP_DEFAULT);
 171
 172         struct scan_field               field;
 173
 174         uint8_t tmp[1];
 175         arm11_setup_field(arm11, 5, &chain, &tmp, &field);
 176
 177         arm11_add_dr_scan_vc(1, &field, state == ARM11_TAP_DEFAULT ? TAP_DRPAUSE : state);
 178
 179         jtag_execute_queue_noclear();
 180
 181         arm11_in_handler_SCAN_N(tmp);
 182
 183         arm11->jtag_info.cur_scan_chain = chain;
 184
 185         return jtag_execute_queue();
 186 }
 187
 188 /** Write an instruction into the ITR register
 189  *
 190  * \param arm11         Target state variable.
 191  * \param inst          An ARM11 processor instruction/opcode.
 192  * \param flag          Optional parameter to retrieve the InstCompl flag
 193  *                                      (this will be written when the JTAG chain is executed).
 194  * \param state         Pass the final TAP state or ARM11_TAP_DEFAULT for the default
 195  *                                      value (Run-Test/Idle).
 196  *
 197  * \remarks                     By default this ends with Run-Test/Idle state
 198  *                                      and causes the instruction to be executed. If
 199  *                                      a subsequent write to DTR is needed before
 200  *                                      executing the instruction then TAP_DRPAUSE should be
 201  *                                      passed to \p state.
 202  *
 203  * \remarks                     This adds to the JTAG command queue but does \em not execute it.
 204  */
 205 static void arm11_add_debug_INST(struct arm11_common * arm11,
 206                 uint32_t inst, uint8_t * flag, tap_state_t state)
 207 {
 208         JTAG_DEBUG("INST <= 0x%08x", (unsigned) inst);
 209
 210         struct scan_field               itr[2];
 211
 212         arm11_setup_field(arm11, 32,    &inst,  NULL, itr + 0);
 213         arm11_setup_field(arm11, 1,         NULL,       flag, itr + 1);
 214
 215         arm11_add_dr_scan_vc(ARRAY_SIZE(itr), itr, state == ARM11_TAP_DEFAULT ? TAP_IDLE : state);
 216 }
 217
 218 /** Read the Debug Status and Control Register (DSCR)
 219  *
 220  * same as CP14 c1
 221  *
 222  * \param arm11         Target state variable.
 223  * \param value         DSCR content
 224  * \return                      Error status
 225  *
 226  * \remarks                     This is a stand-alone function that executes the JTAG command queue.
 227  */
 228 int arm11_read_DSCR(struct arm11_common * arm11, uint32_t *value)
 229 {
 230         int retval;
 231         retval = arm11_add_debug_SCAN_N(arm11, 0x01, ARM11_TAP_DEFAULT);
 232         if (retval != ERROR_OK)
 233                 return retval;
 234
 235         arm11_add_IR(arm11, ARM11_INTEST, ARM11_TAP_DEFAULT);
 236
 237         uint32_t                                dscr;
 238         struct scan_field       chain1_field;
 239
 240         arm11_setup_field(arm11, 32, NULL, &dscr, &chain1_field);
 241
 242         arm11_add_dr_scan_vc(1, &chain1_field, TAP_DRPAUSE);
 243
 244         CHECK_RETVAL(jtag_execute_queue());
 245
 246         if (arm11->last_dscr != dscr)
 247                 JTAG_DEBUG("DSCR  = %08x (OLD %08x)",
 248                                 (unsigned) dscr,
 249                                 (unsigned) arm11->last_dscr);
 250
 251         arm11->last_dscr = dscr;
 252
 253         *value = dscr;
 254
 255         return ERROR_OK;
 256 }
 257
 258 /** Write the Debug Status and Control Register (DSCR)
 259  *
 260  * same as CP14 c1
 261  *
 262  * \param arm11         Target state variable.
 263  * \param dscr          DSCR content
 264  *
 265  * \remarks                     This is a stand-alone function that executes the JTAG command queue.
 266  */
 267 int arm11_write_DSCR(struct arm11_common * arm11, uint32_t dscr)
 268 {
 269         int retval;
 270         retval = arm11_add_debug_SCAN_N(arm11, 0x01, ARM11_TAP_DEFAULT);
 271         if (retval != ERROR_OK)
 272                 return retval;
 273
 274         arm11_add_IR(arm11, ARM11_EXTEST, ARM11_TAP_DEFAULT);
 275
 276         struct scan_field                   chain1_field;
 277
 278         arm11_setup_field(arm11, 32, &dscr, NULL, &chain1_field);
 279
 280         arm11_add_dr_scan_vc(1, &chain1_field, TAP_DRPAUSE);
 281
 282         CHECK_RETVAL(jtag_execute_queue());
 283
 284         JTAG_DEBUG("DSCR <= %08x (OLD %08x)",
 285                         (unsigned) dscr,
 286                         (unsigned) arm11->last_dscr);
 287
 288         arm11->last_dscr = dscr;
 289
 290         return ERROR_OK;
 291 }
 292
 293
 294
 295 /** Get the debug reason from Debug Status and Control Register (DSCR)
 296  *
 297  * \param dscr          DSCR value to analyze
 298  * \return                      Debug reason
 299  *
 300  */
 301 enum target_debug_reason arm11_get_DSCR_debug_reason(uint32_t dscr)
 302 {
 303         switch (dscr & ARM11_DSCR_METHOD_OF_DEBUG_ENTRY_MASK)
 304         {
 305         case ARM11_DSCR_METHOD_OF_DEBUG_ENTRY_HALT:
 306                 LOG_INFO("Debug entry: JTAG HALT");
 307                 return DBG_REASON_DBGRQ;
 308
 309         case ARM11_DSCR_METHOD_OF_DEBUG_ENTRY_BREAKPOINT:
 310                 LOG_INFO("Debug entry: breakpoint");
 311                 return DBG_REASON_BREAKPOINT;
 312
 313         case ARM11_DSCR_METHOD_OF_DEBUG_ENTRY_WATCHPOINT:
 314                 LOG_INFO("Debug entry: watchpoint");
 315                 return DBG_REASON_WATCHPOINT;
 316
 317         case ARM11_DSCR_METHOD_OF_DEBUG_ENTRY_BKPT_INSTRUCTION:
 318                 LOG_INFO("Debug entry: BKPT instruction");
 319                 return DBG_REASON_BREAKPOINT;
 320
 321         case ARM11_DSCR_METHOD_OF_DEBUG_ENTRY_EDBGRQ:
 322                 LOG_INFO("Debug entry: EDBGRQ signal");
 323                 return DBG_REASON_DBGRQ;
 324
 325         case ARM11_DSCR_METHOD_OF_DEBUG_ENTRY_VECTOR_CATCH:
 326                 LOG_INFO("Debug entry: VCR vector catch");
 327                 return DBG_REASON_BREAKPOINT;
 328
 329         default:
 330                 LOG_INFO("Debug entry: unknown");
 331                 return DBG_REASON_DBGRQ;
 332         }
 333 };
 334
 335
 336
 337 /** Prepare the stage for ITR/DTR operations
 338  * from the arm11_run_instr... group of functions.
 339  *
 340  * Put arm11_run_instr_data_prepare() and arm11_run_instr_data_finish()
 341  * around a block of arm11_run_instr_... calls.
 342  *
 343  * Select scan chain 5 to allow quick access to DTR. When scan
 344  * chain 4 is needed to put in a register the ITRSel instruction
 345  * shortcut is used instead of actually changing the Scan_N
 346  * register.
 347  *
 348  * \param arm11         Target state variable.
 349  *
 350  */
 351 int arm11_run_instr_data_prepare(struct arm11_common * arm11)
 352 {
 353         return arm11_add_debug_SCAN_N(arm11, 0x05, ARM11_TAP_DEFAULT);
 354 }
 355
 356 /** Cleanup after ITR/DTR operations
 357  * from the arm11_run_instr... group of functions
 358  *
 359  * Put arm11_run_instr_data_prepare() and arm11_run_instr_data_finish()
 360  * around a block of arm11_run_instr_... calls.
 361  *
 362  * Any IDLE can lead to an instruction execution when
 363  * scan chains 4 or 5 are selected and the IR holds
 364  * INTEST or EXTEST. So we must disable that before
 365  * any following activities lead to an IDLE.
 366  *
 367  * \param arm11         Target state variable.
 368  *
 369  */
 370 int arm11_run_instr_data_finish(struct arm11_common * arm11)
 371 {
 372         return arm11_add_debug_SCAN_N(arm11, 0x00, ARM11_TAP_DEFAULT);
 373 }
 374
 375
 376
 377 /** Execute one or multiple instructions via ITR
 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  *  The executed instruction \em must read data from DTR.
 448  *
 449  * \pre arm11_run_instr_data_prepare() /  arm11_run_instr_data_finish() block
 450  *
 451  * \param arm11         Target state variable.
 452  * \param opcode        ARM opcode
 453  * \param data          Pointer to the data words to be passed to the core
 454  * \param count         Number of data words and instruction repetitions
 455  *
 456  */
 457 int arm11_run_instr_data_to_core(struct arm11_common * arm11, uint32_t opcode, uint32_t * data, size_t count)
 458 {
 459         arm11_add_IR(arm11, ARM11_ITRSEL, ARM11_TAP_DEFAULT);
 460
 461         arm11_add_debug_INST(arm11, opcode, NULL, TAP_DRPAUSE);
 462
 463         arm11_add_IR(arm11, ARM11_EXTEST, ARM11_TAP_DEFAULT);
 464
 465         struct scan_field       chain5_fields[3];
 466
 467         uint32_t                                Data;
 468         uint8_t                         Ready;
 469         uint8_t                         nRetry;
 470
 471         arm11_setup_field(arm11, 32,    &Data,  NULL,           chain5_fields + 0);
 472         arm11_setup_field(arm11,  1,    NULL,   &Ready,         chain5_fields + 1);
 473         arm11_setup_field(arm11,  1,    NULL,   &nRetry,        chain5_fields + 2);
 474
 475         while (count--)
 476         {
 477                 int i = 0;
 478                 do
 479                 {
 480                         Data        = *data;
 481
 482                         arm11_add_dr_scan_vc(ARRAY_SIZE(chain5_fields), chain5_fields, jtag_set_end_state(TAP_IDLE));
 483
 484                         CHECK_RETVAL(jtag_execute_queue());
 485
 486                         JTAG_DEBUG("DTR  Ready %d  nRetry %d", Ready, nRetry);
 487
 488                         long long then = 0;
 489
 490                         if (i == 1000)
 491                         {
 492                                 then = timeval_ms();
 493                         }
 494                         if (i >= 1000)
 495                         {
 496                                 if ((timeval_ms()-then) > 1000)
 497                                 {
 498                                         LOG_WARNING("Timeout (1000ms) waiting for instructions to complete");
 499                                         return ERROR_FAIL;
 500                                 }
 501                         }
 502
 503                         i++;
 504                 }
 505                 while (!Ready);
 506
 507                 data++;
 508         }
 509
 510         arm11_add_IR(arm11, ARM11_INTEST, ARM11_TAP_DEFAULT);
 511
 512         int i = 0;
 513         do
 514         {
 515                 Data        = 0;
 516
 517                 arm11_add_dr_scan_vc(ARRAY_SIZE(chain5_fields), chain5_fields, TAP_DRPAUSE);
 518
 519                 CHECK_RETVAL(jtag_execute_queue());
 520
 521                 JTAG_DEBUG("DTR  Data %08x  Ready %d  nRetry %d",
 522                                 (unsigned) Data, Ready, nRetry);
 523
 524                 long long then = 0;
 525
 526                 if (i == 1000)
 527                 {
 528                         then = timeval_ms();
 529                 }
 530                 if (i >= 1000)
 531                 {
 532                         if ((timeval_ms()-then) > 1000)
 533                         {
 534                                 LOG_WARNING("Timeout (1000ms) waiting for instructions to complete");
 535                                 return ERROR_FAIL;
 536                         }
 537                 }
 538
 539                 i++;
 540         }
 541         while (!Ready);
 542
 543         return ERROR_OK;
 544 }
 545
 546 /** JTAG path for arm11_run_instr_data_to_core_noack
 547  *
 548  *  The repeated TAP_IDLE's do not cause a repeated execution
 549  *  if passed without leaving the state.
 550  *
 551  *  Since this is more than 7 bits (adjustable via adding more
 552  *  TAP_IDLE's) it produces an artificial delay in the lower
 553  *  layer (FT2232) that is long enough to finish execution on
 554  *  the core but still shorter than any manually inducible delays.
 555  *
 556  *  To disable this code, try "memwrite burst false"
 557  *
 558  *  FIX!!! should we use multiple TAP_IDLE here or not???
 559  *
 560  *  https://lists.berlios.de/pipermail/openocd-development/2009-July/009698.html
 561  *  https://lists.berlios.de/pipermail/openocd-development/2009-August/009865.html
 562  */
 563 static const tap_state_t arm11_MOVE_DRPAUSE_IDLE_DRPAUSE_with_delay[] =
 564 {
 565         TAP_DREXIT2, TAP_DRUPDATE, TAP_IDLE, TAP_IDLE, TAP_IDLE, TAP_DRSELECT, TAP_DRCAPTURE, TAP_DRSHIFT
 566 };
 567
 568
 569
 570 /** Execute one instruction via ITR repeatedly while
 571  *  passing data to the core via DTR on each execution.
 572  *
 573  *  No Ready check during transmission.
 574  *
 575  *  The executed instruction \em must read data from DTR.
 576  *
 577  * \pre arm11_run_instr_data_prepare() /  arm11_run_instr_data_finish() block
 578  *
 579  * \param arm11         Target state variable.
 580  * \param opcode        ARM opcode
 581  * \param data          Pointer to the data words to be passed to the core
 582  * \param count         Number of data words and instruction repetitions
 583  *
 584  */
 585 int arm11_run_instr_data_to_core_noack(struct arm11_common * arm11, uint32_t opcode, uint32_t * data, size_t count)
 586 {
 587         arm11_add_IR(arm11, ARM11_ITRSEL, ARM11_TAP_DEFAULT);
 588
 589         arm11_add_debug_INST(arm11, opcode, NULL, TAP_DRPAUSE);
 590
 591         arm11_add_IR(arm11, ARM11_EXTEST, ARM11_TAP_DEFAULT);
 592
 593         struct scan_field       chain5_fields[3];
 594
 595         arm11_setup_field(arm11, 32,    NULL/*&Data*/,  NULL,                           chain5_fields + 0);
 596         arm11_setup_field(arm11,  1,    NULL,                   NULL /*&Ready*/,        chain5_fields + 1);
 597         arm11_setup_field(arm11,  1,    NULL,                   NULL,                           chain5_fields + 2);
 598
 599         uint8_t                 *Readies;
 600         unsigned readiesNum = count + 1;
 601         unsigned bytes = sizeof(*Readies)*readiesNum;
 602
 603         Readies = (uint8_t *) malloc(bytes);
 604         if (Readies == NULL)
 605         {
 606                 LOG_ERROR("Out of memory allocating %u bytes", bytes);
 607                 return ERROR_FAIL;
 608         }
 609
 610         uint8_t *               ReadyPos                        = Readies;
 611
 612         while (count--)
 613         {
 614                 chain5_fields[0].out_value      = (void *)(data++);
 615                 chain5_fields[1].in_value       = ReadyPos++;
 616
 617                 if (count)
 618                 {
 619                         jtag_add_dr_scan(ARRAY_SIZE(chain5_fields), chain5_fields, jtag_set_end_state(TAP_DRPAUSE));
 620                         jtag_add_pathmove(ARRAY_SIZE(arm11_MOVE_DRPAUSE_IDLE_DRPAUSE_with_delay),
 621                                 arm11_MOVE_DRPAUSE_IDLE_DRPAUSE_with_delay);
 622                 }
 623                 else
 624                 {
 625                         jtag_add_dr_scan(ARRAY_SIZE(chain5_fields), chain5_fields, jtag_set_end_state(TAP_IDLE));
 626                 }
 627         }
 628
 629         arm11_add_IR(arm11, ARM11_INTEST, ARM11_TAP_DEFAULT);
 630
 631         chain5_fields[0].out_value      = 0;
 632         chain5_fields[1].in_value   = ReadyPos++;
 633
 634         arm11_add_dr_scan_vc(ARRAY_SIZE(chain5_fields), chain5_fields, TAP_DRPAUSE);
 635
 636         int retval = jtag_execute_queue();
 637         if (retval == ERROR_OK)
 638         {
 639                 unsigned error_count = 0;
 640
 641                 for (size_t i = 0; i < readiesNum; i++)
 642                 {
 643                         if (Readies[i] != 1)
 644                         {
 645                                 error_count++;
 646                         }
 647                 }
 648
 649                 if (error_count > 0 )
 650                         LOG_ERROR("%u words out of %u not transferred",
 651                                 error_count, readiesNum);
 652
 653         }
 654
 655         free(Readies);
 656
 657         return retval;
 658 }
 659
 660
 661 /** Execute an instruction via ITR while handing data into the core via DTR.
 662  *
 663  *  The executed instruction \em must read data from DTR.
 664  *
 665  * \pre arm11_run_instr_data_prepare() /  arm11_run_instr_data_finish() block
 666  *
 667  * \param arm11         Target state variable.
 668  * \param opcode        ARM opcode
 669  * \param data          Data word to be passed to the core via DTR
 670  *
 671  */
 672 int arm11_run_instr_data_to_core1(struct arm11_common * arm11, uint32_t opcode, uint32_t data)
 673 {
 674         return arm11_run_instr_data_to_core(arm11, opcode, &data, 1);
 675 }
 676
 677
 678 /** Execute one instruction via ITR repeatedly while
 679  *  reading data from the core via DTR on each execution.
 680  *
 681  *  The executed instruction \em must write data to DTR.
 682  *
 683  * \pre arm11_run_instr_data_prepare() /  arm11_run_instr_data_finish() block
 684  *
 685  * \param arm11         Target state variable.
 686  * \param opcode        ARM opcode
 687  * \param data          Pointer to an array that receives the data words from the core
 688  * \param count         Number of data words and instruction repetitions
 689  *
 690  */
 691 int arm11_run_instr_data_from_core(struct arm11_common * arm11, uint32_t opcode, uint32_t * data, size_t count)
 692 {
 693         arm11_add_IR(arm11, ARM11_ITRSEL, ARM11_TAP_DEFAULT);
 694
 695         arm11_add_debug_INST(arm11, opcode, NULL, TAP_IDLE);
 696
 697         arm11_add_IR(arm11, ARM11_INTEST, ARM11_TAP_DEFAULT);
 698
 699         struct scan_field       chain5_fields[3];
 700
 701         uint32_t                        Data;
 702         uint8_t                 Ready;
 703         uint8_t                 nRetry;
 704
 705         arm11_setup_field(arm11, 32,    NULL,   &Data,      chain5_fields + 0);
 706         arm11_setup_field(arm11,  1,    NULL,   &Ready,     chain5_fields + 1);
 707         arm11_setup_field(arm11,  1,    NULL,   &nRetry,    chain5_fields + 2);
 708
 709         while (count--)
 710         {
 711                 int i = 0;
 712                 do
 713                 {
 714                         arm11_add_dr_scan_vc(ARRAY_SIZE(chain5_fields), chain5_fields, count ? TAP_IDLE : TAP_DRPAUSE);
 715
 716                         CHECK_RETVAL(jtag_execute_queue());
 717
 718                         JTAG_DEBUG("DTR  Data %08x  Ready %d  nRetry %d",
 719                                         (unsigned) Data, Ready, nRetry);
 720
 721                         long long then = 0;
 722
 723                         if (i == 1000)
 724                         {
 725                                 then = timeval_ms();
 726                         }
 727                         if (i >= 1000)
 728                         {
 729                                 if ((timeval_ms()-then) > 1000)
 730                                 {
 731                                         LOG_WARNING("Timeout (1000ms) waiting for instructions to complete");
 732                                         return ERROR_FAIL;
 733                                 }
 734                         }
 735
 736                         i++;
 737                 }
 738                 while (!Ready);
 739
 740                 *data++ = Data;
 741         }
 742
 743         return ERROR_OK;
 744 }
 745
 746 /** Execute one instruction via ITR
 747  *  then load r0 into DTR and read DTR from core.
 748  *
 749  *  The first executed instruction (\p opcode) should write data to r0.
 750  *
 751  * \pre arm11_run_instr_data_prepare() /  arm11_run_instr_data_finish() block
 752  *
 753  * \param arm11         Target state variable.
 754  * \param opcode        ARM opcode to write r0 with the value of interest
 755  * \param data          Pointer to a data word that receives the value from r0 after \p opcode was executed.
 756  *
 757  */
 758 int arm11_run_instr_data_from_core_via_r0(struct arm11_common * arm11, uint32_t opcode, uint32_t * data)
 759 {
 760         int retval;
 761         retval = arm11_run_instr_no_data1(arm11, opcode);
 762         if (retval != ERROR_OK)
 763                 return retval;
 764
 765         /* MCR p14,0,R0,c0,c5,0 (move r0 -> wDTR -> local var) */
 766         arm11_run_instr_data_from_core(arm11, 0xEE000E15, data, 1);
 767
 768         return ERROR_OK;
 769 }
 770
 771 /** Load data into core via DTR then move it to r0 then
 772  *  execute one instruction via ITR
 773  *
 774  *  The final executed instruction (\p opcode) should read data from r0.
 775  *
 776  * \pre arm11_run_instr_data_prepare() /  arm11_run_instr_data_finish() block
 777  *
 778  * \param arm11         Target state variable.
 779  * \param opcode        ARM opcode to read r0 act upon it
 780  * \param data          Data word that will be written to r0 before \p opcode is executed
 781  *
 782  */
 783 int arm11_run_instr_data_to_core_via_r0(struct arm11_common * arm11, uint32_t opcode, uint32_t data)
 784 {
 785         int retval;
 786         /* MRC p14,0,r0,c0,c5,0 */
 787         retval = arm11_run_instr_data_to_core1(arm11, 0xEE100E15, data);
 788         if (retval != ERROR_OK)
 789                 return retval;
 790
 791         retval = arm11_run_instr_no_data1(arm11, opcode);
 792         if (retval != ERROR_OK)
 793                 return retval;
 794
 795         return ERROR_OK;
 796 }
 797
 798 /** Apply reads and writes to scan chain 7
 799  *
 800  * \see struct arm11_sc7_action
 801  *
 802  * \param arm11         Target state variable.
 803  * \param actions       A list of read and/or write instructions
 804  * \param count         Number of instructions in the list.
 805  *
 806  */
 807 int arm11_sc7_run(struct arm11_common * arm11, struct arm11_sc7_action * actions, size_t count)
 808 {
 809         int retval;
 810
 811         retval = arm11_add_debug_SCAN_N(arm11, 0x07, ARM11_TAP_DEFAULT);
 812         if (retval != ERROR_OK)
 813                 return retval;
 814
 815         arm11_add_IR(arm11, ARM11_EXTEST, ARM11_TAP_DEFAULT);
 816
 817         struct scan_field       chain7_fields[3];
 818
 819         uint8_t                         nRW;
 820         uint32_t                                DataOut;
 821         uint8_t                         AddressOut;
 822         uint8_t                         Ready;
 823         uint32_t                                DataIn;
 824         uint8_t                         AddressIn;
 825
 826         arm11_setup_field(arm11,  1, &nRW,                      &Ready,         chain7_fields + 0);
 827         arm11_setup_field(arm11, 32, &DataOut,          &DataIn,        chain7_fields + 1);
 828         arm11_setup_field(arm11,  7, &AddressOut,       &AddressIn,     chain7_fields + 2);
 829
 830         for (size_t i = 0; i < count + 1; i++)
 831         {
 832                 if (i < count)
 833                 {
 834                         nRW                     = actions[i].write ? 1 : 0;
 835                         DataOut         = actions[i].value;
 836                         AddressOut      = actions[i].address;
 837                 }
 838                 else
 839                 {
 840                         nRW                     = 0;
 841                         DataOut         = 0;
 842                         AddressOut      = 0;
 843                 }
 844
 845                 do
 846                 {
 847                         JTAG_DEBUG("SC7 <= Address %02x  Data %08x    nRW %d",
 848                                         (unsigned) AddressOut,
 849                                         (unsigned) DataOut,
 850                                         nRW);
 851
 852                         arm11_add_dr_scan_vc(ARRAY_SIZE(chain7_fields),
 853                                         chain7_fields, TAP_DRPAUSE);
 854
 855                         CHECK_RETVAL(jtag_execute_queue());
 856
 857                         JTAG_DEBUG("SC7 => Address %02x  Data %08x  Ready %d",
 858                                         (unsigned) AddressIn,
 859                                         (unsigned) DataIn,
 860                                         Ready);
 861                 }
 862                 while (!Ready); /* 'nRW' is 'Ready' on read out */
 863
 864                 if (i > 0)
 865                 {
 866                         if (actions[i - 1].address != AddressIn)
 867                         {
 868                                 LOG_WARNING("Scan chain 7 shifted out unexpected address");
 869                         }
 870
 871                         if (!actions[i - 1].write)
 872                         {
 873                                 actions[i - 1].value = DataIn;
 874                         }
 875                         else
 876                         {
 877                                 if (actions[i - 1].value != DataIn)
 878                                 {
 879                                         LOG_WARNING("Scan chain 7 shifted out unexpected data");
 880                                 }
 881                         }
 882                 }
 883         }
 884
 885         for (size_t i = 0; i < count; i++)
 886         {
 887                 JTAG_DEBUG("SC7 %02d: %02x %s %08x",
 888                         (unsigned) i, actions[i].address,
 889                         actions[i].write ? "<=" : "=>",
 890                         (unsigned) actions[i].value);
 891         }
 892
 893         return ERROR_OK;
 894 }
 895
 896 /** Clear VCR and all breakpoints and watchpoints via scan chain 7
 897  *
 898  * \param arm11         Target state variable.
 899  *
 900  */
 901 void arm11_sc7_clear_vbw(struct arm11_common * arm11)
 902 {
 903         size_t clear_bw_size = arm11->brp + arm11->wrp + 1;
 904         struct arm11_sc7_action         *clear_bw = malloc(sizeof(struct arm11_sc7_action) * clear_bw_size);
 905         struct arm11_sc7_action *       pos = clear_bw;
 906
 907         for (size_t i = 0; i < clear_bw_size; i++)
 908         {
 909                 clear_bw[i].write       = true;
 910                 clear_bw[i].value       = 0;
 911         }
 912
 913         for (size_t i = 0; i < arm11->brp; i++)
 914                 (pos++)->address = ARM11_SC7_BCR0 + i;
 915
 916
 917         for (size_t i = 0; i < arm11->wrp; i++)
 918                 (pos++)->address = ARM11_SC7_WCR0 + i;
 919
 920
 921         (pos++)->address = ARM11_SC7_VCR;
 922
 923         arm11_sc7_run(arm11, clear_bw, clear_bw_size);
 924
 925         free (clear_bw);
 926 }
 927
 928 /** Write VCR register
 929  *
 930  * \param arm11         Target state variable.
 931  * \param value         Value to be written
 932  */
 933 void arm11_sc7_set_vcr(struct arm11_common * arm11, uint32_t value)
 934 {
 935         struct arm11_sc7_action         set_vcr;
 936
 937         set_vcr.write           = true;
 938         set_vcr.address         = ARM11_SC7_VCR;
 939         set_vcr.value           = value;
 940
 941
 942         arm11_sc7_run(arm11, &set_vcr, 1);
 943 }
 944
 945
 946
 947 /** Read word from address
 948  *
 949  * \param arm11         Target state variable.
 950  * \param address       Memory address to be read
 951  * \param result        Pointer where to store result
 952  *
 953  */
 954 int arm11_read_memory_word(struct arm11_common * arm11, uint32_t address, uint32_t * result)
 955 {
 956         int retval;
 957         retval = arm11_run_instr_data_prepare(arm11);
 958         if (retval != ERROR_OK)
 959                 return retval;
 960
 961         /* MRC p14,0,r0,c0,c5,0 (r0 = address) */
 962         CHECK_RETVAL(arm11_run_instr_data_to_core1(arm11, 0xee100e15, address));
 963
 964         /* LDC p14,c5,[R0],#4 (DTR = [r0]) */
 965         CHECK_RETVAL(arm11_run_instr_data_from_core(arm11, 0xecb05e01, result, 1));
 966
 967         return arm11_run_instr_data_finish(arm11);
 968 }
 969
 970
 971 /************************************************************************/
 972
 973 /*
 974  * ARM11 provider for the OpenOCD implementation of the standard
 975  * architectural ARM v6/v7 "Debug Programmer's Model" (DPM).
 976  */
 977
 978 static inline struct arm11_common *dpm_to_arm11(struct arm_dpm *dpm)
 979 {
 980         return container_of(dpm, struct arm11_common, dpm);
 981 }
 982
 983 static int arm11_dpm_prepare(struct arm_dpm *dpm)
 984 {
 985         struct arm11_common *arm11 = dpm_to_arm11(dpm);
 986
 987         arm11 = container_of(dpm->arm, struct arm11_common, arm);
 988
 989         return arm11_run_instr_data_prepare(dpm_to_arm11(dpm));
 990 }
 991
 992 static int arm11_dpm_finish(struct arm_dpm *dpm)
 993 {
 994         return arm11_run_instr_data_finish(dpm_to_arm11(dpm));
 995 }
 996
 997 static int arm11_dpm_instr_write_data_dcc(struct arm_dpm *dpm,
 998                 uint32_t opcode, uint32_t data)
 999 {
1000         return arm11_run_instr_data_to_core(dpm_to_arm11(dpm),
1001                         opcode, &data, 1);
1002 }
1003
1004 static int arm11_dpm_instr_write_data_r0(struct arm_dpm *dpm,
1005                 uint32_t opcode, uint32_t data)
1006 {
1007         return arm11_run_instr_data_to_core_via_r0(dpm_to_arm11(dpm),
1008                         opcode, data);
1009 }
1010
1011 static int arm11_dpm_instr_read_data_dcc(struct arm_dpm *dpm,
1012                 uint32_t opcode, uint32_t *data)
1013 {
1014         return arm11_run_instr_data_from_core(dpm_to_arm11(dpm),
1015                         opcode, data, 1);
1016 }
1017
1018 static int arm11_dpm_instr_read_data_r0(struct arm_dpm *dpm,
1019                 uint32_t opcode, uint32_t *data)
1020 {
1021         return arm11_run_instr_data_from_core_via_r0(dpm_to_arm11(dpm),
1022                         opcode, data);
1023 }
1024
1025
1026 void arm11_dpm_init(struct arm11_common *arm11, uint32_t didr)
1027 {
1028         struct arm_dpm *dpm = &arm11->dpm;
1029
1030         dpm->arm = &arm11->arm;
1031
1032         dpm->didr = didr;
1033
1034         dpm->prepare = arm11_dpm_prepare;
1035         dpm->finish = arm11_dpm_finish;
1036
1037         dpm->instr_write_data_dcc = arm11_dpm_instr_write_data_dcc;
1038         dpm->instr_write_data_r0 = arm11_dpm_instr_write_data_r0;
1039
1040         dpm->instr_read_data_dcc = arm11_dpm_instr_read_data_dcc;
1041         dpm->instr_read_data_r0 = arm11_dpm_instr_read_data_r0;
1042 }