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