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

   1 /***************************************************************************
   2  *   Copyright (C) 2005 by Dominic Rath                                    *
   3  *   Dominic.Rath@gmx.de                                                   *
   4  *                                                                         *
   5  *   Copyright (C) 2007,2008 Øyvind Harboe                                 *
   6  *   oyvind.harboe@zylin.com                                               *
   7  *                                                                         *
   8  *   Copyright (C) 2009 SoftPLC Corporation                                *
   9  *       http://softplc.com                                                    *
  10  *   dick@softplc.com                                                      *
  11  *                                                                         *
  12  *   This program is free software; you can redistribute it and/or modify  *
  13  *   it under the terms of the GNU General Public License as published by  *
  14  *   the Free Software Foundation; either version 2 of the License, or     *
  15  *   (at your option) any later version.                                   *
  16  *                                                                         *
  17  *   This program is distributed in the hope that it will be useful,       *
  18  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  19  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  20  *   GNU General Public License for more details.                          *
  21  *                                                                         *
  22  *   You should have received a copy of the GNU General Public License     *
  23  *   along with this program; if not, write to the                         *
  24  *   Free Software Foundation, Inc.,                                       *
  25  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  26  ***************************************************************************/
  27 #ifdef HAVE_CONFIG_H
  28 #include "config.h"
  29 #endif
  30
  31 #include "replacements.h"
  32
  33 #include "jtag.h"
  34
  35 #include "command.h"
  36 #include "log.h"
  37
  38 #include <stdlib.h>
  39 #include <string.h>
  40 #include <unistd.h>
  41 #ifdef HAVE_STRINGS_H
  42 #include <strings.h>
  43 #endif
  44 #include <assert.h>
  45
  46 int jtag_flush_queue_count; /* count # of flushes for profiling / debugging purposes */
  47
  48 /* note that this is not marked as static as it must be available from outside jtag.c for those
  49    that implement the jtag_xxx() minidriver layer
  50 */
  51 int jtag_error=ERROR_OK;
  52
  53 typedef struct cmd_queue_page_s
  54 {
  55         void *address;
  56         size_t used;
  57         struct cmd_queue_page_s *next;
  58 } cmd_queue_page_t;
  59
  60 #define CMD_QUEUE_PAGE_SIZE (1024 * 1024)
  61 static cmd_queue_page_t *cmd_queue_pages = NULL;
  62
  63 char* jtag_event_strings[] =
  64 {
  65         "JTAG controller reset (RESET or TRST)"
  66 };
  67
  68 const Jim_Nvp nvp_jtag_tap_event[] = {
  69         { .value = JTAG_TAP_EVENT_ENABLE,       .name = "tap-enable" },
  70         { .value = JTAG_TAP_EVENT_DISABLE,      .name = "tap-disable" },
  71
  72         { .name = NULL, .value = -1 }
  73 };
  74
  75 int jtag_trst = 0;
  76 int jtag_srst = 0;
  77
  78 jtag_command_t *jtag_command_queue = NULL;
  79 jtag_command_t **last_comand_pointer = &jtag_command_queue;
  80 static jtag_tap_t *jtag_all_taps = NULL;
  81
  82 enum reset_types jtag_reset_config = RESET_NONE;
  83 tap_state_t cmd_queue_end_state = TAP_RESET;
  84 tap_state_t cmd_queue_cur_state = TAP_RESET;
  85
  86 int jtag_verify_capture_ir = 1;
  87
  88 /* how long the OpenOCD should wait before attempting JTAG communication after reset lines deasserted (in ms) */
  89 static int jtag_nsrst_delay = 0; /* default to no nSRST delay */
  90 static int jtag_ntrst_delay = 0; /* default to no nTRST delay */
  91
  92 /* maximum number of JTAG devices expected in the chain
  93  */
  94 #define JTAG_MAX_CHAIN_SIZE 20
  95
  96 /* callbacks to inform high-level handlers about JTAG state changes */
  97 jtag_event_callback_t *jtag_event_callbacks;
  98
  99 /* speed in kHz*/
 100 static int speed_khz = 0;
 101 /* flag if the kHz speed was defined */
 102 static int hasKHz = 0;
 103
 104 /* jtag interfaces (parport, FTDI-USB, TI-USB, ...)
 105  */
 106
 107 #if BUILD_ECOSBOARD == 1
 108         extern jtag_interface_t zy1000_interface;
 109 #endif
 110
 111 #if BUILD_PARPORT == 1
 112         extern jtag_interface_t parport_interface;
 113 #endif
 114
 115 #if BUILD_DUMMY == 1
 116         extern jtag_interface_t dummy_interface;
 117 #endif
 118
 119 #if BUILD_FT2232_FTD2XX == 1
 120         extern jtag_interface_t ft2232_interface;
 121 #endif
 122
 123 #if BUILD_FT2232_LIBFTDI == 1
 124         extern jtag_interface_t ft2232_interface;
 125 #endif
 126
 127 #if BUILD_AMTJTAGACCEL == 1
 128         extern jtag_interface_t amt_jtagaccel_interface;
 129 #endif
 130
 131 #if BUILD_EP93XX == 1
 132         extern jtag_interface_t ep93xx_interface;
 133 #endif
 134
 135 #if BUILD_AT91RM9200 == 1
 136         extern jtag_interface_t at91rm9200_interface;
 137 #endif
 138
 139 #if BUILD_GW16012 == 1
 140         extern jtag_interface_t gw16012_interface;
 141 #endif
 142
 143 #if BUILD_PRESTO_LIBFTDI == 1 || BUILD_PRESTO_FTD2XX == 1
 144         extern jtag_interface_t presto_interface;
 145 #endif
 146
 147 #if BUILD_USBPROG == 1
 148         extern jtag_interface_t usbprog_interface;
 149 #endif
 150
 151 #if BUILD_JLINK == 1
 152         extern jtag_interface_t jlink_interface;
 153 #endif
 154
 155 #if BUILD_VSLLINK == 1
 156         extern jtag_interface_t vsllink_interface;
 157 #endif
 158
 159 #if BUILD_RLINK == 1
 160         extern jtag_interface_t rlink_interface;
 161 #endif
 162
 163 #if BUILD_ARMJTAGEW == 1
 164         extern jtag_interface_t armjtagew_interface;
 165 #endif
 166
 167 jtag_interface_t *jtag_interfaces[] = {
 168 #if BUILD_ECOSBOARD == 1
 169         &zy1000_interface,
 170 #endif
 171 #if BUILD_PARPORT == 1
 172         &parport_interface,
 173 #endif
 174 #if BUILD_DUMMY == 1
 175         &dummy_interface,
 176 #endif
 177 #if BUILD_FT2232_FTD2XX == 1
 178         &ft2232_interface,
 179 #endif
 180 #if BUILD_FT2232_LIBFTDI == 1
 181         &ft2232_interface,
 182 #endif
 183 #if BUILD_AMTJTAGACCEL == 1
 184         &amt_jtagaccel_interface,
 185 #endif
 186 #if BUILD_EP93XX == 1
 187         &ep93xx_interface,
 188 #endif
 189 #if BUILD_AT91RM9200 == 1
 190         &at91rm9200_interface,
 191 #endif
 192 #if BUILD_GW16012 == 1
 193         &gw16012_interface,
 194 #endif
 195 #if BUILD_PRESTO_LIBFTDI == 1 || BUILD_PRESTO_FTD2XX == 1
 196         &presto_interface,
 197 #endif
 198 #if BUILD_USBPROG == 1
 199         &usbprog_interface,
 200 #endif
 201 #if BUILD_JLINK == 1
 202         &jlink_interface,
 203 #endif
 204 #if BUILD_VSLLINK == 1
 205         &vsllink_interface,
 206 #endif
 207 #if BUILD_RLINK == 1
 208         &rlink_interface,
 209 #endif
 210 #if BUILD_ARMJTAGEW == 1
 211         &armjtagew_interface,
 212 #endif
 213         NULL,
 214 };
 215
 216 jtag_interface_t *jtag = NULL;
 217
 218 /* configuration */
 219 static jtag_interface_t *jtag_interface = NULL;
 220 int jtag_speed = 0;
 221
 222 /* forward declarations */
 223 //void jtag_add_pathmove(int num_states, tap_state_t *path);
 224 //void jtag_add_runtest(int num_cycles, tap_state_t endstate);
 225 //void jtag_add_end_state(tap_state_t endstate);
 226 //void jtag_add_sleep(u32 us);
 227 //int jtag_execute_queue(void);
 228 static tap_state_t tap_state_by_name(const char *name);
 229
 230 /* jtag commands */
 231 static int handle_interface_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
 232 static int handle_jtag_speed_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
 233 static int handle_jtag_khz_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
 234 static int handle_jtag_device_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
 235 static int handle_reset_config_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
 236 static int handle_jtag_nsrst_delay_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
 237 static int handle_jtag_ntrst_delay_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
 238
 239 static int handle_scan_chain_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
 240
 241 static int handle_endstate_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
 242 static int handle_jtag_reset_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
 243 static int handle_runtest_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
 244 static int handle_irscan_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
 245 static int Jim_Command_drscan(Jim_Interp *interp, int argc, Jim_Obj *const *argv);
 246 static int Jim_Command_flush_count(Jim_Interp *interp, int argc, Jim_Obj *const *args);
 247
 248 static int handle_verify_ircapture_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
 249
 250 jtag_tap_t *jtag_AllTaps(void)
 251 {
 252         return jtag_all_taps;
 253 };
 254
 255 int jtag_NumTotalTaps(void)
 256 {
 257         jtag_tap_t *t;
 258         int n;
 259
 260         n = 0;
 261         t = jtag_AllTaps();
 262         while(t){
 263                 n++;
 264                 t = t->next_tap;
 265         }
 266         return n;
 267 }
 268
 269 int jtag_NumEnabledTaps(void)
 270 {
 271         jtag_tap_t *t;
 272         int n;
 273
 274         n = 0;
 275         t = jtag_AllTaps();
 276         while(t){
 277                 if( t->enabled ){
 278                         n++;
 279                 }
 280                 t = t->next_tap;
 281         }
 282         return n;
 283 }
 284
 285 jtag_tap_t *jtag_TapByString( const char *s )
 286 {
 287         jtag_tap_t *t;
 288         char *cp;
 289
 290         t = jtag_AllTaps();
 291         /* try name first */
 292         while(t){
 293                 if( 0 == strcmp( t->dotted_name, s ) ){
 294                         break;
 295                 } else {
 296                         t = t->next_tap;
 297                 }
 298         }
 299         /* backup plan is by number */
 300         if( t == NULL ){
 301                 /* ok - is "s" a number? */
 302                 int n;
 303                 n = strtol( s, &cp, 0 );
 304                 if( (s != cp) && (*cp == 0) ){
 305                         /* Then it is... */
 306                         t = jtag_TapByAbsPosition(n);
 307                 }
 308         }
 309         return t;
 310 }
 311
 312 jtag_tap_t * jtag_TapByJimObj( Jim_Interp *interp, Jim_Obj *o )
 313 {
 314         jtag_tap_t *t;
 315         const char *cp;
 316
 317         cp = Jim_GetString( o, NULL );
 318         if(cp == NULL){
 319                 cp = "(unknown)";
 320                 t = NULL;
 321         }  else {
 322                 t = jtag_TapByString( cp );
 323         }
 324         if( t == NULL ){
 325                 Jim_SetResult_sprintf(interp,"Tap: %s is unknown", cp );
 326         }
 327         return t;
 328 }
 329
 330 /* returns a pointer to the n-th device in the scan chain */
 331 jtag_tap_t * jtag_TapByAbsPosition( int n )
 332 {
 333         int orig_n;
 334         jtag_tap_t *t;
 335
 336         orig_n = n;
 337         t = jtag_AllTaps();
 338
 339         while( t && (n > 0)) {
 340                 n--;
 341                 t = t->next_tap;
 342         }
 343         return t;
 344 }
 345
 346 int jtag_register_event_callback(int (*callback)(enum jtag_event event, void *priv), void *priv)
 347 {
 348         jtag_event_callback_t **callbacks_p = &jtag_event_callbacks;
 349
 350         if (callback == NULL)
 351         {
 352                 return ERROR_INVALID_ARGUMENTS;
 353         }
 354
 355         if (*callbacks_p)
 356         {
 357                 while ((*callbacks_p)->next)
 358                         callbacks_p = &((*callbacks_p)->next);
 359                 callbacks_p = &((*callbacks_p)->next);
 360         }
 361
 362         (*callbacks_p) = malloc(sizeof(jtag_event_callback_t));
 363         (*callbacks_p)->callback = callback;
 364         (*callbacks_p)->priv = priv;
 365         (*callbacks_p)->next = NULL;
 366
 367         return ERROR_OK;
 368 }
 369
 370 int jtag_unregister_event_callback(int (*callback)(enum jtag_event event, void *priv))
 371 {
 372         jtag_event_callback_t **callbacks_p = &jtag_event_callbacks;
 373
 374         if (callback == NULL)
 375         {
 376                 return ERROR_INVALID_ARGUMENTS;
 377         }
 378
 379         while (*callbacks_p)
 380         {
 381                 jtag_event_callback_t **next = &((*callbacks_p)->next);
 382                 if ((*callbacks_p)->callback == callback)
 383                 {
 384                         free(*callbacks_p);
 385                         *callbacks_p = *next;
 386                 }
 387                 callbacks_p = next;
 388         }
 389
 390         return ERROR_OK;
 391 }
 392
 393 int jtag_call_event_callbacks(enum jtag_event event)
 394 {
 395         jtag_event_callback_t *callback = jtag_event_callbacks;
 396
 397         LOG_DEBUG("jtag event: %s", jtag_event_strings[event]);
 398
 399         while (callback)
 400         {
 401                 callback->callback(event, callback->priv);
 402                 callback = callback->next;
 403         }
 404
 405         return ERROR_OK;
 406 }
 407
 408 /* returns a pointer to the pointer of the last command in queue
 409  * this may be a pointer to the root pointer (jtag_command_queue)
 410  * or to the next member of the last but one command
 411  */
 412 jtag_command_t** jtag_get_last_command_p(void)
 413 {
 414 /*      jtag_command_t *cmd = jtag_command_queue;
 415
 416         if (cmd)
 417                 while (cmd->next)
 418                         cmd = cmd->next;
 419         else
 420                 return &jtag_command_queue;
 421
 422         return &cmd->next;*/
 423
 424         return last_comand_pointer;
 425 }
 426
 427 void* cmd_queue_alloc(size_t size)
 428 {
 429         cmd_queue_page_t **p_page = &cmd_queue_pages;
 430         int offset;
 431         u8 *t;
 432
 433         /*
 434          * WARNING:
 435          *    We align/round the *SIZE* per below
 436          *    so that all pointers returned by
 437          *    this function are reasonably well
 438          *    aligned.
 439          *
 440          * If we did not, then an "odd-length" request would cause the
 441          * *next* allocation to be at an *odd* address, and because
 442          * this function has the same type of api as malloc() - we
 443          * must also return pointers that have the same type of
 444          * alignment.
 445          *
 446          * What I do not/have is a reasonable portable means
 447          * to align by...
 448          *
 449          * The solution here, is based on these suggestions.
 450          * http://gcc.gnu.org/ml/gcc-help/2008-12/msg00041.html
 451          *
 452          */
 453         union worse_case_align {
 454                 int i;
 455                 long l;
 456                 float f;
 457                 void *v;
 458         };
 459 #define ALIGN_SIZE  (sizeof(union worse_case_align))
 460
 461         /* The alignment process. */
 462         size = (size + ALIGN_SIZE -1) & (~(ALIGN_SIZE-1));
 463         /* Done... */
 464
 465         if (*p_page)
 466         {
 467                 while ((*p_page)->next)
 468                         p_page = &((*p_page)->next);
 469                 if (CMD_QUEUE_PAGE_SIZE - (*p_page)->used < size)
 470                         p_page = &((*p_page)->next);
 471         }
 472
 473         if (!*p_page)
 474         {
 475                 *p_page = malloc(sizeof(cmd_queue_page_t));
 476                 (*p_page)->used = 0;
 477                 (*p_page)->address = malloc(CMD_QUEUE_PAGE_SIZE);
 478                 (*p_page)->next = NULL;
 479         }
 480
 481         offset = (*p_page)->used;
 482         (*p_page)->used += size;
 483
 484         t=(u8 *)((*p_page)->address);
 485         return t + offset;
 486 }
 487
 488 void cmd_queue_free(void)
 489 {
 490         cmd_queue_page_t *page = cmd_queue_pages;
 491
 492         while (page)
 493         {
 494                 cmd_queue_page_t *last = page;
 495                 free(page->address);
 496                 page = page->next;
 497                 free(last);
 498         }
 499
 500         cmd_queue_pages = NULL;
 501 }
 502
 503 static void jtag_prelude1(void)
 504 {
 505         if (jtag_trst == 1)
 506         {
 507                 LOG_WARNING("JTAG command queued, while TRST is low (TAP in reset)");
 508                 jtag_error=ERROR_JTAG_TRST_ASSERTED;
 509                 return;
 510         }
 511
 512         if (cmd_queue_end_state == TAP_RESET)
 513                 jtag_call_event_callbacks(JTAG_TRST_ASSERTED);
 514 }
 515
 516 static void jtag_prelude(tap_state_t state)
 517 {
 518         jtag_prelude1();
 519
 520         if (state != TAP_INVALID)
 521                 jtag_add_end_state(state);
 522
 523         cmd_queue_cur_state = cmd_queue_end_state;
 524 }
 525
 526 void jtag_add_ir_scan_noverify(int num_fields, scan_field_t *fields, tap_state_t state)
 527 {
 528         int retval;
 529         jtag_prelude(state);
 530
 531         retval=interface_jtag_add_ir_scan(num_fields, fields, cmd_queue_end_state);
 532         if (retval!=ERROR_OK)
 533                 jtag_error=retval;
 534
 535 }
 536
 537
 538 void jtag_add_ir_scan(int num_fields, scan_field_t *fields, tap_state_t state)
 539 {
 540         /* 8 x 32 bit id's is enough for all invoations */
 541         u32 id[8];
 542         int modified[8];
 543
 544
 545         /* if we are to run a verification of the ir scan, we need to get the input back.
 546          * We may have to allocate space if the caller didn't ask for the input back.
 547          *
 548          */
 549         if (jtag_verify_capture_ir)
 550         {
 551                 int j;
 552                 for (j = 0; j < num_fields; j++)
 553                 {
 554                         modified[j]=0;
 555                         if ((fields[j].in_value==NULL)&&(fields[j].num_bits<=32))
 556                         {
 557                                 if (j<8)
 558                                 {
 559                                         modified[j]=1;
 560                                         fields[j].in_value=(u8 *)(id+j);
 561                                 } else
 562                                 {
 563                                         LOG_DEBUG("caller must provide in_value space for verify_capture_ir to work");
 564                                 }
 565                         }
 566                 }
 567         }
 568
 569         jtag_add_ir_scan_noverify(num_fields, fields, state);
 570
 571         if (jtag_verify_capture_ir)
 572         {
 573                 int j;
 574                 for (j = 0; j < num_fields; j++)
 575                 {
 576                         jtag_tap_t *tap=fields[j].tap;
 577                         jtag_check_value_mask(fields+j, tap->expected, tap->expected_mask);
 578
 579                         if (modified[j])
 580                         {
 581                                 fields[j].in_value=NULL;
 582                         }
 583                 }
 584         }
 585
 586 }
 587
 588 int MINIDRIVER(interface_jtag_add_ir_scan)(int num_fields, scan_field_t *fields, tap_state_t state)
 589 {
 590         jtag_command_t **last_cmd;
 591         jtag_tap_t *tap;
 592         int j;
 593         int x;
 594         int nth_tap;
 595         int scan_size = 0;
 596
 597         last_cmd = jtag_get_last_command_p();
 598
 599         /* allocate memory for a new list member */
 600         *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
 601         (*last_cmd)->next = NULL;
 602         last_comand_pointer = &((*last_cmd)->next);
 603         (*last_cmd)->type = JTAG_SCAN;
 604
 605         /* allocate memory for ir scan command */
 606         (*last_cmd)->cmd.scan = cmd_queue_alloc(sizeof(scan_command_t));
 607         (*last_cmd)->cmd.scan->ir_scan = 1;
 608         x = jtag_NumEnabledTaps();
 609         (*last_cmd)->cmd.scan->num_fields = x;  /* one field per device */
 610         (*last_cmd)->cmd.scan->fields = cmd_queue_alloc(x  * sizeof(scan_field_t));
 611         (*last_cmd)->cmd.scan->end_state = state;
 612
 613         nth_tap = -1;
 614         tap = NULL;
 615         for(;;){
 616                 int found = 0;
 617
 618                 /* do this here so it is not forgotten */
 619                 tap = jtag_NextEnabledTap(tap);
 620                 if( tap == NULL ){
 621                         break;
 622                 }
 623                 nth_tap++;
 624                 scan_size = tap->ir_length;
 625                 (*last_cmd)->cmd.scan->fields[nth_tap].tap = tap;
 626                 (*last_cmd)->cmd.scan->fields[nth_tap].num_bits = scan_size;
 627                 (*last_cmd)->cmd.scan->fields[nth_tap].in_value = fields[nth_tap].in_value;
 628
 629                 /* search the list */
 630                 for (j = 0; j < num_fields; j++)
 631                 {
 632                         if (tap == fields[j].tap)
 633                         {
 634                                 found = 1;
 635                                 (*last_cmd)->cmd.scan->fields[nth_tap].out_value = buf_cpy(fields[j].out_value, cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);
 636
 637                                 tap->bypass = 0;
 638                                 break;
 639                         }
 640                 }
 641
 642                 if (!found)
 643                 {
 644                         /* if a tap isn't listed, set it to BYPASS */
 645                         (*last_cmd)->cmd.scan->fields[nth_tap].out_value = buf_set_ones(cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);
 646                         tap->bypass = 1;
 647                 }
 648
 649                 /* update device information */
 650                 buf_cpy((*last_cmd)->cmd.scan->fields[nth_tap].out_value, tap->cur_instr, scan_size);
 651         }
 652
 653         return ERROR_OK;
 654 }
 655
 656 void jtag_add_plain_ir_scan(int num_fields, scan_field_t *fields, tap_state_t state)
 657 {
 658         int retval;
 659
 660         jtag_prelude(state);
 661
 662         retval=interface_jtag_add_plain_ir_scan(num_fields, fields, cmd_queue_end_state);
 663         if (retval!=ERROR_OK)
 664                 jtag_error=retval;
 665 }
 666
 667 int MINIDRIVER(interface_jtag_add_plain_ir_scan)(int num_fields, scan_field_t *fields, tap_state_t state)
 668 {
 669         int i;
 670         jtag_command_t **last_cmd;
 671
 672         last_cmd = jtag_get_last_command_p();
 673
 674         /* allocate memory for a new list member */
 675         *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
 676         (*last_cmd)->next = NULL;
 677         last_comand_pointer = &((*last_cmd)->next);
 678         (*last_cmd)->type = JTAG_SCAN;
 679
 680         /* allocate memory for ir scan command */
 681         (*last_cmd)->cmd.scan = cmd_queue_alloc(sizeof(scan_command_t));
 682         (*last_cmd)->cmd.scan->ir_scan = 1;
 683         (*last_cmd)->cmd.scan->num_fields = num_fields;
 684         (*last_cmd)->cmd.scan->fields = cmd_queue_alloc(num_fields * sizeof(scan_field_t));
 685         (*last_cmd)->cmd.scan->end_state = state;
 686
 687         for( i = 0 ; i < num_fields ; i++ ){
 688                 int num_bits = fields[i].num_bits;
 689                 int num_bytes = CEIL(fields[i].num_bits, 8);
 690                 (*last_cmd)->cmd.scan->fields[i].tap = fields[i].tap;
 691                 (*last_cmd)->cmd.scan->fields[i].num_bits = num_bits;
 692                 (*last_cmd)->cmd.scan->fields[i].out_value = buf_cpy(fields[i].out_value, cmd_queue_alloc(num_bytes), num_bits);
 693                 (*last_cmd)->cmd.scan->fields[i].in_value = fields[i].in_value;
 694         }
 695         return ERROR_OK;
 696 }
 697
 698 void jtag_add_dr_scan(int num_fields, scan_field_t *fields, tap_state_t state)
 699 {
 700         int retval;
 701
 702         jtag_prelude(state);
 703
 704         retval=interface_jtag_add_dr_scan(num_fields, fields, cmd_queue_end_state);
 705         if (retval!=ERROR_OK)
 706                 jtag_error=retval;
 707 }
 708
 709 void jtag_add_dr_scan_now(int num_fields, scan_field_t *fields, tap_state_t state)
 710 {
 711         jtag_add_dr_scan(num_fields, fields, state);
 712         jtag_execute_queue_noclear();
 713 }
 714
 715 int MINIDRIVER(interface_jtag_add_dr_scan)(int num_fields, scan_field_t *fields, tap_state_t state)
 716 {
 717         int j;
 718         int nth_tap;
 719         int bypass_devices = 0;
 720         int field_count = 0;
 721         int scan_size;
 722
 723         jtag_command_t **last_cmd = jtag_get_last_command_p();
 724         jtag_tap_t *tap;
 725
 726         /* count devices in bypass */
 727         tap = NULL;
 728         bypass_devices = 0;
 729         for(;;){
 730                 tap = jtag_NextEnabledTap(tap);
 731                 if( tap == NULL ){
 732                         break;
 733                 }
 734                 if( tap->bypass ){
 735                         bypass_devices++;
 736                 }
 737         }
 738
 739         /* allocate memory for a new list member */
 740         *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
 741         last_comand_pointer = &((*last_cmd)->next);
 742         (*last_cmd)->next = NULL;
 743         (*last_cmd)->type = JTAG_SCAN;
 744
 745         /* allocate memory for dr scan command */
 746         (*last_cmd)->cmd.scan = cmd_queue_alloc(sizeof(scan_command_t));
 747         (*last_cmd)->cmd.scan->ir_scan = 0;
 748         (*last_cmd)->cmd.scan->num_fields = num_fields + bypass_devices;
 749         (*last_cmd)->cmd.scan->fields = cmd_queue_alloc((num_fields + bypass_devices) * sizeof(scan_field_t));
 750         (*last_cmd)->cmd.scan->end_state = state;
 751
 752         tap = NULL;
 753         nth_tap = -1;
 754         for(;;){
 755                 nth_tap++;
 756                 tap = jtag_NextEnabledTap(tap);
 757                 if( tap == NULL ){
 758                         break;
 759                 }
 760                 int found = 0;
 761                 (*last_cmd)->cmd.scan->fields[field_count].tap = tap;
 762
 763                 for (j = 0; j < num_fields; j++)
 764                 {
 765                         if (tap == fields[j].tap)
 766                         {
 767                                 found = 1;
 768                                 scan_size = fields[j].num_bits;
 769                                 (*last_cmd)->cmd.scan->fields[field_count].num_bits = scan_size;
 770                                 (*last_cmd)->cmd.scan->fields[field_count].out_value = buf_cpy(fields[j].out_value, cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);
 771                                 (*last_cmd)->cmd.scan->fields[field_count].in_value = fields[j].in_value;
 772                                 field_count++;
 773                         }
 774                 }
 775                 if (!found)
 776                 {
 777 #ifdef _DEBUG_JTAG_IO_
 778                         /* if a device isn't listed, the BYPASS register should be selected */
 779                         if (! tap->bypass)
 780                         {
 781                                 LOG_ERROR("BUG: no scan data for a device not in BYPASS");
 782                                 exit(-1);
 783                         }
 784 #endif
 785                         /* program the scan field to 1 bit length, and ignore it's value */
 786                         (*last_cmd)->cmd.scan->fields[field_count].num_bits = 1;
 787                         (*last_cmd)->cmd.scan->fields[field_count].out_value = NULL;
 788                         (*last_cmd)->cmd.scan->fields[field_count].in_value = NULL;
 789                         field_count++;
 790                 }
 791                 else
 792                 {
 793 #ifdef _DEBUG_JTAG_IO_
 794                         /* if a device is listed, the BYPASS register must not be selected */
 795                         if (tap->bypass)
 796                         {
 797                                 LOG_ERROR("BUG: scan data for a device in BYPASS");
 798                                 exit(-1);
 799                         }
 800 #endif
 801                 }
 802         }
 803         return ERROR_OK;
 804 }
 805
 806 void MINIDRIVER(interface_jtag_add_dr_out)(jtag_tap_t *target_tap,
 807                 int num_fields,
 808                 const int *num_bits,
 809                 const u32 *value,
 810                 tap_state_t end_state)
 811 {
 812         int nth_tap;
 813         int field_count = 0;
 814         int scan_size;
 815         int bypass_devices = 0;
 816
 817         jtag_command_t **last_cmd = jtag_get_last_command_p();
 818         jtag_tap_t *tap;
 819
 820         /* count devices in bypass */
 821         tap = NULL;
 822         bypass_devices = 0;
 823         for(;;){
 824                 tap = jtag_NextEnabledTap(tap);
 825                 if( tap == NULL ){
 826                         break;
 827                 }
 828                 if( tap->bypass ){
 829                         bypass_devices++;
 830                 }
 831         }
 832
 833         /* allocate memory for a new list member */
 834         *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
 835         last_comand_pointer = &((*last_cmd)->next);
 836         (*last_cmd)->next = NULL;
 837         (*last_cmd)->type = JTAG_SCAN;
 838
 839         /* allocate memory for dr scan command */
 840         (*last_cmd)->cmd.scan = cmd_queue_alloc(sizeof(scan_command_t));
 841         (*last_cmd)->cmd.scan->ir_scan = 0;
 842         (*last_cmd)->cmd.scan->num_fields = num_fields + bypass_devices;
 843         (*last_cmd)->cmd.scan->fields = cmd_queue_alloc((num_fields + bypass_devices) * sizeof(scan_field_t));
 844         (*last_cmd)->cmd.scan->end_state = end_state;
 845
 846         tap = NULL;
 847         nth_tap = -1;
 848         for(;;){
 849                 tap = jtag_NextEnabledTap(tap);
 850                 if( tap == NULL ){
 851                         break;
 852                 }
 853                 nth_tap++;
 854                 (*last_cmd)->cmd.scan->fields[field_count].tap = tap;
 855
 856                 if (tap == target_tap)
 857                 {
 858                         int j;
 859 #ifdef _DEBUG_JTAG_IO_
 860                         /* if a device is listed, the BYPASS register must not be selected */
 861                         if (tap->bypass)
 862                         {
 863                                 LOG_ERROR("BUG: scan data for a device in BYPASS");
 864                                 exit(-1);
 865                         }
 866 #endif
 867                         for (j = 0; j < num_fields; j++)
 868                         {
 869                                 u8 out_value[4];
 870                                 scan_size = num_bits[j];
 871                                 buf_set_u32(out_value, 0, scan_size, value[j]);
 872                                 (*last_cmd)->cmd.scan->fields[field_count].num_bits = scan_size;
 873                                 (*last_cmd)->cmd.scan->fields[field_count].out_value = buf_cpy(out_value, cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);
 874                                 (*last_cmd)->cmd.scan->fields[field_count].in_value = NULL;
 875                                 field_count++;
 876                         }
 877                 } else
 878                 {
 879 #ifdef _DEBUG_JTAG_IO_
 880                         /* if a device isn't listed, the BYPASS register should be selected */
 881                         if (! tap->bypass)
 882                         {
 883                                 LOG_ERROR("BUG: no scan data for a device not in BYPASS");
 884                                 exit(-1);
 885                         }
 886 #endif
 887                         /* program the scan field to 1 bit length, and ignore it's value */
 888                         (*last_cmd)->cmd.scan->fields[field_count].num_bits = 1;
 889                         (*last_cmd)->cmd.scan->fields[field_count].out_value = NULL;
 890                         (*last_cmd)->cmd.scan->fields[field_count].in_value = NULL;
 891                         field_count++;
 892                 }
 893         }
 894 }
 895
 896 void jtag_add_plain_dr_scan(int num_fields, scan_field_t *fields, tap_state_t state)
 897 {
 898         int retval;
 899
 900         jtag_prelude(state);
 901
 902         retval=interface_jtag_add_plain_dr_scan(num_fields, fields, cmd_queue_end_state);
 903         if (retval!=ERROR_OK)
 904                 jtag_error=retval;
 905 }
 906
 907 int MINIDRIVER(interface_jtag_add_plain_dr_scan)(int num_fields, scan_field_t *fields, tap_state_t state)
 908 {
 909         int i;
 910         jtag_command_t **last_cmd = jtag_get_last_command_p();
 911
 912         /* allocate memory for a new list member */
 913         *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
 914         last_comand_pointer = &((*last_cmd)->next);
 915         (*last_cmd)->next = NULL;
 916         (*last_cmd)->type = JTAG_SCAN;
 917
 918         /* allocate memory for scan command */
 919         (*last_cmd)->cmd.scan = cmd_queue_alloc(sizeof(scan_command_t));
 920         (*last_cmd)->cmd.scan->ir_scan = 0;
 921         (*last_cmd)->cmd.scan->num_fields = num_fields;
 922         (*last_cmd)->cmd.scan->fields = cmd_queue_alloc(num_fields * sizeof(scan_field_t));
 923         (*last_cmd)->cmd.scan->end_state = state;
 924
 925         for (i = 0; i < num_fields; i++)
 926         {
 927                 int num_bits = fields[i].num_bits;
 928                 int num_bytes = CEIL(fields[i].num_bits, 8);
 929                 (*last_cmd)->cmd.scan->fields[i].tap = fields[i].tap;
 930                 (*last_cmd)->cmd.scan->fields[i].num_bits = num_bits;
 931                 (*last_cmd)->cmd.scan->fields[i].out_value = buf_cpy(fields[i].out_value, cmd_queue_alloc(num_bytes), num_bits);
 932                 (*last_cmd)->cmd.scan->fields[i].in_value = fields[i].in_value;
 933         }
 934
 935         return ERROR_OK;
 936 }
 937
 938 void jtag_add_tlr(void)
 939 {
 940         jtag_prelude(TAP_RESET);
 941
 942         int retval;
 943         retval=interface_jtag_add_tlr();
 944         if (retval!=ERROR_OK)
 945                 jtag_error=retval;
 946 }
 947
 948 int MINIDRIVER(interface_jtag_add_tlr)(void)
 949 {
 950         tap_state_t state = TAP_RESET;
 951         jtag_command_t **last_cmd = jtag_get_last_command_p();
 952
 953         /* allocate memory for a new list member */
 954         *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
 955         last_comand_pointer = &((*last_cmd)->next);
 956         (*last_cmd)->next = NULL;
 957         (*last_cmd)->type = JTAG_STATEMOVE;
 958
 959         (*last_cmd)->cmd.statemove = cmd_queue_alloc(sizeof(statemove_command_t));
 960         (*last_cmd)->cmd.statemove->end_state = state;
 961
 962         return ERROR_OK;
 963 }
 964
 965 void jtag_add_pathmove(int num_states, tap_state_t *path)
 966 {
 967         tap_state_t cur_state = cmd_queue_cur_state;
 968         int i;
 969         int retval;
 970
 971         /* the last state has to be a stable state */
 972         if (!tap_is_state_stable(path[num_states - 1]))
 973         {
 974                 LOG_ERROR("BUG: TAP path doesn't finish in a stable state");
 975                 exit(-1);
 976         }
 977
 978         for (i=0; i<num_states; i++)
 979         {
 980                 if (path[i] == TAP_RESET)
 981                 {
 982                         LOG_ERROR("BUG: TAP_RESET is not a valid state for pathmove sequences");
 983                         exit(-1);
 984                 }
 985
 986                 if ( tap_state_transition(cur_state, true)  != path[i]
 987                   && tap_state_transition(cur_state, false) != path[i])
 988                 {
 989                         LOG_ERROR("BUG: %s -> %s isn't a valid TAP transition", tap_state_name(cur_state), tap_state_name(path[i]));
 990                         exit(-1);
 991                 }
 992                 cur_state = path[i];
 993         }
 994
 995         jtag_prelude1();
 996
 997         retval = interface_jtag_add_pathmove(num_states, path);
 998         cmd_queue_cur_state = path[num_states - 1];
 999         if (retval!=ERROR_OK)
1000                 jtag_error=retval;
1001 }
1002
1003 int MINIDRIVER(interface_jtag_add_pathmove)(int num_states, tap_state_t *path)
1004 {
1005         jtag_command_t **last_cmd = jtag_get_last_command_p();
1006         int i;
1007
1008         /* allocate memory for a new list member */
1009         *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
1010         last_comand_pointer = &((*last_cmd)->next);
1011         (*last_cmd)->next = NULL;
1012         (*last_cmd)->type = JTAG_PATHMOVE;
1013
1014         (*last_cmd)->cmd.pathmove = cmd_queue_alloc(sizeof(pathmove_command_t));
1015         (*last_cmd)->cmd.pathmove->num_states = num_states;
1016         (*last_cmd)->cmd.pathmove->path = cmd_queue_alloc(sizeof(tap_state_t) * num_states);
1017
1018         for (i = 0; i < num_states; i++)
1019                 (*last_cmd)->cmd.pathmove->path[i] = path[i];
1020
1021         return ERROR_OK;
1022 }
1023
1024 int MINIDRIVER(interface_jtag_add_runtest)(int num_cycles, tap_state_t state)
1025 {
1026         jtag_command_t **last_cmd = jtag_get_last_command_p();
1027
1028         /* allocate memory for a new list member */
1029         *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
1030         (*last_cmd)->next = NULL;
1031         last_comand_pointer = &((*last_cmd)->next);
1032         (*last_cmd)->type = JTAG_RUNTEST;
1033
1034         (*last_cmd)->cmd.runtest = cmd_queue_alloc(sizeof(runtest_command_t));
1035         (*last_cmd)->cmd.runtest->num_cycles = num_cycles;
1036         (*last_cmd)->cmd.runtest->end_state = state;
1037
1038         return ERROR_OK;
1039 }
1040
1041 void jtag_add_runtest(int num_cycles, tap_state_t state)
1042 {
1043         int retval;
1044
1045         jtag_prelude(state);
1046
1047         /* executed by sw or hw fifo */
1048         retval=interface_jtag_add_runtest(num_cycles, cmd_queue_end_state);
1049         if (retval!=ERROR_OK)
1050                 jtag_error=retval;
1051 }
1052
1053
1054 int MINIDRIVER(interface_jtag_add_clocks)( int num_cycles )
1055 {
1056         jtag_command_t **last_cmd = jtag_get_last_command_p();
1057
1058         /* allocate memory for a new list member */
1059         *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
1060         (*last_cmd)->next = NULL;
1061         last_comand_pointer = &((*last_cmd)->next);
1062         (*last_cmd)->type = JTAG_STABLECLOCKS;
1063
1064         (*last_cmd)->cmd.stableclocks = cmd_queue_alloc(sizeof(stableclocks_command_t));
1065         (*last_cmd)->cmd.stableclocks->num_cycles = num_cycles;
1066         return ERROR_OK;
1067 }
1068
1069 void jtag_add_clocks( int num_cycles )
1070 {
1071         int retval;
1072
1073         if( !tap_is_state_stable(cmd_queue_cur_state) )
1074         {
1075                  LOG_ERROR( "jtag_add_clocks() was called with TAP in non-stable state \"%s\"",
1076                                  tap_state_name(cmd_queue_cur_state) );
1077                  jtag_error = ERROR_JTAG_NOT_STABLE_STATE;
1078                  return;
1079         }
1080
1081         if( num_cycles > 0 )
1082         {
1083                 jtag_prelude1();
1084
1085                 retval = interface_jtag_add_clocks(num_cycles);
1086                 if (retval != ERROR_OK)
1087                         jtag_error=retval;
1088         }
1089 }
1090
1091 void jtag_add_reset(int req_tlr_or_trst, int req_srst)
1092 {
1093         int trst_with_tlr = 0;
1094         int retval;
1095
1096         /* FIX!!! there are *many* different cases here. A better
1097          * approach is needed for legal combinations of transitions...
1098          */
1099         if ((jtag_reset_config & RESET_HAS_SRST)&&
1100                         (jtag_reset_config & RESET_HAS_TRST)&&
1101                         ((jtag_reset_config & RESET_SRST_PULLS_TRST)==0))
1102         {
1103                 if (((req_tlr_or_trst&&!jtag_trst)||
1104                                 (!req_tlr_or_trst&&jtag_trst))&&
1105                                 ((req_srst&&!jtag_srst)||
1106                                                 (!req_srst&&jtag_srst)))
1107                 {
1108                         /* FIX!!! srst_pulls_trst allows 1,1 => 0,0 transition.... */
1109                         //LOG_ERROR("BUG: transition of req_tlr_or_trst and req_srst in the same jtag_add_reset() call is undefined");
1110                 }
1111         }
1112
1113         /* Make sure that jtag_reset_config allows the requested reset */
1114         /* if SRST pulls TRST, we can't fulfill srst == 1 with trst == 0 */
1115         if (((jtag_reset_config & RESET_SRST_PULLS_TRST) && (req_srst == 1)) && (!req_tlr_or_trst))
1116         {
1117                 LOG_ERROR("BUG: requested reset would assert trst");
1118                 jtag_error=ERROR_FAIL;
1119                 return;
1120         }
1121
1122         /* if TRST pulls SRST, we reset with TAP T-L-R */
1123         if (((jtag_reset_config & RESET_TRST_PULLS_SRST) && (req_tlr_or_trst)) && (req_srst == 0))
1124         {
1125                 trst_with_tlr = 1;
1126         }
1127
1128         if (req_srst && !(jtag_reset_config & RESET_HAS_SRST))
1129         {
1130                 LOG_ERROR("BUG: requested SRST assertion, but the current configuration doesn't support this");
1131                 jtag_error=ERROR_FAIL;
1132                 return;
1133         }
1134
1135         if (req_tlr_or_trst)
1136         {
1137                 if (!trst_with_tlr && (jtag_reset_config & RESET_HAS_TRST))
1138                 {
1139                         jtag_trst = 1;
1140                 } else
1141                 {
1142                         trst_with_tlr = 1;
1143                 }
1144         } else
1145         {
1146                 jtag_trst = 0;
1147         }
1148
1149         jtag_srst = req_srst;
1150
1151         retval = interface_jtag_add_reset(jtag_trst, jtag_srst);
1152         if (retval!=ERROR_OK)
1153         {
1154                 jtag_error=retval;
1155                 return;
1156         }
1157
1158         if (jtag_srst)
1159         {
1160                 LOG_DEBUG("SRST line asserted");
1161         }
1162         else
1163         {
1164                 LOG_DEBUG("SRST line released");
1165                 if (jtag_nsrst_delay)
1166                         jtag_add_sleep(jtag_nsrst_delay * 1000);
1167         }
1168
1169         if (trst_with_tlr)
1170         {
1171                 LOG_DEBUG("JTAG reset with RESET instead of TRST");
1172                 jtag_add_end_state(TAP_RESET);
1173                 jtag_add_tlr();
1174                 jtag_call_event_callbacks(JTAG_TRST_ASSERTED);
1175                 return;
1176         }
1177
1178         if (jtag_trst)
1179         {
1180                 /* we just asserted nTRST, so we're now in Test-Logic-Reset,
1181                  * and inform possible listeners about this
1182                  */
1183                 LOG_DEBUG("TRST line asserted");
1184                 cmd_queue_cur_state = TAP_RESET;
1185                 jtag_call_event_callbacks(JTAG_TRST_ASSERTED);
1186         }
1187         else
1188         {
1189                 if (jtag_ntrst_delay)
1190                         jtag_add_sleep(jtag_ntrst_delay * 1000);
1191         }
1192 }
1193
1194 int MINIDRIVER(interface_jtag_add_reset)(int req_trst, int req_srst)
1195 {
1196         jtag_command_t **last_cmd = jtag_get_last_command_p();
1197
1198         /* allocate memory for a new list member */
1199         *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
1200         (*last_cmd)->next = NULL;
1201         last_comand_pointer = &((*last_cmd)->next);
1202         (*last_cmd)->type = JTAG_RESET;
1203
1204         (*last_cmd)->cmd.reset = cmd_queue_alloc(sizeof(reset_command_t));
1205         (*last_cmd)->cmd.reset->trst = req_trst;
1206         (*last_cmd)->cmd.reset->srst = req_srst;
1207
1208         return ERROR_OK;
1209 }
1210
1211 void jtag_add_end_state(tap_state_t state)
1212 {
1213         cmd_queue_end_state = state;
1214         if ((cmd_queue_end_state == TAP_DRSHIFT)||(cmd_queue_end_state == TAP_IRSHIFT))
1215         {
1216                 LOG_ERROR("BUG: TAP_DRSHIFT/IRSHIFT can't be end state. Calling code should use a larger scan field");
1217         }
1218 }
1219
1220 int MINIDRIVER(interface_jtag_add_sleep)(u32 us)
1221 {
1222         jtag_command_t **last_cmd = jtag_get_last_command_p();
1223
1224         /* allocate memory for a new list member */
1225         *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
1226         (*last_cmd)->next = NULL;
1227         last_comand_pointer = &((*last_cmd)->next);
1228         (*last_cmd)->type = JTAG_SLEEP;
1229
1230         (*last_cmd)->cmd.sleep = cmd_queue_alloc(sizeof(sleep_command_t));
1231         (*last_cmd)->cmd.sleep->us = us;
1232
1233         return ERROR_OK;
1234 }
1235
1236 void jtag_add_sleep(u32 us)
1237 {
1238         keep_alive(); /* we might be running on a very slow JTAG clk */
1239         int retval=interface_jtag_add_sleep(us);
1240         if (retval!=ERROR_OK)
1241                 jtag_error=retval;
1242         return;
1243 }
1244
1245 int jtag_scan_size(scan_command_t *cmd)
1246 {
1247         int bit_count = 0;
1248         int i;
1249
1250         /* count bits in scan command */
1251         for (i = 0; i < cmd->num_fields; i++)
1252         {
1253                 bit_count += cmd->fields[i].num_bits;
1254         }
1255
1256         return bit_count;
1257 }
1258
1259 int jtag_build_buffer(scan_command_t *cmd, u8 **buffer)
1260 {
1261         int bit_count = 0;
1262         int i;
1263
1264         bit_count = jtag_scan_size(cmd);
1265         *buffer = malloc(CEIL(bit_count, 8));
1266
1267         bit_count = 0;
1268
1269 #ifdef _DEBUG_JTAG_IO_
1270         LOG_DEBUG("%s num_fields: %i", cmd->ir_scan ? "IRSCAN" : "DRSCAN", cmd->num_fields);
1271 #endif
1272
1273         for (i = 0; i < cmd->num_fields; i++)
1274         {
1275                 if (cmd->fields[i].out_value)
1276                 {
1277 #ifdef _DEBUG_JTAG_IO_
1278                         char* char_buf = buf_to_str(cmd->fields[i].out_value, (cmd->fields[i].num_bits > DEBUG_JTAG_IOZ) ? DEBUG_JTAG_IOZ : cmd->fields[i].num_bits, 16);
1279 #endif
1280                         buf_set_buf(cmd->fields[i].out_value, 0, *buffer, bit_count, cmd->fields[i].num_bits);
1281 #ifdef _DEBUG_JTAG_IO_
1282                         LOG_DEBUG("fields[%i].out_value[%i]: 0x%s", i, cmd->fields[i].num_bits, char_buf);
1283                         free(char_buf);
1284 #endif
1285                 }
1286                 else
1287                 {
1288 #ifdef _DEBUG_JTAG_IO_
1289                         LOG_DEBUG("fields[%i].out_value[%i]: NULL", i, cmd->fields[i].num_bits);
1290 #endif
1291                 }
1292
1293                 bit_count += cmd->fields[i].num_bits;
1294         }
1295
1296 #ifdef _DEBUG_JTAG_IO_
1297         //LOG_DEBUG("bit_count totalling: %i",  bit_count );
1298 #endif
1299
1300         return bit_count;
1301 }
1302
1303 int jtag_read_buffer(u8 *buffer, scan_command_t *cmd)
1304 {
1305         int i;
1306         int bit_count = 0;
1307         int retval;
1308
1309         /* we return ERROR_OK, unless a check fails, or a handler reports a problem */
1310         retval = ERROR_OK;
1311
1312         for (i = 0; i < cmd->num_fields; i++)
1313         {
1314                 /* if neither in_value nor in_handler
1315                  * are specified we don't have to examine this field
1316                  */
1317                 if (cmd->fields[i].in_value)
1318                 {
1319                         int num_bits = cmd->fields[i].num_bits;
1320                         u8 *captured = buf_set_buf(buffer, bit_count, malloc(CEIL(num_bits, 8)), 0, num_bits);
1321
1322 #ifdef _DEBUG_JTAG_IO_
1323                         char *char_buf = buf_to_str(captured, (num_bits > DEBUG_JTAG_IOZ) ? DEBUG_JTAG_IOZ : num_bits, 16);
1324                         LOG_DEBUG("fields[%i].in_value[%i]: 0x%s", i, num_bits, char_buf);
1325                         free(char_buf);
1326 #endif
1327
1328                         if (cmd->fields[i].in_value)
1329                         {
1330                                 buf_cpy(captured, cmd->fields[i].in_value, num_bits);
1331                         }
1332
1333                         free(captured);
1334                 }
1335                 bit_count += cmd->fields[i].num_bits;
1336         }
1337
1338         return retval;
1339 }
1340
1341 static const char *jtag_tap_name(jtag_tap_t *tap)
1342 {
1343         return (tap == NULL) ? "(unknown)" : tap->dotted_name;
1344 }
1345
1346 int jtag_check_value_inner(u8 *captured, scan_field_t *field, u8 *in_check_value, u8 *in_check_mask)
1347 {
1348         int retval = ERROR_OK;
1349         int num_bits = field->num_bits;
1350
1351         int compare_failed = 0;
1352
1353         if (in_check_mask)
1354                 compare_failed = buf_cmp_mask(captured, in_check_value, in_check_mask, num_bits);
1355         else
1356                 compare_failed = buf_cmp(captured, in_check_value, num_bits);
1357
1358         if (compare_failed){
1359                 /* An error handler could have caught the failing check
1360                  * only report a problem when there wasn't a handler, or if the handler
1361                  * acknowledged the error
1362                  */
1363                 LOG_WARNING("TAP %s:",
1364                                         jtag_tap_name(field->tap));
1365                 if (compare_failed)
1366                 {
1367                         char *captured_char = buf_to_str(captured, (num_bits > DEBUG_JTAG_IOZ) ? DEBUG_JTAG_IOZ : num_bits, 16);
1368                         char *in_check_value_char = buf_to_str(in_check_value, (num_bits > DEBUG_JTAG_IOZ) ? DEBUG_JTAG_IOZ : num_bits, 16);
1369
1370                         if (in_check_mask)
1371                         {
1372                                 char *in_check_mask_char;
1373                                 in_check_mask_char = buf_to_str(in_check_mask, (num_bits > DEBUG_JTAG_IOZ) ? DEBUG_JTAG_IOZ : num_bits, 16);
1374                                 LOG_WARNING("value captured during scan didn't pass the requested check:");
1375                                 LOG_WARNING("captured: 0x%s check_value: 0x%s check_mask: 0x%s",
1376                                                         captured_char, in_check_value_char, in_check_mask_char);
1377                                 free(in_check_mask_char);
1378                         }
1379                         else
1380                         {
1381                                 LOG_WARNING("value captured during scan didn't pass the requested check: captured: 0x%s check_value: 0x%s", captured_char, in_check_value_char);
1382                         }
1383
1384                         free(captured_char);
1385                         free(in_check_value_char);
1386
1387                         retval = ERROR_JTAG_QUEUE_FAILED;
1388                 }
1389
1390         }
1391         return retval;
1392 }
1393
1394 void jtag_check_value_mask(scan_field_t *field, u8 *value, u8 *mask)
1395 {
1396         assert(field->in_value != NULL);
1397
1398         if (value==NULL)
1399         {
1400                 /* no checking to do */
1401                 return;
1402         }
1403
1404         jtag_execute_queue_noclear();
1405
1406         int retval=jtag_check_value_inner(field->in_value, field, value, mask);
1407         jtag_set_error(retval);
1408 }
1409
1410
1411
1412 enum scan_type jtag_scan_type(scan_command_t *cmd)
1413 {
1414         int i;
1415         int type = 0;
1416
1417         for (i = 0; i < cmd->num_fields; i++)
1418         {
1419                 if (cmd->fields[i].in_value)
1420                         type |= SCAN_IN;
1421                 if (cmd->fields[i].out_value)
1422                         type |= SCAN_OUT;
1423         }
1424
1425         return type;
1426 }
1427
1428 int MINIDRIVER(interface_jtag_execute_queue)(void)
1429 {
1430         int retval;
1431
1432         if (jtag==NULL)
1433         {
1434                 LOG_ERROR("No JTAG interface configured yet. Issue 'init' command in startup scripts before communicating with targets.");
1435                 return ERROR_FAIL;
1436         }
1437
1438         retval = jtag->execute_queue();
1439
1440         cmd_queue_free();
1441
1442         jtag_command_queue = NULL;
1443         last_comand_pointer = &jtag_command_queue;
1444
1445         return retval;
1446 }
1447
1448 void jtag_execute_queue_noclear(void)
1449 {
1450         /* each flush can take as much as 1-2ms on high bandwidth low latency interfaces.
1451          * E.g. a JTAG over TCP/IP or USB....
1452          */
1453         jtag_flush_queue_count++;
1454
1455         int retval=interface_jtag_execute_queue();
1456         /* we keep the first error */
1457         if ((jtag_error==ERROR_OK)&&(retval!=ERROR_OK))
1458         {
1459                 jtag_error=retval;
1460         }
1461 }
1462
1463 int jtag_execute_queue(void)
1464 {
1465         int retval;
1466         jtag_execute_queue_noclear();
1467         retval=jtag_error;
1468         jtag_error=ERROR_OK;
1469         return retval;
1470 }
1471
1472 int jtag_reset_callback(enum jtag_event event, void *priv)
1473 {
1474         jtag_tap_t *tap = priv;
1475
1476         LOG_DEBUG("-");
1477
1478         if (event == JTAG_TRST_ASSERTED)
1479         {
1480                 buf_set_ones(tap->cur_instr, tap->ir_length);
1481                 tap->bypass = 1;
1482         }
1483
1484         return ERROR_OK;
1485 }
1486
1487 void jtag_sleep(u32 us)
1488 {
1489         alive_sleep(us/1000);
1490 }
1491
1492 /* Try to examine chain layout according to IEEE 1149.1 §12
1493  */
1494 int jtag_examine_chain(void)
1495 {
1496         jtag_tap_t *tap;
1497         scan_field_t field;
1498         u8 idcode_buffer[JTAG_MAX_CHAIN_SIZE * 4];
1499         int i;
1500         int bit_count;
1501         int device_count = 0;
1502         u8 zero_check = 0x0;
1503         u8 one_check = 0xff;
1504
1505         field.tap = NULL;
1506         field.num_bits = sizeof(idcode_buffer) * 8;
1507         field.out_value = idcode_buffer;
1508
1509         field.in_value = idcode_buffer;
1510
1511
1512
1513
1514         for (i = 0; i < JTAG_MAX_CHAIN_SIZE; i++)
1515         {
1516                 buf_set_u32(idcode_buffer, i * 32, 32, 0x000000FF);
1517         }
1518
1519         jtag_add_plain_dr_scan(1, &field, TAP_RESET);
1520         jtag_execute_queue();
1521
1522         for (i = 0; i < JTAG_MAX_CHAIN_SIZE * 4; i++)
1523         {
1524                 zero_check |= idcode_buffer[i];
1525                 one_check &= idcode_buffer[i];
1526         }
1527
1528         /* if there wasn't a single non-zero bit or if all bits were one, the scan isn't valid */
1529         if ((zero_check == 0x00) || (one_check == 0xff))
1530         {
1531                 LOG_ERROR("JTAG communication failure, check connection, JTAG interface, target power etc.");
1532                 return ERROR_JTAG_INIT_FAILED;
1533         }
1534
1535         /* point at the 1st tap */
1536         tap = jtag_NextEnabledTap(NULL);
1537         if( tap == NULL ){
1538                 LOG_ERROR("JTAG: No taps enabled?");
1539                 return ERROR_JTAG_INIT_FAILED;
1540         }
1541
1542         for (bit_count = 0; bit_count < (JTAG_MAX_CHAIN_SIZE * 32) - 31;)
1543         {
1544                 u32 idcode = buf_get_u32(idcode_buffer, bit_count, 32);
1545                 if ((idcode & 1) == 0)
1546                 {
1547                         /* LSB must not be 0, this indicates a device in bypass */
1548                         LOG_WARNING("Tap/Device does not have IDCODE");
1549                         idcode=0;
1550
1551                         bit_count += 1;
1552                 }
1553                 else
1554                 {
1555                         u32 manufacturer;
1556                         u32 part;
1557                         u32 version;
1558
1559                         /* some devices, such as AVR will output all 1's instead of TDI
1560                         input value at end of chain. */
1561                         if ((idcode == 0x000000FF)||(idcode == 0xFFFFFFFF))
1562                         {
1563                                 int unexpected=0;
1564                                 /* End of chain (invalid manufacturer ID)
1565                                  *
1566                                  * The JTAG examine is the very first thing that happens
1567                                  *
1568                                  * A single JTAG device requires only 64 bits to be read back correctly.
1569                                  *
1570                                  * The code below adds a check that the rest of the data scanned (640 bits)
1571                                  * are all as expected. This helps diagnose/catch problems with the JTAG chain
1572                                  *
1573                                  * earlier and gives more helpful/explicit error messages.
1574                                  */
1575                                 for (bit_count += 32; bit_count < (JTAG_MAX_CHAIN_SIZE * 32) - 31;bit_count += 32)
1576                                 {
1577                                         idcode = buf_get_u32(idcode_buffer, bit_count, 32);
1578                                         if (unexpected||((idcode != 0x000000FF)&&(idcode != 0xFFFFFFFF)))
1579                                         {
1580                                                 LOG_WARNING("Unexpected idcode after end of chain! %d 0x%08x", bit_count, idcode);
1581                                                 unexpected = 1;
1582                                         }
1583                                 }
1584
1585                                 break;
1586                         }
1587
1588 #define EXTRACT_MFG(X)  (((X) & 0xffe) >> 1)
1589                         manufacturer = EXTRACT_MFG(idcode);
1590 #define EXTRACT_PART(X) (((X) & 0xffff000) >> 12)
1591                         part = EXTRACT_PART(idcode);
1592 #define EXTRACT_VER(X)  (((X) & 0xf0000000) >> 28)
1593                         version = EXTRACT_VER(idcode);
1594
1595                         LOG_INFO("JTAG tap: %s tap/device found: 0x%8.8x (Manufacturer: 0x%3.3x, Part: 0x%4.4x, Version: 0x%1.1x)",
1596                                          ((tap != NULL) ? (tap->dotted_name) : "(not-named)"),
1597                                 idcode, manufacturer, part, version);
1598
1599                         bit_count += 32;
1600                 }
1601                 if (tap)
1602                 {
1603                         tap->idcode = idcode;
1604
1605                         if (tap->expected_ids_cnt > 0) {
1606                                 /* Loop over the expected identification codes and test for a match */
1607                                 u8 ii;
1608                                 for (ii = 0; ii < tap->expected_ids_cnt; ii++) {
1609                                         if( tap->idcode == tap->expected_ids[ii] ){
1610                                                 break;
1611                                         }
1612                                 }
1613
1614                                 /* If none of the expected ids matched, log an error */
1615                                 if (ii == tap->expected_ids_cnt) {
1616                                         LOG_ERROR("JTAG tap: %s             got: 0x%08x (mfg: 0x%3.3x, part: 0x%4.4x, ver: 0x%1.1x)",
1617                                                           tap->dotted_name,
1618                                                           idcode,
1619                                                           EXTRACT_MFG( tap->idcode ),
1620                                                           EXTRACT_PART( tap->idcode ),
1621                                                           EXTRACT_VER( tap->idcode ) );
1622                                         for (ii = 0; ii < tap->expected_ids_cnt; ii++) {
1623                                                 LOG_ERROR("JTAG tap: %s expected %hhu of %hhu: 0x%08x (mfg: 0x%3.3x, part: 0x%4.4x, ver: 0x%1.1x)",
1624                                                                   tap->dotted_name,
1625                                                                   ii + 1,
1626                                                                   tap->expected_ids_cnt,
1627                                                                   tap->expected_ids[ii],
1628                                                                   EXTRACT_MFG( tap->expected_ids[ii] ),
1629                                                                   EXTRACT_PART( tap->expected_ids[ii] ),
1630                                                                   EXTRACT_VER( tap->expected_ids[ii] ) );
1631                                         }
1632
1633                                         return ERROR_JTAG_INIT_FAILED;
1634                                 } else {
1635                                         LOG_INFO("JTAG Tap/device matched");
1636                                 }
1637                         } else {
1638 #if 0
1639                                 LOG_INFO("JTAG TAP ID: 0x%08x - Unknown - please report (A) chipname and (B) idcode to the openocd project",
1640                                                  tap->idcode);
1641 #endif
1642                         }
1643                         tap = jtag_NextEnabledTap(tap);
1644                 }
1645                 device_count++;
1646         }
1647
1648         /* see if number of discovered devices matches configuration */
1649         if (device_count != jtag_NumEnabledTaps())
1650         {
1651                 LOG_ERROR("number of discovered devices in JTAG chain (%i) doesn't match (enabled) configuration (%i), total taps: %d",
1652                                   device_count, jtag_NumEnabledTaps(), jtag_NumTotalTaps());
1653                 LOG_ERROR("check the config file and ensure proper JTAG communication (connections, speed, ...)");
1654                 return ERROR_JTAG_INIT_FAILED;
1655         }
1656
1657         return ERROR_OK;
1658 }
1659
1660 int jtag_validate_chain(void)
1661 {
1662         jtag_tap_t *tap;
1663         int total_ir_length = 0;
1664         u8 *ir_test = NULL;
1665         scan_field_t field;
1666         int chain_pos = 0;
1667
1668         tap = NULL;
1669         total_ir_length = 0;
1670         for(;;){
1671                 tap = jtag_NextEnabledTap(tap);
1672                 if( tap == NULL ){
1673                         break;
1674                 }
1675                 total_ir_length += tap->ir_length;
1676         }
1677
1678         total_ir_length += 2;
1679         ir_test = malloc(CEIL(total_ir_length, 8));
1680         buf_set_ones(ir_test, total_ir_length);
1681
1682         field.tap = NULL;
1683         field.num_bits = total_ir_length;
1684         field.out_value = ir_test;
1685         field.in_value = ir_test;
1686
1687
1688         jtag_add_plain_ir_scan(1, &field, TAP_RESET);
1689         jtag_execute_queue();
1690
1691         tap = NULL;
1692         chain_pos = 0;
1693         int val;
1694         for(;;){
1695                 tap = jtag_NextEnabledTap(tap);
1696                 if( tap == NULL ){
1697                         break;
1698                 }
1699
1700                 val = buf_get_u32(ir_test, chain_pos, 2);
1701                 if (val != 0x1)
1702                 {
1703                         char *cbuf = buf_to_str(ir_test, total_ir_length, 16);
1704                         LOG_ERROR("Could not validate JTAG scan chain, IR mismatch, scan returned 0x%s. tap=%s pos=%d expected 0x1 got %0x", cbuf, jtag_tap_name(tap), chain_pos, val);
1705                         free(cbuf);
1706                         free(ir_test);
1707                         return ERROR_JTAG_INIT_FAILED;
1708                 }
1709                 chain_pos += tap->ir_length;
1710         }
1711
1712         val = buf_get_u32(ir_test, chain_pos, 2);
1713         if (val != 0x3)
1714         {
1715                 char *cbuf = buf_to_str(ir_test, total_ir_length, 16);
1716                 LOG_ERROR("Could not validate end of JTAG scan chain, IR mismatch, scan returned 0x%s. pos=%d expected 0x3 got %0x", cbuf, chain_pos, val);
1717                 free(cbuf);
1718                 free(ir_test);
1719                 return ERROR_JTAG_INIT_FAILED;
1720         }
1721
1722         free(ir_test);
1723
1724         return ERROR_OK;
1725 }
1726
1727 enum jtag_tap_cfg_param {
1728         JCFG_EVENT
1729 };
1730
1731 static Jim_Nvp nvp_config_opts[] = {
1732         { .name = "-event",      .value = JCFG_EVENT },
1733
1734         { .name = NULL,          .value = -1 }
1735 };
1736
1737 static int jtag_tap_configure_cmd( Jim_GetOptInfo *goi, jtag_tap_t * tap)
1738 {
1739         Jim_Nvp *n;
1740         Jim_Obj *o;
1741         int e;
1742
1743         /* parse config or cget options */
1744         while (goi->argc > 0) {
1745                 Jim_SetEmptyResult (goi->interp);
1746
1747                 e = Jim_GetOpt_Nvp(goi, nvp_config_opts, &n);
1748                 if (e != JIM_OK) {
1749                         Jim_GetOpt_NvpUnknown(goi, nvp_config_opts, 0);
1750                         return e;
1751                 }
1752
1753                 switch (n->value) {
1754                         case JCFG_EVENT:
1755                                 if (goi->argc == 0) {
1756                                         Jim_WrongNumArgs( goi->interp, goi->argc, goi->argv, "-event ?event-name? ..." );
1757                                         return JIM_ERR;
1758                                 }
1759
1760                                 e = Jim_GetOpt_Nvp( goi, nvp_jtag_tap_event, &n );
1761                                 if (e != JIM_OK) {
1762                                         Jim_GetOpt_NvpUnknown(goi, nvp_jtag_tap_event, 1);
1763                                         return e;
1764                                 }
1765
1766                                 if (goi->isconfigure) {
1767                                         if (goi->argc != 1) {
1768                                                 Jim_WrongNumArgs(goi->interp, goi->argc, goi->argv, "-event ?event-name? ?EVENT-BODY?");
1769                                                 return JIM_ERR;
1770                                         }
1771                                 } else {
1772                                         if (goi->argc != 0) {
1773                                                 Jim_WrongNumArgs(goi->interp, goi->argc, goi->argv, "-event ?event-name?");
1774                                                 return JIM_ERR;
1775                                         }
1776                                 }
1777
1778                                 {
1779                                         jtag_tap_event_action_t *jteap;
1780
1781                                         jteap = tap->event_action;
1782                                         /* replace existing? */
1783                                         while (jteap) {
1784                                                 if (jteap->event == (enum jtag_tap_event)n->value) {
1785                                                         break;
1786                                                 }
1787                                                 jteap = jteap->next;
1788                                         }
1789
1790                                         if (goi->isconfigure) {
1791                                                 if (jteap == NULL) {
1792                                                         /* create new */
1793                                                         jteap = calloc(1, sizeof (*jteap));
1794                                                 }
1795                                                 jteap->event = n->value;
1796                                                 Jim_GetOpt_Obj( goi, &o);
1797                                                 if (jteap->body) {
1798                                                         Jim_DecrRefCount(interp, jteap->body);
1799                                                 }
1800                                                 jteap->body = Jim_DuplicateObj(goi->interp, o);
1801                                                 Jim_IncrRefCount(jteap->body);
1802
1803                                                 /* add to head of event list */
1804                                                 jteap->next = tap->event_action;
1805                                                 tap->event_action = jteap;
1806                                                 Jim_SetEmptyResult(goi->interp);
1807                                         } else {
1808                                                 /* get */
1809                                                 if (jteap == NULL) {
1810                                                         Jim_SetEmptyResult(goi->interp);
1811                                                 } else {
1812                                                         Jim_SetResult(goi->interp, Jim_DuplicateObj(goi->interp, jteap->body));
1813                                                 }
1814                                         }
1815                                 }
1816                                 /* loop for more */
1817                                 break;
1818                 }
1819         } /* while (goi->argc) */
1820
1821         return JIM_OK;
1822 }
1823
1824 static int jim_newtap_cmd( Jim_GetOptInfo *goi )
1825 {
1826         jtag_tap_t *pTap;
1827         jtag_tap_t **ppTap;
1828         jim_wide w;
1829         int x;
1830         int e;
1831         int reqbits;
1832         Jim_Nvp *n;
1833         char *cp;
1834         const Jim_Nvp opts[] = {
1835 #define NTAP_OPT_IRLEN     0
1836                 { .name = "-irlen"                      ,       .value = NTAP_OPT_IRLEN },
1837 #define NTAP_OPT_IRMASK    1
1838                 { .name = "-irmask"                     ,       .value = NTAP_OPT_IRMASK },
1839 #define NTAP_OPT_IRCAPTURE 2
1840                 { .name = "-ircapture"          ,       .value = NTAP_OPT_IRCAPTURE },
1841 #define NTAP_OPT_ENABLED   3
1842                 { .name = "-enable"                     ,       .value = NTAP_OPT_ENABLED },
1843 #define NTAP_OPT_DISABLED  4
1844                 { .name = "-disable"            ,       .value = NTAP_OPT_DISABLED },
1845 #define NTAP_OPT_EXPECTED_ID 5
1846                 { .name = "-expected-id"        ,       .value = NTAP_OPT_EXPECTED_ID },
1847                 { .name = NULL                          ,       .value = -1 },
1848         };
1849
1850         pTap = malloc( sizeof(jtag_tap_t) );
1851         memset( pTap, 0, sizeof(*pTap) );
1852         if( !pTap ){
1853                 Jim_SetResult_sprintf( goi->interp, "no memory");
1854                 return JIM_ERR;
1855         }
1856         /*
1857          * we expect CHIP + TAP + OPTIONS
1858          * */
1859         if( goi->argc < 3 ){
1860                 Jim_SetResult_sprintf(goi->interp, "Missing CHIP TAP OPTIONS ....");
1861                 return JIM_ERR;
1862         }
1863         Jim_GetOpt_String( goi, &cp, NULL );
1864         pTap->chip = strdup(cp);
1865
1866         Jim_GetOpt_String( goi, &cp, NULL );
1867         pTap->tapname = strdup(cp);
1868
1869         /* name + dot + name + null */
1870         x = strlen(pTap->chip) + 1 + strlen(pTap->tapname) + 1;
1871         cp = malloc( x );
1872         sprintf( cp, "%s.%s", pTap->chip, pTap->tapname );
1873         pTap->dotted_name = cp;
1874
1875         LOG_DEBUG("Creating New Tap, Chip: %s, Tap: %s, Dotted: %s, %d params",
1876                           pTap->chip, pTap->tapname, pTap->dotted_name, goi->argc);
1877
1878         /* default is enabled */
1879         pTap->enabled = 1;
1880
1881         /* deal with options */
1882 #define NTREQ_IRLEN      1
1883 #define NTREQ_IRCAPTURE  2
1884 #define NTREQ_IRMASK     4
1885
1886         /* clear them as we find them */
1887         reqbits = (NTREQ_IRLEN | NTREQ_IRCAPTURE | NTREQ_IRMASK);
1888
1889         while( goi->argc ){
1890                 e = Jim_GetOpt_Nvp( goi, opts, &n );
1891                 if( e != JIM_OK ){
1892                         Jim_GetOpt_NvpUnknown( goi, opts, 0 );
1893                         return e;
1894                 }
1895                 LOG_DEBUG("Processing option: %s", n->name );
1896                 switch( n->value ){
1897                 case NTAP_OPT_ENABLED:
1898                         pTap->enabled = 1;
1899                         break;
1900                 case NTAP_OPT_DISABLED:
1901                         pTap->enabled = 0;
1902                         break;
1903                 case NTAP_OPT_EXPECTED_ID:
1904                 {
1905                         u32 *new_expected_ids;
1906
1907                         e = Jim_GetOpt_Wide( goi, &w );
1908                         if( e != JIM_OK) {
1909                                 Jim_SetResult_sprintf(goi->interp, "option: %s bad parameter", n->name);
1910                                 return e;
1911                         }
1912
1913                         new_expected_ids = malloc(sizeof(u32) * (pTap->expected_ids_cnt + 1));
1914                         if (new_expected_ids == NULL) {
1915                                 Jim_SetResult_sprintf( goi->interp, "no memory");
1916                                 return JIM_ERR;
1917                         }
1918
1919                         memcpy(new_expected_ids, pTap->expected_ids, sizeof(u32) * pTap->expected_ids_cnt);
1920
1921                         new_expected_ids[pTap->expected_ids_cnt] = w;
1922
1923                         free(pTap->expected_ids);
1924                         pTap->expected_ids = new_expected_ids;
1925                         pTap->expected_ids_cnt++;
1926                         break;
1927                 }
1928                 case NTAP_OPT_IRLEN:
1929                 case NTAP_OPT_IRMASK:
1930                 case NTAP_OPT_IRCAPTURE:
1931                         e = Jim_GetOpt_Wide( goi, &w );
1932                         if( e != JIM_OK ){
1933                                 Jim_SetResult_sprintf( goi->interp, "option: %s bad parameter", n->name );
1934                                 return e;
1935                         }
1936                         if( (w < 0) || (w > 0xffff) ){
1937                                 /* wacky value */
1938                                 Jim_SetResult_sprintf( goi->interp, "option: %s - wacky value: %d (0x%x)",
1939                                                                            n->name, (int)(w), (int)(w));
1940                                 return JIM_ERR;
1941                         }
1942                         switch(n->value){
1943                         case NTAP_OPT_IRLEN:
1944                                 pTap->ir_length = w;
1945                                 reqbits &= (~(NTREQ_IRLEN));
1946                                 break;
1947                         case NTAP_OPT_IRMASK:
1948                                 pTap->ir_capture_mask = w;
1949                                 reqbits &= (~(NTREQ_IRMASK));
1950                                 break;
1951                         case NTAP_OPT_IRCAPTURE:
1952                                 pTap->ir_capture_value = w;
1953                                 reqbits &= (~(NTREQ_IRCAPTURE));
1954                                 break;
1955                         }
1956                 } /* switch(n->value) */
1957         } /* while( goi->argc ) */
1958
1959         /* Did we get all the options? */
1960         if( reqbits ){
1961                 // no
1962                 Jim_SetResult_sprintf( goi->interp,
1963                                                            "newtap: %s missing required parameters",
1964                                                            pTap->dotted_name);
1965                 /* TODO: Tell user what is missing :-( */
1966                 /* no memory leaks pelase */
1967                 free(((void *)(pTap->expected_ids)));
1968                 free(((void *)(pTap->chip)));
1969                 free(((void *)(pTap->tapname)));
1970                 free(((void *)(pTap->dotted_name)));
1971                 free(((void *)(pTap)));
1972                 return JIM_ERR;
1973         }
1974
1975         pTap->expected      = malloc( pTap->ir_length );
1976         pTap->expected_mask = malloc( pTap->ir_length );
1977         pTap->cur_instr     = malloc( pTap->ir_length );
1978
1979         buf_set_u32( pTap->expected,
1980                                  0,
1981                                  pTap->ir_length,
1982                                  pTap->ir_capture_value );
1983         buf_set_u32( pTap->expected_mask,
1984                                  0,
1985                                  pTap->ir_length,
1986                                  pTap->ir_capture_mask );
1987         buf_set_ones( pTap->cur_instr,
1988                                   pTap->ir_length );
1989
1990         pTap->bypass = 1;
1991
1992         jtag_register_event_callback(jtag_reset_callback, pTap );
1993
1994         ppTap = &(jtag_all_taps);
1995         while( (*ppTap) != NULL ){
1996                 ppTap = &((*ppTap)->next_tap);
1997         }
1998         *ppTap = pTap;
1999         {
2000                 static int n_taps = 0;
2001                 pTap->abs_chain_position = n_taps++;
2002         }
2003         LOG_DEBUG( "Created Tap: %s @ abs position %d, irlen %d, capture: 0x%x mask: 0x%x",
2004                                 (*ppTap)->dotted_name,
2005                                 (*ppTap)->abs_chain_position,
2006                                 (*ppTap)->ir_length,
2007                                 (*ppTap)->ir_capture_value,
2008                                 (*ppTap)->ir_capture_mask );
2009
2010         return ERROR_OK;
2011 }
2012
2013 static int jim_jtag_command( Jim_Interp *interp, int argc, Jim_Obj *const *argv )
2014 {
2015         Jim_GetOptInfo goi;
2016         int e;
2017         Jim_Nvp *n;
2018         Jim_Obj *o;
2019         struct command_context_s *context;
2020
2021         enum {
2022                 JTAG_CMD_INTERFACE,
2023                 JTAG_CMD_INIT_RESET,
2024                 JTAG_CMD_NEWTAP,
2025                 JTAG_CMD_TAPENABLE,
2026                 JTAG_CMD_TAPDISABLE,
2027                 JTAG_CMD_TAPISENABLED,
2028                 JTAG_CMD_CONFIGURE,
2029                 JTAG_CMD_CGET
2030         };
2031
2032         const Jim_Nvp jtag_cmds[] = {
2033                 { .name = "interface"     , .value = JTAG_CMD_INTERFACE },
2034                 { .name = "arp_init-reset", .value = JTAG_CMD_INIT_RESET },
2035                 { .name = "newtap"        , .value = JTAG_CMD_NEWTAP },
2036                 { .name = "tapisenabled"     , .value = JTAG_CMD_TAPISENABLED },
2037                 { .name = "tapenable"     , .value = JTAG_CMD_TAPENABLE },
2038                 { .name = "tapdisable"    , .value = JTAG_CMD_TAPDISABLE },
2039                 { .name = "configure"     , .value = JTAG_CMD_CONFIGURE },
2040                 { .name = "cget"          , .value = JTAG_CMD_CGET },
2041
2042                 { .name = NULL, .value = -1 },
2043         };
2044
2045         context = Jim_GetAssocData(interp, "context");
2046         /* go past the command */
2047         Jim_GetOpt_Setup( &goi, interp, argc-1, argv+1 );
2048
2049         e = Jim_GetOpt_Nvp( &goi, jtag_cmds, &n );
2050         if( e != JIM_OK ){
2051                 Jim_GetOpt_NvpUnknown( &goi, jtag_cmds, 0 );
2052                 return e;
2053         }
2054                 Jim_SetEmptyResult( goi.interp );
2055         switch( n->value ){
2056         case JTAG_CMD_INTERFACE:
2057                 /* return the name of the interface */
2058                 /* TCL code might need to know the exact type... */
2059                 /* FUTURE: we allow this as a means to "set" the interface. */
2060                 if( goi.argc != 0 ){
2061                         Jim_WrongNumArgs( goi.interp, 1, goi.argv-1, "(no params)");
2062                         return JIM_ERR;
2063                 }
2064                 Jim_SetResultString( goi.interp, jtag_interface->name, -1 );
2065                 return JIM_OK;
2066         case JTAG_CMD_INIT_RESET:
2067                 if( goi.argc != 0 ){
2068                         Jim_WrongNumArgs( goi.interp, 1, goi.argv-1, "(no params)");
2069                         return JIM_ERR;
2070                 }
2071                 e = jtag_init_reset(context);
2072                 if( e != ERROR_OK ){
2073                         Jim_SetResult_sprintf( goi.interp, "error: %d", e);
2074                         return JIM_ERR;
2075                 }
2076                 return JIM_OK;
2077         case JTAG_CMD_NEWTAP:
2078                 return jim_newtap_cmd( &goi );
2079                 break;
2080         case JTAG_CMD_TAPISENABLED:
2081         case JTAG_CMD_TAPENABLE:
2082         case JTAG_CMD_TAPDISABLE:
2083                 if( goi.argc != 1 ){
2084                         Jim_SetResultString( goi.interp, "Too many parameters",-1 );
2085                         return JIM_ERR;
2086                 }
2087
2088                 {
2089                         jtag_tap_t *t;
2090                         t = jtag_TapByJimObj( goi.interp, goi.argv[0] );
2091                         if( t == NULL ){
2092                                 return JIM_ERR;
2093                         }
2094                         switch( n->value ){
2095                         case JTAG_CMD_TAPISENABLED:
2096                                 e = t->enabled;
2097                                 break;
2098                         case JTAG_CMD_TAPENABLE:
2099                                 jtag_tap_handle_event( t, JTAG_TAP_EVENT_ENABLE);
2100                                 e = 1;
2101                                 t->enabled = e;
2102                                 break;
2103                         case JTAG_CMD_TAPDISABLE:
2104                                 jtag_tap_handle_event( t, JTAG_TAP_EVENT_DISABLE);
2105                                 e = 0;
2106                                 t->enabled = e;
2107                                 break;
2108                         }
2109                         Jim_SetResult( goi.interp, Jim_NewIntObj( goi.interp, e ) );
2110                         return JIM_OK;
2111                 }
2112                 break;
2113
2114         case JTAG_CMD_CGET:
2115                 if( goi.argc < 2 ){
2116                         Jim_WrongNumArgs( goi.interp, 0, NULL, "?tap-name? -option ...");
2117                         return JIM_ERR;
2118                 }
2119
2120                 {
2121                         jtag_tap_t *t;
2122
2123                         Jim_GetOpt_Obj(&goi, &o);
2124                         t = jtag_TapByJimObj( goi.interp, o );
2125                         if( t == NULL ){
2126                                 return JIM_ERR;
2127                         }
2128
2129                         goi.isconfigure = 0;
2130                         return jtag_tap_configure_cmd( &goi, t);
2131                 }
2132                 break;
2133
2134         case JTAG_CMD_CONFIGURE:
2135                 if( goi.argc < 3 ){
2136                         Jim_WrongNumArgs( goi.interp, 0, NULL, "?tap-name? -option ?VALUE? ...");
2137                         return JIM_ERR;
2138                 }
2139
2140                 {
2141                         jtag_tap_t *t;
2142
2143                         Jim_GetOpt_Obj(&goi, &o);
2144                         t = jtag_TapByJimObj( goi.interp, o );
2145                         if( t == NULL ){
2146                                 return JIM_ERR;
2147                         }
2148
2149                         goi.isconfigure = 1;
2150                         return jtag_tap_configure_cmd( &goi, t);
2151                 }
2152         }
2153
2154         return JIM_ERR;
2155 }
2156
2157 int jtag_register_commands(struct command_context_s *cmd_ctx)
2158 {
2159         register_jim( cmd_ctx, "jtag", jim_jtag_command, "perform jtag tap actions");
2160
2161         register_command(cmd_ctx, NULL, "interface", handle_interface_command,
2162                 COMMAND_CONFIG, "try to configure interface");
2163         register_command(cmd_ctx, NULL, "jtag_speed", handle_jtag_speed_command,
2164                 COMMAND_ANY, "set jtag speed (if supported)");
2165         register_command(cmd_ctx, NULL, "jtag_khz", handle_jtag_khz_command,
2166                 COMMAND_ANY, "same as jtag_speed, except it takes maximum khz as arguments. 0 KHz = RTCK.");
2167         register_command(cmd_ctx, NULL, "jtag_device", handle_jtag_device_command,
2168                 COMMAND_CONFIG, "jtag_device <ir_length> <ir_expected> <ir_mask>");
2169         register_command(cmd_ctx, NULL, "reset_config", handle_reset_config_command,
2170                 COMMAND_ANY,
2171                 "[none/trst_only/srst_only/trst_and_srst] [srst_pulls_trst/trst_pulls_srst] [combined/separate] [trst_push_pull/trst_open_drain] [srst_push_pull/srst_open_drain]");
2172         register_command(cmd_ctx, NULL, "jtag_nsrst_delay", handle_jtag_nsrst_delay_command,
2173                 COMMAND_ANY, "jtag_nsrst_delay <ms> - delay after deasserting srst in ms");
2174         register_command(cmd_ctx, NULL, "jtag_ntrst_delay", handle_jtag_ntrst_delay_command,
2175                 COMMAND_ANY, "jtag_ntrst_delay <ms> - delay after deasserting trst in ms");
2176
2177         register_command(cmd_ctx, NULL, "scan_chain", handle_scan_chain_command,
2178                 COMMAND_EXEC, "print current scan chain configuration");
2179
2180         register_command(cmd_ctx, NULL, "endstate", handle_endstate_command,
2181                 COMMAND_EXEC, "finish JTAG operations in <tap_state>");
2182         register_command(cmd_ctx, NULL, "jtag_reset", handle_jtag_reset_command,
2183                 COMMAND_EXEC, "toggle reset lines <trst> <srst>");
2184         register_command(cmd_ctx, NULL, "runtest", handle_runtest_command,
2185                 COMMAND_EXEC, "move to Run-Test/Idle, and execute <num_cycles>");
2186         register_command(cmd_ctx, NULL, "irscan", handle_irscan_command,
2187                 COMMAND_EXEC, "execute IR scan <device> <instr> [dev2] [instr2] ...");
2188         register_jim(cmd_ctx, "drscan", Jim_Command_drscan, "execute DR scan <device> <num_bits> <value> <num_bits1> <value2> ...");
2189         register_jim(cmd_ctx, "flush_count", Jim_Command_flush_count, "returns number of times the JTAG queue has been flushed");
2190
2191         register_command(cmd_ctx, NULL, "verify_ircapture", handle_verify_ircapture_command,
2192                 COMMAND_ANY, "verify value captured during Capture-IR <enable|disable>");
2193         return ERROR_OK;
2194 }
2195
2196 int jtag_interface_init(struct command_context_s *cmd_ctx)
2197 {
2198         if (jtag)
2199                 return ERROR_OK;
2200
2201         if (!jtag_interface)
2202         {
2203                 /* nothing was previously specified by "interface" command */
2204                 LOG_ERROR("JTAG interface has to be specified, see \"interface\" command");
2205                 return ERROR_JTAG_INVALID_INTERFACE;
2206         }
2207         if(hasKHz)
2208         {
2209                 jtag_interface->khz(speed_khz, &jtag_speed);
2210                 hasKHz = 0;
2211         }
2212
2213         if (jtag_interface->init() != ERROR_OK)
2214                 return ERROR_JTAG_INIT_FAILED;
2215
2216         jtag = jtag_interface;
2217         return ERROR_OK;
2218 }
2219
2220 static int jtag_init_inner(struct command_context_s *cmd_ctx)
2221 {
2222         jtag_tap_t *tap;
2223         int retval;
2224
2225         LOG_DEBUG("Init JTAG chain");
2226
2227         tap = jtag_NextEnabledTap(NULL);
2228         if( tap == NULL ){
2229                 LOG_ERROR("There are no enabled taps?");
2230                 return ERROR_JTAG_INIT_FAILED;
2231         }
2232
2233         jtag_add_tlr();
2234         if ((retval=jtag_execute_queue())!=ERROR_OK)
2235                 return retval;
2236
2237         /* examine chain first, as this could discover the real chain layout */
2238         if (jtag_examine_chain() != ERROR_OK)
2239         {
2240                 LOG_ERROR("trying to validate configured JTAG chain anyway...");
2241         }
2242
2243         if (jtag_validate_chain() != ERROR_OK)
2244         {
2245                 LOG_WARNING("Could not validate JTAG chain, continuing anyway...");
2246         }
2247
2248         return ERROR_OK;
2249 }
2250
2251 int jtag_init_reset(struct command_context_s *cmd_ctx)
2252 {
2253         int retval;
2254
2255         if ((retval=jtag_interface_init(cmd_ctx)) != ERROR_OK)
2256                 return retval;
2257
2258         LOG_DEBUG("Trying to bring the JTAG controller to life by asserting TRST / RESET");
2259
2260         /* Reset can happen after a power cycle.
2261          *
2262          * Ideally we would only assert TRST or run RESET before the target reset.
2263          *
2264          * However w/srst_pulls_trst, trst is asserted together with the target
2265          * reset whether we want it or not.
2266          *
2267          * NB! Some targets have JTAG circuitry disabled until a
2268          * trst & srst has been asserted.
2269          *
2270          * NB! here we assume nsrst/ntrst delay are sufficient!
2271          *
2272          * NB! order matters!!!! srst *can* disconnect JTAG circuitry
2273          *
2274          */
2275         jtag_add_reset(1, 0); /* RESET or TRST */
2276         if (jtag_reset_config & RESET_HAS_SRST)
2277         {
2278                 jtag_add_reset(1, 1);
2279                 if ((jtag_reset_config & RESET_SRST_PULLS_TRST)==0)
2280                         jtag_add_reset(0, 1);
2281         }
2282         jtag_add_reset(0, 0);
2283         if ((retval = jtag_execute_queue()) != ERROR_OK)
2284                 return retval;
2285
2286         /* Check that we can communication on the JTAG chain + eventually we want to
2287          * be able to perform enumeration only after OpenOCD has started
2288          * telnet and GDB server
2289          *
2290          * That would allow users to more easily perform any magic they need to before
2291          * reset happens.
2292          */
2293         return jtag_init_inner(cmd_ctx);
2294 }
2295
2296 int jtag_init(struct command_context_s *cmd_ctx)
2297 {
2298         int retval;
2299         if ((retval=jtag_interface_init(cmd_ctx)) != ERROR_OK)
2300                 return retval;
2301         if (jtag_init_inner(cmd_ctx)==ERROR_OK)
2302         {
2303                 return ERROR_OK;
2304         }
2305         return jtag_init_reset(cmd_ctx);
2306 }
2307
2308 static int default_khz(int khz, int *jtag_speed)
2309 {
2310         LOG_ERROR("Translation from khz to jtag_speed not implemented");
2311         return ERROR_FAIL;
2312 }
2313
2314 static int default_speed_div(int speed, int *khz)
2315 {
2316         LOG_ERROR("Translation from jtag_speed to khz not implemented");
2317         return ERROR_FAIL;
2318 }
2319
2320 static int default_power_dropout(int *dropout)
2321 {
2322         *dropout=0; /* by default we can't detect power dropout */
2323         return ERROR_OK;
2324 }
2325
2326 static int default_srst_asserted(int *srst_asserted)
2327 {
2328         *srst_asserted=0; /* by default we can't detect srst asserted */
2329         return ERROR_OK;
2330 }
2331
2332 static int handle_interface_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2333 {
2334         int i;
2335         int retval;
2336
2337         /* check whether the interface is already configured */
2338         if (jtag_interface)
2339         {
2340                 LOG_WARNING("Interface already configured, ignoring");
2341                 return ERROR_OK;
2342         }
2343
2344         /* interface name is a mandatory argument */
2345         if (argc < 1 || args[0][0] == '\0')
2346         {
2347                 return ERROR_COMMAND_SYNTAX_ERROR;
2348         }
2349
2350         for (i=0; jtag_interfaces[i]; i++)
2351         {
2352                 if (strcmp(args[0], jtag_interfaces[i]->name) == 0)
2353                 {
2354                         if ((retval = jtag_interfaces[i]->register_commands(cmd_ctx)) != ERROR_OK)
2355                         {
2356                                 return retval;
2357                         }
2358
2359                         jtag_interface = jtag_interfaces[i];
2360
2361                         if (jtag_interface->khz == NULL)
2362                         {
2363                                 jtag_interface->khz = default_khz;
2364                         }
2365                         if (jtag_interface->speed_div == NULL)
2366                         {
2367                                 jtag_interface->speed_div = default_speed_div;
2368                         }
2369                         if (jtag_interface->power_dropout == NULL)
2370                         {
2371                                 jtag_interface->power_dropout = default_power_dropout;
2372                         }
2373                         if (jtag_interface->srst_asserted == NULL)
2374                         {
2375                                 jtag_interface->srst_asserted = default_srst_asserted;
2376                         }
2377
2378                         return ERROR_OK;
2379                 }
2380         }
2381
2382         /* no valid interface was found (i.e. the configuration option,
2383          * didn't match one of the compiled-in interfaces
2384          */
2385         LOG_ERROR("No valid jtag interface found (%s)", args[0]);
2386         LOG_ERROR("compiled-in jtag interfaces:");
2387         for (i = 0; jtag_interfaces[i]; i++)
2388         {
2389                 LOG_ERROR("%i: %s", i, jtag_interfaces[i]->name);
2390         }
2391
2392         return ERROR_JTAG_INVALID_INTERFACE;
2393 }
2394
2395 static int handle_jtag_device_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2396 {
2397         int e;
2398         char buf[1024];
2399         Jim_Obj *newargs[ 10 ];
2400         /*
2401          * CONVERT SYNTAX
2402          * argv[-1] = command
2403          * argv[ 0] = ir length
2404          * argv[ 1] = ir capture
2405          * argv[ 2] = ir mask
2406          * argv[ 3] = not actually used by anything but in the docs
2407          */
2408
2409         if( argc < 4 ){
2410                 command_print( cmd_ctx, "OLD DEPRECATED SYNTAX: Please use the NEW syntax");
2411                 return ERROR_OK;
2412         }
2413         command_print( cmd_ctx, "OLD SYNTAX: DEPRECATED - translating to new syntax");
2414         command_print( cmd_ctx, "jtag newtap CHIP TAP -irlen %s -ircapture %s -irvalue %s",
2415                                    args[0],
2416                                    args[1],
2417                                    args[2] );
2418         command_print( cmd_ctx, "Example: STM32 has 2 taps, the cortexM3(len4) + boundaryscan(len5)");
2419         command_print( cmd_ctx, "jtag newtap stm32 cortexm3 ....., thus creating the tap: \"stm32.cortexm3\"");
2420         command_print( cmd_ctx, "jtag newtap stm32 boundary ....., and the tap: \"stm32.boundary\"");
2421         command_print( cmd_ctx, "And then refer to the taps by the dotted name.");
2422
2423         newargs[0] = Jim_NewStringObj( interp, "jtag", -1   );
2424         newargs[1] = Jim_NewStringObj( interp, "newtap", -1 );
2425         sprintf( buf, "chip%d", jtag_NumTotalTaps() );
2426         newargs[2] = Jim_NewStringObj( interp, buf, -1 );
2427         sprintf( buf, "tap%d", jtag_NumTotalTaps() );
2428         newargs[3] = Jim_NewStringObj( interp, buf, -1  );
2429         newargs[4] = Jim_NewStringObj( interp, "-irlen", -1  );
2430         newargs[5] = Jim_NewStringObj( interp, args[0], -1  );
2431         newargs[6] = Jim_NewStringObj( interp, "-ircapture", -1  );
2432         newargs[7] = Jim_NewStringObj( interp, args[1], -1  );
2433         newargs[8] = Jim_NewStringObj( interp, "-irmask", -1  );
2434         newargs[9] = Jim_NewStringObj( interp, args[2], -1  );
2435
2436         command_print( cmd_ctx, "NEW COMMAND:");
2437         sprintf( buf, "%s %s %s %s %s %s %s %s %s %s",
2438                          Jim_GetString( newargs[0], NULL ),
2439                          Jim_GetString( newargs[1], NULL ),
2440                          Jim_GetString( newargs[2], NULL ),
2441                          Jim_GetString( newargs[3], NULL ),
2442                          Jim_GetString( newargs[4], NULL ),
2443                          Jim_GetString( newargs[5], NULL ),
2444                          Jim_GetString( newargs[6], NULL ),
2445                          Jim_GetString( newargs[7], NULL ),
2446                          Jim_GetString( newargs[8], NULL ),
2447                          Jim_GetString( newargs[9], NULL ) );
2448
2449         e = jim_jtag_command( interp, 10, newargs );
2450         if( e != JIM_OK ){
2451                 command_print( cmd_ctx, "%s", Jim_GetString( Jim_GetResult(interp), NULL ) );
2452         }
2453         return e;
2454 }
2455
2456 static int handle_scan_chain_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2457 {
2458         jtag_tap_t *tap;
2459
2460         tap = jtag_all_taps;
2461         command_print(cmd_ctx, "     TapName            | Enabled |   IdCode      Expected    IrLen IrCap  IrMask Instr     ");
2462         command_print(cmd_ctx, "---|--------------------|---------|------------|------------|------|------|------|---------");
2463
2464         while( tap ){
2465                 u32 expected, expected_mask, cur_instr, ii;
2466                 expected = buf_get_u32(tap->expected, 0, tap->ir_length);
2467                 expected_mask = buf_get_u32(tap->expected_mask, 0, tap->ir_length);
2468                 cur_instr = buf_get_u32(tap->cur_instr, 0, tap->ir_length);
2469
2470                 command_print(cmd_ctx,
2471                                           "%2d | %-18s |    %c    | 0x%08x | 0x%08x | 0x%02x | 0x%02x | 0x%02x | 0x%02x",
2472                                           tap->abs_chain_position,
2473                                           tap->dotted_name,
2474                                           tap->enabled ? 'Y' : 'n',
2475                                           tap->idcode,
2476                                           (tap->expected_ids_cnt > 0 ? tap->expected_ids[0] : 0),
2477                                           tap->ir_length,
2478                                           expected,
2479                                           expected_mask,
2480                                           cur_instr);
2481
2482                 for (ii = 1; ii < tap->expected_ids_cnt; ii++) {
2483                         command_print(cmd_ctx, "   |                    |         |            | 0x%08x |      |      |      |         ",
2484                                                   tap->expected_ids[ii]);
2485                 }
2486
2487                 tap = tap->next_tap;
2488         }
2489
2490         return ERROR_OK;
2491 }
2492
2493 static int handle_reset_config_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2494 {
2495         if (argc < 1)
2496                 return ERROR_COMMAND_SYNTAX_ERROR;
2497
2498         if (argc >= 1)
2499         {
2500                 if (strcmp(args[0], "none") == 0)
2501                         jtag_reset_config = RESET_NONE;
2502                 else if (strcmp(args[0], "trst_only") == 0)
2503                         jtag_reset_config = RESET_HAS_TRST;
2504                 else if (strcmp(args[0], "srst_only") == 0)
2505                         jtag_reset_config = RESET_HAS_SRST;
2506                 else if (strcmp(args[0], "trst_and_srst") == 0)
2507                         jtag_reset_config = RESET_TRST_AND_SRST;
2508                 else
2509                 {
2510                         LOG_ERROR("(1) invalid reset_config argument (%s), defaulting to none", args[0]);
2511                         jtag_reset_config = RESET_NONE;
2512                         return ERROR_INVALID_ARGUMENTS;
2513                 }
2514         }
2515
2516         if (argc >= 2)
2517         {
2518                 if (strcmp(args[1], "separate") == 0)
2519                 {
2520                         /* seperate reset lines - default */
2521                 } else
2522                 {
2523                         if (strcmp(args[1], "srst_pulls_trst") == 0)
2524                                 jtag_reset_config |= RESET_SRST_PULLS_TRST;
2525                         else if (strcmp(args[1], "trst_pulls_srst") == 0)
2526                                 jtag_reset_config |= RESET_TRST_PULLS_SRST;
2527                         else if (strcmp(args[1], "combined") == 0)
2528                                 jtag_reset_config |= RESET_SRST_PULLS_TRST | RESET_TRST_PULLS_SRST;
2529                         else
2530                         {
2531                                 LOG_ERROR("(2) invalid reset_config argument (%s), defaulting to none", args[1]);
2532                                 jtag_reset_config = RESET_NONE;
2533                                 return ERROR_INVALID_ARGUMENTS;
2534                         }
2535                 }
2536         }
2537
2538         if (argc >= 3)
2539         {
2540                 if (strcmp(args[2], "trst_open_drain") == 0)
2541                         jtag_reset_config |= RESET_TRST_OPEN_DRAIN;
2542                 else if (strcmp(args[2], "trst_push_pull") == 0)
2543                         jtag_reset_config &= ~RESET_TRST_OPEN_DRAIN;
2544                 else
2545                 {
2546                         LOG_ERROR("(3) invalid reset_config argument (%s) defaulting to none", args[2] );
2547                         jtag_reset_config = RESET_NONE;
2548                         return ERROR_INVALID_ARGUMENTS;
2549                 }
2550         }
2551
2552         if (argc >= 4)
2553         {
2554                 if (strcmp(args[3], "srst_push_pull") == 0)
2555                         jtag_reset_config |= RESET_SRST_PUSH_PULL;
2556                 else if (strcmp(args[3], "srst_open_drain") == 0)
2557                         jtag_reset_config &= ~RESET_SRST_PUSH_PULL;
2558                 else
2559                 {
2560                         LOG_ERROR("(4) invalid reset_config argument (%s), defaulting to none", args[3]);
2561                         jtag_reset_config = RESET_NONE;
2562                         return ERROR_INVALID_ARGUMENTS;
2563                 }
2564         }
2565
2566         return ERROR_OK;
2567 }
2568
2569 static int handle_jtag_nsrst_delay_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2570 {
2571         if (argc < 1)
2572         {
2573                 LOG_ERROR("jtag_nsrst_delay <ms> command takes one required argument");
2574                 exit(-1);
2575         }
2576         else
2577         {
2578                 jtag_nsrst_delay = strtoul(args[0], NULL, 0);
2579         }
2580
2581         return ERROR_OK;
2582 }
2583
2584 static int handle_jtag_ntrst_delay_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2585 {
2586         if (argc < 1)
2587         {
2588                 LOG_ERROR("jtag_ntrst_delay <ms> command takes one required argument");
2589                 exit(-1);
2590         }
2591         else
2592         {
2593                 jtag_ntrst_delay = strtoul(args[0], NULL, 0);
2594         }
2595
2596         return ERROR_OK;
2597 }
2598
2599 static int handle_jtag_speed_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2600 {
2601         int retval=ERROR_OK;
2602
2603         if (argc == 1)
2604         {
2605                 LOG_DEBUG("handle jtag speed");
2606
2607                 int cur_speed = 0;
2608                 cur_speed = jtag_speed = strtoul(args[0], NULL, 0);
2609
2610                 /* this command can be called during CONFIG,
2611                  * in which case jtag isn't initialized */
2612                 if (jtag)
2613                 {
2614                         retval=jtag->speed(cur_speed);
2615                 }
2616         } else if (argc == 0)
2617         {
2618         } else
2619         {
2620                 return ERROR_COMMAND_SYNTAX_ERROR;
2621         }
2622         command_print(cmd_ctx, "jtag_speed: %d", jtag_speed);
2623
2624         return retval;
2625 }
2626
2627 static int handle_jtag_khz_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2628 {
2629         int retval=ERROR_OK;
2630         LOG_DEBUG("handle jtag khz");
2631
2632         if(argc == 1)
2633         {
2634                 speed_khz = strtoul(args[0], NULL, 0);
2635                 if (jtag != NULL)
2636                 {
2637                         int cur_speed = 0;
2638                         LOG_DEBUG("have interface set up");
2639                         int speed_div1;
2640                         if ((retval=jtag->khz(speed_khz, &speed_div1))!=ERROR_OK)
2641                         {
2642                                 speed_khz = 0;
2643                                 return retval;
2644                         }
2645
2646                         cur_speed = jtag_speed = speed_div1;
2647
2648                         retval=jtag->speed(cur_speed);
2649                 } else
2650                 {
2651                         hasKHz = 1;
2652                 }
2653         } else if (argc==0)
2654         {
2655         } else
2656         {
2657                 return ERROR_COMMAND_SYNTAX_ERROR;
2658         }
2659
2660         if (jtag!=NULL)
2661         {
2662                 if ((retval=jtag->speed_div(jtag_speed, &speed_khz))!=ERROR_OK)
2663                         return retval;
2664         }
2665
2666         if (speed_khz==0)
2667         {
2668                 command_print(cmd_ctx, "RCLK - adaptive");
2669         } else
2670         {
2671                 command_print(cmd_ctx, "%d kHz", speed_khz);
2672         }
2673         return retval;
2674
2675 }
2676
2677 static int handle_endstate_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2678 {
2679         tap_state_t state;
2680
2681         if (argc < 1)
2682         {
2683                 return ERROR_COMMAND_SYNTAX_ERROR;
2684         }
2685         else
2686         {
2687                 state = tap_state_by_name( args[0] );
2688                 if( state < 0 ){
2689                         command_print( cmd_ctx, "Invalid state name: %s\n", args[0] );
2690                         return ERROR_COMMAND_SYNTAX_ERROR;
2691                 }
2692                 jtag_add_end_state(state);
2693                 jtag_execute_queue();
2694         }
2695         command_print(cmd_ctx, "current endstate: %s", tap_state_name(cmd_queue_end_state));
2696
2697         return ERROR_OK;
2698 }
2699
2700 static int handle_jtag_reset_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2701 {
2702         int trst = -1;
2703         int srst = -1;
2704
2705         if (argc < 2)
2706         {
2707                 return ERROR_COMMAND_SYNTAX_ERROR;
2708         }
2709
2710         if (args[0][0] == '1')
2711                 trst = 1;
2712         else if (args[0][0] == '0')
2713                 trst = 0;
2714         else
2715         {
2716                 return ERROR_COMMAND_SYNTAX_ERROR;
2717         }
2718
2719         if (args[1][0] == '1')
2720                 srst = 1;
2721         else if (args[1][0] == '0')
2722                 srst = 0;
2723         else
2724         {
2725                 return ERROR_COMMAND_SYNTAX_ERROR;
2726         }
2727
2728         if (jtag_interface_init(cmd_ctx) != ERROR_OK)
2729                 return ERROR_JTAG_INIT_FAILED;
2730
2731         jtag_add_reset(trst, srst);
2732         jtag_execute_queue();
2733
2734         return ERROR_OK;
2735 }
2736
2737 static int handle_runtest_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2738 {
2739         if (argc < 1)
2740         {
2741                 return ERROR_COMMAND_SYNTAX_ERROR;
2742         }
2743
2744         jtag_add_runtest(strtol(args[0], NULL, 0), TAP_INVALID);
2745         jtag_execute_queue();
2746
2747         return ERROR_OK;
2748
2749 }
2750
2751 static int handle_irscan_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2752 {
2753         int i;
2754         scan_field_t *fields;
2755         jtag_tap_t *tap;
2756         tap_state_t endstate;
2757
2758         if ((argc < 2) || (argc % 2))
2759         {
2760                 return ERROR_COMMAND_SYNTAX_ERROR;
2761         }
2762
2763         /* optional "-endstate" */
2764         /*          "statename" */
2765         /* at the end of the arguments. */
2766         /* assume none. */
2767         endstate = TAP_INVALID;
2768         if( argc >= 4 ){
2769                 /* have at least one pair of numbers. */
2770                 /* is last pair the magic text? */
2771                 if( 0 == strcmp( "-endstate", args[ argc - 2 ] ) ){
2772                         const char *cpA;
2773                         const char *cpS;
2774                         cpA = args[ argc-1 ];
2775                         for( endstate = 0 ; endstate < TAP_NUM_STATES ; endstate++ ){
2776                                 cpS = tap_state_name( endstate );
2777                                 if( 0 == strcmp( cpA, cpS ) ){
2778                                         break;
2779                                 }
2780                         }
2781                         if( endstate >= TAP_NUM_STATES ){
2782                                 return ERROR_COMMAND_SYNTAX_ERROR;
2783                         } else {
2784                                 /* found - remove the last 2 args */
2785                                 argc -= 2;
2786                         }
2787                 }
2788         }
2789
2790         fields = malloc(sizeof(scan_field_t) * argc / 2);
2791
2792         for (i = 0; i < argc / 2; i++)
2793         {
2794                 tap = jtag_TapByString( args[i*2] );
2795                 if (tap==NULL)
2796                 {
2797                         command_print( cmd_ctx, "Tap: %s unknown", args[i*2] );
2798                         return ERROR_FAIL;
2799                 }
2800                 int field_size = tap->ir_length;
2801                 fields[i].tap = tap;
2802                 fields[i].out_value = malloc(CEIL(field_size, 8));
2803                 buf_set_u32(fields[i].out_value, 0, field_size, strtoul(args[i*2+1], NULL, 0));
2804                 fields[i].in_value = NULL;
2805         }
2806
2807         jtag_add_ir_scan(argc / 2, fields, TAP_INVALID);
2808         /* did we have an endstate? */
2809         if (endstate != TAP_INVALID)
2810                 jtag_add_end_state(endstate);
2811
2812         jtag_execute_queue();
2813
2814         for (i = 0; i < argc / 2; i++)
2815                 free(fields[i].out_value);
2816
2817         free (fields);
2818
2819         return ERROR_OK;
2820 }
2821
2822 static int Jim_Command_drscan(Jim_Interp *interp, int argc, Jim_Obj *const *args)
2823 {
2824         int retval;
2825         scan_field_t *fields;
2826         int num_fields;
2827         int field_count = 0;
2828         int i, e;
2829         jtag_tap_t *tap;
2830         tap_state_t endstate;
2831
2832         /* args[1] = device
2833          * args[2] = num_bits
2834          * args[3] = hex string
2835          * ... repeat num bits and hex string ...
2836          *
2837          * .. optionally:
2838         *     args[N-2] = "-endstate"
2839          *     args[N-1] = statename
2840          */
2841         if ((argc < 4) || ((argc % 2)!=0))
2842         {
2843                 Jim_WrongNumArgs(interp, 1, args, "wrong arguments");
2844                 return JIM_ERR;
2845         }
2846
2847         /* assume no endstate */
2848         endstate = TAP_INVALID;
2849         /* validate arguments as numbers */
2850         e = JIM_OK;
2851         for (i = 2; i < argc; i+=2)
2852         {
2853                 long bits;
2854                 const char *cp;
2855
2856                 e = Jim_GetLong(interp, args[i], &bits);
2857                 /* If valid - try next arg */
2858                 if( e == JIM_OK ){
2859                         continue;
2860                 }
2861
2862                 /* Not valid.. are we at the end? */
2863                 if ( ((i+2) != argc) ){
2864                         /* nope, then error */
2865                         return e;
2866                 }
2867
2868                 /* it could be: "-endstate FOO" */
2869
2870                 /* get arg as a string. */
2871                 cp = Jim_GetString( args[i], NULL );
2872                 /* is it the magic? */
2873                 if( 0 == strcmp( "-endstate", cp ) ){
2874                         /* is the statename valid? */
2875                         cp = Jim_GetString( args[i+1], NULL );
2876
2877                         /* see if it is a valid state name */
2878                         endstate = tap_state_by_name(cp);
2879                         if( endstate < 0 ){
2880                                 /* update the error message */
2881                                 Jim_SetResult_sprintf(interp,"endstate: %s invalid", cp );
2882                         } else {
2883                                 /* valid - so clear the error */
2884                                 e = JIM_OK;
2885                                 /* and remove the last 2 args */
2886                                 argc -= 2;
2887                         }
2888                 }
2889
2890                 /* Still an error? */
2891                 if( e != JIM_OK ){
2892                         return e; /* too bad */
2893                 }
2894         } /* validate args */
2895
2896         tap = jtag_TapByJimObj( interp, args[1] );
2897         if( tap == NULL ){
2898                 return JIM_ERR;
2899         }
2900
2901         num_fields=(argc-2)/2;
2902         fields = malloc(sizeof(scan_field_t) * num_fields);
2903         for (i = 2; i < argc; i+=2)
2904         {
2905                 long bits;
2906                 int len;
2907                 const char *str;
2908
2909                 Jim_GetLong(interp, args[i], &bits);
2910                 str = Jim_GetString(args[i+1], &len);
2911
2912                 fields[field_count].tap = tap;
2913                 fields[field_count].num_bits = bits;
2914                 fields[field_count].out_value = malloc(CEIL(bits, 8));
2915                 str_to_buf(str, len, fields[field_count].out_value, bits, 0);
2916                 fields[field_count].in_value = fields[field_count].out_value;
2917                 field_count++;
2918         }
2919
2920         jtag_add_dr_scan(num_fields, fields, TAP_INVALID);
2921         /* did we get an end state? */
2922         if (endstate != TAP_INVALID)
2923                 jtag_add_end_state(endstate);
2924
2925         retval = jtag_execute_queue();
2926         if (retval != ERROR_OK)
2927         {
2928                 Jim_SetResultString(interp, "drscan: jtag execute failed",-1);
2929                 return JIM_ERR;
2930         }
2931
2932         field_count=0;
2933         Jim_Obj *list = Jim_NewListObj(interp, NULL, 0);
2934         for (i = 2; i < argc; i+=2)
2935         {
2936                 long bits;
2937                 char *str;
2938
2939                 Jim_GetLong(interp, args[i], &bits);
2940                 str = buf_to_str(fields[field_count].in_value, bits, 16);
2941                 free(fields[field_count].out_value);
2942
2943                 Jim_ListAppendElement(interp, list, Jim_NewStringObj(interp, str, strlen(str)));
2944                 free(str);
2945                 field_count++;
2946         }
2947
2948         Jim_SetResult(interp, list);
2949
2950         free(fields);
2951
2952         return JIM_OK;
2953 }
2954
2955
2956 static int Jim_Command_flush_count(Jim_Interp *interp, int argc, Jim_Obj *const *args)
2957 {
2958         Jim_SetResult(interp, Jim_NewIntObj(interp, jtag_flush_queue_count));
2959
2960         return JIM_OK;
2961 }
2962
2963
2964 static int handle_verify_ircapture_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2965 {
2966         if (argc == 1)
2967         {
2968                 if (strcmp(args[0], "enable") == 0)
2969                 {
2970                         jtag_verify_capture_ir = 1;
2971                 }
2972                 else if (strcmp(args[0], "disable") == 0)
2973                 {
2974                         jtag_verify_capture_ir = 0;
2975                 } else
2976                 {
2977                         return ERROR_COMMAND_SYNTAX_ERROR;
2978                 }
2979         } else if (argc != 0)
2980         {
2981                 return ERROR_COMMAND_SYNTAX_ERROR;
2982         }
2983
2984         command_print(cmd_ctx, "verify Capture-IR is %s", (jtag_verify_capture_ir) ? "enabled": "disabled");
2985
2986         return ERROR_OK;
2987 }
2988
2989 int jtag_power_dropout(int *dropout)
2990 {
2991         return jtag->power_dropout(dropout);
2992 }
2993
2994 int jtag_srst_asserted(int *srst_asserted)
2995 {
2996         return jtag->srst_asserted(srst_asserted);
2997 }
2998
2999 void jtag_tap_handle_event( jtag_tap_t * tap, enum jtag_tap_event e)
3000 {
3001         jtag_tap_event_action_t * jteap;
3002         int done;
3003
3004         jteap = tap->event_action;
3005
3006         done = 0;
3007         while (jteap) {
3008                 if (jteap->event == e) {
3009                         done = 1;
3010                         LOG_DEBUG( "JTAG tap: %s event: %d (%s) action: %s\n",
3011                                         tap->dotted_name,
3012                                         e,
3013                                         Jim_Nvp_value2name_simple(nvp_jtag_tap_event, e)->name,
3014                                         Jim_GetString(jteap->body, NULL) );
3015                         if (Jim_EvalObj(interp, jteap->body) != JIM_OK) {
3016                                 Jim_PrintErrorMessage(interp);
3017                         }
3018                 }
3019
3020                 jteap = jteap->next;
3021         }
3022
3023         if (!done) {
3024                 LOG_DEBUG( "event %d %s - no action",
3025                                 e,
3026                                 Jim_Nvp_value2name_simple( nvp_jtag_tap_event, e)->name);
3027         }
3028 }
3029
3030 /*-----<Cable Helper API>---------------------------------------*/
3031
3032 /*  these Cable Helper API functions are all documented in the jtag.h header file,
3033         using a Doxygen format.  And since Doxygen's configuration file "Doxyfile",
3034         is setup to prefer its docs in the header file, no documentation is here, for
3035         if it were, it would have to be doubly maintained.
3036 */
3037
3038 /**
3039  * @see tap_set_state() and tap_get_state() accessors.
3040  * Actual name is not important since accessors hide it.
3041  */
3042 static tap_state_t state_follower = TAP_RESET;
3043
3044 void tap_set_state_impl( tap_state_t new_state )
3045 {
3046         /* this is the state we think the TAPs are in now, was cur_state */
3047         state_follower = new_state;
3048 }
3049
3050 tap_state_t tap_get_state()
3051 {
3052         return state_follower;
3053 }
3054
3055 /**
3056  * @see tap_set_end_state() and tap_get_end_state() accessors.
3057  * Actual name is not important because accessors hide it.
3058  */
3059 static tap_state_t end_state_follower = TAP_RESET;
3060
3061 void tap_set_end_state( tap_state_t new_end_state )
3062 {
3063         /* this is the state we think the TAPs will be in at completion of the
3064            current TAP operation, was end_state
3065         */
3066         end_state_follower = new_end_state;
3067 }
3068
3069 tap_state_t tap_get_end_state()
3070 {
3071         return end_state_follower;
3072 }
3073
3074
3075 int tap_move_ndx( tap_state_t astate )
3076 {
3077         /* given a stable state, return the index into the tms_seqs[] array within tap_get_tms_path() */
3078
3079         int ndx;
3080
3081         switch( astate )
3082         {
3083         case TAP_RESET:         ndx = 0;                        break;
3084         case TAP_DRSHIFT:       ndx = 2;                        break;
3085         case TAP_DRPAUSE:       ndx = 3;                        break;
3086         case TAP_IDLE:          ndx = 1;                        break;
3087         case TAP_IRSHIFT:       ndx = 4;                        break;
3088         case TAP_IRPAUSE:       ndx = 5;                        break;
3089         default:
3090                 LOG_ERROR( "fatal: unstable state \"%s\" used in tap_move_ndx()", tap_state_name(astate) );
3091                 exit(1);
3092         }
3093
3094         return ndx;
3095 }
3096
3097
3098 /* tap_move[i][j]: tap movement command to go from state i to state j
3099  * 0: Test-Logic-Reset
3100  * 1: Run-Test/Idle
3101  * 2: Shift-DR
3102  * 3: Pause-DR
3103  * 4: Shift-IR
3104  * 5: Pause-IR
3105  *
3106  * DRSHIFT->DRSHIFT and IRSHIFT->IRSHIFT have to be caught in interface specific code
3107  */
3108 static struct
3109 {
3110         u8      bits;
3111         u8      bit_count;
3112
3113 } tms_seqs[6][6] =              /*  [from_state_ndx][to_state_ndx] */
3114 {
3115         /* value clocked to TMS to move from one of six stable states to another.
3116          * N.B. OOCD clocks TMS from LSB first, so read these right-to-left.
3117          * N.B. These values are tightly bound to the table in tap_get_tms_path_len().
3118          * N.B. Reset only needs to be 0b11111, but in JLink an even byte of 1's is more stable.
3119          *              These extra ones cause no TAP state problem, because we go into reset and stay in reset.
3120          */
3121
3122 /*
3123  * These macros allow us to specify TMS state transitions by bits rather than hex bytes.
3124  * Read the bits from LSBit first to MSBit last (right-to-left).
3125  */
3126 #define HEX__(n) 0x##n##LU
3127
3128 #define B8__(x) \
3129          (((x) & 0x0000000FLU)?(1<<0):0) \
3130         +(((x) & 0x000000F0LU)?(1<<1):0) \
3131         +(((x) & 0x00000F00LU)?(1<<2):0) \
3132         +(((x) & 0x0000F000LU)?(1<<3):0) \
3133         +(((x) & 0x000F0000LU)?(1<<4):0) \
3134         +(((x) & 0x00F00000LU)?(1<<5):0) \
3135         +(((x) & 0x0F000000LU)?(1<<6):0) \
3136         +(((x) & 0xF0000000LU)?(1<<7):0)
3137
3138 #define B8(bits,count)          { ((u8)B8__(HEX__(bits))), (count) }
3139
3140 #if 0 && ((BUILD_FT2232_FTD2XX==1) || (BUILD_FT2232_LIBFTDI==1) || (BUILD_JLINK==1))
3141         /*      this is the table submitted by Jeff Williams on 3/30/2009 with this comment:
3142
3143                 OK, I added Peter's version of the state table, and it works OK for
3144                 me on MC1322x. I've recreated the jlink portion of patch with this
3145                 new state table. His changes to my state table are pretty minor in
3146                 terms of total transitions, but Peter feels that his version fixes
3147                 some long-standing problems.
3148                 Jeff
3149
3150                 I added the bit count into the table, reduced RESET column to 7 bits from 8.
3151                 Dick
3152
3153                 state specific comments:
3154                 ------------------------
3155                 *->RESET                   tried the 5 bit reset and it gave me problems, 7 bits seems to
3156                                            work better on ARM9 with ft2232 driver.  (Dick)
3157
3158                 RESET->DRSHIFT add 1 extra clock cycles in the RESET state before advancing.
3159                                                 needed on ARM9 with ft2232 driver.  (Dick)
3160
3161                 RESET->IRSHIFT add 1 extra clock cycles in the RESET state before advancing.
3162                                                 needed on ARM9 with ft2232 driver.  (Dick)
3163         */
3164
3165         /* to state: */
3166         /*      RESET                   IDLE                            DRSHIFT                 DRPAUSE                 IRSHIFT                 IRPAUSE */                      /* from state: */
3167         {       B8(1111111,7),  B8(0,1),                        B8(00101,5),    B8(01010,5),            B8(001101,6),   B8(010110,6) },         /* RESET */
3168         {       B8(1111111,7),  B8(0,1),                        B8(001,3),              B8(0101,4),             B8(0011,4),     B8(01011,5) },          /* IDLE */
3169         {       B8(1111111,7),  B8(011,3),              B8(00111,5),            B8(01,2),               B8(001111,6),   B8(0101111,7) },                /* DRSHIFT */
3170         {       B8(1111111,7),  B8(011,3),              B8(01,2),               B8(0,1),                        B8(001111,6),   B8(0101111,7) },                /* DRPAUSE */
3171         {       B8(1111111,7),  B8(011,3),              B8(00111,5),            B8(010111,6),   B8(001111,6),   B8(01,2) },                     /* IRSHIFT */
3172         {       B8(1111111,7),  B8(011,3),              B8(00111,5),            B8(010111,6),   B8(01,2),               B8(0,1) }                       /* IRPAUSE */
3173
3174 #else   /* this is the old table, converted from hex and with the bit_count set to 7 for each combo, like before */
3175
3176         /* to state: */
3177         /*      RESET                   IDLE                            DRSHIFT                 DRPAUSE                 IRSHIFT                 IRPAUSE */                      /* from state: */
3178         {       B8(1111111,7),  B8(0000000,7),  B8(0010111,7),  B8(0001010,7),  B8(0011011,7),  B8(0010110,7) },                /* RESET */
3179         {       B8(1111111,7),  B8(0000000,7),  B8(0100101,7),  B8(0000101,7),  B8(0101011,7),  B8(0001011,7) },                /* IDLE */
3180         {       B8(1111111,7),  B8(0110001,7),  B8(0000000,7),  B8(0000001,7),  B8(0001111,7),  B8(0101111,7) },                /* DRSHIFT */
3181         {       B8(1111111,7),  B8(0110000,7),  B8(0100000,7),  B8(0010111,7),  B8(0011110,7),  B8(0101111,7) },                /* DRPAUSE */
3182         {       B8(1111111,7),  B8(0110001,7),  B8(0000111,7),  B8(0010111,7),  B8(0000000,7),  B8(0000001,7) },                /* IRSHIFT */
3183         {       B8(1111111,7),  B8(0110000,7),  B8(0011100,7),  B8(0010111,7),  B8(0100000,7),  B8(0101111,7) }                 /* IRPAUSE */
3184
3185 #endif
3186
3187 #if 0 /* keeping old hex stuff for awhile, for reference */
3188         /* RESET                        IDLE                    DRSHIFT                 DRPAUSE                 IRSHIFT                 IRPAUSE */
3189         {  0x7f,                        0x00,                   0x17,                   0x0a,                   0x1b,                   0x16 }, /* RESET */
3190         {  0x7f,                        0x00,                   0x25,                   0x05,                   0x2b,                   0x0b }, /* IDLE */
3191         {  0x7f,                        0x31,                   0x00,                   0x01,                   0x0f,                   0x2f }, /* DRSHIFT  */
3192         {  0x7f,                        0x30,                   0x20,                   0x17,                   0x1e,                   0x2f }, /* DRPAUSE  */
3193         {  0x7f,                        0x31,                   0x07,                   0x17,                   0x00,                   0x01 }, /* IRSHIFT  */
3194         {  0x7f,                        0x30,                   0x1c,                   0x17,                   0x20,                   0x2f }  /* IRPAUSE  */
3195 #endif
3196 };
3197
3198
3199 int tap_get_tms_path( tap_state_t from, tap_state_t to )
3200 {
3201         return tms_seqs[tap_move_ndx(from)][tap_move_ndx(to)].bits;
3202 }
3203
3204
3205 int tap_get_tms_path_len( tap_state_t from, tap_state_t to )
3206 {
3207         return tms_seqs[tap_move_ndx(from)][tap_move_ndx(to)].bit_count;
3208 }
3209
3210
3211 bool tap_is_state_stable(tap_state_t astate)
3212 {
3213         bool is_stable;
3214
3215         /*      A switch() is used because it is symbol dependent
3216                 (not value dependent like an array), and can also check bounds.
3217         */
3218         switch( astate )
3219         {
3220         case TAP_RESET:
3221         case TAP_IDLE:
3222         case TAP_DRSHIFT:
3223         case TAP_DRPAUSE:
3224         case TAP_IRSHIFT:
3225         case TAP_IRPAUSE:
3226                 is_stable = true;
3227                 break;
3228         default:
3229                 is_stable = false;
3230         }
3231
3232         return is_stable;
3233 }
3234
3235 tap_state_t tap_state_transition(tap_state_t cur_state, bool tms)
3236 {
3237         tap_state_t new_state;
3238
3239         /*      A switch is used because it is symbol dependent and not value dependent
3240                 like an array.  Also it can check for out of range conditions.
3241         */
3242
3243         if (tms)
3244         {
3245                 switch (cur_state)
3246                 {
3247                 case TAP_RESET:
3248                         new_state = cur_state;
3249                         break;
3250                 case TAP_IDLE:
3251                 case TAP_DRUPDATE:
3252                 case TAP_IRUPDATE:
3253                         new_state = TAP_DRSELECT;
3254                         break;
3255                 case TAP_DRSELECT:
3256                         new_state = TAP_IRSELECT;
3257                         break;
3258                 case TAP_DRCAPTURE:
3259                 case TAP_DRSHIFT:
3260                         new_state = TAP_DREXIT1;
3261                         break;
3262                 case TAP_DREXIT1:
3263                 case TAP_DREXIT2:
3264                         new_state = TAP_DRUPDATE;
3265                         break;
3266                 case TAP_DRPAUSE:
3267                         new_state = TAP_DREXIT2;
3268                         break;
3269                 case TAP_IRSELECT:
3270                         new_state = TAP_RESET;
3271                         break;
3272                 case TAP_IRCAPTURE:
3273                 case TAP_IRSHIFT:
3274                         new_state = TAP_IREXIT1;
3275                         break;
3276                 case TAP_IREXIT1:
3277                 case TAP_IREXIT2:
3278                         new_state = TAP_IRUPDATE;
3279                         break;
3280                 case TAP_IRPAUSE:
3281                         new_state = TAP_IREXIT2;
3282                         break;
3283                 default:
3284                         LOG_ERROR( "fatal: invalid argument cur_state=%d", cur_state );
3285                         exit(1);
3286                         break;
3287                 }
3288         }
3289         else
3290         {
3291                 switch (cur_state)
3292                 {
3293                 case TAP_RESET:
3294                 case TAP_IDLE:
3295                 case TAP_DRUPDATE:
3296                 case TAP_IRUPDATE:
3297                         new_state = TAP_IDLE;
3298                         break;
3299                 case TAP_DRSELECT:
3300                         new_state = TAP_DRCAPTURE;
3301                         break;
3302                 case TAP_DRCAPTURE:
3303                 case TAP_DRSHIFT:
3304                 case TAP_DREXIT2:
3305                         new_state = TAP_DRSHIFT;
3306                         break;
3307                 case TAP_DREXIT1:
3308                 case TAP_DRPAUSE:
3309                         new_state = TAP_DRPAUSE;
3310                         break;
3311                 case TAP_IRSELECT:
3312                         new_state = TAP_IRCAPTURE;
3313                         break;
3314                 case TAP_IRCAPTURE:
3315                 case TAP_IRSHIFT:
3316                 case TAP_IREXIT2:
3317                         new_state = TAP_IRSHIFT;
3318                         break;
3319                 case TAP_IREXIT1:
3320                 case TAP_IRPAUSE:
3321                         new_state = TAP_IRPAUSE;
3322                         break;
3323                 default:
3324                         LOG_ERROR( "fatal: invalid argument cur_state=%d", cur_state );
3325                         exit(1);
3326                         break;
3327                 }
3328         }
3329
3330         return new_state;
3331 }
3332
3333 const char* tap_state_name(tap_state_t state)
3334 {
3335         const char* ret;
3336
3337         switch( state )
3338         {
3339         case TAP_RESET:         ret = "RESET";                  break;
3340         case TAP_IDLE:          ret = "RUN/IDLE";               break;
3341         case TAP_DRSELECT:      ret = "DRSELECT";               break;
3342         case TAP_DRCAPTURE: ret = "DRCAPTURE";          break;
3343         case TAP_DRSHIFT:       ret = "DRSHIFT";                        break;
3344         case TAP_DREXIT1:       ret = "DREXIT1";                        break;
3345         case TAP_DRPAUSE:       ret = "DRPAUSE";                        break;
3346         case TAP_DREXIT2:       ret = "DREXIT2";                        break;
3347         case TAP_DRUPDATE:      ret = "DRUPDATE";               break;
3348         case TAP_IRSELECT:      ret = "IRSELECT";               break;
3349         case TAP_IRCAPTURE: ret = "IRCAPTURE";          break;
3350         case TAP_IRSHIFT:       ret = "IRSHIFT";                        break;
3351         case TAP_IREXIT1:       ret = "IREXIT1";                        break;
3352         case TAP_IRPAUSE:       ret = "IRPAUSE";                        break;
3353         case TAP_IREXIT2:       ret = "IREXIT2";                        break;
3354         case TAP_IRUPDATE:      ret = "IRUPDATE";               break;
3355         default:                                ret = "???";
3356         }
3357
3358         return ret;
3359 }
3360
3361 static tap_state_t tap_state_by_name( const char *name )
3362 {
3363         tap_state_t x;
3364
3365         for( x = 0 ; x < TAP_NUM_STATES ; x++ ){
3366                 /* be nice to the human */
3367                 if( 0 == strcasecmp( name, tap_state_name(x) ) ){
3368                         return x;
3369                 }
3370         }
3371         /* not found */
3372         return TAP_INVALID;
3373 }
3374
3375 #ifdef _DEBUG_JTAG_IO_
3376
3377 #define JTAG_DEBUG_STATE_APPEND(buf, len, bit) \
3378                 do { buf[len] = bit ? '1' : '0'; } while(0)
3379 #define JTAG_DEBUG_STATE_PRINT(a, b, astr, bstr) \
3380                 DEBUG_JTAG_IO("TAP/SM: %9s -> %5s\tTMS: %s\tTDI: %s", \
3381                         tap_state_name(a), tap_state_name(b), astr, bstr)
3382
3383 tap_state_t jtag_debug_state_machine(const void *tms_buf, const void *tdi_buf,
3384                 unsigned tap_bits, tap_state_t next_state)
3385 {
3386         const u8 *tms_buffer;
3387         const u8 *tdi_buffer;
3388         unsigned tap_bytes;
3389         unsigned cur_byte;
3390         unsigned cur_bit;
3391
3392         unsigned tap_out_bits;
3393         char tms_str[33];
3394         char tdi_str[33];
3395
3396         tap_state_t last_state;
3397
3398         // set startstate (and possibly last, if tap_bits == 0)
3399         last_state = next_state;
3400         DEBUG_JTAG_IO("TAP/SM: START state: %s", tap_state_name(next_state));
3401
3402         tms_buffer = (const u8 *)tms_buf;
3403         tdi_buffer = (const u8 *)tdi_buf;
3404
3405         tap_bytes = TAP_SCAN_BYTES(tap_bits);
3406         DEBUG_JTAG_IO("TAP/SM: TMS bits: %u (bytes: %u)", tap_bits, tap_bytes);
3407
3408         tap_out_bits = 0;
3409         for(cur_byte = 0; cur_byte < tap_bytes; cur_byte++)
3410         {
3411                 for(cur_bit = 0; cur_bit < 8; cur_bit++)
3412                 {
3413                         // make sure we do not run off the end of the buffers
3414                         unsigned tap_bit = cur_byte * 8 + cur_bit;
3415                         if (tap_bit == tap_bits)
3416                                 break;
3417
3418                         // check and save TMS bit
3419                         tap_bit = !!(tms_buffer[cur_byte] & (1 << cur_bit));
3420                         JTAG_DEBUG_STATE_APPEND(tms_str, tap_out_bits, tap_bit);
3421
3422                         // use TMS bit to find the next TAP state
3423                         next_state = tap_state_transition(last_state, tap_bit);
3424
3425                         // check and store TDI bit
3426                         tap_bit = !!(tdi_buffer[cur_byte] & (1 << cur_bit));
3427                         JTAG_DEBUG_STATE_APPEND(tdi_str, tap_out_bits, tap_bit);
3428
3429                         // increment TAP bits
3430                         tap_out_bits++;
3431
3432                         // Only show TDO bits on state transitions, or
3433                         // after some number of bits in the same state.
3434                         if ((next_state == last_state) && (tap_out_bits < 32))
3435                                 continue;
3436
3437                         // terminate strings and display state transition
3438                         tms_str[tap_out_bits] = tdi_str[tap_out_bits] = 0;
3439                         JTAG_DEBUG_STATE_PRINT(last_state, next_state, tms_str, tdi_str);
3440
3441                         // reset state
3442                         last_state = next_state;
3443                         tap_out_bits = 0;
3444                 }
3445         }
3446
3447         if (tap_out_bits)
3448         {
3449                 // terminate strings and display state transition
3450                 tms_str[tap_out_bits] = tdi_str[tap_out_bits] = 0;
3451                 JTAG_DEBUG_STATE_PRINT(last_state, next_state, tms_str, tdi_str);
3452         }
3453
3454         DEBUG_JTAG_IO("TAP/SM: FINAL state: %s", tap_state_name(next_state));
3455
3456         return next_state;
3457 }
3458 #endif // _DEBUG_JTAG_IO_
3459
3460 /*-----</Cable Helper API>--------------------------------------*/