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