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