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

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