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