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 ...)    DEBUG(expr)
  34 #else
  35 #define JTAG_DEBUG(expr ...)    do {} 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", 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)", dscr, arm11->last_dscr);
 248
 249         arm11->last_dscr = dscr;
 250
 251         *value = dscr;
 252
 253         return ERROR_OK;
 254 }
 255
 256 /** Write the Debug Status and Control Register (DSCR)
 257  *
 258  * same as CP14 c1
 259  *
 260  * \param arm11         Target state variable.
 261  * \param dscr          DSCR content
 262  *
 263  * \remarks                     This is a stand-alone function that executes the JTAG command queue.
 264  */
 265 int arm11_write_DSCR(struct arm11_common * arm11, uint32_t dscr)
 266 {
 267         int retval;
 268         retval = arm11_add_debug_SCAN_N(arm11, 0x01, ARM11_TAP_DEFAULT);
 269         if (retval != ERROR_OK)
 270                 return retval;
 271
 272         arm11_add_IR(arm11, ARM11_EXTEST, ARM11_TAP_DEFAULT);
 273
 274         struct scan_field                   chain1_field;
 275
 276         arm11_setup_field(arm11, 32, &dscr, NULL, &chain1_field);
 277
 278         arm11_add_dr_scan_vc(1, &chain1_field, TAP_DRPAUSE);
 279
 280         CHECK_RETVAL(jtag_execute_queue());
 281
 282         JTAG_DEBUG("DSCR <= %08x (OLD %08x)", dscr, arm11->last_dscr);
 283
 284         arm11->last_dscr = dscr;
 285
 286         return ERROR_OK;
 287 }
 288
 289
 290
 291 /** Get the debug reason from Debug Status and Control Register (DSCR)
 292  *
 293  * \param dscr          DSCR value to analyze
 294  * \return                      Debug reason
 295  *
 296  */
 297 enum target_debug_reason arm11_get_DSCR_debug_reason(uint32_t dscr)
 298 {
 299         switch (dscr & ARM11_DSCR_METHOD_OF_DEBUG_ENTRY_MASK)
 300         {
 301         case ARM11_DSCR_METHOD_OF_DEBUG_ENTRY_HALT:
 302                 LOG_INFO("Debug entry: JTAG HALT");
 303                 return DBG_REASON_DBGRQ;
 304
 305         case ARM11_DSCR_METHOD_OF_DEBUG_ENTRY_BREAKPOINT:
 306                 LOG_INFO("Debug entry: breakpoint");
 307                 return DBG_REASON_BREAKPOINT;
 308
 309         case ARM11_DSCR_METHOD_OF_DEBUG_ENTRY_WATCHPOINT:
 310                 LOG_INFO("Debug entry: watchpoint");
 311                 return DBG_REASON_WATCHPOINT;
 312
 313         case ARM11_DSCR_METHOD_OF_DEBUG_ENTRY_BKPT_INSTRUCTION:
 314                 LOG_INFO("Debug entry: BKPT instruction");
 315                 return DBG_REASON_BREAKPOINT;
 316
 317         case ARM11_DSCR_METHOD_OF_DEBUG_ENTRY_EDBGRQ:
 318                 LOG_INFO("Debug entry: EDBGRQ signal");
 319                 return DBG_REASON_DBGRQ;
 320
 321         case ARM11_DSCR_METHOD_OF_DEBUG_ENTRY_VECTOR_CATCH:
 322                 LOG_INFO("Debug entry: VCR vector catch");
 323                 return DBG_REASON_BREAKPOINT;
 324
 325         default:
 326                 LOG_INFO("Debug entry: unknown");
 327                 return DBG_REASON_DBGRQ;
 328         }
 329 };
 330
 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 /** Execute one or multiple instructions via ITR
 374  *
 375  * \pre arm11_run_instr_data_prepare() /  arm11_run_instr_data_finish() block
 376  *
 377  * \param arm11         Target state variable.
 378  * \param opcode        Pointer to sequence of ARM opcodes
 379  * \param count         Number of opcodes to execute
 380  *
 381  */
 382 static
 383 int arm11_run_instr_no_data(struct arm11_common * arm11,
 384                 uint32_t * opcode, size_t count)
 385 {
 386         arm11_add_IR(arm11, ARM11_ITRSEL, ARM11_TAP_DEFAULT);
 387
 388         while (count--)
 389         {
 390                 arm11_add_debug_INST(arm11, *opcode++, NULL, TAP_IDLE);
 391
 392                 int i = 0;
 393                 while (1)
 394                 {
 395                         uint8_t flag;
 396
 397                         arm11_add_debug_INST(arm11, 0, &flag, count ? TAP_IDLE : TAP_DRPAUSE);
 398
 399                         CHECK_RETVAL(jtag_execute_queue());
 400
 401                         if (flag)
 402                                 break;
 403
 404                         long long then = 0;
 405
 406                         if (i == 1000)
 407                         {
 408                                 then = timeval_ms();
 409                         }
 410                         if (i >= 1000)
 411                         {
 412                                 if ((timeval_ms()-then) > 1000)
 413                                 {
 414                                         LOG_WARNING("Timeout (1000ms) waiting for instructions to complete");
 415                                         return ERROR_FAIL;
 416                                 }
 417                         }
 418
 419                         i++;
 420                 }
 421         }
 422
 423         return ERROR_OK;
 424 }
 425
 426 /** Execute one instruction via ITR
 427  *
 428  * \pre arm11_run_instr_data_prepare() /  arm11_run_instr_data_finish() block
 429  *
 430  * \param arm11         Target state variable.
 431  * \param opcode        ARM opcode
 432  *
 433  */
 434 int arm11_run_instr_no_data1(struct arm11_common * arm11, uint32_t opcode)
 435 {
 436         return arm11_run_instr_no_data(arm11, &opcode, 1);
 437 }
 438
 439
 440 /** Execute one instruction via ITR repeatedly while
 441  *  passing data to the core via DTR on each execution.
 442  *
 443  *  The executed instruction \em must read data from DTR.
 444  *
 445  * \pre arm11_run_instr_data_prepare() /  arm11_run_instr_data_finish() block
 446  *
 447  * \param arm11         Target state variable.
 448  * \param opcode        ARM opcode
 449  * \param data          Pointer to the data words to be passed to the core
 450  * \param count         Number of data words and instruction repetitions
 451  *
 452  */
 453 int arm11_run_instr_data_to_core(struct arm11_common * arm11, uint32_t opcode, uint32_t * data, size_t count)
 454 {
 455         arm11_add_IR(arm11, ARM11_ITRSEL, ARM11_TAP_DEFAULT);
 456
 457         arm11_add_debug_INST(arm11, opcode, NULL, TAP_DRPAUSE);
 458
 459         arm11_add_IR(arm11, ARM11_EXTEST, ARM11_TAP_DEFAULT);
 460
 461         struct scan_field       chain5_fields[3];
 462
 463         uint32_t                                Data;
 464         uint8_t                         Ready;
 465         uint8_t                         nRetry;
 466
 467         arm11_setup_field(arm11, 32,    &Data,  NULL,           chain5_fields + 0);
 468         arm11_setup_field(arm11,  1,    NULL,   &Ready,         chain5_fields + 1);
 469         arm11_setup_field(arm11,  1,    NULL,   &nRetry,        chain5_fields + 2);
 470
 471         while (count--)
 472         {
 473                 int i = 0;
 474                 do
 475                 {
 476                         Data        = *data;
 477
 478                         arm11_add_dr_scan_vc(ARRAY_SIZE(chain5_fields), chain5_fields, jtag_set_end_state(TAP_IDLE));
 479
 480                         CHECK_RETVAL(jtag_execute_queue());
 481
 482                         JTAG_DEBUG("DTR  Ready %d  nRetry %d", Ready, nRetry);
 483
 484                         long long then = 0;
 485
 486                         if (i == 1000)
 487                         {
 488                                 then = timeval_ms();
 489                         }
 490                         if (i >= 1000)
 491                         {
 492                                 if ((timeval_ms()-then) > 1000)
 493                                 {
 494                                         LOG_WARNING("Timeout (1000ms) waiting for instructions to complete");
 495                                         return ERROR_FAIL;
 496                                 }
 497                         }
 498
 499                         i++;
 500                 }
 501                 while (!Ready);
 502
 503                 data++;
 504         }
 505
 506         arm11_add_IR(arm11, ARM11_INTEST, ARM11_TAP_DEFAULT);
 507
 508         int i = 0;
 509         do
 510         {
 511                 Data        = 0;
 512
 513                 arm11_add_dr_scan_vc(ARRAY_SIZE(chain5_fields), chain5_fields, TAP_DRPAUSE);
 514
 515                 CHECK_RETVAL(jtag_execute_queue());
 516
 517                 JTAG_DEBUG("DTR  Data %08x  Ready %d  nRetry %d", Data, Ready, nRetry);
 518
 519                 long long then = 0;
 520
 521                 if (i == 1000)
 522                 {
 523                         then = timeval_ms();
 524                 }
 525                 if (i >= 1000)
 526                 {
 527                         if ((timeval_ms()-then) > 1000)
 528                         {
 529                                 LOG_WARNING("Timeout (1000ms) waiting for instructions to complete");
 530                                 return ERROR_FAIL;
 531                         }
 532                 }
 533
 534                 i++;
 535         }
 536         while (!Ready);
 537
 538         return ERROR_OK;
 539 }
 540
 541 /** JTAG path for arm11_run_instr_data_to_core_noack
 542  *
 543  *  The repeated TAP_IDLE's do not cause a repeated execution
 544  *  if passed without leaving the state.
 545  *
 546  *  Since this is more than 7 bits (adjustable via adding more
 547  *  TAP_IDLE's) it produces an artificial delay in the lower
 548  *  layer (FT2232) that is long enough to finish execution on
 549  *  the core but still shorter than any manually inducible delays.
 550  *
 551  *  To disable this code, try "memwrite burst false"
 552  *
 553  *  FIX!!! should we use multiple TAP_IDLE here or not???
 554  *
 555  *  https://lists.berlios.de/pipermail/openocd-development/2009-July/009698.html
 556  *  https://lists.berlios.de/pipermail/openocd-development/2009-August/009865.html
 557  */
 558 static const tap_state_t arm11_MOVE_DRPAUSE_IDLE_DRPAUSE_with_delay[] =
 559 {
 560         TAP_DREXIT2, TAP_DRUPDATE, TAP_IDLE, TAP_IDLE, TAP_IDLE, TAP_DRSELECT, TAP_DRCAPTURE, TAP_DRSHIFT
 561 };
 562
 563
 564
 565 /** Execute one instruction via ITR repeatedly while
 566  *  passing data to the core via DTR on each execution.
 567  *
 568  *  No Ready check during transmission.
 569  *
 570  *  The executed instruction \em must read data from DTR.
 571  *
 572  * \pre arm11_run_instr_data_prepare() /  arm11_run_instr_data_finish() block
 573  *
 574  * \param arm11         Target state variable.
 575  * \param opcode        ARM opcode
 576  * \param data          Pointer to the data words to be passed to the core
 577  * \param count         Number of data words and instruction repetitions
 578  *
 579  */
 580 int arm11_run_instr_data_to_core_noack(struct arm11_common * arm11, uint32_t opcode, uint32_t * data, size_t count)
 581 {
 582         arm11_add_IR(arm11, ARM11_ITRSEL, ARM11_TAP_DEFAULT);
 583
 584         arm11_add_debug_INST(arm11, opcode, NULL, TAP_DRPAUSE);
 585
 586         arm11_add_IR(arm11, ARM11_EXTEST, ARM11_TAP_DEFAULT);
 587
 588         struct scan_field       chain5_fields[3];
 589
 590         arm11_setup_field(arm11, 32,    NULL/*&Data*/,  NULL,                           chain5_fields + 0);
 591         arm11_setup_field(arm11,  1,    NULL,                   NULL /*&Ready*/,        chain5_fields + 1);
 592         arm11_setup_field(arm11,  1,    NULL,                   NULL,                           chain5_fields + 2);
 593
 594         uint8_t                 *Readies;
 595         unsigned readiesNum = count + 1;
 596         unsigned bytes = sizeof(*Readies)*readiesNum;
 597
 598         Readies = (uint8_t *) malloc(bytes);
 599         if (Readies == NULL)
 600         {
 601                 LOG_ERROR("Out of memory allocating %u bytes", bytes);
 602                 return ERROR_FAIL;
 603         }
 604
 605         uint8_t *               ReadyPos                        = Readies;
 606
 607         while (count--)
 608         {
 609                 chain5_fields[0].out_value      = (void *)(data++);
 610                 chain5_fields[1].in_value       = ReadyPos++;
 611
 612                 if (count)
 613                 {
 614                         jtag_add_dr_scan(ARRAY_SIZE(chain5_fields), chain5_fields, jtag_set_end_state(TAP_DRPAUSE));
 615                         jtag_add_pathmove(ARRAY_SIZE(arm11_MOVE_DRPAUSE_IDLE_DRPAUSE_with_delay),
 616                                 arm11_MOVE_DRPAUSE_IDLE_DRPAUSE_with_delay);
 617                 }
 618                 else
 619                 {
 620                         jtag_add_dr_scan(ARRAY_SIZE(chain5_fields), chain5_fields, jtag_set_end_state(TAP_IDLE));
 621                 }
 622         }
 623
 624         arm11_add_IR(arm11, ARM11_INTEST, ARM11_TAP_DEFAULT);
 625
 626         chain5_fields[0].out_value      = 0;
 627         chain5_fields[1].in_value   = ReadyPos++;
 628
 629         arm11_add_dr_scan_vc(ARRAY_SIZE(chain5_fields), chain5_fields, TAP_DRPAUSE);
 630
 631         int retval = jtag_execute_queue();
 632         if (retval == ERROR_OK)
 633         {
 634                 unsigned error_count = 0;
 635
 636                 for (size_t i = 0; i < readiesNum; i++)
 637                 {
 638                         if (Readies[i] != 1)
 639                         {
 640                                 error_count++;
 641                         }
 642                 }
 643
 644                 if (error_count > 0 )
 645                         LOG_ERROR("%u words out of %u not transferred",
 646                                 error_count, readiesNum);
 647
 648         }
 649
 650         free(Readies);
 651
 652         return retval;
 653 }
 654
 655
 656 /** Execute an instruction via ITR while handing data into the core via DTR.
 657  *
 658  *  The executed instruction \em must read data from DTR.
 659  *
 660  * \pre arm11_run_instr_data_prepare() /  arm11_run_instr_data_finish() block
 661  *
 662  * \param arm11         Target state variable.
 663  * \param opcode        ARM opcode
 664  * \param data          Data word to be passed to the core via DTR
 665  *
 666  */
 667 int arm11_run_instr_data_to_core1(struct arm11_common * arm11, uint32_t opcode, uint32_t data)
 668 {
 669         return arm11_run_instr_data_to_core(arm11, opcode, &data, 1);
 670 }
 671
 672
 673 /** Execute one instruction via ITR repeatedly while
 674  *  reading data from the core via DTR on each execution.
 675  *
 676  *  The executed instruction \em must write data to DTR.
 677  *
 678  * \pre arm11_run_instr_data_prepare() /  arm11_run_instr_data_finish() block
 679  *
 680  * \param arm11         Target state variable.
 681  * \param opcode        ARM opcode
 682  * \param data          Pointer to an array that receives the data words from the core
 683  * \param count         Number of data words and instruction repetitions
 684  *
 685  */
 686 int arm11_run_instr_data_from_core(struct arm11_common * arm11, uint32_t opcode, uint32_t * data, size_t count)
 687 {
 688         arm11_add_IR(arm11, ARM11_ITRSEL, ARM11_TAP_DEFAULT);
 689
 690         arm11_add_debug_INST(arm11, opcode, NULL, TAP_IDLE);
 691
 692         arm11_add_IR(arm11, ARM11_INTEST, ARM11_TAP_DEFAULT);
 693
 694         struct scan_field       chain5_fields[3];
 695
 696         uint32_t                        Data;
 697         uint8_t                 Ready;
 698         uint8_t                 nRetry;
 699
 700         arm11_setup_field(arm11, 32,    NULL,   &Data,      chain5_fields + 0);
 701         arm11_setup_field(arm11,  1,    NULL,   &Ready,     chain5_fields + 1);
 702         arm11_setup_field(arm11,  1,    NULL,   &nRetry,    chain5_fields + 2);
 703
 704         while (count--)
 705         {
 706                 int i = 0;
 707                 do
 708                 {
 709                         arm11_add_dr_scan_vc(ARRAY_SIZE(chain5_fields), chain5_fields, count ? TAP_IDLE : TAP_DRPAUSE);
 710
 711                         CHECK_RETVAL(jtag_execute_queue());
 712
 713                         JTAG_DEBUG("DTR  Data %08x  Ready %d  nRetry %d", Data, Ready, nRetry);
 714
 715                         long long then = 0;
 716
 717                         if (i == 1000)
 718                         {
 719                                 then = timeval_ms();
 720                         }
 721                         if (i >= 1000)
 722                         {
 723                                 if ((timeval_ms()-then) > 1000)
 724                                 {
 725                                         LOG_WARNING("Timeout (1000ms) waiting for instructions to complete");
 726                                         return ERROR_FAIL;
 727                                 }
 728                         }
 729
 730                         i++;
 731                 }
 732                 while (!Ready);
 733
 734                 *data++ = Data;
 735         }
 736
 737         return ERROR_OK;
 738 }
 739
 740 /** Execute one instruction via ITR
 741  *  then load r0 into DTR and read DTR from core.
 742  *
 743  *  The first executed instruction (\p opcode) should write data to r0.
 744  *
 745  * \pre arm11_run_instr_data_prepare() /  arm11_run_instr_data_finish() block
 746  *
 747  * \param arm11         Target state variable.
 748  * \param opcode        ARM opcode to write r0 with the value of interest
 749  * \param data          Pointer to a data word that receives the value from r0 after \p opcode was executed.
 750  *
 751  */
 752 int arm11_run_instr_data_from_core_via_r0(struct arm11_common * arm11, uint32_t opcode, uint32_t * data)
 753 {
 754         int retval;
 755         retval = arm11_run_instr_no_data1(arm11, opcode);
 756         if (retval != ERROR_OK)
 757                 return retval;
 758
 759         /* MCR p14,0,R0,c0,c5,0 (move r0 -> wDTR -> local var) */
 760         arm11_run_instr_data_from_core(arm11, 0xEE000E15, data, 1);
 761
 762         return ERROR_OK;
 763 }
 764
 765 /** Load data into core via DTR then move it to r0 then
 766  *  execute one instruction via ITR
 767  *
 768  *  The final executed instruction (\p opcode) should read data from r0.
 769  *
 770  * \pre arm11_run_instr_data_prepare() /  arm11_run_instr_data_finish() block
 771  *
 772  * \param arm11         Target state variable.
 773  * \param opcode        ARM opcode to read r0 act upon it
 774  * \param data          Data word that will be written to r0 before \p opcode is executed
 775  *
 776  */
 777 int arm11_run_instr_data_to_core_via_r0(struct arm11_common * arm11, uint32_t opcode, uint32_t data)
 778 {
 779         int retval;
 780         /* MRC p14,0,r0,c0,c5,0 */
 781         retval = arm11_run_instr_data_to_core1(arm11, 0xEE100E15, data);
 782         if (retval != ERROR_OK)
 783                 return retval;
 784
 785         retval = arm11_run_instr_no_data1(arm11, opcode);
 786         if (retval != ERROR_OK)
 787                 return retval;
 788
 789         return ERROR_OK;
 790 }
 791
 792 /** Apply reads and writes to scan chain 7
 793  *
 794  * \see struct arm11_sc7_action
 795  *
 796  * \param arm11         Target state variable.
 797  * \param actions       A list of read and/or write instructions
 798  * \param count         Number of instructions in the list.
 799  *
 800  */
 801 int arm11_sc7_run(struct arm11_common * arm11, struct arm11_sc7_action * actions, size_t count)
 802 {
 803         int retval;
 804
 805         retval = arm11_add_debug_SCAN_N(arm11, 0x07, ARM11_TAP_DEFAULT);
 806         if (retval != ERROR_OK)
 807                 return retval;
 808
 809         arm11_add_IR(arm11, ARM11_EXTEST, ARM11_TAP_DEFAULT);
 810
 811         struct scan_field       chain7_fields[3];
 812
 813         uint8_t                         nRW;
 814         uint32_t                                DataOut;
 815         uint8_t                         AddressOut;
 816         uint8_t                         Ready;
 817         uint32_t                                DataIn;
 818         uint8_t                         AddressIn;
 819
 820         arm11_setup_field(arm11,  1, &nRW,                      &Ready,         chain7_fields + 0);
 821         arm11_setup_field(arm11, 32, &DataOut,          &DataIn,        chain7_fields + 1);
 822         arm11_setup_field(arm11,  7, &AddressOut,       &AddressIn,     chain7_fields + 2);
 823
 824         for (size_t i = 0; i < count + 1; i++)
 825         {
 826                 if (i < count)
 827                 {
 828                         nRW                     = actions[i].write ? 1 : 0;
 829                         DataOut         = actions[i].value;
 830                         AddressOut      = actions[i].address;
 831                 }
 832                 else
 833                 {
 834                         nRW                     = 0;
 835                         DataOut         = 0;
 836                         AddressOut      = 0;
 837                 }
 838
 839                 do
 840                 {
 841                         JTAG_DEBUG("SC7 <= Address %02x  Data %08x    nRW %d", AddressOut, DataOut, nRW);
 842
 843                         arm11_add_dr_scan_vc(ARRAY_SIZE(chain7_fields), chain7_fields, TAP_DRPAUSE);
 844
 845                         CHECK_RETVAL(jtag_execute_queue());
 846
 847                         JTAG_DEBUG("SC7 => Address %02x  Data %08x  Ready %d", AddressIn, DataIn, Ready);
 848                 }
 849                 while (!Ready); /* 'nRW' is 'Ready' on read out */
 850
 851                 if (i > 0)
 852                 {
 853                         if (actions[i - 1].address != AddressIn)
 854                         {
 855                                 LOG_WARNING("Scan chain 7 shifted out unexpected address");
 856                         }
 857
 858                         if (!actions[i - 1].write)
 859                         {
 860                                 actions[i - 1].value = DataIn;
 861                         }
 862                         else
 863                         {
 864                                 if (actions[i - 1].value != DataIn)
 865                                 {
 866                                         LOG_WARNING("Scan chain 7 shifted out unexpected data");
 867                                 }
 868                         }
 869                 }
 870         }
 871
 872         for (size_t i = 0; i < count; i++)
 873         {
 874                 JTAG_DEBUG("SC7 %02d: %02x %s %08x", i, actions[i].address, actions[i].write ? "<=" : "=>", actions[i].value);
 875         }
 876
 877         return ERROR_OK;
 878 }
 879
 880 /** Clear VCR and all breakpoints and watchpoints via scan chain 7
 881  *
 882  * \param arm11         Target state variable.
 883  *
 884  */
 885 void arm11_sc7_clear_vbw(struct arm11_common * arm11)
 886 {
 887         size_t clear_bw_size = arm11->brp + arm11->wrp + 1;
 888         struct arm11_sc7_action         *clear_bw = malloc(sizeof(struct arm11_sc7_action) * clear_bw_size);
 889         struct arm11_sc7_action *       pos = clear_bw;
 890
 891         for (size_t i = 0; i < clear_bw_size; i++)
 892         {
 893                 clear_bw[i].write       = true;
 894                 clear_bw[i].value       = 0;
 895         }
 896
 897         for (size_t i = 0; i < arm11->brp; i++)
 898                 (pos++)->address = ARM11_SC7_BCR0 + i;
 899
 900
 901         for (size_t i = 0; i < arm11->wrp; i++)
 902                 (pos++)->address = ARM11_SC7_WCR0 + i;
 903
 904
 905         (pos++)->address = ARM11_SC7_VCR;
 906
 907         arm11_sc7_run(arm11, clear_bw, clear_bw_size);
 908
 909         free (clear_bw);
 910 }
 911
 912 /** Write VCR register
 913  *
 914  * \param arm11         Target state variable.
 915  * \param value         Value to be written
 916  */
 917 void arm11_sc7_set_vcr(struct arm11_common * arm11, uint32_t value)
 918 {
 919         struct arm11_sc7_action         set_vcr;
 920
 921         set_vcr.write           = true;
 922         set_vcr.address         = ARM11_SC7_VCR;
 923         set_vcr.value           = value;
 924
 925
 926         arm11_sc7_run(arm11, &set_vcr, 1);
 927 }
 928
 929
 930
 931 /** Read word from address
 932  *
 933  * \param arm11         Target state variable.
 934  * \param address       Memory address to be read
 935  * \param result        Pointer where to store result
 936  *
 937  */
 938 int arm11_read_memory_word(struct arm11_common * arm11, uint32_t address, uint32_t * result)
 939 {
 940         int retval;
 941         retval = arm11_run_instr_data_prepare(arm11);
 942         if (retval != ERROR_OK)
 943                 return retval;
 944
 945         /* MRC p14,0,r0,c0,c5,0 (r0 = address) */
 946         CHECK_RETVAL(arm11_run_instr_data_to_core1(arm11, 0xee100e15, address));
 947
 948         /* LDC p14,c5,[R0],#4 (DTR = [r0]) */
 949         CHECK_RETVAL(arm11_run_instr_data_from_core(arm11, 0xecb05e01, result, 1));
 950
 951         return arm11_run_instr_data_finish(arm11);
 952 }
 953