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 "arm11.h"
  28
  29 #include "time_support.h"
  30
  31 #if 0
  32 #define JTAG_DEBUG(expr ...)    DEBUG(expr)
  33 #else
  34 #define JTAG_DEBUG(expr ...)    do {} while (0)
  35 #endif
  36
  37 /*
  38 This pathmove goes from Pause-IR to Shift-IR while avoiding RTI. The
  39 behavior of the FTDI driver IIRC was to go via RTI.
  40
  41 Conversely there may be other places in this code where the ARM11 code relies
  42 on the driver to hit through RTI when coming from Update-?R.
  43 */
  44 tap_state_t arm11_move_pi_to_si_via_ci[] =
  45 {
  46     TAP_IREXIT2, TAP_IRUPDATE, TAP_DRSELECT, TAP_IRSELECT, TAP_IRCAPTURE, TAP_IRSHIFT
  47 };
  48
  49
  50 int arm11_add_ir_scan_vc(int num_fields, scan_field_t *fields, tap_state_t state)
  51 {
  52         if (cmd_queue_cur_state == TAP_IRPAUSE)
  53                 jtag_add_pathmove(asizeof(arm11_move_pi_to_si_via_ci), arm11_move_pi_to_si_via_ci);
  54
  55         jtag_add_ir_scan(num_fields, fields, state);
  56         return ERROR_OK;
  57 }
  58
  59 tap_state_t arm11_move_pd_to_sd_via_cd[] =
  60 {
  61         TAP_DREXIT2, TAP_DRUPDATE, TAP_DRSELECT, TAP_DRCAPTURE, TAP_DRSHIFT
  62 };
  63
  64 int arm11_add_dr_scan_vc(int num_fields, scan_field_t *fields, tap_state_t state)
  65 {
  66         if (cmd_queue_cur_state == TAP_DRPAUSE)
  67                 jtag_add_pathmove(asizeof(arm11_move_pd_to_sd_via_cd), arm11_move_pd_to_sd_via_cd);
  68
  69         jtag_add_dr_scan(num_fields, fields, state);
  70         return ERROR_OK;
  71 }
  72
  73
  74 /** Code de-clutter: Construct scan_field_t to write out a value
  75  *
  76  * \param arm11                 Target state variable.
  77  * \param num_bits              Length of the data field
  78  * \param out_data              pointer to the data that will be sent out
  79  *                                              <em > (data is read when it is added to the JTAG queue)</em>
  80  * \param in_data               pointer to the memory that will receive data that was clocked in
  81  *                                              <em > (data is written when the JTAG queue is executed)</em>
  82  * \param field                 target data structure that will be initialized
  83  */
  84 void arm11_setup_field(arm11_common_t * arm11, int num_bits, void * out_data, void * in_data, scan_field_t * field)
  85 {
  86         field->tap                      = arm11->target->tap;
  87         field->num_bits                 = num_bits;
  88         field->out_value                = out_data;
  89         field->in_value                 = in_data;
  90 }
  91
  92
  93 /** Write JTAG instruction register
  94  *
  95  * \param arm11         Target state variable.
  96  * \param instr         An ARM11 DBGTAP instruction. Use enum #arm11_instructions.
  97  * \param state         Pass the final TAP state or ARM11_TAP_DEFAULT for the default value (Pause-IR).
  98  *
  99  * \remarks                     This adds to the JTAG command queue but does \em not execute it.
 100  */
 101 void arm11_add_IR(arm11_common_t * arm11, uint8_t instr, tap_state_t state)
 102 {
 103         jtag_tap_t *tap;
 104         tap = arm11->target->tap;
 105
 106         if (buf_get_u32(tap->cur_instr, 0, 5) == instr)
 107         {
 108                 JTAG_DEBUG("IR <= 0x%02x SKIPPED", instr);
 109                 return;
 110         }
 111
 112         JTAG_DEBUG("IR <= 0x%02x", instr);
 113
 114         scan_field_t field;
 115
 116         arm11_setup_field(arm11, 5, &instr, NULL, &field);
 117
 118         arm11_add_ir_scan_vc(1, &field, state == ARM11_TAP_DEFAULT ? TAP_IRPAUSE : state);
 119 }
 120
 121 /** Verify shifted out data from Scan Chain Register (SCREG)
 122  *  Used as parameter to scan_field_t::in_handler in
 123  *  arm11_add_debug_SCAN_N().
 124  *
 125  */
 126 static void arm11_in_handler_SCAN_N(uint8_t *in_value)
 127 {
 128         /** \todo TODO: clarify why this isnt properly masked in core.c jtag_read_buffer() */
 129         uint8_t v = *in_value & 0x1F;
 130
 131         if (v != 0x10)
 132         {
 133                 LOG_ERROR("'arm11 target' JTAG communication error SCREG SCAN OUT 0x%02x (expected 0x10)", v);
 134                 jtag_set_error(ERROR_FAIL);
 135         }
 136
 137         JTAG_DEBUG("SCREG SCAN OUT 0x%02x", v);
 138 }
 139
 140 /** Select and write to Scan Chain Register (SCREG)
 141  *
 142  * This function sets the instruction register to SCAN_N and writes
 143  * the data register with the selected chain number.
 144  *
 145  * http://infocenter.arm.com/help/topic/com.arm.doc.ddi0301f/Cacbjhfg.html
 146  *
 147  * \param arm11     Target state variable.
 148  * \param chain     Scan chain that will be selected.
 149  * \param state     Pass the final TAP state or ARM11_TAP_DEFAULT for the default
 150  *                                      value (Pause-DR).
 151  *
 152  * The chain takes effect when Update-DR is passed (usually when subsequently
 153  * the INTEXT/EXTEST instructions are written).
 154  *
 155  * \warning                     (Obsolete) Using this twice in a row will \em fail. The first
 156  *                                      call will end in Pause-DR. The second call, due to the IR
 157  *                                      caching, will not go through Capture-DR when shifting in the
 158  *                                      new scan chain number. As a result the verification in
 159  *                                      arm11_in_handler_SCAN_N() must fail.
 160  *
 161  * \remarks                     This adds to the JTAG command queue but does \em not execute it.
 162  */
 163
 164 void arm11_add_debug_SCAN_N(arm11_common_t * arm11, uint8_t chain, tap_state_t state)
 165 {
 166         JTAG_DEBUG("SCREG <= 0x%02x", chain);
 167
 168         arm11_add_IR(arm11, ARM11_SCAN_N, ARM11_TAP_DEFAULT);
 169
 170         scan_field_t            field;
 171
 172         uint8_t tmp[1];
 173         arm11_setup_field(arm11, 5, &chain, &tmp, &field);
 174
 175         arm11_add_dr_scan_vc(1, &field, state == ARM11_TAP_DEFAULT ? TAP_DRPAUSE : state);
 176
 177         jtag_execute_queue_noclear();
 178
 179         arm11_in_handler_SCAN_N(tmp);
 180 }
 181
 182 /** Write an instruction into the ITR register
 183  *
 184  * \param arm11         Target state variable.
 185  * \param inst          An ARM11 processor instruction/opcode.
 186  * \param flag          Optional parameter to retrieve the InstCompl flag
 187  *                                      (this will be written when the JTAG chain is executed).
 188  * \param state         Pass the final TAP state or ARM11_TAP_DEFAULT for the default
 189  *                                      value (Run-Test/Idle).
 190  *
 191  * \remarks                     By default this ends with Run-Test/Idle state
 192  *                                      and causes the instruction to be executed. If
 193  *                                      a subsequent write to DTR is needed before
 194  *                                      executing the instruction then TAP_DRPAUSE should be
 195  *                                      passed to \p state.
 196  *
 197  * \remarks                     This adds to the JTAG command queue but does \em not execute it.
 198  */
 199 void arm11_add_debug_INST(arm11_common_t * arm11, uint32_t inst, uint8_t * flag, tap_state_t state)
 200 {
 201         JTAG_DEBUG("INST <= 0x%08x", inst);
 202
 203         scan_field_t            itr[2];
 204
 205         arm11_setup_field(arm11, 32,    &inst,  NULL, itr + 0);
 206         arm11_setup_field(arm11, 1,         NULL,       flag, itr + 1);
 207
 208         arm11_add_dr_scan_vc(asizeof(itr), itr, state == ARM11_TAP_DEFAULT ? TAP_IDLE : state);
 209 }
 210
 211 /** Read the Debug Status and Control Register (DSCR)
 212  *
 213  * same as CP14 c1
 214  *
 215  * \param arm11         Target state variable.
 216  * \param value         DSCR content
 217  * \return                      Error status
 218  *
 219  * \remarks                     This is a stand-alone function that executes the JTAG command queue.
 220  */
 221 int arm11_read_DSCR(arm11_common_t * arm11, uint32_t *value)
 222 {
 223         arm11_add_debug_SCAN_N(arm11, 0x01, ARM11_TAP_DEFAULT);
 224
 225         arm11_add_IR(arm11, ARM11_INTEST, ARM11_TAP_DEFAULT);
 226
 227         uint32_t                                dscr;
 228         scan_field_t    chain1_field;
 229
 230         arm11_setup_field(arm11, 32, NULL, &dscr, &chain1_field);
 231
 232         arm11_add_dr_scan_vc(1, &chain1_field, TAP_DRPAUSE);
 233
 234         CHECK_RETVAL(jtag_execute_queue());
 235
 236         if (arm11->last_dscr != dscr)
 237                 JTAG_DEBUG("DSCR  = %08x (OLD %08x)", dscr, arm11->last_dscr);
 238
 239         arm11->last_dscr = dscr;
 240
 241         *value = dscr;
 242
 243         return ERROR_OK;
 244 }
 245
 246 /** Write the Debug Status and Control Register (DSCR)
 247  *
 248  * same as CP14 c1
 249  *
 250  * \param arm11         Target state variable.
 251  * \param dscr          DSCR content
 252  *
 253  * \remarks                     This is a stand-alone function that executes the JTAG command queue.
 254  */
 255 int arm11_write_DSCR(arm11_common_t * arm11, uint32_t dscr)
 256 {
 257         arm11_add_debug_SCAN_N(arm11, 0x01, ARM11_TAP_DEFAULT);
 258
 259         arm11_add_IR(arm11, ARM11_EXTEST, ARM11_TAP_DEFAULT);
 260
 261         scan_field_t                chain1_field;
 262
 263         arm11_setup_field(arm11, 32, &dscr, NULL, &chain1_field);
 264
 265         arm11_add_dr_scan_vc(1, &chain1_field, TAP_DRPAUSE);
 266
 267         CHECK_RETVAL(jtag_execute_queue());
 268
 269         JTAG_DEBUG("DSCR <= %08x (OLD %08x)", dscr, arm11->last_dscr);
 270
 271         arm11->last_dscr = dscr;
 272
 273         return ERROR_OK;
 274 }
 275
 276
 277
 278 /** Get the debug reason from Debug Status and Control Register (DSCR)
 279  *
 280  * \param dscr          DSCR value to analyze
 281  * \return                      Debug reason
 282  *
 283  */
 284 enum target_debug_reason arm11_get_DSCR_debug_reason(uint32_t dscr)
 285 {
 286         switch (dscr & ARM11_DSCR_METHOD_OF_DEBUG_ENTRY_MASK)
 287         {
 288         case ARM11_DSCR_METHOD_OF_DEBUG_ENTRY_HALT:
 289                 LOG_INFO("Debug entry: JTAG HALT");
 290                 return DBG_REASON_DBGRQ;
 291
 292         case ARM11_DSCR_METHOD_OF_DEBUG_ENTRY_BREAKPOINT:
 293                 LOG_INFO("Debug entry: breakpoint");
 294                 return DBG_REASON_BREAKPOINT;
 295
 296         case ARM11_DSCR_METHOD_OF_DEBUG_ENTRY_WATCHPOINT:
 297                 LOG_INFO("Debug entry: watchpoint");
 298                 return DBG_REASON_WATCHPOINT;
 299
 300         case ARM11_DSCR_METHOD_OF_DEBUG_ENTRY_BKPT_INSTRUCTION:
 301                 LOG_INFO("Debug entry: BKPT instruction");
 302                 return DBG_REASON_BREAKPOINT;
 303
 304         case ARM11_DSCR_METHOD_OF_DEBUG_ENTRY_EDBGRQ:
 305                 LOG_INFO("Debug entry: EDBGRQ signal");
 306                 return DBG_REASON_DBGRQ;
 307
 308         case ARM11_DSCR_METHOD_OF_DEBUG_ENTRY_VECTOR_CATCH:
 309                 LOG_INFO("Debug entry: VCR vector catch");
 310                 return DBG_REASON_BREAKPOINT;
 311
 312         default:
 313                 LOG_INFO("Debug entry: unknown");
 314                 return DBG_REASON_DBGRQ;
 315         }
 316 };
 317
 318
 319
 320 /** Prepare the stage for ITR/DTR operations
 321  * from the arm11_run_instr... group of functions.
 322  *
 323  * Put arm11_run_instr_data_prepare() and arm11_run_instr_data_finish()
 324  * around a block of arm11_run_instr_... calls.
 325  *
 326  * Select scan chain 5 to allow quick access to DTR. When scan
 327  * chain 4 is needed to put in a register the ITRSel instruction
 328  * shortcut is used instead of actually changing the Scan_N
 329  * register.
 330  *
 331  * \param arm11         Target state variable.
 332  *
 333  */
 334 void arm11_run_instr_data_prepare(arm11_common_t * arm11)
 335 {
 336         arm11_add_debug_SCAN_N(arm11, 0x05, ARM11_TAP_DEFAULT);
 337 }
 338
 339 /** Cleanup after ITR/DTR operations
 340  * from the arm11_run_instr... group of functions
 341  *
 342  * Put arm11_run_instr_data_prepare() and arm11_run_instr_data_finish()
 343  * around a block of arm11_run_instr_... calls.
 344  *
 345  * Any IDLE can lead to an instruction execution when
 346  * scan chains 4 or 5 are selected and the IR holds
 347  * INTEST or EXTEST. So we must disable that before
 348  * any following activities lead to an IDLE.
 349  *
 350  * \param arm11         Target state variable.
 351  *
 352  */
 353 void arm11_run_instr_data_finish(arm11_common_t * arm11)
 354 {
 355         arm11_add_debug_SCAN_N(arm11, 0x00, ARM11_TAP_DEFAULT);
 356 }
 357
 358
 359
 360 /** Execute one or multiple instructions via ITR
 361  *
 362  * \pre arm11_run_instr_data_prepare() /  arm11_run_instr_data_finish() block
 363  *
 364  * \param arm11         Target state variable.
 365  * \param opcode        Pointer to sequence of ARM opcodes
 366  * \param count         Number of opcodes to execute
 367  *
 368  */
 369 int arm11_run_instr_no_data(arm11_common_t * arm11, uint32_t * opcode, size_t count)
 370 {
 371         arm11_add_IR(arm11, ARM11_ITRSEL, ARM11_TAP_DEFAULT);
 372
 373         while (count--)
 374         {
 375                 arm11_add_debug_INST(arm11, *opcode++, NULL, TAP_IDLE);
 376
 377                 int i = 0;
 378                 while (1)
 379                 {
 380                         uint8_t flag;
 381
 382                         arm11_add_debug_INST(arm11, 0, &flag, count ? TAP_IDLE : TAP_DRPAUSE);
 383
 384                         CHECK_RETVAL(jtag_execute_queue());
 385
 386                         if (flag)
 387                                 break;
 388
 389                         long long then = 0;
 390
 391                         if (i == 1000)
 392                         {
 393                                 then = timeval_ms();
 394                         }
 395                         if (i >= 1000)
 396                         {
 397                                 if ((timeval_ms()-then) > 1000)
 398                                 {
 399                                         LOG_WARNING("Timeout (1000ms) waiting for instructions to complete");
 400                                         return ERROR_FAIL;
 401                                 }
 402                         }
 403
 404                         i++;
 405                 }
 406         }
 407
 408         return ERROR_OK;
 409 }
 410
 411 /** Execute one instruction via ITR
 412  *
 413  * \pre arm11_run_instr_data_prepare() /  arm11_run_instr_data_finish() block
 414  *
 415  * \param arm11         Target state variable.
 416  * \param opcode        ARM opcode
 417  *
 418  */
 419 int arm11_run_instr_no_data1(arm11_common_t * arm11, uint32_t opcode)
 420 {
 421         return arm11_run_instr_no_data(arm11, &opcode, 1);
 422 }
 423
 424
 425 /** Execute one instruction via ITR repeatedly while
 426  *  passing data to the core via DTR on each execution.
 427  *
 428  *  The executed instruction \em must read data from DTR.
 429  *
 430  * \pre arm11_run_instr_data_prepare() /  arm11_run_instr_data_finish() block
 431  *
 432  * \param arm11         Target state variable.
 433  * \param opcode        ARM opcode
 434  * \param data          Pointer to the data words to be passed to the core
 435  * \param count         Number of data words and instruction repetitions
 436  *
 437  */
 438 int arm11_run_instr_data_to_core(arm11_common_t * arm11, uint32_t opcode, uint32_t * data, size_t count)
 439 {
 440         arm11_add_IR(arm11, ARM11_ITRSEL, ARM11_TAP_DEFAULT);
 441
 442         arm11_add_debug_INST(arm11, opcode, NULL, TAP_DRPAUSE);
 443
 444         arm11_add_IR(arm11, ARM11_EXTEST, ARM11_TAP_DEFAULT);
 445
 446         scan_field_t    chain5_fields[3];
 447
 448         uint32_t                                Data;
 449         uint8_t                         Ready;
 450         uint8_t                         nRetry;
 451
 452         arm11_setup_field(arm11, 32,    &Data,  NULL,           chain5_fields + 0);
 453         arm11_setup_field(arm11,  1,    NULL,   &Ready,         chain5_fields + 1);
 454         arm11_setup_field(arm11,  1,    NULL,   &nRetry,        chain5_fields + 2);
 455
 456         while (count--)
 457         {
 458                 int i = 0;
 459                 do
 460                 {
 461                         Data        = *data;
 462
 463                         arm11_add_dr_scan_vc(asizeof(chain5_fields), chain5_fields, jtag_set_end_state(TAP_IDLE));
 464
 465                         CHECK_RETVAL(jtag_execute_queue());
 466
 467                         JTAG_DEBUG("DTR  Ready %d  nRetry %d", Ready, nRetry);
 468
 469                         long long then = 0;
 470
 471                         if (i == 1000)
 472                         {
 473                                 then = timeval_ms();
 474                         }
 475                         if (i >= 1000)
 476                         {
 477                                 if ((timeval_ms()-then) > 1000)
 478                                 {
 479                                         LOG_WARNING("Timeout (1000ms) waiting for instructions to complete");
 480                                         return ERROR_FAIL;
 481                                 }
 482                         }
 483
 484                         i++;
 485                 }
 486                 while (!Ready);
 487
 488                 data++;
 489         }
 490
 491         arm11_add_IR(arm11, ARM11_INTEST, ARM11_TAP_DEFAULT);
 492
 493         int i = 0;
 494         do
 495         {
 496                 Data        = 0;
 497
 498                 arm11_add_dr_scan_vc(asizeof(chain5_fields), chain5_fields, TAP_DRPAUSE);
 499
 500                 CHECK_RETVAL(jtag_execute_queue());
 501
 502                 JTAG_DEBUG("DTR  Data %08x  Ready %d  nRetry %d", Data, Ready, nRetry);
 503
 504                 long long then = 0;
 505
 506                 if (i == 1000)
 507                 {
 508                         then = timeval_ms();
 509                 }
 510                 if (i >= 1000)
 511                 {
 512                         if ((timeval_ms()-then) > 1000)
 513                         {
 514                                 LOG_WARNING("Timeout (1000ms) waiting for instructions to complete");
 515                                 return ERROR_FAIL;
 516                         }
 517                 }
 518
 519                 i++;
 520         }
 521         while (!Ready);
 522
 523         return ERROR_OK;
 524 }
 525
 526 /** JTAG path for arm11_run_instr_data_to_core_noack
 527  *
 528  *  The repeated TAP_IDLE's do not cause a repeated execution
 529  *  if passed without leaving the state.
 530  *
 531  *  Since this is more than 7 bits (adjustable via adding more
 532  *  TAP_IDLE's) it produces an artificial delay in the lower
 533  *  layer (FT2232) that is long enough to finish execution on
 534  *  the core but still shorter than any manually inducible delays.
 535  *
 536  *  To disable this code, try "memwrite burst false"
 537  *
 538  *  FIX!!! should we use multiple TAP_IDLE here or not???
 539  *
 540  *  https://lists.berlios.de/pipermail/openocd-development/2009-July/009698.html
 541  *  https://lists.berlios.de/pipermail/openocd-development/2009-August/009865.html
 542  */
 543 tap_state_t arm11_MOVE_DRPAUSE_IDLE_DRPAUSE_with_delay[] =
 544 {
 545         TAP_DREXIT2, TAP_DRUPDATE, TAP_IDLE, TAP_IDLE, TAP_IDLE, TAP_DRSELECT, TAP_DRCAPTURE, TAP_DRSHIFT
 546 };
 547
 548
 549
 550 /** Execute one instruction via ITR repeatedly while
 551  *  passing data to the core via DTR on each execution.
 552  *
 553  *  No Ready check during transmission.
 554  *
 555  *  The executed instruction \em must read data from DTR.
 556  *
 557  * \pre arm11_run_instr_data_prepare() /  arm11_run_instr_data_finish() block
 558  *
 559  * \param arm11         Target state variable.
 560  * \param opcode        ARM opcode
 561  * \param data          Pointer to the data words to be passed to the core
 562  * \param count         Number of data words and instruction repetitions
 563  *
 564  */
 565 int arm11_run_instr_data_to_core_noack(arm11_common_t * arm11, uint32_t opcode, uint32_t * data, size_t count)
 566 {
 567         arm11_add_IR(arm11, ARM11_ITRSEL, ARM11_TAP_DEFAULT);
 568
 569         arm11_add_debug_INST(arm11, opcode, NULL, TAP_DRPAUSE);
 570
 571         arm11_add_IR(arm11, ARM11_EXTEST, ARM11_TAP_DEFAULT);
 572
 573         scan_field_t    chain5_fields[3];
 574
 575         arm11_setup_field(arm11, 32,    NULL/*&Data*/,  NULL,                           chain5_fields + 0);
 576         arm11_setup_field(arm11,  1,    NULL,                   NULL /*&Ready*/,        chain5_fields + 1);
 577         arm11_setup_field(arm11,  1,    NULL,                   NULL,                           chain5_fields + 2);
 578
 579         uint8_t                 *Readies;
 580         int bytes = sizeof(*Readies)*(count + 1);
 581         Readies = (uint8_t *) malloc(bytes);
 582         if (Readies == NULL)
 583         {
 584                 LOG_ERROR("Out of memory allocating %d bytes", bytes);
 585                 return ERROR_FAIL;
 586         }
 587
 588         uint8_t *               ReadyPos                        = Readies;
 589
 590         while (count--)
 591         {
 592                 chain5_fields[0].out_value      = (void *)(data++);
 593                 chain5_fields[1].in_value       = ReadyPos++;
 594
 595                 if (count)
 596                 {
 597                         jtag_add_dr_scan(asizeof(chain5_fields), chain5_fields, jtag_set_end_state(TAP_DRPAUSE));
 598                         jtag_add_pathmove(asizeof(arm11_MOVE_DRPAUSE_IDLE_DRPAUSE_with_delay),
 599                                 arm11_MOVE_DRPAUSE_IDLE_DRPAUSE_with_delay);
 600                 }
 601                 else
 602                 {
 603                         jtag_add_dr_scan(asizeof(chain5_fields), chain5_fields, jtag_set_end_state(TAP_IDLE));
 604                 }
 605         }
 606
 607         arm11_add_IR(arm11, ARM11_INTEST, ARM11_TAP_DEFAULT);
 608
 609         chain5_fields[0].out_value      = 0;
 610         chain5_fields[1].in_value   = ReadyPos++;
 611
 612         arm11_add_dr_scan_vc(asizeof(chain5_fields), chain5_fields, TAP_DRPAUSE);
 613
 614         int retval = jtag_execute_queue();
 615         if (retval == ERROR_OK)
 616         {
 617
 618                 size_t error_count = 0;
 619
 620                 for (size_t i = 0; i < asizeof(Readies); i++)
 621                 {
 622                         if (Readies[i] != 1)
 623                         {
 624                                 error_count++;
 625                         }
 626                 }
 627
 628                 if (error_count)
 629                         LOG_ERROR("Transfer errors " ZU, error_count);
 630
 631         }
 632
 633         free(Readies);
 634
 635         return retval;
 636 }
 637
 638
 639 /** Execute an instruction via ITR while handing data into the core via DTR.
 640  *
 641  *  The executed instruction \em must read data from DTR.
 642  *
 643  * \pre arm11_run_instr_data_prepare() /  arm11_run_instr_data_finish() block
 644  *
 645  * \param arm11         Target state variable.
 646  * \param opcode        ARM opcode
 647  * \param data          Data word to be passed to the core via DTR
 648  *
 649  */
 650 int arm11_run_instr_data_to_core1(arm11_common_t * arm11, uint32_t opcode, uint32_t data)
 651 {
 652         return arm11_run_instr_data_to_core(arm11, opcode, &data, 1);
 653 }
 654
 655
 656 /** Execute one instruction via ITR repeatedly while
 657  *  reading data from the core via DTR on each execution.
 658  *
 659  *  The executed instruction \em must write data to DTR.
 660  *
 661  * \pre arm11_run_instr_data_prepare() /  arm11_run_instr_data_finish() block
 662  *
 663  * \param arm11         Target state variable.
 664  * \param opcode        ARM opcode
 665  * \param data          Pointer to an array that receives the data words from the core
 666  * \param count         Number of data words and instruction repetitions
 667  *
 668  */
 669 int arm11_run_instr_data_from_core(arm11_common_t * arm11, uint32_t opcode, uint32_t * data, size_t count)
 670 {
 671         arm11_add_IR(arm11, ARM11_ITRSEL, ARM11_TAP_DEFAULT);
 672
 673         arm11_add_debug_INST(arm11, opcode, NULL, TAP_IDLE);
 674
 675         arm11_add_IR(arm11, ARM11_INTEST, ARM11_TAP_DEFAULT);
 676
 677         scan_field_t    chain5_fields[3];
 678
 679         uint32_t                        Data;
 680         uint8_t                 Ready;
 681         uint8_t                 nRetry;
 682
 683         arm11_setup_field(arm11, 32,    NULL,   &Data,      chain5_fields + 0);
 684         arm11_setup_field(arm11,  1,    NULL,   &Ready,     chain5_fields + 1);
 685         arm11_setup_field(arm11,  1,    NULL,   &nRetry,    chain5_fields + 2);
 686
 687         while (count--)
 688         {
 689                 int i = 0;
 690                 do
 691                 {
 692                         arm11_add_dr_scan_vc(asizeof(chain5_fields), chain5_fields, count ? TAP_IDLE : TAP_DRPAUSE);
 693
 694                         CHECK_RETVAL(jtag_execute_queue());
 695
 696                         JTAG_DEBUG("DTR  Data %08x  Ready %d  nRetry %d", Data, Ready, nRetry);
 697
 698                         long long then = 0;
 699
 700                         if (i == 1000)
 701                         {
 702                                 then = timeval_ms();
 703                         }
 704                         if (i >= 1000)
 705                         {
 706                                 if ((timeval_ms()-then) > 1000)
 707                                 {
 708                                         LOG_WARNING("Timeout (1000ms) waiting for instructions to complete");
 709                                         return ERROR_FAIL;
 710                                 }
 711                         }
 712
 713                         i++;
 714                 }
 715                 while (!Ready);
 716
 717                 *data++ = Data;
 718         }
 719
 720         return ERROR_OK;
 721 }
 722
 723 /** Execute one instruction via ITR
 724  *  then load r0 into DTR and read DTR from core.
 725  *
 726  *  The first executed instruction (\p opcode) should write data to r0.
 727  *
 728  * \pre arm11_run_instr_data_prepare() /  arm11_run_instr_data_finish() block
 729  *
 730  * \param arm11         Target state variable.
 731  * \param opcode        ARM opcode to write r0 with the value of interest
 732  * \param data          Pointer to a data word that receives the value from r0 after \p opcode was executed.
 733  *
 734  */
 735 int arm11_run_instr_data_from_core_via_r0(arm11_common_t * arm11, uint32_t opcode, uint32_t * data)
 736 {
 737         int retval;
 738         retval = arm11_run_instr_no_data1(arm11, opcode);
 739         if (retval != ERROR_OK)
 740                 return retval;
 741
 742         /* MCR p14,0,R0,c0,c5,0 (move r0 -> wDTR -> local var) */
 743         arm11_run_instr_data_from_core(arm11, 0xEE000E15, data, 1);
 744
 745         return ERROR_OK;
 746 }
 747
 748 /** Load data into core via DTR then move it to r0 then
 749  *  execute one instruction via ITR
 750  *
 751  *  The final executed instruction (\p opcode) should read data from r0.
 752  *
 753  * \pre arm11_run_instr_data_prepare() /  arm11_run_instr_data_finish() block
 754  *
 755  * \param arm11         Target state variable.
 756  * \param opcode        ARM opcode to read r0 act upon it
 757  * \param data          Data word that will be written to r0 before \p opcode is executed
 758  *
 759  */
 760 void arm11_run_instr_data_to_core_via_r0(arm11_common_t * arm11, uint32_t opcode, uint32_t data)
 761 {
 762         /* MRC p14,0,r0,c0,c5,0 */
 763         arm11_run_instr_data_to_core1(arm11, 0xEE100E15, data);
 764
 765         arm11_run_instr_no_data1(arm11, opcode);
 766 }
 767
 768 /** Apply reads and writes to scan chain 7
 769  *
 770  * \see arm11_sc7_action_t
 771  *
 772  * \param arm11         Target state variable.
 773  * \param actions       A list of read and/or write instructions
 774  * \param count         Number of instructions in the list.
 775  *
 776  */
 777 int arm11_sc7_run(arm11_common_t * arm11, arm11_sc7_action_t * actions, size_t count)
 778 {
 779         arm11_add_debug_SCAN_N(arm11, 0x07, ARM11_TAP_DEFAULT);
 780
 781         arm11_add_IR(arm11, ARM11_EXTEST, ARM11_TAP_DEFAULT);
 782
 783         scan_field_t    chain7_fields[3];
 784
 785         uint8_t                         nRW;
 786         uint32_t                                DataOut;
 787         uint8_t                         AddressOut;
 788         uint8_t                         Ready;
 789         uint32_t                                DataIn;
 790         uint8_t                         AddressIn;
 791
 792         arm11_setup_field(arm11,  1, &nRW,                      &Ready,         chain7_fields + 0);
 793         arm11_setup_field(arm11, 32, &DataOut,          &DataIn,        chain7_fields + 1);
 794         arm11_setup_field(arm11,  7, &AddressOut,       &AddressIn,     chain7_fields + 2);
 795
 796         for (size_t i = 0; i < count + 1; i++)
 797         {
 798                 if (i < count)
 799                 {
 800                         nRW                     = actions[i].write ? 1 : 0;
 801                         DataOut         = actions[i].value;
 802                         AddressOut      = actions[i].address;
 803                 }
 804                 else
 805                 {
 806                         nRW                     = 0;
 807                         DataOut         = 0;
 808                         AddressOut      = 0;
 809                 }
 810
 811                 do
 812                 {
 813                         JTAG_DEBUG("SC7 <= Address %02x  Data %08x    nRW %d", AddressOut, DataOut, nRW);
 814
 815                         arm11_add_dr_scan_vc(asizeof(chain7_fields), chain7_fields, TAP_DRPAUSE);
 816
 817                         CHECK_RETVAL(jtag_execute_queue());
 818
 819                         JTAG_DEBUG("SC7 => Address %02x  Data %08x  Ready %d", AddressIn, DataIn, Ready);
 820                 }
 821                 while (!Ready); /* 'nRW' is 'Ready' on read out */
 822
 823                 if (i > 0)
 824                 {
 825                         if (actions[i - 1].address != AddressIn)
 826                         {
 827                                 LOG_WARNING("Scan chain 7 shifted out unexpected address");
 828                         }
 829
 830                         if (!actions[i - 1].write)
 831                         {
 832                                 actions[i - 1].value = DataIn;
 833                         }
 834                         else
 835                         {
 836                                 if (actions[i - 1].value != DataIn)
 837                                 {
 838                                         LOG_WARNING("Scan chain 7 shifted out unexpected data");
 839                                 }
 840                         }
 841                 }
 842         }
 843
 844         for (size_t i = 0; i < count; i++)
 845         {
 846                 JTAG_DEBUG("SC7 %02d: %02x %s %08x", i, actions[i].address, actions[i].write ? "<=" : "=>", actions[i].value);
 847         }
 848
 849         return ERROR_OK;
 850 }
 851
 852 /** Clear VCR and all breakpoints and watchpoints via scan chain 7
 853  *
 854  * \param arm11         Target state variable.
 855  *
 856  */
 857 void arm11_sc7_clear_vbw(arm11_common_t * arm11)
 858 {
 859         arm11_sc7_action_t              clear_bw[arm11->brp + arm11->wrp + 1];
 860         arm11_sc7_action_t *    pos = clear_bw;
 861
 862         for (size_t i = 0; i < asizeof(clear_bw); i++)
 863         {
 864                 clear_bw[i].write       = true;
 865                 clear_bw[i].value       = 0;
 866         }
 867
 868         for (size_t i = 0; i < arm11->brp; i++)
 869                 (pos++)->address = ARM11_SC7_BCR0 + i;
 870
 871
 872         for (size_t i = 0; i < arm11->wrp; i++)
 873                 (pos++)->address = ARM11_SC7_WCR0 + i;
 874
 875
 876         (pos++)->address = ARM11_SC7_VCR;
 877
 878         arm11_sc7_run(arm11, clear_bw, asizeof(clear_bw));
 879 }
 880
 881 /** Write VCR register
 882  *
 883  * \param arm11         Target state variable.
 884  * \param value         Value to be written
 885  */
 886 void arm11_sc7_set_vcr(arm11_common_t * arm11, uint32_t value)
 887 {
 888         arm11_sc7_action_t              set_vcr;
 889
 890         set_vcr.write           = true;
 891         set_vcr.address         = ARM11_SC7_VCR;
 892         set_vcr.value           = value;
 893
 894
 895         arm11_sc7_run(arm11, &set_vcr, 1);
 896 }
 897
 898
 899
 900 /** Read word from address
 901  *
 902  * \param arm11         Target state variable.
 903  * \param address       Memory address to be read
 904  * \param result        Pointer where to store result
 905  *
 906  */
 907 int arm11_read_memory_word(arm11_common_t * arm11, uint32_t address, uint32_t * result)
 908 {
 909         arm11_run_instr_data_prepare(arm11);
 910
 911         /* MRC p14,0,r0,c0,c5,0 (r0 = address) */
 912         CHECK_RETVAL(arm11_run_instr_data_to_core1(arm11, 0xee100e15, address));
 913
 914         /* LDC p14,c5,[R0],#4 (DTR = [r0]) */
 915         CHECK_RETVAL(arm11_run_instr_data_from_core(arm11, 0xecb05e01, result, 1));
 916
 917         arm11_run_instr_data_finish(arm11);
 918
 919         return ERROR_OK;
 920 }
 921
 922