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