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

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