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 static const char *arm11_ir_to_string(uint8_t ir)
  95 {
  96         const char *s = "unknown";
  97
  98         switch (ir) {
  99         case ARM11_EXTEST:
 100                 s = "EXTEST";
 101                 break;
 102         case ARM11_SCAN_N:
 103                 s = "SCAN_N";
 104                 break;
 105         case ARM11_RESTART:
 106                 s = "RESTART";
 107                 break;
 108         case ARM11_HALT:
 109                 s = "HALT";
 110                 break;
 111         case ARM11_INTEST:
 112                 s = "INTEST";
 113                 break;
 114         case ARM11_ITRSEL:
 115                 s = "ITRSEL";
 116                 break;
 117         case ARM11_IDCODE:
 118                 s = "IDCODE";
 119                 break;
 120         case ARM11_BYPASS:
 121                 s = "BYPASS";
 122                 break;
 123         }
 124         return s;
 125 }
 126
 127 /** Write JTAG instruction register
 128  *
 129  * \param arm11         Target state variable.
 130  * \param instr         An ARM11 DBGTAP instruction. Use enum #arm11_instructions.
 131  * \param state         Pass the final TAP state or ARM11_TAP_DEFAULT for the default value (Pause-IR).
 132  *
 133  * \remarks                     This adds to the JTAG command queue but does \em not execute it.
 134  */
 135 void arm11_add_IR(struct arm11_common * arm11, uint8_t instr, tap_state_t state)
 136 {
 137         struct jtag_tap *tap = arm11->arm.target->tap;
 138
 139         if (buf_get_u32(tap->cur_instr, 0, 5) == instr)
 140         {
 141                 JTAG_DEBUG("IR <= 0x%02x SKIPPED", instr);
 142                 return;
 143         }
 144
 145         JTAG_DEBUG("IR <= %s (0x%02x)", arm11_ir_to_string(instr), instr);
 146
 147         struct scan_field field;
 148
 149         arm11_setup_field(arm11, 5, &instr, NULL, &field);
 150
 151         arm11_add_ir_scan_vc(1, &field, state == ARM11_TAP_DEFAULT ? TAP_IRPAUSE : state);
 152 }
 153
 154 /** Verify shifted out data from Scan Chain Register (SCREG)
 155  *  Used as parameter to struct scan_field::in_handler in
 156  *  arm11_add_debug_SCAN_N().
 157  *
 158  */
 159 static void arm11_in_handler_SCAN_N(uint8_t *in_value)
 160 {
 161         /** \todo TODO: clarify why this isnt properly masked in core.c jtag_read_buffer() */
 162         uint8_t v = *in_value & 0x1F;
 163
 164         if (v != 0x10)
 165         {
 166                 LOG_ERROR("'arm11 target' JTAG communication error SCREG SCAN OUT 0x%02x (expected 0x10)", v);
 167                 jtag_set_error(ERROR_FAIL);
 168         }
 169
 170         if (v != 0x10)
 171                 JTAG_DEBUG("SCREG SCAN OUT 0x%02x", v);
 172 }
 173
 174 /** Select and write to Scan Chain Register (SCREG)
 175  *
 176  * This function sets the instruction register to SCAN_N and writes
 177  * the data register with the selected chain number.
 178  *
 179  * http://infocenter.arm.com/help/topic/com.arm.doc.ddi0301f/Cacbjhfg.html
 180  *
 181  * \param arm11     Target state variable.
 182  * \param chain     Scan chain that will be selected.
 183  * \param state     Pass the final TAP state or ARM11_TAP_DEFAULT for the default
 184  *                                      value (Pause-DR).
 185  *
 186  * Changes the current scan chain if needed, transitions to the specified
 187  * TAP state, and leaves the IR undefined.
 188  *
 189  * The chain takes effect when Update-DR is passed (usually when subsequently
 190  * the INTEXT/EXTEST instructions are written).
 191  *
 192  * \warning                     (Obsolete) Using this twice in a row will \em fail. The first
 193  *                                      call will end in Pause-DR. The second call, due to the IR
 194  *                                      caching, will not go through Capture-DR when shifting in the
 195  *                                      new scan chain number. As a result the verification in
 196  *                                      arm11_in_handler_SCAN_N() must fail.
 197  *
 198  * \remarks                     This adds to the JTAG command queue but does \em not execute it.
 199  */
 200
 201 int arm11_add_debug_SCAN_N(struct arm11_common *arm11,
 202                 uint8_t chain, tap_state_t state)
 203 {
 204         /* Don't needlessly switch the scan chain.
 205          * NOTE:  the ITRSEL instruction fakes SCREG changing;
 206          * but leaves its actual value unchanged.
 207          */
 208         if (arm11->jtag_info.cur_scan_chain == chain) {
 209                 JTAG_DEBUG("SCREG <= %d SKIPPED", chain);
 210                 return jtag_add_statemove((state == ARM11_TAP_DEFAULT)
 211                                         ? TAP_DRPAUSE : state);
 212         }
 213         JTAG_DEBUG("SCREG <= %d", chain);
 214
 215         arm11_add_IR(arm11, ARM11_SCAN_N, ARM11_TAP_DEFAULT);
 216
 217         struct scan_field               field;
 218
 219         uint8_t tmp[1];
 220         arm11_setup_field(arm11, 5, &chain, &tmp, &field);
 221
 222         arm11_add_dr_scan_vc(1, &field, state == ARM11_TAP_DEFAULT ? TAP_DRPAUSE : state);
 223
 224         jtag_execute_queue_noclear();
 225
 226         arm11_in_handler_SCAN_N(tmp);
 227
 228         arm11->jtag_info.cur_scan_chain = chain;
 229
 230         return jtag_execute_queue();
 231 }
 232
 233 /**
 234  * Queue a DR scan of the ITR register.  Caller must have selected
 235  * scan chain 4 (ITR), possibly using ITRSEL.
 236  *
 237  * \param arm11         Target state variable.
 238  * \param inst          An ARM11 processor instruction/opcode.
 239  * \param flag          Optional parameter to retrieve the Ready flag;
 240  *      this address will be written when the JTAG chain is scanned.
 241  * \param state         The TAP state to enter after the DR scan.
 242  *
 243  * Going through the TAP_DRUPDATE state writes ITR only if Ready was
 244  * previously set.  Only the Ready flag is readable by the scan.
 245  *
 246  * An instruction loaded into ITR is executed when going through the
 247  * TAP_IDLE state only if Ready was previously set and the debug state
 248  * is properly set up.  Depending on the instruction, you may also need
 249  * to ensure that the rDTR is ready before that Run-Test/Idle state.
 250  */
 251 static void arm11_add_debug_INST(struct arm11_common * arm11,
 252                 uint32_t inst, uint8_t * flag, tap_state_t state)
 253 {
 254         JTAG_DEBUG("INST <= 0x%08x", (unsigned) inst);
 255
 256         struct scan_field               itr[2];
 257
 258         arm11_setup_field(arm11, 32,    &inst,  NULL, itr + 0);
 259         arm11_setup_field(arm11, 1,         NULL,       flag, itr + 1);
 260
 261         arm11_add_dr_scan_vc(ARRAY_SIZE(itr), itr, state);
 262 }
 263
 264 /**
 265  * Read and save the Debug Status and Control Register (DSCR).
 266  *
 267  * \param arm11         Target state variable.
 268  * \return Error status; arm11->dscr is updated on success.
 269  *
 270  * \remarks This is a stand-alone function that executes the JTAG
 271  * command queue.  It does not require the ARM11 debug TAP to be
 272  * in any particular state.
 273  */
 274 int arm11_read_DSCR(struct arm11_common *arm11)
 275 {
 276         int retval;
 277
 278         retval = arm11_add_debug_SCAN_N(arm11, 0x01, ARM11_TAP_DEFAULT);
 279         if (retval != ERROR_OK)
 280                 return retval;
 281
 282         arm11_add_IR(arm11, ARM11_INTEST, ARM11_TAP_DEFAULT);
 283
 284         uint32_t                                dscr;
 285         struct scan_field       chain1_field;
 286
 287         arm11_setup_field(arm11, 32, NULL, &dscr, &chain1_field);
 288
 289         arm11_add_dr_scan_vc(1, &chain1_field, TAP_DRPAUSE);
 290
 291         CHECK_RETVAL(jtag_execute_queue());
 292
 293         if (arm11->dscr != dscr)
 294                 JTAG_DEBUG("DSCR  = %08x (OLD %08x)",
 295                                 (unsigned) dscr,
 296                                 (unsigned) arm11->dscr);
 297
 298         arm11->dscr = dscr;
 299
 300         return ERROR_OK;
 301 }
 302
 303 /** Write the Debug Status and Control Register (DSCR)
 304  *
 305  * same as CP14 c1
 306  *
 307  * \param arm11         Target state variable.
 308  * \param dscr          DSCR content
 309  *
 310  * \remarks                     This is a stand-alone function that executes the JTAG command queue.
 311  */
 312 int arm11_write_DSCR(struct arm11_common * arm11, uint32_t dscr)
 313 {
 314         int retval;
 315         retval = arm11_add_debug_SCAN_N(arm11, 0x01, ARM11_TAP_DEFAULT);
 316         if (retval != ERROR_OK)
 317                 return retval;
 318
 319         arm11_add_IR(arm11, ARM11_EXTEST, ARM11_TAP_DEFAULT);
 320
 321         struct scan_field                   chain1_field;
 322
 323         arm11_setup_field(arm11, 32, &dscr, NULL, &chain1_field);
 324
 325         arm11_add_dr_scan_vc(1, &chain1_field, TAP_DRPAUSE);
 326
 327         CHECK_RETVAL(jtag_execute_queue());
 328
 329         JTAG_DEBUG("DSCR <= %08x (OLD %08x)",
 330                         (unsigned) dscr,
 331                         (unsigned) arm11->dscr);
 332
 333         arm11->dscr = dscr;
 334
 335         return ERROR_OK;
 336 }
 337
 338 /** Prepare the stage for ITR/DTR operations
 339  * from the arm11_run_instr... group of functions.
 340  *
 341  * Put arm11_run_instr_data_prepare() and arm11_run_instr_data_finish()
 342  * around a block of arm11_run_instr_... calls.
 343  *
 344  * Select scan chain 5 to allow quick access to DTR. When scan
 345  * chain 4 is needed to put in a register the ITRSel instruction
 346  * shortcut is used instead of actually changing the Scan_N
 347  * register.
 348  *
 349  * \param arm11         Target state variable.
 350  *
 351  */
 352 int arm11_run_instr_data_prepare(struct arm11_common * arm11)
 353 {
 354         return arm11_add_debug_SCAN_N(arm11, 0x05, ARM11_TAP_DEFAULT);
 355 }
 356
 357 /** Cleanup after ITR/DTR operations
 358  * from the arm11_run_instr... group of functions
 359  *
 360  * Put arm11_run_instr_data_prepare() and arm11_run_instr_data_finish()
 361  * around a block of arm11_run_instr_... calls.
 362  *
 363  * Any IDLE can lead to an instruction execution when
 364  * scan chains 4 or 5 are selected and the IR holds
 365  * INTEST or EXTEST. So we must disable that before
 366  * any following activities lead to an IDLE.
 367  *
 368  * \param arm11         Target state variable.
 369  *
 370  */
 371 int arm11_run_instr_data_finish(struct arm11_common * arm11)
 372 {
 373         return arm11_add_debug_SCAN_N(arm11, 0x00, ARM11_TAP_DEFAULT);
 374 }
 375
 376
 377
 378 /**
 379  * Execute one or more instructions via ITR.
 380  * Caller guarantees that processor is in debug state, that DSCR_ITR_EN
 381  * is set, the ITR Ready flag is set (as seen on the previous entry to
 382  * TAP_DRCAPTURE), and the DSCR sticky abort flag is clear.
 383  *
 384  * \pre arm11_run_instr_data_prepare() /  arm11_run_instr_data_finish() block
 385  *
 386  * \param arm11         Target state variable.
 387  * \param opcode        Pointer to sequence of ARM opcodes
 388  * \param count         Number of opcodes to execute
 389  *
 390  */
 391 static
 392 int arm11_run_instr_no_data(struct arm11_common * arm11,
 393                 uint32_t * opcode, size_t count)
 394 {
 395         arm11_add_IR(arm11, ARM11_ITRSEL, ARM11_TAP_DEFAULT);
 396
 397         while (count--)
 398         {
 399                 arm11_add_debug_INST(arm11, *opcode++, NULL, TAP_IDLE);
 400
 401                 int i = 0;
 402                 while (1)
 403                 {
 404                         uint8_t flag;
 405
 406                         arm11_add_debug_INST(arm11, 0, &flag, count ? TAP_IDLE : TAP_DRPAUSE);
 407
 408                         CHECK_RETVAL(jtag_execute_queue());
 409
 410                         if (flag)
 411                                 break;
 412
 413                         long long then = 0;
 414
 415                         if (i == 1000)
 416                         {
 417                                 then = timeval_ms();
 418                         }
 419                         if (i >= 1000)
 420                         {
 421                                 if ((timeval_ms()-then) > 1000)
 422                                 {
 423                                         LOG_WARNING("Timeout (1000ms) waiting for instructions to complete");
 424                                         return ERROR_FAIL;
 425                                 }
 426                         }
 427
 428                         i++;
 429                 }
 430         }
 431
 432         return ERROR_OK;
 433 }
 434
 435 /** Execute one instruction via ITR
 436  *
 437  * \pre arm11_run_instr_data_prepare() /  arm11_run_instr_data_finish() block
 438  *
 439  * \param arm11         Target state variable.
 440  * \param opcode        ARM opcode
 441  *
 442  */
 443 int arm11_run_instr_no_data1(struct arm11_common * arm11, uint32_t opcode)
 444 {
 445         return arm11_run_instr_no_data(arm11, &opcode, 1);
 446 }
 447
 448
 449 /** Execute one instruction via ITR repeatedly while
 450  *  passing data to the core via DTR on each execution.
 451  *
 452  * Caller guarantees that processor is in debug state, that DSCR_ITR_EN
 453  * is set, the ITR Ready flag is set (as seen on the previous entry to
 454  * TAP_DRCAPTURE), and the DSCR sticky abort flag is clear.
 455  *
 456  *  The executed instruction \em must read data from DTR.
 457  *
 458  * \pre arm11_run_instr_data_prepare() /  arm11_run_instr_data_finish() block
 459  *
 460  * \param arm11         Target state variable.
 461  * \param opcode        ARM opcode
 462  * \param data          Pointer to the data words to be passed to the core
 463  * \param count         Number of data words and instruction repetitions
 464  *
 465  */
 466 int arm11_run_instr_data_to_core(struct arm11_common * arm11, uint32_t opcode, uint32_t * data, size_t count)
 467 {
 468         arm11_add_IR(arm11, ARM11_ITRSEL, ARM11_TAP_DEFAULT);
 469
 470         arm11_add_debug_INST(arm11, opcode, NULL, TAP_DRPAUSE);
 471
 472         arm11_add_IR(arm11, ARM11_EXTEST, ARM11_TAP_DEFAULT);
 473
 474         struct scan_field       chain5_fields[3];
 475
 476         uint32_t                                Data;
 477         uint8_t                         Ready;
 478         uint8_t                         nRetry;
 479
 480         arm11_setup_field(arm11, 32,    &Data,  NULL,           chain5_fields + 0);
 481         arm11_setup_field(arm11,  1,    NULL,   &Ready,         chain5_fields + 1);
 482         arm11_setup_field(arm11,  1,    NULL,   &nRetry,        chain5_fields + 2);
 483
 484         while (count--)
 485         {
 486                 int i = 0;
 487                 do
 488                 {
 489                         Data        = *data;
 490
 491                         arm11_add_dr_scan_vc(ARRAY_SIZE(chain5_fields), chain5_fields, jtag_set_end_state(TAP_IDLE));
 492
 493                         CHECK_RETVAL(jtag_execute_queue());
 494
 495                         JTAG_DEBUG("DTR  Ready %d  nRetry %d", Ready, nRetry);
 496
 497                         long long then = 0;
 498
 499                         if (i == 1000)
 500                         {
 501                                 then = timeval_ms();
 502                         }
 503                         if (i >= 1000)
 504                         {
 505                                 if ((timeval_ms()-then) > 1000)
 506                                 {
 507                                         LOG_WARNING("Timeout (1000ms) waiting for instructions to complete");
 508                                         return ERROR_FAIL;
 509                                 }
 510                         }
 511
 512                         i++;
 513                 }
 514                 while (!Ready);
 515
 516                 data++;
 517         }
 518
 519         arm11_add_IR(arm11, ARM11_INTEST, ARM11_TAP_DEFAULT);
 520
 521         int i = 0;
 522         do
 523         {
 524                 Data        = 0;
 525
 526                 arm11_add_dr_scan_vc(ARRAY_SIZE(chain5_fields), chain5_fields, TAP_DRPAUSE);
 527
 528                 CHECK_RETVAL(jtag_execute_queue());
 529
 530                 JTAG_DEBUG("DTR  Data %08x  Ready %d  nRetry %d",
 531                                 (unsigned) Data, Ready, nRetry);
 532
 533                 long long then = 0;
 534
 535                 if (i == 1000)
 536                 {
 537                         then = timeval_ms();
 538                 }
 539                 if (i >= 1000)
 540                 {
 541                         if ((timeval_ms()-then) > 1000)
 542                         {
 543                                 LOG_WARNING("Timeout (1000ms) waiting for instructions to complete");
 544                                 return ERROR_FAIL;
 545                         }
 546                 }
 547
 548                 i++;
 549         }
 550         while (!Ready);
 551
 552         return ERROR_OK;
 553 }
 554
 555 /** JTAG path for arm11_run_instr_data_to_core_noack
 556  *
 557  *  The repeated TAP_IDLE's do not cause a repeated execution
 558  *  if passed without leaving the state.
 559  *
 560  *  Since this is more than 7 bits (adjustable via adding more
 561  *  TAP_IDLE's) it produces an artificial delay in the lower
 562  *  layer (FT2232) that is long enough to finish execution on
 563  *  the core but still shorter than any manually inducible delays.
 564  *
 565  *  To disable this code, try "memwrite burst false"
 566  *
 567  *  FIX!!! should we use multiple TAP_IDLE here or not???
 568  *
 569  *  https://lists.berlios.de/pipermail/openocd-development/2009-July/009698.html
 570  *  https://lists.berlios.de/pipermail/openocd-development/2009-August/009865.html
 571  */
 572 static const tap_state_t arm11_MOVE_DRPAUSE_IDLE_DRPAUSE_with_delay[] =
 573 {
 574         TAP_DREXIT2, TAP_DRUPDATE, TAP_IDLE, TAP_IDLE, TAP_IDLE, TAP_DRSELECT, TAP_DRCAPTURE, TAP_DRSHIFT
 575 };
 576
 577
 578
 579 /** Execute one instruction via ITR repeatedly while
 580  *  passing data to the core via DTR on each execution.
 581  *
 582  * Caller guarantees that processor is in debug state, that DSCR_ITR_EN
 583  * is set, the ITR Ready flag is set (as seen on the previous entry to
 584  * TAP_DRCAPTURE), and the DSCR sticky abort flag is clear.
 585  *
 586  *  No Ready check during transmission.
 587  *
 588  *  The executed instruction \em must read data from DTR.
 589  *
 590  * \pre arm11_run_instr_data_prepare() /  arm11_run_instr_data_finish() block
 591  *
 592  * \param arm11         Target state variable.
 593  * \param opcode        ARM opcode
 594  * \param data          Pointer to the data words to be passed to the core
 595  * \param count         Number of data words and instruction repetitions
 596  *
 597  */
 598 int arm11_run_instr_data_to_core_noack(struct arm11_common * arm11, uint32_t opcode, uint32_t * data, size_t count)
 599 {
 600         arm11_add_IR(arm11, ARM11_ITRSEL, ARM11_TAP_DEFAULT);
 601
 602         arm11_add_debug_INST(arm11, opcode, NULL, TAP_DRPAUSE);
 603
 604         arm11_add_IR(arm11, ARM11_EXTEST, ARM11_TAP_DEFAULT);
 605
 606         struct scan_field       chain5_fields[3];
 607
 608         arm11_setup_field(arm11, 32,    NULL/*&Data*/,  NULL,                           chain5_fields + 0);
 609         arm11_setup_field(arm11,  1,    NULL,                   NULL /*&Ready*/,        chain5_fields + 1);
 610         arm11_setup_field(arm11,  1,    NULL,                   NULL,                           chain5_fields + 2);
 611
 612         uint8_t                 *Readies;
 613         unsigned readiesNum = count + 1;
 614         unsigned bytes = sizeof(*Readies)*readiesNum;
 615
 616         Readies = (uint8_t *) malloc(bytes);
 617         if (Readies == NULL)
 618         {
 619                 LOG_ERROR("Out of memory allocating %u bytes", bytes);
 620                 return ERROR_FAIL;
 621         }
 622
 623         uint8_t *               ReadyPos                        = Readies;
 624
 625         while (count--)
 626         {
 627                 chain5_fields[0].out_value      = (void *)(data++);
 628                 chain5_fields[1].in_value       = ReadyPos++;
 629
 630                 if (count)
 631                 {
 632                         jtag_add_dr_scan(ARRAY_SIZE(chain5_fields), chain5_fields, jtag_set_end_state(TAP_DRPAUSE));
 633                         jtag_add_pathmove(ARRAY_SIZE(arm11_MOVE_DRPAUSE_IDLE_DRPAUSE_with_delay),
 634                                 arm11_MOVE_DRPAUSE_IDLE_DRPAUSE_with_delay);
 635                 }
 636                 else
 637                 {
 638                         jtag_add_dr_scan(ARRAY_SIZE(chain5_fields), chain5_fields, jtag_set_end_state(TAP_IDLE));
 639                 }
 640         }
 641
 642         arm11_add_IR(arm11, ARM11_INTEST, ARM11_TAP_DEFAULT);
 643
 644         chain5_fields[0].out_value      = 0;
 645         chain5_fields[1].in_value   = ReadyPos++;
 646
 647         arm11_add_dr_scan_vc(ARRAY_SIZE(chain5_fields), chain5_fields, TAP_DRPAUSE);
 648
 649         int retval = jtag_execute_queue();
 650         if (retval == ERROR_OK)
 651         {
 652                 unsigned error_count = 0;
 653
 654                 for (size_t i = 0; i < readiesNum; i++)
 655                 {
 656                         if (Readies[i] != 1)
 657                         {
 658                                 error_count++;
 659                         }
 660                 }
 661
 662                 if (error_count > 0 )
 663                         LOG_ERROR("%u words out of %u not transferred",
 664                                 error_count, readiesNum);
 665
 666         }
 667
 668         free(Readies);
 669
 670         return retval;
 671 }
 672
 673
 674 /** Execute an instruction via ITR while handing data into the core via DTR.
 675  *
 676  *  The executed instruction \em must read data from DTR.
 677  *
 678  * \pre arm11_run_instr_data_prepare() /  arm11_run_instr_data_finish() block
 679  *
 680  * \param arm11         Target state variable.
 681  * \param opcode        ARM opcode
 682  * \param data          Data word to be passed to the core via DTR
 683  *
 684  */
 685 int arm11_run_instr_data_to_core1(struct arm11_common * arm11, uint32_t opcode, uint32_t data)
 686 {
 687         return arm11_run_instr_data_to_core(arm11, opcode, &data, 1);
 688 }
 689
 690
 691 /** Execute one instruction via ITR repeatedly while
 692  *  reading data from the core via DTR on each execution.
 693  *
 694  * Caller guarantees that processor is in debug state, that DSCR_ITR_EN
 695  * is set, the ITR Ready flag is set (as seen on the previous entry to
 696  * TAP_DRCAPTURE), and the DSCR sticky abort flag is clear.
 697  *
 698  *  The executed instruction \em must write data to DTR.
 699  *
 700  * \pre arm11_run_instr_data_prepare() /  arm11_run_instr_data_finish() block
 701  *
 702  * \param arm11         Target state variable.
 703  * \param opcode        ARM opcode
 704  * \param data          Pointer to an array that receives the data words from the core
 705  * \param count         Number of data words and instruction repetitions
 706  *
 707  */
 708 int arm11_run_instr_data_from_core(struct arm11_common * arm11, uint32_t opcode, uint32_t * data, size_t count)
 709 {
 710         arm11_add_IR(arm11, ARM11_ITRSEL, ARM11_TAP_DEFAULT);
 711
 712         arm11_add_debug_INST(arm11, opcode, NULL, TAP_IDLE);
 713
 714         arm11_add_IR(arm11, ARM11_INTEST, ARM11_TAP_DEFAULT);
 715
 716         struct scan_field       chain5_fields[3];
 717
 718         uint32_t                        Data;
 719         uint8_t                 Ready;
 720         uint8_t                 nRetry;
 721
 722         arm11_setup_field(arm11, 32,    NULL,   &Data,      chain5_fields + 0);
 723         arm11_setup_field(arm11,  1,    NULL,   &Ready,     chain5_fields + 1);
 724         arm11_setup_field(arm11,  1,    NULL,   &nRetry,    chain5_fields + 2);
 725
 726         while (count--)
 727         {
 728                 int i = 0;
 729                 do
 730                 {
 731                         arm11_add_dr_scan_vc(ARRAY_SIZE(chain5_fields), chain5_fields, count ? TAP_IDLE : TAP_DRPAUSE);
 732
 733                         CHECK_RETVAL(jtag_execute_queue());
 734
 735                         JTAG_DEBUG("DTR  Data %08x  Ready %d  nRetry %d",
 736                                         (unsigned) Data, Ready, nRetry);
 737
 738                         long long then = 0;
 739
 740                         if (i == 1000)
 741                         {
 742                                 then = timeval_ms();
 743                         }
 744                         if (i >= 1000)
 745                         {
 746                                 if ((timeval_ms()-then) > 1000)
 747                                 {
 748                                         LOG_WARNING("Timeout (1000ms) waiting for instructions to complete");
 749                                         return ERROR_FAIL;
 750                                 }
 751                         }
 752
 753                         i++;
 754                 }
 755                 while (!Ready);
 756
 757                 *data++ = Data;
 758         }
 759
 760         return ERROR_OK;
 761 }
 762
 763 /** Execute one instruction via ITR
 764  *  then load r0 into DTR and read DTR from core.
 765  *
 766  *  The first executed instruction (\p opcode) should write data to r0.
 767  *
 768  * \pre arm11_run_instr_data_prepare() /  arm11_run_instr_data_finish() block
 769  *
 770  * \param arm11         Target state variable.
 771  * \param opcode        ARM opcode to write r0 with the value of interest
 772  * \param data          Pointer to a data word that receives the value from r0 after \p opcode was executed.
 773  *
 774  */
 775 int arm11_run_instr_data_from_core_via_r0(struct arm11_common * arm11, uint32_t opcode, uint32_t * data)
 776 {
 777         int retval;
 778         retval = arm11_run_instr_no_data1(arm11, opcode);
 779         if (retval != ERROR_OK)
 780                 return retval;
 781
 782         /* MCR p14,0,R0,c0,c5,0 (move r0 -> wDTR -> local var) */
 783         arm11_run_instr_data_from_core(arm11, 0xEE000E15, data, 1);
 784
 785         return ERROR_OK;
 786 }
 787
 788 /** Load data into core via DTR then move it to r0 then
 789  *  execute one instruction via ITR
 790  *
 791  *  The final executed instruction (\p opcode) should read data from r0.
 792  *
 793  * \pre arm11_run_instr_data_prepare() /  arm11_run_instr_data_finish() block
 794  *
 795  * \param arm11         Target state variable.
 796  * \param opcode        ARM opcode to read r0 act upon it
 797  * \param data          Data word that will be written to r0 before \p opcode is executed
 798  *
 799  */
 800 int arm11_run_instr_data_to_core_via_r0(struct arm11_common * arm11, uint32_t opcode, uint32_t data)
 801 {
 802         int retval;
 803         /* MRC p14,0,r0,c0,c5,0 */
 804         retval = arm11_run_instr_data_to_core1(arm11, 0xEE100E15, data);
 805         if (retval != ERROR_OK)
 806                 return retval;
 807
 808         retval = arm11_run_instr_no_data1(arm11, opcode);
 809         if (retval != ERROR_OK)
 810                 return retval;
 811
 812         return ERROR_OK;
 813 }
 814
 815 /** Apply reads and writes to scan chain 7
 816  *
 817  * \see struct arm11_sc7_action
 818  *
 819  * \param arm11         Target state variable.
 820  * \param actions       A list of read and/or write instructions
 821  * \param count         Number of instructions in the list.
 822  *
 823  */
 824 int arm11_sc7_run(struct arm11_common * arm11, struct arm11_sc7_action * actions, size_t count)
 825 {
 826         int retval;
 827
 828         retval = arm11_add_debug_SCAN_N(arm11, 0x07, ARM11_TAP_DEFAULT);
 829         if (retval != ERROR_OK)
 830                 return retval;
 831
 832         arm11_add_IR(arm11, ARM11_EXTEST, ARM11_TAP_DEFAULT);
 833
 834         struct scan_field       chain7_fields[3];
 835
 836         uint8_t                         nRW;
 837         uint32_t                                DataOut;
 838         uint8_t                         AddressOut;
 839         uint8_t                         Ready;
 840         uint32_t                                DataIn;
 841         uint8_t                         AddressIn;
 842
 843         arm11_setup_field(arm11,  1, &nRW,                      &Ready,         chain7_fields + 0);
 844         arm11_setup_field(arm11, 32, &DataOut,          &DataIn,        chain7_fields + 1);
 845         arm11_setup_field(arm11,  7, &AddressOut,       &AddressIn,     chain7_fields + 2);
 846
 847         for (size_t i = 0; i < count + 1; i++)
 848         {
 849                 if (i < count)
 850                 {
 851                         nRW                     = actions[i].write ? 1 : 0;
 852                         DataOut         = actions[i].value;
 853                         AddressOut      = actions[i].address;
 854                 }
 855                 else
 856                 {
 857                         nRW                     = 1;
 858                         DataOut         = 0;
 859                         AddressOut      = 0;
 860                 }
 861
 862                 do
 863                 {
 864                         JTAG_DEBUG("SC7 <= c%-3d Data %08x %s",
 865                                         (unsigned) AddressOut,
 866                                         (unsigned) DataOut,
 867                                         nRW ? "write" : "read");
 868
 869                         arm11_add_dr_scan_vc(ARRAY_SIZE(chain7_fields),
 870                                         chain7_fields, TAP_DRPAUSE);
 871
 872                         CHECK_RETVAL(jtag_execute_queue());
 873
 874                         if (!Ready)
 875                                 JTAG_DEBUG("SC7 => !ready");
 876                 }
 877                 while (!Ready); /* 'nRW' is 'Ready' on read out */
 878
 879                 if (!nRW)
 880                         JTAG_DEBUG("SC7 => Data %08x", (unsigned) DataIn);
 881
 882                 if (i > 0)
 883                 {
 884                         if (actions[i - 1].address != AddressIn)
 885                         {
 886                                 LOG_WARNING("Scan chain 7 shifted out unexpected address");
 887                         }
 888
 889                         if (!actions[i - 1].write)
 890                         {
 891                                 actions[i - 1].value = DataIn;
 892                         }
 893                         else
 894                         {
 895                                 if (actions[i - 1].value != DataIn)
 896                                 {
 897                                         LOG_WARNING("Scan chain 7 shifted out unexpected data");
 898                                 }
 899                         }
 900                 }
 901         }
 902         return ERROR_OK;
 903 }
 904
 905 /** Clear VCR and all breakpoints and watchpoints via scan chain 7
 906  *
 907  * \param arm11         Target state variable.
 908  *
 909  */
 910 void arm11_sc7_clear_vbw(struct arm11_common * arm11)
 911 {
 912         size_t clear_bw_size = arm11->brp + 1;
 913         struct arm11_sc7_action         *clear_bw = malloc(sizeof(struct arm11_sc7_action) * clear_bw_size);
 914         struct arm11_sc7_action *       pos = clear_bw;
 915
 916         for (size_t i = 0; i < clear_bw_size; i++)
 917         {
 918                 clear_bw[i].write       = true;
 919                 clear_bw[i].value       = 0;
 920         }
 921
 922         for (size_t i = 0; i < arm11->brp; i++)
 923                 (pos++)->address = ARM11_SC7_BCR0 + i;
 924
 925         (pos++)->address = ARM11_SC7_VCR;
 926
 927         arm11_sc7_run(arm11, clear_bw, clear_bw_size);
 928
 929         free (clear_bw);
 930 }
 931
 932 /** Write VCR register
 933  *
 934  * \param arm11         Target state variable.
 935  * \param value         Value to be written
 936  */
 937 void arm11_sc7_set_vcr(struct arm11_common * arm11, uint32_t value)
 938 {
 939         struct arm11_sc7_action         set_vcr;
 940
 941         set_vcr.write           = true;
 942         set_vcr.address         = ARM11_SC7_VCR;
 943         set_vcr.value           = value;
 944
 945         arm11_sc7_run(arm11, &set_vcr, 1);
 946 }
 947
 948
 949
 950 /** Read word from address
 951  *
 952  * \param arm11         Target state variable.
 953  * \param address       Memory address to be read
 954  * \param result        Pointer where to store result
 955  *
 956  */
 957 int arm11_read_memory_word(struct arm11_common * arm11, uint32_t address, uint32_t * result)
 958 {
 959         int retval;
 960         retval = arm11_run_instr_data_prepare(arm11);
 961         if (retval != ERROR_OK)
 962                 return retval;
 963
 964         /* MRC p14,0,r0,c0,c5,0 (r0 = address) */
 965         CHECK_RETVAL(arm11_run_instr_data_to_core1(arm11, 0xee100e15, address));
 966
 967         /* LDC p14,c5,[R0],#4 (DTR = [r0]) */
 968         CHECK_RETVAL(arm11_run_instr_data_from_core(arm11, 0xecb05e01, result, 1));
 969
 970         return arm11_run_instr_data_finish(arm11);
 971 }
 972
 973
 974 /************************************************************************/
 975
 976 /*
 977  * ARM11 provider for the OpenOCD implementation of the standard
 978  * architectural ARM v6/v7 "Debug Programmer's Model" (DPM).
 979  */
 980
 981 static inline struct arm11_common *dpm_to_arm11(struct arm_dpm *dpm)
 982 {
 983         return container_of(dpm, struct arm11_common, dpm);
 984 }
 985
 986 static int arm11_dpm_prepare(struct arm_dpm *dpm)
 987 {
 988         struct arm11_common *arm11 = dpm_to_arm11(dpm);
 989
 990         arm11 = container_of(dpm->arm, struct arm11_common, arm);
 991
 992         return arm11_run_instr_data_prepare(dpm_to_arm11(dpm));
 993 }
 994
 995 static int arm11_dpm_finish(struct arm_dpm *dpm)
 996 {
 997         return arm11_run_instr_data_finish(dpm_to_arm11(dpm));
 998 }
 999
1000 static int arm11_dpm_instr_write_data_dcc(struct arm_dpm *dpm,
1001                 uint32_t opcode, uint32_t data)
1002 {
1003         return arm11_run_instr_data_to_core(dpm_to_arm11(dpm),
1004                         opcode, &data, 1);
1005 }
1006
1007 static int arm11_dpm_instr_write_data_r0(struct arm_dpm *dpm,
1008                 uint32_t opcode, uint32_t data)
1009 {
1010         return arm11_run_instr_data_to_core_via_r0(dpm_to_arm11(dpm),
1011                         opcode, data);
1012 }
1013
1014 static int arm11_dpm_instr_read_data_dcc(struct arm_dpm *dpm,
1015                 uint32_t opcode, uint32_t *data)
1016 {
1017         return arm11_run_instr_data_from_core(dpm_to_arm11(dpm),
1018                         opcode, data, 1);
1019 }
1020
1021 static int arm11_dpm_instr_read_data_r0(struct arm_dpm *dpm,
1022                 uint32_t opcode, uint32_t *data)
1023 {
1024         return arm11_run_instr_data_from_core_via_r0(dpm_to_arm11(dpm),
1025                         opcode, data);
1026 }
1027
1028 /* Because arm11_sc7_run() takes a vector of actions, we batch breakpoint
1029  * and watchpoint operations instead of running them right away.  Since we
1030  * pre-allocated our vector, we don't need to worry about space.
1031  */
1032 static int arm11_bpwp_enable(struct arm_dpm *dpm, unsigned index,
1033                 uint32_t addr, uint32_t control)
1034 {
1035         struct arm11_common *arm11 = dpm_to_arm11(dpm);
1036         struct arm11_sc7_action *action;
1037
1038         action = arm11->bpwp_actions + arm11->bpwp_n;
1039
1040         /* Invariant:  this bp/wp is disabled.
1041          * It also happens that the core is halted here, but for
1042          * DPM-based cores we don't actually care about that.
1043          */
1044
1045         action[0].write = action[1].write = true;
1046
1047         action[0].value = addr;
1048         action[1].value = control;
1049
1050         switch (index) {
1051         case 0 ... 15:
1052                 action[0].address = ARM11_SC7_BVR0 + index;
1053                 action[1].address = ARM11_SC7_BCR0 + index;
1054                 break;
1055         case 16 ... 32:
1056                 index -= 16;
1057                 action[0].address = ARM11_SC7_WVR0 + index;
1058                 action[1].address = ARM11_SC7_WCR0 + index;
1059                 break;
1060         default:
1061                 return ERROR_FAIL;
1062         }
1063
1064         arm11->bpwp_n += 2;
1065
1066         return ERROR_OK;
1067 }
1068
1069 static int arm11_bpwp_disable(struct arm_dpm *dpm, unsigned index)
1070 {
1071         struct arm11_common *arm11 = dpm_to_arm11(dpm);
1072         struct arm11_sc7_action *action;
1073
1074         action = arm11->bpwp_actions + arm11->bpwp_n;
1075
1076         action[0].write = true;
1077         action[0].value = 0;
1078
1079         switch (index) {
1080         case 0 ... 15:
1081                 action[0].address = ARM11_SC7_BCR0 + index;
1082                 break;
1083         case 16 ... 32:
1084                 index -= 16;
1085                 action[0].address = ARM11_SC7_WCR0 + index;
1086                 break;
1087         default:
1088                 return ERROR_FAIL;
1089         }
1090
1091         arm11->bpwp_n += 1;
1092
1093         return ERROR_OK;
1094 }
1095
1096 /** Flush any pending breakpoint and watchpoint updates. */
1097 int arm11_bpwp_flush(struct arm11_common *arm11)
1098 {
1099         int retval;
1100
1101         if (!arm11->bpwp_n)
1102                 return ERROR_OK;
1103
1104         retval = arm11_sc7_run(arm11, arm11->bpwp_actions, arm11->bpwp_n);
1105         arm11->bpwp_n = 0;
1106
1107         return retval;
1108 }
1109
1110 /** Set up high-level debug module utilities */
1111 int arm11_dpm_init(struct arm11_common *arm11, uint32_t didr)
1112 {
1113         struct arm_dpm *dpm = &arm11->dpm;
1114         int retval;
1115
1116         dpm->arm = &arm11->arm;
1117
1118         dpm->didr = didr;
1119
1120         dpm->prepare = arm11_dpm_prepare;
1121         dpm->finish = arm11_dpm_finish;
1122
1123         dpm->instr_write_data_dcc = arm11_dpm_instr_write_data_dcc;
1124         dpm->instr_write_data_r0 = arm11_dpm_instr_write_data_r0;
1125
1126         dpm->instr_read_data_dcc = arm11_dpm_instr_read_data_dcc;
1127         dpm->instr_read_data_r0 = arm11_dpm_instr_read_data_r0;
1128
1129         dpm->bpwp_enable = arm11_bpwp_enable;
1130         dpm->bpwp_disable = arm11_bpwp_disable;
1131
1132         retval = arm_dpm_setup(dpm);
1133         if (retval != ERROR_OK)
1134                 return retval;
1135
1136         /* alloc enough to enable all breakpoints and watchpoints at once */
1137         arm11->bpwp_actions = calloc(2 * (dpm->nbp + dpm->nwp),
1138                         sizeof *arm11->bpwp_actions);
1139         if (!arm11->bpwp_actions)
1140                 return ERROR_FAIL;
1141
1142         retval = arm_dpm_initialize(dpm);
1143         if (retval != ERROR_OK)
1144                 return retval;
1145
1146         return arm11_bpwp_flush(arm11);
1147 }