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_dbgtap.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 static const 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 static const 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         struct jtag_tap *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 int 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         return jtag_execute_queue();
 182 }
 183
 184 /** Write an instruction into the ITR register
 185  *
 186  * \param arm11         Target state variable.
 187  * \param inst          An ARM11 processor instruction/opcode.
 188  * \param flag          Optional parameter to retrieve the InstCompl flag
 189  *                                      (this will be written when the JTAG chain is executed).
 190  * \param state         Pass the final TAP state or ARM11_TAP_DEFAULT for the default
 191  *                                      value (Run-Test/Idle).
 192  *
 193  * \remarks                     By default this ends with Run-Test/Idle state
 194  *                                      and causes the instruction to be executed. If
 195  *                                      a subsequent write to DTR is needed before
 196  *                                      executing the instruction then TAP_DRPAUSE should be
 197  *                                      passed to \p state.
 198  *
 199  * \remarks                     This adds to the JTAG command queue but does \em not execute it.
 200  */
 201 void arm11_add_debug_INST(arm11_common_t * arm11, uint32_t inst, uint8_t * flag, tap_state_t state)
 202 {
 203         JTAG_DEBUG("INST <= 0x%08x", inst);
 204
 205         scan_field_t            itr[2];
 206
 207         arm11_setup_field(arm11, 32,    &inst,  NULL, itr + 0);
 208         arm11_setup_field(arm11, 1,         NULL,       flag, itr + 1);
 209
 210         arm11_add_dr_scan_vc(asizeof(itr), itr, state == ARM11_TAP_DEFAULT ? TAP_IDLE : state);
 211 }
 212
 213 /** Read the Debug Status and Control Register (DSCR)
 214  *
 215  * same as CP14 c1
 216  *
 217  * \param arm11         Target state variable.
 218  * \param value         DSCR content
 219  * \return                      Error status
 220  *
 221  * \remarks                     This is a stand-alone function that executes the JTAG command queue.
 222  */
 223 int arm11_read_DSCR(arm11_common_t * arm11, uint32_t *value)
 224 {
 225         int retval;
 226         retval = arm11_add_debug_SCAN_N(arm11, 0x01, ARM11_TAP_DEFAULT);
 227         if (retval != ERROR_OK)
 228                 return retval;
 229
 230         arm11_add_IR(arm11, ARM11_INTEST, ARM11_TAP_DEFAULT);
 231
 232         uint32_t                                dscr;
 233         scan_field_t    chain1_field;
 234
 235         arm11_setup_field(arm11, 32, NULL, &dscr, &chain1_field);
 236
 237         arm11_add_dr_scan_vc(1, &chain1_field, TAP_DRPAUSE);
 238
 239         CHECK_RETVAL(jtag_execute_queue());
 240
 241         if (arm11->last_dscr != dscr)
 242                 JTAG_DEBUG("DSCR  = %08x (OLD %08x)", dscr, arm11->last_dscr);
 243
 244         arm11->last_dscr = dscr;
 245
 246         *value = dscr;
 247
 248         return ERROR_OK;
 249 }
 250
 251 /** Write the Debug Status and Control Register (DSCR)
 252  *
 253  * same as CP14 c1
 254  *
 255  * \param arm11         Target state variable.
 256  * \param dscr          DSCR content
 257  *
 258  * \remarks                     This is a stand-alone function that executes the JTAG command queue.
 259  */
 260 int arm11_write_DSCR(arm11_common_t * arm11, uint32_t dscr)
 261 {
 262         int retval;
 263         retval = arm11_add_debug_SCAN_N(arm11, 0x01, ARM11_TAP_DEFAULT);
 264         if (retval != ERROR_OK)
 265                 return retval;
 266
 267         arm11_add_IR(arm11, ARM11_EXTEST, ARM11_TAP_DEFAULT);
 268
 269         scan_field_t                chain1_field;
 270
 271         arm11_setup_field(arm11, 32, &dscr, NULL, &chain1_field);
 272
 273         arm11_add_dr_scan_vc(1, &chain1_field, TAP_DRPAUSE);
 274
 275         CHECK_RETVAL(jtag_execute_queue());
 276
 277         JTAG_DEBUG("DSCR <= %08x (OLD %08x)", dscr, arm11->last_dscr);
 278
 279         arm11->last_dscr = dscr;
 280
 281         return ERROR_OK;
 282 }
 283
 284
 285
 286 /** Get the debug reason from Debug Status and Control Register (DSCR)
 287  *
 288  * \param dscr          DSCR value to analyze
 289  * \return                      Debug reason
 290  *
 291  */
 292 enum target_debug_reason arm11_get_DSCR_debug_reason(uint32_t dscr)
 293 {
 294         switch (dscr & ARM11_DSCR_METHOD_OF_DEBUG_ENTRY_MASK)
 295         {
 296         case ARM11_DSCR_METHOD_OF_DEBUG_ENTRY_HALT:
 297                 LOG_INFO("Debug entry: JTAG HALT");
 298                 return DBG_REASON_DBGRQ;
 299
 300         case ARM11_DSCR_METHOD_OF_DEBUG_ENTRY_BREAKPOINT:
 301                 LOG_INFO("Debug entry: breakpoint");
 302                 return DBG_REASON_BREAKPOINT;
 303
 304         case ARM11_DSCR_METHOD_OF_DEBUG_ENTRY_WATCHPOINT:
 305                 LOG_INFO("Debug entry: watchpoint");
 306                 return DBG_REASON_WATCHPOINT;
 307
 308         case ARM11_DSCR_METHOD_OF_DEBUG_ENTRY_BKPT_INSTRUCTION:
 309                 LOG_INFO("Debug entry: BKPT instruction");
 310                 return DBG_REASON_BREAKPOINT;
 311
 312         case ARM11_DSCR_METHOD_OF_DEBUG_ENTRY_EDBGRQ:
 313                 LOG_INFO("Debug entry: EDBGRQ signal");
 314                 return DBG_REASON_DBGRQ;
 315
 316         case ARM11_DSCR_METHOD_OF_DEBUG_ENTRY_VECTOR_CATCH:
 317                 LOG_INFO("Debug entry: VCR vector catch");
 318                 return DBG_REASON_BREAKPOINT;
 319
 320         default:
 321                 LOG_INFO("Debug entry: unknown");
 322                 return DBG_REASON_DBGRQ;
 323         }
 324 };
 325
 326
 327
 328 /** Prepare the stage for ITR/DTR operations
 329  * from the arm11_run_instr... group of functions.
 330  *
 331  * Put arm11_run_instr_data_prepare() and arm11_run_instr_data_finish()
 332  * around a block of arm11_run_instr_... calls.
 333  *
 334  * Select scan chain 5 to allow quick access to DTR. When scan
 335  * chain 4 is needed to put in a register the ITRSel instruction
 336  * shortcut is used instead of actually changing the Scan_N
 337  * register.
 338  *
 339  * \param arm11         Target state variable.
 340  *
 341  */
 342 int arm11_run_instr_data_prepare(arm11_common_t * arm11)
 343 {
 344         return arm11_add_debug_SCAN_N(arm11, 0x05, ARM11_TAP_DEFAULT);
 345 }
 346
 347 /** Cleanup after ITR/DTR operations
 348  * from the arm11_run_instr... group of functions
 349  *
 350  * Put arm11_run_instr_data_prepare() and arm11_run_instr_data_finish()
 351  * around a block of arm11_run_instr_... calls.
 352  *
 353  * Any IDLE can lead to an instruction execution when
 354  * scan chains 4 or 5 are selected and the IR holds
 355  * INTEST or EXTEST. So we must disable that before
 356  * any following activities lead to an IDLE.
 357  *
 358  * \param arm11         Target state variable.
 359  *
 360  */
 361 int arm11_run_instr_data_finish(arm11_common_t * arm11)
 362 {
 363         return arm11_add_debug_SCAN_N(arm11, 0x00, ARM11_TAP_DEFAULT);
 364 }
 365
 366
 367
 368 /** Execute one or multiple instructions via ITR
 369  *
 370  * \pre arm11_run_instr_data_prepare() /  arm11_run_instr_data_finish() block
 371  *
 372  * \param arm11         Target state variable.
 373  * \param opcode        Pointer to sequence of ARM opcodes
 374  * \param count         Number of opcodes to execute
 375  *
 376  */
 377 int arm11_run_instr_no_data(arm11_common_t * arm11, uint32_t * opcode, size_t count)
 378 {
 379         arm11_add_IR(arm11, ARM11_ITRSEL, ARM11_TAP_DEFAULT);
 380
 381         while (count--)
 382         {
 383                 arm11_add_debug_INST(arm11, *opcode++, NULL, TAP_IDLE);
 384
 385                 int i = 0;
 386                 while (1)
 387                 {
 388                         uint8_t flag;
 389
 390                         arm11_add_debug_INST(arm11, 0, &flag, count ? TAP_IDLE : TAP_DRPAUSE);
 391
 392                         CHECK_RETVAL(jtag_execute_queue());
 393
 394                         if (flag)
 395                                 break;
 396
 397                         long long then = 0;
 398
 399                         if (i == 1000)
 400                         {
 401                                 then = timeval_ms();
 402                         }
 403                         if (i >= 1000)
 404                         {
 405                                 if ((timeval_ms()-then) > 1000)
 406                                 {
 407                                         LOG_WARNING("Timeout (1000ms) waiting for instructions to complete");
 408                                         return ERROR_FAIL;
 409                                 }
 410                         }
 411
 412                         i++;
 413                 }
 414         }
 415
 416         return ERROR_OK;
 417 }
 418
 419 /** Execute one instruction via ITR
 420  *
 421  * \pre arm11_run_instr_data_prepare() /  arm11_run_instr_data_finish() block
 422  *
 423  * \param arm11         Target state variable.
 424  * \param opcode        ARM opcode
 425  *
 426  */
 427 int arm11_run_instr_no_data1(arm11_common_t * arm11, uint32_t opcode)
 428 {
 429         return arm11_run_instr_no_data(arm11, &opcode, 1);
 430 }
 431
 432
 433 /** Execute one instruction via ITR repeatedly while
 434  *  passing data to the core via DTR on each execution.
 435  *
 436  *  The executed instruction \em must read data from DTR.
 437  *
 438  * \pre arm11_run_instr_data_prepare() /  arm11_run_instr_data_finish() block
 439  *
 440  * \param arm11         Target state variable.
 441  * \param opcode        ARM opcode
 442  * \param data          Pointer to the data words to be passed to the core
 443  * \param count         Number of data words and instruction repetitions
 444  *
 445  */
 446 int arm11_run_instr_data_to_core(arm11_common_t * arm11, uint32_t opcode, uint32_t * data, size_t count)
 447 {
 448         arm11_add_IR(arm11, ARM11_ITRSEL, ARM11_TAP_DEFAULT);
 449
 450         arm11_add_debug_INST(arm11, opcode, NULL, TAP_DRPAUSE);
 451
 452         arm11_add_IR(arm11, ARM11_EXTEST, ARM11_TAP_DEFAULT);
 453
 454         scan_field_t    chain5_fields[3];
 455
 456         uint32_t                                Data;
 457         uint8_t                         Ready;
 458         uint8_t                         nRetry;
 459
 460         arm11_setup_field(arm11, 32,    &Data,  NULL,           chain5_fields + 0);
 461         arm11_setup_field(arm11,  1,    NULL,   &Ready,         chain5_fields + 1);
 462         arm11_setup_field(arm11,  1,    NULL,   &nRetry,        chain5_fields + 2);
 463
 464         while (count--)
 465         {
 466                 int i = 0;
 467                 do
 468                 {
 469                         Data        = *data;
 470
 471                         arm11_add_dr_scan_vc(asizeof(chain5_fields), chain5_fields, jtag_set_end_state(TAP_IDLE));
 472
 473                         CHECK_RETVAL(jtag_execute_queue());
 474
 475                         JTAG_DEBUG("DTR  Ready %d  nRetry %d", Ready, nRetry);
 476
 477                         long long then = 0;
 478
 479                         if (i == 1000)
 480                         {
 481                                 then = timeval_ms();
 482                         }
 483                         if (i >= 1000)
 484                         {
 485                                 if ((timeval_ms()-then) > 1000)
 486                                 {
 487                                         LOG_WARNING("Timeout (1000ms) waiting for instructions to complete");
 488                                         return ERROR_FAIL;
 489                                 }
 490                         }
 491
 492                         i++;
 493                 }
 494                 while (!Ready);
 495
 496                 data++;
 497         }
 498
 499         arm11_add_IR(arm11, ARM11_INTEST, ARM11_TAP_DEFAULT);
 500
 501         int i = 0;
 502         do
 503         {
 504                 Data        = 0;
 505
 506                 arm11_add_dr_scan_vc(asizeof(chain5_fields), chain5_fields, TAP_DRPAUSE);
 507
 508                 CHECK_RETVAL(jtag_execute_queue());
 509
 510                 JTAG_DEBUG("DTR  Data %08x  Ready %d  nRetry %d", Data, Ready, nRetry);
 511
 512                 long long then = 0;
 513
 514                 if (i == 1000)
 515                 {
 516                         then = timeval_ms();
 517                 }
 518                 if (i >= 1000)
 519                 {
 520                         if ((timeval_ms()-then) > 1000)
 521                         {
 522                                 LOG_WARNING("Timeout (1000ms) waiting for instructions to complete");
 523                                 return ERROR_FAIL;
 524                         }
 525                 }
 526
 527                 i++;
 528         }
 529         while (!Ready);
 530
 531         return ERROR_OK;
 532 }
 533
 534 /** JTAG path for arm11_run_instr_data_to_core_noack
 535  *
 536  *  The repeated TAP_IDLE's do not cause a repeated execution
 537  *  if passed without leaving the state.
 538  *
 539  *  Since this is more than 7 bits (adjustable via adding more
 540  *  TAP_IDLE's) it produces an artificial delay in the lower
 541  *  layer (FT2232) that is long enough to finish execution on
 542  *  the core but still shorter than any manually inducible delays.
 543  *
 544  *  To disable this code, try "memwrite burst false"
 545  *
 546  *  FIX!!! should we use multiple TAP_IDLE here or not???
 547  *
 548  *  https://lists.berlios.de/pipermail/openocd-development/2009-July/009698.html
 549  *  https://lists.berlios.de/pipermail/openocd-development/2009-August/009865.html
 550  */
 551 static const tap_state_t arm11_MOVE_DRPAUSE_IDLE_DRPAUSE_with_delay[] =
 552 {
 553         TAP_DREXIT2, TAP_DRUPDATE, TAP_IDLE, TAP_IDLE, TAP_IDLE, TAP_DRSELECT, TAP_DRCAPTURE, TAP_DRSHIFT
 554 };
 555
 556
 557
 558 /** Execute one instruction via ITR repeatedly while
 559  *  passing data to the core via DTR on each execution.
 560  *
 561  *  No Ready check during transmission.
 562  *
 563  *  The executed instruction \em must read data from DTR.
 564  *
 565  * \pre arm11_run_instr_data_prepare() /  arm11_run_instr_data_finish() block
 566  *
 567  * \param arm11         Target state variable.
 568  * \param opcode        ARM opcode
 569  * \param data          Pointer to the data words to be passed to the core
 570  * \param count         Number of data words and instruction repetitions
 571  *
 572  */
 573 int arm11_run_instr_data_to_core_noack(arm11_common_t * arm11, uint32_t opcode, uint32_t * data, size_t count)
 574 {
 575         arm11_add_IR(arm11, ARM11_ITRSEL, ARM11_TAP_DEFAULT);
 576
 577         arm11_add_debug_INST(arm11, opcode, NULL, TAP_DRPAUSE);
 578
 579         arm11_add_IR(arm11, ARM11_EXTEST, ARM11_TAP_DEFAULT);
 580
 581         scan_field_t    chain5_fields[3];
 582
 583         arm11_setup_field(arm11, 32,    NULL/*&Data*/,  NULL,                           chain5_fields + 0);
 584         arm11_setup_field(arm11,  1,    NULL,                   NULL /*&Ready*/,        chain5_fields + 1);
 585         arm11_setup_field(arm11,  1,    NULL,                   NULL,                           chain5_fields + 2);
 586
 587         uint8_t                 *Readies;
 588         size_t readiesNum = (count + 1);
 589         size_t bytes = sizeof(*Readies)*readiesNum;
 590         Readies = (uint8_t *) malloc(bytes);
 591         if (Readies == NULL)
 592         {
 593                 LOG_ERROR("Out of memory allocating " ZU " bytes", bytes);
 594                 return ERROR_FAIL;
 595         }
 596
 597         uint8_t *               ReadyPos                        = Readies;
 598
 599         while (count--)
 600         {
 601                 chain5_fields[0].out_value      = (void *)(data++);
 602                 chain5_fields[1].in_value       = ReadyPos++;
 603
 604                 if (count)
 605                 {
 606                         jtag_add_dr_scan(asizeof(chain5_fields), chain5_fields, jtag_set_end_state(TAP_DRPAUSE));
 607                         jtag_add_pathmove(asizeof(arm11_MOVE_DRPAUSE_IDLE_DRPAUSE_with_delay),
 608                                 arm11_MOVE_DRPAUSE_IDLE_DRPAUSE_with_delay);
 609                 }
 610                 else
 611                 {
 612                         jtag_add_dr_scan(asizeof(chain5_fields), chain5_fields, jtag_set_end_state(TAP_IDLE));
 613                 }
 614         }
 615
 616         arm11_add_IR(arm11, ARM11_INTEST, ARM11_TAP_DEFAULT);
 617
 618         chain5_fields[0].out_value      = 0;
 619         chain5_fields[1].in_value   = ReadyPos++;
 620
 621         arm11_add_dr_scan_vc(asizeof(chain5_fields), chain5_fields, TAP_DRPAUSE);
 622
 623         int retval = jtag_execute_queue();
 624         if (retval == ERROR_OK)
 625         {
 626                 size_t error_count = 0;
 627
 628                 for (size_t i = 0; i < readiesNum; i++)
 629                 {
 630                         if (Readies[i] != 1)
 631                         {
 632                                 error_count++;
 633                         }
 634                 }
 635
 636                 if (error_count > 0 )
 637                         LOG_ERROR(ZU " words out of " ZU " not transferred", error_count, readiesNum);
 638
 639         }
 640
 641         free(Readies);
 642
 643         return retval;
 644 }
 645
 646
 647 /** Execute an instruction via ITR while handing data into the core via DTR.
 648  *
 649  *  The executed instruction \em must read data from DTR.
 650  *
 651  * \pre arm11_run_instr_data_prepare() /  arm11_run_instr_data_finish() block
 652  *
 653  * \param arm11         Target state variable.
 654  * \param opcode        ARM opcode
 655  * \param data          Data word to be passed to the core via DTR
 656  *
 657  */
 658 int arm11_run_instr_data_to_core1(arm11_common_t * arm11, uint32_t opcode, uint32_t data)
 659 {
 660         return arm11_run_instr_data_to_core(arm11, opcode, &data, 1);
 661 }
 662
 663
 664 /** Execute one instruction via ITR repeatedly while
 665  *  reading data from the core via DTR on each execution.
 666  *
 667  *  The executed instruction \em must write data to DTR.
 668  *
 669  * \pre arm11_run_instr_data_prepare() /  arm11_run_instr_data_finish() block
 670  *
 671  * \param arm11         Target state variable.
 672  * \param opcode        ARM opcode
 673  * \param data          Pointer to an array that receives the data words from the core
 674  * \param count         Number of data words and instruction repetitions
 675  *
 676  */
 677 int arm11_run_instr_data_from_core(arm11_common_t * arm11, uint32_t opcode, uint32_t * data, size_t count)
 678 {
 679         arm11_add_IR(arm11, ARM11_ITRSEL, ARM11_TAP_DEFAULT);
 680
 681         arm11_add_debug_INST(arm11, opcode, NULL, TAP_IDLE);
 682
 683         arm11_add_IR(arm11, ARM11_INTEST, ARM11_TAP_DEFAULT);
 684
 685         scan_field_t    chain5_fields[3];
 686
 687         uint32_t                        Data;
 688         uint8_t                 Ready;
 689         uint8_t                 nRetry;
 690
 691         arm11_setup_field(arm11, 32,    NULL,   &Data,      chain5_fields + 0);
 692         arm11_setup_field(arm11,  1,    NULL,   &Ready,     chain5_fields + 1);
 693         arm11_setup_field(arm11,  1,    NULL,   &nRetry,    chain5_fields + 2);
 694
 695         while (count--)
 696         {
 697                 int i = 0;
 698                 do
 699                 {
 700                         arm11_add_dr_scan_vc(asizeof(chain5_fields), chain5_fields, count ? TAP_IDLE : TAP_DRPAUSE);
 701
 702                         CHECK_RETVAL(jtag_execute_queue());
 703
 704                         JTAG_DEBUG("DTR  Data %08x  Ready %d  nRetry %d", Data, Ready, nRetry);
 705
 706                         long long then = 0;
 707
 708                         if (i == 1000)
 709                         {
 710                                 then = timeval_ms();
 711                         }
 712                         if (i >= 1000)
 713                         {
 714                                 if ((timeval_ms()-then) > 1000)
 715                                 {
 716                                         LOG_WARNING("Timeout (1000ms) waiting for instructions to complete");
 717                                         return ERROR_FAIL;
 718                                 }
 719                         }
 720
 721                         i++;
 722                 }
 723                 while (!Ready);
 724
 725                 *data++ = Data;
 726         }
 727
 728         return ERROR_OK;
 729 }
 730
 731 /** Execute one instruction via ITR
 732  *  then load r0 into DTR and read DTR from core.
 733  *
 734  *  The first executed instruction (\p opcode) should write data to r0.
 735  *
 736  * \pre arm11_run_instr_data_prepare() /  arm11_run_instr_data_finish() block
 737  *
 738  * \param arm11         Target state variable.
 739  * \param opcode        ARM opcode to write r0 with the value of interest
 740  * \param data          Pointer to a data word that receives the value from r0 after \p opcode was executed.
 741  *
 742  */
 743 int arm11_run_instr_data_from_core_via_r0(arm11_common_t * arm11, uint32_t opcode, uint32_t * data)
 744 {
 745         int retval;
 746         retval = arm11_run_instr_no_data1(arm11, opcode);
 747         if (retval != ERROR_OK)
 748                 return retval;
 749
 750         /* MCR p14,0,R0,c0,c5,0 (move r0 -> wDTR -> local var) */
 751         arm11_run_instr_data_from_core(arm11, 0xEE000E15, data, 1);
 752
 753         return ERROR_OK;
 754 }
 755
 756 /** Load data into core via DTR then move it to r0 then
 757  *  execute one instruction via ITR
 758  *
 759  *  The final executed instruction (\p opcode) should read data from r0.
 760  *
 761  * \pre arm11_run_instr_data_prepare() /  arm11_run_instr_data_finish() block
 762  *
 763  * \param arm11         Target state variable.
 764  * \param opcode        ARM opcode to read r0 act upon it
 765  * \param data          Data word that will be written to r0 before \p opcode is executed
 766  *
 767  */
 768 int arm11_run_instr_data_to_core_via_r0(arm11_common_t * arm11, uint32_t opcode, uint32_t data)
 769 {
 770         int retval;
 771         /* MRC p14,0,r0,c0,c5,0 */
 772         retval = arm11_run_instr_data_to_core1(arm11, 0xEE100E15, data);
 773         if (retval != ERROR_OK)
 774                 return retval;
 775
 776         retval = arm11_run_instr_no_data1(arm11, opcode);
 777         if (retval != ERROR_OK)
 778                 return retval;
 779
 780         return ERROR_OK;
 781 }
 782
 783 /** Apply reads and writes to scan chain 7
 784  *
 785  * \see arm11_sc7_action_t
 786  *
 787  * \param arm11         Target state variable.
 788  * \param actions       A list of read and/or write instructions
 789  * \param count         Number of instructions in the list.
 790  *
 791  */
 792 int arm11_sc7_run(arm11_common_t * arm11, arm11_sc7_action_t * actions, size_t count)
 793 {
 794         int retval;
 795
 796         retval = arm11_add_debug_SCAN_N(arm11, 0x07, ARM11_TAP_DEFAULT);
 797         if (retval != ERROR_OK)
 798                 return retval;
 799
 800         arm11_add_IR(arm11, ARM11_EXTEST, ARM11_TAP_DEFAULT);
 801
 802         scan_field_t    chain7_fields[3];
 803
 804         uint8_t                         nRW;
 805         uint32_t                                DataOut;
 806         uint8_t                         AddressOut;
 807         uint8_t                         Ready;
 808         uint32_t                                DataIn;
 809         uint8_t                         AddressIn;
 810
 811         arm11_setup_field(arm11,  1, &nRW,                      &Ready,         chain7_fields + 0);
 812         arm11_setup_field(arm11, 32, &DataOut,          &DataIn,        chain7_fields + 1);
 813         arm11_setup_field(arm11,  7, &AddressOut,       &AddressIn,     chain7_fields + 2);
 814
 815         for (size_t i = 0; i < count + 1; i++)
 816         {
 817                 if (i < count)
 818                 {
 819                         nRW                     = actions[i].write ? 1 : 0;
 820                         DataOut         = actions[i].value;
 821                         AddressOut      = actions[i].address;
 822                 }
 823                 else
 824                 {
 825                         nRW                     = 0;
 826                         DataOut         = 0;
 827                         AddressOut      = 0;
 828                 }
 829
 830                 do
 831                 {
 832                         JTAG_DEBUG("SC7 <= Address %02x  Data %08x    nRW %d", AddressOut, DataOut, nRW);
 833
 834                         arm11_add_dr_scan_vc(asizeof(chain7_fields), chain7_fields, TAP_DRPAUSE);
 835
 836                         CHECK_RETVAL(jtag_execute_queue());
 837
 838                         JTAG_DEBUG("SC7 => Address %02x  Data %08x  Ready %d", AddressIn, DataIn, Ready);
 839                 }
 840                 while (!Ready); /* 'nRW' is 'Ready' on read out */
 841
 842                 if (i > 0)
 843                 {
 844                         if (actions[i - 1].address != AddressIn)
 845                         {
 846                                 LOG_WARNING("Scan chain 7 shifted out unexpected address");
 847                         }
 848
 849                         if (!actions[i - 1].write)
 850                         {
 851                                 actions[i - 1].value = DataIn;
 852                         }
 853                         else
 854                         {
 855                                 if (actions[i - 1].value != DataIn)
 856                                 {
 857                                         LOG_WARNING("Scan chain 7 shifted out unexpected data");
 858                                 }
 859                         }
 860                 }
 861         }
 862
 863         for (size_t i = 0; i < count; i++)
 864         {
 865                 JTAG_DEBUG("SC7 %02d: %02x %s %08x", i, actions[i].address, actions[i].write ? "<=" : "=>", actions[i].value);
 866         }
 867
 868         return ERROR_OK;
 869 }
 870
 871 /** Clear VCR and all breakpoints and watchpoints via scan chain 7
 872  *
 873  * \param arm11         Target state variable.
 874  *
 875  */
 876 void arm11_sc7_clear_vbw(arm11_common_t * arm11)
 877 {
 878         arm11_sc7_action_t              clear_bw[arm11->brp + arm11->wrp + 1];
 879         arm11_sc7_action_t *    pos = clear_bw;
 880
 881         for (size_t i = 0; i < asizeof(clear_bw); i++)
 882         {
 883                 clear_bw[i].write       = true;
 884                 clear_bw[i].value       = 0;
 885         }
 886
 887         for (size_t i = 0; i < arm11->brp; i++)
 888                 (pos++)->address = ARM11_SC7_BCR0 + i;
 889
 890
 891         for (size_t i = 0; i < arm11->wrp; i++)
 892                 (pos++)->address = ARM11_SC7_WCR0 + i;
 893
 894
 895         (pos++)->address = ARM11_SC7_VCR;
 896
 897         arm11_sc7_run(arm11, clear_bw, asizeof(clear_bw));
 898 }
 899
 900 /** Write VCR register
 901  *
 902  * \param arm11         Target state variable.
 903  * \param value         Value to be written
 904  */
 905 void arm11_sc7_set_vcr(arm11_common_t * arm11, uint32_t value)
 906 {
 907         arm11_sc7_action_t              set_vcr;
 908
 909         set_vcr.write           = true;
 910         set_vcr.address         = ARM11_SC7_VCR;
 911         set_vcr.value           = value;
 912
 913
 914         arm11_sc7_run(arm11, &set_vcr, 1);
 915 }
 916
 917
 918
 919 /** Read word from address
 920  *
 921  * \param arm11         Target state variable.
 922  * \param address       Memory address to be read
 923  * \param result        Pointer where to store result
 924  *
 925  */
 926 int arm11_read_memory_word(arm11_common_t * arm11, uint32_t address, uint32_t * result)
 927 {
 928         int retval;
 929         retval = arm11_run_instr_data_prepare(arm11);
 930         if (retval != ERROR_OK)
 931                 return retval;
 932
 933         /* MRC p14,0,r0,c0,c5,0 (r0 = address) */
 934         CHECK_RETVAL(arm11_run_instr_data_to_core1(arm11, 0xee100e15, address));
 935
 936         /* LDC p14,c5,[R0],#4 (DTR = [r0]) */
 937         CHECK_RETVAL(arm11_run_instr_data_from_core(arm11, 0xecb05e01, result, 1));
 938
 939         return arm11_run_instr_data_finish(arm11);
 940 }
 941
 942
 943 /** Write Embedded Trace Macrocell (ETM) via Scan chain 6
 944  *
 945  * http://infocenter.arm.com/help/topic/com.arm.doc.ddi0318e/Bcfddjeh.html#Bcfggcbe
 946  *
 947  * \param arm11         Target state variable.
 948  * \param address       7 bit ETM register address
 949  * \param value         Value to be written
 950  *
 951  * \return                      Error status
 952  *
 953  * \remarks                     This is a stand-alone function that executes the JTAG command queue.
 954  */
 955 int arm11_write_etm(arm11_common_t * arm11, uint8_t address, uint32_t value)
 956 {
 957         CHECK_RETVAL(arm11_add_debug_SCAN_N(arm11, 0x06, ARM11_TAP_DEFAULT));
 958
 959         /* Uses INTEST for read and write */
 960         arm11_add_IR(arm11, ARM11_INTEST, ARM11_TAP_DEFAULT);
 961
 962         scan_field_t            chain6_fields[3];
 963
 964         uint8_t                         nRW                     = 1;
 965
 966         arm11_setup_field(arm11, 32, &value,            NULL,           chain6_fields + 0);
 967         arm11_setup_field(arm11,  7, &address,          NULL,           chain6_fields + 1);
 968         arm11_setup_field(arm11,  1, &nRW,                      NULL,           chain6_fields + 2);
 969
 970         arm11_add_dr_scan_vc(asizeof(chain6_fields), chain6_fields, TAP_IDLE);
 971
 972         CHECK_RETVAL(jtag_execute_queue());
 973
 974         return ERROR_OK;
 975 }
 976
 977 /** Read Embedded Trace Macrocell (ETM) via Scan chain 6
 978  *
 979  * http://infocenter.arm.com/help/topic/com.arm.doc.ddi0318e/Bcfddjeh.html#Bcfggcbe
 980  *
 981  * \param arm11         Target state variable.
 982  * \param address       7 bit ETM register address
 983  * \param value         Pointer that receives value that was read
 984  *
 985  * \return                      Error status
 986  *
 987  * \remarks                     This is a stand-alone function that executes the JTAG command queue.
 988  */
 989 int arm11_read_etm(arm11_common_t * arm11, uint8_t address, uint32_t * value)
 990 {
 991         CHECK_RETVAL(arm11_add_debug_SCAN_N(arm11, 0x06, ARM11_TAP_DEFAULT));
 992
 993         /* Uses INTEST for read and write */
 994         arm11_add_IR(arm11, ARM11_INTEST, ARM11_TAP_DEFAULT);
 995
 996         scan_field_t            chain6_fields[3];
 997
 998         uint8_t                         nRW                     = 0;
 999
1000         arm11_setup_field(arm11, 32, NULL,                      NULL,           chain6_fields + 0);
1001         arm11_setup_field(arm11,  7, &address,          NULL,           chain6_fields + 1);
1002         arm11_setup_field(arm11,  1, &nRW,                      NULL,           chain6_fields + 2);
1003
1004         arm11_add_dr_scan_vc(asizeof(chain6_fields), chain6_fields, TAP_IDLE);
1005
1006         /* Data is made available in Capture-DR and shifted out on the next access */
1007
1008         arm11_setup_field(arm11, 32, NULL,                      value,          chain6_fields + 0);
1009         arm11_setup_field(arm11,  7, &address,          NULL,           chain6_fields + 1);
1010         arm11_setup_field(arm11,  1, &nRW,                      NULL,           chain6_fields + 2);
1011
1012         arm11_add_dr_scan_vc(asizeof(chain6_fields), chain6_fields, TAP_IDLE);
1013
1014         CHECK_RETVAL(jtag_execute_queue());
1015
1016         return ERROR_OK;
1017 }
1018