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