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  *   This program is free software; you can redistribute it and/or modify  *
   9  *   it under the terms of the GNU General Public License as published by  *
  10  *   the Free Software Foundation; either version 2 of the License, or     *
  11  *   (at your option) any later version.                                   *
  12  *                                                                         *
  13  *   This program is distributed in the hope that it will be useful,       *
  14  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  15  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  16  *   GNU General Public License for more details.                          *
  17  *                                                                         *
  18  *   You should have received a copy of the GNU General Public License     *
  19  *   along with this program; if not, write to the                         *
  20  *   Free Software Foundation, Inc.,                                       *
  21  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  22  ***************************************************************************/
  23 #ifdef HAVE_CONFIG_H
  24 #include "config.h"
  25 #endif
  26
  27 #include "replacements.h"
  28
  29 #include "jtag.h"
  30
  31 #include "command.h"
  32 #include "log.h"
  33
  34 #include "stdlib.h"
  35 #include "string.h"
  36 #include <unistd.h>
  37
  38 /* note that this is not marked as static as it must be available from outside jtag.c for those
  39    that implement the jtag_xxx() minidriver layer
  40 */
  41 int jtag_error=ERROR_OK;
  42
  43
  44 char* tap_state_strings[16] =
  45 {
  46         "tlr",
  47         "sds", "cd", "sd", "e1d", "pd", "e2d", "ud",
  48         "rti",
  49         "sis", "ci", "si", "e1i", "pi", "e2i", "ui"
  50 };
  51
  52 typedef struct cmd_queue_page_s
  53 {
  54         void *address;
  55         size_t used;
  56         struct cmd_queue_page_s *next;
  57 } cmd_queue_page_t;
  58
  59 #define CMD_QUEUE_PAGE_SIZE (1024 * 1024)
  60 static cmd_queue_page_t *cmd_queue_pages = NULL;
  61
  62 /* tap_move[i][j]: tap movement command to go from state i to state j
  63  * 0: Test-Logic-Reset
  64  * 1: Run-Test/Idle
  65  * 2: Shift-DR
  66  * 3: Pause-DR
  67  * 4: Shift-IR
  68  * 5: Pause-IR
  69  *
  70  * SD->SD and SI->SI have to be caught in interface specific code
  71  */
  72 u8 tap_move[6][6] =
  73 {
  74 /*        TLR   RTI   SD    PD    SI    PI             */
  75         {0x7f, 0x00, 0x17, 0x0a, 0x1b, 0x16},   /* TLR */
  76         {0x7f, 0x00, 0x25, 0x05, 0x2b, 0x0b},   /* RTI */
  77         {0x7f, 0x31, 0x00, 0x01, 0x0f, 0x2f},   /* SD  */
  78         {0x7f, 0x30, 0x20, 0x17, 0x1e, 0x2f},   /* PD  */
  79         {0x7f, 0x31, 0x07, 0x17, 0x00, 0x01},   /* SI  */
  80         {0x7f, 0x30, 0x1c, 0x17, 0x20, 0x2f}    /* PI  */
  81 };
  82
  83 int tap_move_map[16] = {
  84         0, -1, -1,  2, -1,  3, -1, -1,
  85         1, -1, -1,  4, -1,  5, -1, -1
  86 };
  87
  88 tap_transition_t tap_transitions[16] =
  89 {
  90         {TAP_TLR, TAP_RTI},             /* TLR */
  91         {TAP_SIS, TAP_CD},              /* SDS */
  92         {TAP_E1D, TAP_SD},              /* CD  */
  93         {TAP_E1D, TAP_SD},              /* SD  */
  94         {TAP_UD,  TAP_PD},              /* E1D */
  95         {TAP_E2D, TAP_PD},              /* PD  */
  96         {TAP_UD,  TAP_SD},              /* E2D */
  97         {TAP_SDS, TAP_RTI},             /* UD  */
  98         {TAP_SDS, TAP_RTI},             /* RTI */
  99         {TAP_TLR, TAP_CI},              /* SIS */
 100         {TAP_E1I, TAP_SI},              /* CI  */
 101         {TAP_E1I, TAP_SI},              /* SI  */
 102         {TAP_UI,  TAP_PI},              /* E1I */
 103         {TAP_E2I, TAP_PI},              /* PI  */
 104         {TAP_UI,  TAP_SI},              /* E2I */
 105         {TAP_SDS, TAP_RTI}              /* UI  */
 106 };
 107
 108 char* jtag_event_strings[] =
 109 {
 110         "JTAG controller reset (TLR or TRST)"
 111 };
 112
 113 /* kludge!!!! these are just global variables that the
 114  * interface use internally. They really belong
 115  * inside the drivers, but we don't want to break
 116  * linking the drivers!!!!
 117  */
 118 enum tap_state end_state = TAP_TLR;
 119 enum tap_state cur_state = TAP_TLR;
 120 int jtag_trst = 0;
 121 int jtag_srst = 0;
 122
 123 jtag_command_t *jtag_command_queue = NULL;
 124 jtag_command_t **last_comand_pointer = &jtag_command_queue;
 125 static jtag_tap_t *jtag_all_taps = NULL;
 126
 127 enum reset_types jtag_reset_config = RESET_NONE;
 128 enum tap_state cmd_queue_end_state = TAP_TLR;
 129 enum tap_state cmd_queue_cur_state = TAP_TLR;
 130
 131 int jtag_verify_capture_ir = 1;
 132
 133 /* how long the OpenOCD should wait before attempting JTAG communication after reset lines deasserted (in ms) */
 134 int jtag_nsrst_delay = 0; /* default to no nSRST delay */
 135 int jtag_ntrst_delay = 0; /* default to no nTRST delay */
 136
 137 /* maximum number of JTAG devices expected in the chain
 138  */
 139 #define JTAG_MAX_CHAIN_SIZE 20
 140
 141 /* callbacks to inform high-level handlers about JTAG state changes */
 142 jtag_event_callback_t *jtag_event_callbacks;
 143
 144 /* speed in kHz*/
 145 static int speed_khz = 0;
 146 /* flag if the kHz speed was defined */
 147 static int hasKHz = 0;
 148
 149 /* jtag interfaces (parport, FTDI-USB, TI-USB, ...)
 150  */
 151
 152 #if BUILD_ECOSBOARD == 1
 153         extern jtag_interface_t zy1000_interface;
 154 #endif
 155
 156 #if BUILD_PARPORT == 1
 157         extern jtag_interface_t parport_interface;
 158 #endif
 159
 160 #if BUILD_DUMMY == 1
 161         extern jtag_interface_t dummy_interface;
 162 #endif
 163
 164 #if BUILD_FT2232_FTD2XX == 1
 165         extern jtag_interface_t ft2232_interface;
 166 #endif
 167
 168 #if BUILD_FT2232_LIBFTDI == 1
 169         extern jtag_interface_t ft2232_interface;
 170 #endif
 171
 172 #if BUILD_AMTJTAGACCEL == 1
 173         extern jtag_interface_t amt_jtagaccel_interface;
 174 #endif
 175
 176 #if BUILD_EP93XX == 1
 177         extern jtag_interface_t ep93xx_interface;
 178 #endif
 179
 180 #if BUILD_AT91RM9200 == 1
 181         extern jtag_interface_t at91rm9200_interface;
 182 #endif
 183
 184 #if BUILD_GW16012 == 1
 185         extern jtag_interface_t gw16012_interface;
 186 #endif
 187
 188 #if BUILD_PRESTO_LIBFTDI == 1 || BUILD_PRESTO_FTD2XX == 1
 189         extern jtag_interface_t presto_interface;
 190 #endif
 191
 192 #if BUILD_USBPROG == 1
 193         extern jtag_interface_t usbprog_interface;
 194 #endif
 195
 196 #if BUILD_JLINK == 1
 197         extern jtag_interface_t jlink_interface;
 198 #endif
 199
 200 jtag_interface_t *jtag_interfaces[] = {
 201 #if BUILD_ECOSBOARD == 1
 202         &zy1000_interface,
 203 #endif
 204 #if BUILD_PARPORT == 1
 205         &parport_interface,
 206 #endif
 207 #if BUILD_DUMMY == 1
 208         &dummy_interface,
 209 #endif
 210 #if BUILD_FT2232_FTD2XX == 1
 211         &ft2232_interface,
 212 #endif
 213 #if BUILD_FT2232_LIBFTDI == 1
 214         &ft2232_interface,
 215 #endif
 216 #if BUILD_AMTJTAGACCEL == 1
 217         &amt_jtagaccel_interface,
 218 #endif
 219 #if BUILD_EP93XX == 1
 220         &ep93xx_interface,
 221 #endif
 222 #if BUILD_AT91RM9200 == 1
 223         &at91rm9200_interface,
 224 #endif
 225 #if BUILD_GW16012 == 1
 226         &gw16012_interface,
 227 #endif
 228 #if BUILD_PRESTO_LIBFTDI == 1 || BUILD_PRESTO_FTD2XX == 1
 229         &presto_interface,
 230 #endif
 231 #if BUILD_USBPROG == 1
 232         &usbprog_interface,
 233 #endif
 234 #if BUILD_JLINK == 1
 235         &jlink_interface,
 236 #endif
 237         NULL,
 238 };
 239
 240 jtag_interface_t *jtag = NULL;
 241
 242 /* configuration */
 243 jtag_interface_t *jtag_interface = NULL;
 244 int jtag_speed = 0;
 245
 246
 247
 248 /* forward declarations */
 249 void jtag_add_pathmove(int num_states, enum tap_state *path);
 250 void jtag_add_runtest(int num_cycles, enum tap_state endstate);
 251 void jtag_add_end_state(enum tap_state endstate);
 252 void jtag_add_sleep(u32 us);
 253 int jtag_execute_queue(void);
 254
 255
 256 /* jtag commands */
 257 int handle_interface_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
 258 int handle_jtag_speed_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
 259 int handle_jtag_khz_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
 260 int handle_jtag_device_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
 261 int handle_reset_config_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
 262 int handle_jtag_nsrst_delay_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
 263 int handle_jtag_ntrst_delay_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
 264
 265 int handle_scan_chain_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
 266
 267 int handle_endstate_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
 268 int handle_jtag_reset_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
 269 int handle_runtest_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
 270 int handle_irscan_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
 271 int Jim_Command_drscan(Jim_Interp *interp, int argc, Jim_Obj *const *argv);
 272
 273 int handle_verify_ircapture_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
 274
 275
 276 jtag_tap_t *jtag_AllTaps(void)
 277 {
 278   return jtag_all_taps;
 279 };
 280
 281 int
 282 jtag_NumTotalTaps(void)
 283 {
 284         jtag_tap_t *t;
 285         int n;
 286
 287         n = 0;
 288         t = jtag_AllTaps();
 289         while(t){
 290                 n++;
 291                 t = t->next_tap;
 292         }
 293         return n;
 294 }
 295
 296 int
 297 jtag_NumEnabledTaps(void)
 298 {
 299         jtag_tap_t *t;
 300         int n;
 301
 302         n = 0;
 303         t = jtag_AllTaps();
 304         while(t){
 305                 if( t->enabled ){
 306                         n++;
 307                 }
 308                 t = t->next_tap;
 309         }
 310         return n;
 311 }
 312
 313
 314 jtag_tap_t *jtag_TapByString( const char *s )
 315 {
 316         jtag_tap_t *t;
 317         char *cp;
 318
 319         t = jtag_AllTaps();
 320         // try name first
 321         while(t){
 322                 if( 0 == strcmp( t->dotted_name, s ) ){
 323                         break;
 324                 } else {
 325                         t = t->next_tap;
 326                 }
 327         }
 328         // backup plan is by number
 329         if( t == NULL ){
 330                 /* ok - is "s" a number? */
 331                 int n;
 332                 n = strtol( s, &cp, 0 );
 333                 if( (s != cp) && (*cp == 0) ){
 334                         /* Then it is... */
 335                         t = jtag_TapByAbsPosition(n);
 336                 }
 337         }
 338         return t;
 339 }
 340
 341 jtag_tap_t *
 342 jtag_TapByJimObj( Jim_Interp *interp, Jim_Obj *o )
 343 {
 344         jtag_tap_t *t;
 345         const char *cp;
 346
 347         cp = Jim_GetString( o, NULL );
 348         if(cp == NULL){
 349                 cp = "(unknown)";
 350                 t = NULL;
 351         }  else {
 352                 t = jtag_TapByString( cp );
 353         }
 354         if( t == NULL ){
 355                 Jim_SetResult_sprintf(interp,"Tap: %s is unknown", cp );
 356         }
 357         return t;
 358 }
 359
 360 /* returns a pointer to the n-th device in the scan chain */
 361 jtag_tap_t *
 362 jtag_TapByAbsPosition( int n )
 363 {
 364         int orig_n;
 365         jtag_tap_t *t;
 366
 367         orig_n = n;
 368         t = jtag_AllTaps();
 369
 370         while( t && (n > 0)) {
 371                 n--;
 372                 t = t->next_tap;
 373     }
 374         return t;
 375 }
 376
 377
 378 int jtag_register_event_callback(int (*callback)(enum jtag_event event, void *priv), void *priv)
 379 {
 380         jtag_event_callback_t **callbacks_p = &jtag_event_callbacks;
 381
 382         if (callback == NULL)
 383         {
 384                 return ERROR_INVALID_ARGUMENTS;
 385         }
 386
 387         if (*callbacks_p)
 388         {
 389                 while ((*callbacks_p)->next)
 390                         callbacks_p = &((*callbacks_p)->next);
 391                 callbacks_p = &((*callbacks_p)->next);
 392         }
 393
 394         (*callbacks_p) = malloc(sizeof(jtag_event_callback_t));
 395         (*callbacks_p)->callback = callback;
 396         (*callbacks_p)->priv = priv;
 397         (*callbacks_p)->next = NULL;
 398
 399         return ERROR_OK;
 400 }
 401
 402 int jtag_unregister_event_callback(int (*callback)(enum jtag_event event, void *priv))
 403 {
 404         jtag_event_callback_t **callbacks_p = &jtag_event_callbacks;
 405
 406         if (callback == NULL)
 407         {
 408                 return ERROR_INVALID_ARGUMENTS;
 409         }
 410
 411         while (*callbacks_p)
 412         {
 413                 jtag_event_callback_t **next = &((*callbacks_p)->next);
 414                 if ((*callbacks_p)->callback == callback)
 415                 {
 416                         free(*callbacks_p);
 417                         *callbacks_p = *next;
 418                 }
 419                 callbacks_p = next;
 420         }
 421
 422         return ERROR_OK;
 423 }
 424
 425 int jtag_call_event_callbacks(enum jtag_event event)
 426 {
 427         jtag_event_callback_t *callback = jtag_event_callbacks;
 428
 429         LOG_DEBUG("jtag event: %s", jtag_event_strings[event]);
 430
 431         while (callback)
 432         {
 433                 callback->callback(event, callback->priv);
 434                 callback = callback->next;
 435         }
 436
 437         return ERROR_OK;
 438 }
 439
 440 /* returns a pointer to the pointer of the last command in queue
 441  * this may be a pointer to the root pointer (jtag_command_queue)
 442  * or to the next member of the last but one command
 443  */
 444 jtag_command_t** jtag_get_last_command_p(void)
 445 {
 446 /*      jtag_command_t *cmd = jtag_command_queue;
 447
 448         if (cmd)
 449                 while (cmd->next)
 450                         cmd = cmd->next;
 451         else
 452                 return &jtag_command_queue;
 453
 454         return &cmd->next;*/
 455
 456         return last_comand_pointer;
 457 }
 458
 459
 460 void* cmd_queue_alloc(size_t size)
 461 {
 462         cmd_queue_page_t **p_page = &cmd_queue_pages;
 463         int offset;
 464         u8 *t;
 465
 466         /*
 467          * WARNING:
 468          *    We align/round the *SIZE* per below
 469          *    so that all pointers returned by
 470          *    this function are reasonably well
 471          *    aligned.
 472          *
 473          * If we did not, then an "odd-length" request would cause the
 474          * *next* allocation to be at an *odd* address, and because
 475          * this function has the same type of api as malloc() - we
 476          * must also return pointers that have the same type of
 477          * alignment.
 478          *
 479          * What I do not/have is a reasonable portable means
 480          * to align by...
 481          *
 482          * The solution here, is based on these suggestions.
 483          * http://gcc.gnu.org/ml/gcc-help/2008-12/msg00041.html
 484          *
 485          */
 486         union worse_case_align {
 487           int i;
 488           long l;
 489           float f;
 490           void *v;
 491         };
 492 #define ALIGN_SIZE  (sizeof(union worse_case_align))
 493
 494         // The alignment process.
 495         size = (size + ALIGN_SIZE -1) & (~(ALIGN_SIZE-1));
 496         // Done...
 497
 498
 499         if (*p_page)
 500         {
 501                 while ((*p_page)->next)
 502                         p_page = &((*p_page)->next);
 503                 if (CMD_QUEUE_PAGE_SIZE - (*p_page)->used < size)
 504                         p_page = &((*p_page)->next);
 505         }
 506
 507         if (!*p_page)
 508         {
 509                 *p_page = malloc(sizeof(cmd_queue_page_t));
 510                 (*p_page)->used = 0;
 511                 (*p_page)->address = malloc(CMD_QUEUE_PAGE_SIZE);
 512                 (*p_page)->next = NULL;
 513         }
 514
 515         offset = (*p_page)->used;
 516         (*p_page)->used += size;
 517
 518         t=(u8 *)((*p_page)->address);
 519         return t + offset;
 520 }
 521
 522 void cmd_queue_free(void)
 523 {
 524         cmd_queue_page_t *page = cmd_queue_pages;
 525
 526         while (page)
 527         {
 528                 cmd_queue_page_t *last = page;
 529                 free(page->address);
 530                 page = page->next;
 531                 free(last);
 532         }
 533
 534         cmd_queue_pages = NULL;
 535 }
 536
 537 static void jtag_prelude1(void)
 538 {
 539         if (jtag_trst == 1)
 540         {
 541                 LOG_WARNING("JTAG command queued, while TRST is low (TAP in reset)");
 542                 jtag_error=ERROR_JTAG_TRST_ASSERTED;
 543                 return;
 544         }
 545
 546         if (cmd_queue_end_state == TAP_TLR)
 547                 jtag_call_event_callbacks(JTAG_TRST_ASSERTED);
 548 }
 549
 550 static void jtag_prelude(enum tap_state state)
 551 {
 552         jtag_prelude1();
 553
 554         if (state != -1)
 555                 jtag_add_end_state(state);
 556
 557         cmd_queue_cur_state = cmd_queue_end_state;
 558 }
 559
 560 void jtag_add_ir_scan(int num_fields, scan_field_t *fields, enum tap_state state)
 561 {
 562         int retval;
 563
 564         jtag_prelude(state);
 565
 566         retval=interface_jtag_add_ir_scan(num_fields, fields, cmd_queue_end_state);
 567         if (retval!=ERROR_OK)
 568                 jtag_error=retval;
 569 }
 570
 571 int MINIDRIVER(interface_jtag_add_ir_scan)(int num_fields, scan_field_t *fields, enum tap_state state)
 572 {
 573         jtag_command_t **last_cmd;
 574         jtag_tap_t *tap;
 575         int j;
 576         int x;
 577         int nth_tap;
 578         int scan_size = 0;
 579
 580
 581         last_cmd = jtag_get_last_command_p();
 582
 583         /* allocate memory for a new list member */
 584         *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
 585         (*last_cmd)->next = NULL;
 586         last_comand_pointer = &((*last_cmd)->next);
 587         (*last_cmd)->type = JTAG_SCAN;
 588
 589         /* allocate memory for ir scan command */
 590         (*last_cmd)->cmd.scan = cmd_queue_alloc(sizeof(scan_command_t));
 591         (*last_cmd)->cmd.scan->ir_scan = 1;
 592         x = jtag_NumEnabledTaps();
 593         (*last_cmd)->cmd.scan->num_fields = x;  /* one field per device */
 594         (*last_cmd)->cmd.scan->fields = cmd_queue_alloc(x  * sizeof(scan_field_t));
 595         (*last_cmd)->cmd.scan->end_state = state;
 596
 597         nth_tap = -1;
 598         tap = NULL;
 599         for(;;){
 600                 int found = 0;
 601
 602                 // do this here so it is not forgotten
 603                 tap = jtag_NextEnabledTap(tap);
 604                 if( tap == NULL ){
 605                         break;
 606                 }
 607                 nth_tap++;
 608                 scan_size = tap->ir_length;
 609                 (*last_cmd)->cmd.scan->fields[nth_tap].tap = tap;
 610                 (*last_cmd)->cmd.scan->fields[nth_tap].num_bits = scan_size;
 611                 (*last_cmd)->cmd.scan->fields[nth_tap].in_value = NULL;
 612                 (*last_cmd)->cmd.scan->fields[nth_tap].in_handler = NULL;       /* disable verification by default */
 613
 614                 /* search the list */
 615                 for (j = 0; j < num_fields; j++)
 616                 {
 617                         if (tap == fields[j].tap)
 618                         {
 619                                 found = 1;
 620                                 (*last_cmd)->cmd.scan->fields[nth_tap].out_value = buf_cpy(fields[j].out_value, cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);
 621                                 (*last_cmd)->cmd.scan->fields[nth_tap].out_mask = buf_cpy(fields[j].out_mask, cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);
 622
 623                                 if (jtag_verify_capture_ir)
 624                                 {
 625                                         if (fields[j].in_handler==NULL)
 626                                         {
 627                                                 jtag_set_check_value((*last_cmd)->cmd.scan->fields+nth_tap, tap->expected, tap->expected_mask, NULL);
 628                                         } else
 629                                         {
 630                                                 (*last_cmd)->cmd.scan->fields[nth_tap].in_handler = fields[j].in_handler;
 631                                                 (*last_cmd)->cmd.scan->fields[nth_tap].in_handler_priv = fields[j].in_handler_priv;
 632                                                 (*last_cmd)->cmd.scan->fields[nth_tap].in_check_value = tap->expected;
 633                                                 (*last_cmd)->cmd.scan->fields[nth_tap].in_check_mask = tap->expected_mask;
 634                                         }
 635                                 }
 636
 637                                 tap->bypass = 0;
 638                                 break;
 639                         }
 640                 }
 641
 642                 if (!found)
 643                 {
 644                         /* if a tap isn't listed, set it to BYPASS */
 645                         (*last_cmd)->cmd.scan->fields[nth_tap].out_value = buf_set_ones(cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);
 646                         (*last_cmd)->cmd.scan->fields[nth_tap].out_mask = NULL;
 647                         tap->bypass = 1;
 648
 649                 }
 650
 651                 /* update device information */
 652                 buf_cpy((*last_cmd)->cmd.scan->fields[nth_tap].out_value, tap->cur_instr, scan_size);
 653         }
 654
 655         return ERROR_OK;
 656 }
 657
 658 void jtag_add_plain_ir_scan(int num_fields, scan_field_t *fields, enum tap_state state)
 659 {
 660         int retval;
 661
 662         jtag_prelude(state);
 663
 664         retval=interface_jtag_add_plain_ir_scan(num_fields, fields, cmd_queue_end_state);
 665         if (retval!=ERROR_OK)
 666                 jtag_error=retval;
 667 }
 668
 669 int MINIDRIVER(interface_jtag_add_plain_ir_scan)(int num_fields, scan_field_t *fields, enum tap_state state)
 670 {
 671         int i;
 672         jtag_command_t **last_cmd;
 673
 674         last_cmd = jtag_get_last_command_p();
 675
 676         /* allocate memory for a new list member */
 677         *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
 678         (*last_cmd)->next = NULL;
 679         last_comand_pointer = &((*last_cmd)->next);
 680         (*last_cmd)->type = JTAG_SCAN;
 681
 682         /* allocate memory for ir scan command */
 683         (*last_cmd)->cmd.scan = cmd_queue_alloc(sizeof(scan_command_t));
 684         (*last_cmd)->cmd.scan->ir_scan = 1;
 685         (*last_cmd)->cmd.scan->num_fields = num_fields;
 686         (*last_cmd)->cmd.scan->fields = cmd_queue_alloc(num_fields * sizeof(scan_field_t));
 687         (*last_cmd)->cmd.scan->end_state = state;
 688
 689         for( i = 0 ; i < num_fields ; i++ ){
 690                 int num_bits = fields[i].num_bits;
 691                 int num_bytes = CEIL(fields[i].num_bits, 8);
 692                 (*last_cmd)->cmd.scan->fields[i].tap = fields[i].tap;
 693                 (*last_cmd)->cmd.scan->fields[i].num_bits = num_bits;
 694                 (*last_cmd)->cmd.scan->fields[i].out_value = buf_cpy(fields[i].out_value, cmd_queue_alloc(num_bytes), num_bits);
 695                 (*last_cmd)->cmd.scan->fields[i].out_mask = buf_cpy(fields[i].out_mask, cmd_queue_alloc(num_bytes), num_bits);
 696                 (*last_cmd)->cmd.scan->fields[i].in_value = fields[i].in_value;
 697                 (*last_cmd)->cmd.scan->fields[i].in_check_value = fields[i].in_check_value;
 698                 (*last_cmd)->cmd.scan->fields[i].in_check_mask = fields[i].in_check_mask;
 699                 (*last_cmd)->cmd.scan->fields[i].in_handler = NULL;
 700                 (*last_cmd)->cmd.scan->fields[i].in_handler_priv = NULL;
 701         }
 702         return ERROR_OK;
 703 }
 704
 705 void jtag_add_dr_scan(int num_fields, scan_field_t *fields, enum tap_state state)
 706 {
 707         int retval;
 708
 709         jtag_prelude(state);
 710
 711         retval=interface_jtag_add_dr_scan(num_fields, fields, cmd_queue_end_state);
 712         if (retval!=ERROR_OK)
 713                 jtag_error=retval;
 714 }
 715
 716 int MINIDRIVER(interface_jtag_add_dr_scan)(int num_fields, scan_field_t *fields, enum tap_state state)
 717 {
 718         int j;
 719         int nth_tap;
 720         int bypass_devices = 0;
 721         int field_count = 0;
 722         int scan_size;
 723
 724         jtag_command_t **last_cmd = jtag_get_last_command_p();
 725         jtag_tap_t *tap;
 726
 727         /* count devices in bypass */
 728         tap = NULL;
 729         bypass_devices = 0;
 730         for(;;){
 731                 tap = jtag_NextEnabledTap(tap);
 732                 if( tap == NULL ){
 733                         break;
 734                 }
 735                 if( tap->bypass ){
 736                         bypass_devices++;
 737                 }
 738         }
 739
 740         /* allocate memory for a new list member */
 741         *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
 742         last_comand_pointer = &((*last_cmd)->next);
 743         (*last_cmd)->next = NULL;
 744         (*last_cmd)->type = JTAG_SCAN;
 745
 746         /* allocate memory for dr scan command */
 747         (*last_cmd)->cmd.scan = cmd_queue_alloc(sizeof(scan_command_t));
 748         (*last_cmd)->cmd.scan->ir_scan = 0;
 749         (*last_cmd)->cmd.scan->num_fields = num_fields + bypass_devices;
 750         (*last_cmd)->cmd.scan->fields = cmd_queue_alloc((num_fields + bypass_devices) * sizeof(scan_field_t));
 751         (*last_cmd)->cmd.scan->end_state = state;
 752
 753         tap = NULL;
 754         nth_tap = -1;
 755         for(;;){
 756                 nth_tap++;
 757                 tap = jtag_NextEnabledTap(tap);
 758                 if( tap == NULL ){
 759                         break;
 760                 }
 761                 int found = 0;
 762                 (*last_cmd)->cmd.scan->fields[field_count].tap = tap;
 763
 764                 for (j = 0; j < num_fields; j++)
 765                 {
 766                         if (tap == fields[j].tap)
 767                         {
 768                                 found = 1;
 769                                 scan_size = fields[j].num_bits;
 770                                 (*last_cmd)->cmd.scan->fields[field_count].num_bits = scan_size;
 771                                 (*last_cmd)->cmd.scan->fields[field_count].out_value = buf_cpy(fields[j].out_value, cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);
 772                                 (*last_cmd)->cmd.scan->fields[field_count].out_mask = buf_cpy(fields[j].out_mask, cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);
 773                                 (*last_cmd)->cmd.scan->fields[field_count].in_value = fields[j].in_value;
 774                                 (*last_cmd)->cmd.scan->fields[field_count].in_check_value = fields[j].in_check_value;
 775                                 (*last_cmd)->cmd.scan->fields[field_count].in_check_mask = fields[j].in_check_mask;
 776                                 (*last_cmd)->cmd.scan->fields[field_count].in_handler = fields[j].in_handler;
 777                                 (*last_cmd)->cmd.scan->fields[field_count++].in_handler_priv = fields[j].in_handler_priv;
 778                         }
 779                 }
 780                 if (!found)
 781                 {
 782 #ifdef _DEBUG_JTAG_IO_
 783                         /* if a device isn't listed, the BYPASS register should be selected */
 784                         if (! tap->bypass)
 785                         {
 786                                 LOG_ERROR("BUG: no scan data for a device not in BYPASS");
 787                                 exit(-1);
 788                         }
 789 #endif
 790                         /* program the scan field to 1 bit length, and ignore it's value */
 791                         (*last_cmd)->cmd.scan->fields[field_count].num_bits = 1;
 792                         (*last_cmd)->cmd.scan->fields[field_count].out_value = NULL;
 793                         (*last_cmd)->cmd.scan->fields[field_count].out_mask = NULL;
 794                         (*last_cmd)->cmd.scan->fields[field_count].in_value = NULL;
 795                         (*last_cmd)->cmd.scan->fields[field_count].in_check_value = NULL;
 796                         (*last_cmd)->cmd.scan->fields[field_count].in_check_mask = NULL;
 797                         (*last_cmd)->cmd.scan->fields[field_count].in_handler = NULL;
 798                         (*last_cmd)->cmd.scan->fields[field_count++].in_handler_priv = NULL;
 799                 }
 800                 else
 801                 {
 802 #ifdef _DEBUG_JTAG_IO_
 803                         /* if a device is listed, the BYPASS register must not be selected */
 804                         if (tap->bypass)
 805                         {
 806                                 LOG_ERROR("BUG: scan data for a device in BYPASS");
 807                                 exit(-1);
 808                         }
 809 #endif
 810                 }
 811         }
 812         return ERROR_OK;
 813 }
 814
 815 void MINIDRIVER(interface_jtag_add_dr_out)(jtag_tap_t *target_tap,
 816                 int num_fields,
 817                 const int *num_bits,
 818                 const u32 *value,
 819                 enum tap_state end_state)
 820 {
 821         int nth_tap;
 822         int field_count = 0;
 823         int scan_size;
 824         int bypass_devices = 0;
 825
 826         jtag_command_t **last_cmd = jtag_get_last_command_p();
 827         jtag_tap_t *tap;
 828
 829         /* count devices in bypass */
 830         tap = NULL;
 831         bypass_devices = 0;
 832         for(;;){
 833                 tap = jtag_NextEnabledTap(tap);
 834                 if( tap == NULL ){
 835                         break;
 836                 }
 837                 if( tap->bypass ){
 838                         bypass_devices++;
 839                 }
 840         }
 841
 842         /* allocate memory for a new list member */
 843         *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
 844         last_comand_pointer = &((*last_cmd)->next);
 845         (*last_cmd)->next = NULL;
 846         (*last_cmd)->type = JTAG_SCAN;
 847
 848         /* allocate memory for dr scan command */
 849         (*last_cmd)->cmd.scan = cmd_queue_alloc(sizeof(scan_command_t));
 850         (*last_cmd)->cmd.scan->ir_scan = 0;
 851         (*last_cmd)->cmd.scan->num_fields = num_fields + bypass_devices;
 852         (*last_cmd)->cmd.scan->fields = cmd_queue_alloc((num_fields + bypass_devices) * sizeof(scan_field_t));
 853         (*last_cmd)->cmd.scan->end_state = end_state;
 854
 855         tap = NULL;
 856         nth_tap = -1;
 857         for(;;){
 858                 tap = jtag_NextEnabledTap(tap);
 859                 if( tap == NULL ){
 860                         break;
 861                 }
 862                 nth_tap++;
 863                 (*last_cmd)->cmd.scan->fields[field_count].tap = tap;
 864
 865                 if (tap == target_tap)
 866                 {
 867                         int j;
 868 #ifdef _DEBUG_JTAG_IO_
 869                         /* if a device is listed, the BYPASS register must not be selected */
 870                         if (tap->bypass)
 871                         {
 872                                 LOG_ERROR("BUG: scan data for a device in BYPASS");
 873                                 exit(-1);
 874                         }
 875 #endif
 876                         for (j = 0; j < num_fields; j++)
 877                         {
 878                                 u8 out_value[4];
 879                                 scan_size = num_bits[j];
 880                                 buf_set_u32(out_value, 0, scan_size, value[j]);
 881                                 (*last_cmd)->cmd.scan->fields[field_count].num_bits = scan_size;
 882                                 (*last_cmd)->cmd.scan->fields[field_count].out_value = buf_cpy(out_value, cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);
 883                                 (*last_cmd)->cmd.scan->fields[field_count].out_mask = NULL;
 884                                 (*last_cmd)->cmd.scan->fields[field_count].in_value = NULL;
 885                                 (*last_cmd)->cmd.scan->fields[field_count].in_check_value = NULL;
 886                                 (*last_cmd)->cmd.scan->fields[field_count].in_check_mask = NULL;
 887                                 (*last_cmd)->cmd.scan->fields[field_count].in_handler = NULL;
 888                                 (*last_cmd)->cmd.scan->fields[field_count++].in_handler_priv = NULL;
 889                         }
 890                 } else
 891                 {
 892 #ifdef _DEBUG_JTAG_IO_
 893                         /* if a device isn't listed, the BYPASS register should be selected */
 894                         if (! tap->bypass)
 895                         {
 896                                 LOG_ERROR("BUG: no scan data for a device not in BYPASS");
 897                                 exit(-1);
 898                         }
 899 #endif
 900                         /* program the scan field to 1 bit length, and ignore it's value */
 901                         (*last_cmd)->cmd.scan->fields[field_count].num_bits = 1;
 902                         (*last_cmd)->cmd.scan->fields[field_count].out_value = NULL;
 903                         (*last_cmd)->cmd.scan->fields[field_count].out_mask = NULL;
 904                         (*last_cmd)->cmd.scan->fields[field_count].in_value = NULL;
 905                         (*last_cmd)->cmd.scan->fields[field_count].in_check_value = NULL;
 906                         (*last_cmd)->cmd.scan->fields[field_count].in_check_mask = NULL;
 907                         (*last_cmd)->cmd.scan->fields[field_count].in_handler = NULL;
 908                         (*last_cmd)->cmd.scan->fields[field_count++].in_handler_priv = NULL;
 909                 }
 910         }
 911 }
 912
 913 void jtag_add_plain_dr_scan(int num_fields, scan_field_t *fields, enum tap_state state)
 914 {
 915         int retval;
 916
 917         jtag_prelude(state);
 918
 919         retval=interface_jtag_add_plain_dr_scan(num_fields, fields, cmd_queue_end_state);
 920         if (retval!=ERROR_OK)
 921                 jtag_error=retval;
 922 }
 923
 924 int MINIDRIVER(interface_jtag_add_plain_dr_scan)(int num_fields, scan_field_t *fields, enum tap_state state)
 925 {
 926         int i;
 927         jtag_command_t **last_cmd = jtag_get_last_command_p();
 928
 929         /* allocate memory for a new list member */
 930         *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
 931         last_comand_pointer = &((*last_cmd)->next);
 932         (*last_cmd)->next = NULL;
 933         (*last_cmd)->type = JTAG_SCAN;
 934
 935         /* allocate memory for scan command */
 936         (*last_cmd)->cmd.scan = cmd_queue_alloc(sizeof(scan_command_t));
 937         (*last_cmd)->cmd.scan->ir_scan = 0;
 938         (*last_cmd)->cmd.scan->num_fields = num_fields;
 939         (*last_cmd)->cmd.scan->fields = cmd_queue_alloc(num_fields * sizeof(scan_field_t));
 940         (*last_cmd)->cmd.scan->end_state = state;
 941
 942         for (i = 0; i < num_fields; i++)
 943         {
 944                 int num_bits = fields[i].num_bits;
 945                 int num_bytes = CEIL(fields[i].num_bits, 8);
 946                 (*last_cmd)->cmd.scan->fields[i].tap = fields[i].tap;
 947                 (*last_cmd)->cmd.scan->fields[i].num_bits = num_bits;
 948                 (*last_cmd)->cmd.scan->fields[i].out_value = buf_cpy(fields[i].out_value, cmd_queue_alloc(num_bytes), num_bits);
 949                 (*last_cmd)->cmd.scan->fields[i].out_mask = buf_cpy(fields[i].out_mask, cmd_queue_alloc(num_bytes), num_bits);
 950                 (*last_cmd)->cmd.scan->fields[i].in_value = fields[i].in_value;
 951                 (*last_cmd)->cmd.scan->fields[i].in_check_value = fields[i].in_check_value;
 952                 (*last_cmd)->cmd.scan->fields[i].in_check_mask = fields[i].in_check_mask;
 953                 (*last_cmd)->cmd.scan->fields[i].in_handler = fields[i].in_handler;
 954                 (*last_cmd)->cmd.scan->fields[i].in_handler_priv = fields[i].in_handler_priv;
 955         }
 956
 957         return ERROR_OK;
 958 }
 959
 960 void jtag_add_tlr(void)
 961 {
 962         jtag_prelude(TAP_TLR);
 963
 964         int retval;
 965         retval=interface_jtag_add_tlr();
 966         if (retval!=ERROR_OK)
 967                 jtag_error=retval;
 968 }
 969
 970 int MINIDRIVER(interface_jtag_add_tlr)()
 971 {
 972         enum tap_state state = TAP_TLR;
 973         jtag_command_t **last_cmd = jtag_get_last_command_p();
 974
 975         /* allocate memory for a new list member */
 976         *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
 977         last_comand_pointer = &((*last_cmd)->next);
 978         (*last_cmd)->next = NULL;
 979         (*last_cmd)->type = JTAG_STATEMOVE;
 980
 981         (*last_cmd)->cmd.statemove = cmd_queue_alloc(sizeof(statemove_command_t));
 982         (*last_cmd)->cmd.statemove->end_state = state;
 983
 984
 985         return ERROR_OK;
 986 }
 987
 988 void jtag_add_pathmove(int num_states, enum tap_state *path)
 989 {
 990         enum tap_state cur_state=cmd_queue_cur_state;
 991         int i;
 992         int retval;
 993
 994         /* the last state has to be a stable state */
 995         if (tap_move_map[path[num_states - 1]] == -1)
 996         {
 997                 LOG_ERROR("BUG: TAP path doesn't finish in a stable state");
 998                 exit(-1);
 999         }
1000
1001         for (i=0; i<num_states; i++)
1002         {
1003                 if (path[i] == TAP_TLR)
1004                 {
1005                         LOG_ERROR("BUG: TAP_TLR is not a valid state for pathmove sequences");
1006                         exit(-1);
1007                 }
1008                 if ((tap_transitions[cur_state].low != path[i])&&
1009                                 (tap_transitions[cur_state].high != path[i]))
1010                 {
1011                         LOG_ERROR("BUG: %s -> %s isn't a valid TAP transition", tap_state_strings[cur_state], tap_state_strings[path[i]]);
1012                         exit(-1);
1013                 }
1014                 cur_state = path[i];
1015         }
1016
1017         jtag_prelude1();
1018
1019
1020         retval=interface_jtag_add_pathmove(num_states, path);
1021         cmd_queue_cur_state = path[num_states - 1];
1022         if (retval!=ERROR_OK)
1023                 jtag_error=retval;
1024 }
1025
1026 int MINIDRIVER(interface_jtag_add_pathmove)(int num_states, enum tap_state *path)
1027 {
1028         jtag_command_t **last_cmd = jtag_get_last_command_p();
1029         int i;
1030
1031         /* allocate memory for a new list member */
1032         *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
1033         last_comand_pointer = &((*last_cmd)->next);
1034         (*last_cmd)->next = NULL;
1035         (*last_cmd)->type = JTAG_PATHMOVE;
1036
1037         (*last_cmd)->cmd.pathmove = cmd_queue_alloc(sizeof(pathmove_command_t));
1038         (*last_cmd)->cmd.pathmove->num_states = num_states;
1039         (*last_cmd)->cmd.pathmove->path = cmd_queue_alloc(sizeof(enum tap_state) * num_states);
1040
1041         for (i = 0; i < num_states; i++)
1042                 (*last_cmd)->cmd.pathmove->path[i] = path[i];
1043
1044         return ERROR_OK;
1045 }
1046
1047 int MINIDRIVER(interface_jtag_add_runtest)(int num_cycles, enum tap_state state)
1048 {
1049         jtag_command_t **last_cmd = jtag_get_last_command_p();
1050
1051         /* allocate memory for a new list member */
1052         *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
1053         (*last_cmd)->next = NULL;
1054         last_comand_pointer = &((*last_cmd)->next);
1055         (*last_cmd)->type = JTAG_RUNTEST;
1056
1057         (*last_cmd)->cmd.runtest = cmd_queue_alloc(sizeof(runtest_command_t));
1058         (*last_cmd)->cmd.runtest->num_cycles = num_cycles;
1059         (*last_cmd)->cmd.runtest->end_state = state;
1060
1061         return ERROR_OK;
1062 }
1063
1064 void jtag_add_runtest(int num_cycles, enum tap_state state)
1065 {
1066         int retval;
1067
1068         jtag_prelude(state);
1069
1070         /* executed by sw or hw fifo */
1071         retval=interface_jtag_add_runtest(num_cycles, cmd_queue_end_state);
1072         if (retval!=ERROR_OK)
1073                 jtag_error=retval;
1074 }
1075
1076 void jtag_add_reset(int req_tlr_or_trst, int req_srst)
1077 {
1078         int trst_with_tlr = 0;
1079         int retval;
1080
1081         /* FIX!!! there are *many* different cases here. A better
1082          * approach is needed for legal combinations of transitions...
1083          */
1084         if ((jtag_reset_config & RESET_HAS_SRST)&&
1085                         (jtag_reset_config & RESET_HAS_TRST)&&
1086                         ((jtag_reset_config & RESET_SRST_PULLS_TRST)==0))
1087         {
1088                 if (((req_tlr_or_trst&&!jtag_trst)||
1089                                 (!req_tlr_or_trst&&jtag_trst))&&
1090                                 ((req_srst&&!jtag_srst)||
1091                                                 (!req_srst&&jtag_srst)))
1092                 {
1093                         /* FIX!!! srst_pulls_trst allows 1,1 => 0,0 transition.... */
1094                         //LOG_ERROR("BUG: transition of req_tlr_or_trst and req_srst in the same jtag_add_reset() call is undefined");
1095                 }
1096         }
1097
1098         /* Make sure that jtag_reset_config allows the requested reset */
1099         /* if SRST pulls TRST, we can't fulfill srst == 1 with trst == 0 */
1100         if (((jtag_reset_config & RESET_SRST_PULLS_TRST) && (req_srst == 1)) && (!req_tlr_or_trst))
1101         {
1102                 LOG_ERROR("BUG: requested reset would assert trst");
1103                 jtag_error=ERROR_FAIL;
1104                 return;
1105         }
1106
1107         /* if TRST pulls SRST, we reset with TAP T-L-R */
1108         if (((jtag_reset_config & RESET_TRST_PULLS_SRST) && (req_tlr_or_trst)) && (req_srst == 0))
1109         {
1110                 trst_with_tlr = 1;
1111         }
1112
1113         if (req_srst && !(jtag_reset_config & RESET_HAS_SRST))
1114         {
1115                 LOG_ERROR("BUG: requested SRST assertion, but the current configuration doesn't support this");
1116                 jtag_error=ERROR_FAIL;
1117                 return;
1118         }
1119
1120         if (req_tlr_or_trst)
1121         {
1122                 if (!trst_with_tlr && (jtag_reset_config & RESET_HAS_TRST))
1123                 {
1124                         jtag_trst = 1;
1125                 } else
1126                 {
1127                         trst_with_tlr = 1;
1128                 }
1129         } else
1130         {
1131                 jtag_trst = 0;
1132         }
1133
1134         jtag_srst = req_srst;
1135
1136         retval = interface_jtag_add_reset(jtag_trst, jtag_srst);
1137         if (retval!=ERROR_OK)
1138         {
1139                 jtag_error=retval;
1140                 return;
1141         }
1142
1143         if (jtag_srst)
1144         {
1145                 LOG_DEBUG("SRST line asserted");
1146         }
1147         else
1148         {
1149                 LOG_DEBUG("SRST line released");
1150                 if (jtag_nsrst_delay)
1151                         jtag_add_sleep(jtag_nsrst_delay * 1000);
1152         }
1153
1154         if (trst_with_tlr)
1155         {
1156                 LOG_DEBUG("JTAG reset with TLR instead of TRST");
1157                 jtag_add_end_state(TAP_TLR);
1158                 jtag_add_tlr();
1159                 jtag_call_event_callbacks(JTAG_TRST_ASSERTED);
1160                 return;
1161         }
1162
1163         if (jtag_trst)
1164         {
1165                 /* we just asserted nTRST, so we're now in Test-Logic-Reset,
1166                  * and inform possible listeners about this
1167                  */
1168                 LOG_DEBUG("TRST line asserted");
1169                 cmd_queue_cur_state = TAP_TLR;
1170                 jtag_call_event_callbacks(JTAG_TRST_ASSERTED);
1171         }
1172         else
1173         {
1174                 if (jtag_ntrst_delay)
1175                         jtag_add_sleep(jtag_ntrst_delay * 1000);
1176         }
1177 }
1178
1179 int MINIDRIVER(interface_jtag_add_reset)(int req_trst, int req_srst)
1180 {
1181         jtag_command_t **last_cmd = jtag_get_last_command_p();
1182
1183         /* allocate memory for a new list member */
1184         *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
1185         (*last_cmd)->next = NULL;
1186         last_comand_pointer = &((*last_cmd)->next);
1187         (*last_cmd)->type = JTAG_RESET;
1188
1189         (*last_cmd)->cmd.reset = cmd_queue_alloc(sizeof(reset_command_t));
1190         (*last_cmd)->cmd.reset->trst = req_trst;
1191         (*last_cmd)->cmd.reset->srst = req_srst;
1192
1193         return ERROR_OK;
1194 }
1195
1196 void jtag_add_end_state(enum tap_state state)
1197 {
1198         cmd_queue_end_state = state;
1199         if ((cmd_queue_end_state == TAP_SD)||(cmd_queue_end_state == TAP_SI))
1200         {
1201                 LOG_ERROR("BUG: TAP_SD/SI can't be end state. Calling code should use a larger scan field");
1202         }
1203 }
1204
1205 int MINIDRIVER(interface_jtag_add_sleep)(u32 us)
1206 {
1207         jtag_command_t **last_cmd = jtag_get_last_command_p();
1208
1209         /* allocate memory for a new list member */
1210         *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
1211         (*last_cmd)->next = NULL;
1212         last_comand_pointer = &((*last_cmd)->next);
1213         (*last_cmd)->type = JTAG_SLEEP;
1214
1215         (*last_cmd)->cmd.sleep = cmd_queue_alloc(sizeof(sleep_command_t));
1216         (*last_cmd)->cmd.sleep->us = us;
1217
1218         return ERROR_OK;
1219 }
1220
1221 void jtag_add_sleep(u32 us)
1222 {
1223         keep_alive(); /* we might be running on a very slow JTAG clk */
1224         int retval=interface_jtag_add_sleep(us);
1225         if (retval!=ERROR_OK)
1226                 jtag_error=retval;
1227         return;
1228 }
1229
1230 int jtag_scan_size(scan_command_t *cmd)
1231 {
1232         int bit_count = 0;
1233         int i;
1234
1235         /* count bits in scan command */
1236         for (i = 0; i < cmd->num_fields; i++)
1237         {
1238                 bit_count += cmd->fields[i].num_bits;
1239         }
1240
1241         return bit_count;
1242 }
1243
1244 int jtag_build_buffer(scan_command_t *cmd, u8 **buffer)
1245 {
1246         int bit_count = 0;
1247         int i;
1248
1249         bit_count = jtag_scan_size(cmd);
1250         *buffer = malloc(CEIL(bit_count, 8));
1251
1252         bit_count = 0;
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 > 64) ? 64 : 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: 0x%s", i, char_buf);
1264                         free(char_buf);
1265 #endif
1266                 }
1267
1268                 bit_count += cmd->fields[i].num_bits;
1269         }
1270
1271         return bit_count;
1272 }
1273
1274 int jtag_read_buffer(u8 *buffer, scan_command_t *cmd)
1275 {
1276         int i;
1277         int bit_count = 0;
1278         int retval;
1279
1280         /* we return ERROR_OK, unless a check fails, or a handler reports a problem */
1281         retval = ERROR_OK;
1282
1283         for (i = 0; i < cmd->num_fields; i++)
1284         {
1285                 /* if neither in_value nor in_handler
1286                  * are specified we don't have to examine this field
1287                  */
1288                 if (cmd->fields[i].in_value || cmd->fields[i].in_handler)
1289                 {
1290                         int num_bits = cmd->fields[i].num_bits;
1291                         u8 *captured = buf_set_buf(buffer, bit_count, malloc(CEIL(num_bits, 8)), 0, num_bits);
1292
1293 #ifdef _DEBUG_JTAG_IO_
1294                         char *char_buf;
1295
1296                         char_buf = buf_to_str(captured, (num_bits > 64) ? 64 : num_bits, 16);
1297                         LOG_DEBUG("fields[%i].in_value: 0x%s", i, char_buf);
1298                         free(char_buf);
1299 #endif
1300
1301                         if (cmd->fields[i].in_value)
1302                         {
1303                                 buf_cpy(captured, cmd->fields[i].in_value, num_bits);
1304
1305                                 if (cmd->fields[i].in_handler)
1306                                 {
1307                                         if (cmd->fields[i].in_handler(cmd->fields[i].in_value, cmd->fields[i].in_handler_priv, cmd->fields+i) != ERROR_OK)
1308                                         {
1309                                                 LOG_WARNING("in_handler reported a failed check");
1310                                                 retval = ERROR_JTAG_QUEUE_FAILED;
1311                                         }
1312                                 }
1313                         }
1314
1315                         /* no in_value specified, but a handler takes care of the scanned data */
1316                         if (cmd->fields[i].in_handler && (!cmd->fields[i].in_value))
1317                         {
1318                                 if (cmd->fields[i].in_handler(captured, cmd->fields[i].in_handler_priv, cmd->fields+i) != ERROR_OK)
1319                                 {
1320                                         /* We're going to call the error:handler later, but if the in_handler
1321                                          * reported an error we report this failure upstream
1322                                          */
1323                                         LOG_WARNING("in_handler reported a failed check");
1324                                         retval = ERROR_JTAG_QUEUE_FAILED;
1325                                 }
1326                         }
1327
1328                         free(captured);
1329                 }
1330                 bit_count += cmd->fields[i].num_bits;
1331         }
1332
1333         return retval;
1334 }
1335
1336 int jtag_check_value(u8 *captured, void *priv, scan_field_t *field)
1337 {
1338         int retval = ERROR_OK;
1339         int num_bits = field->num_bits;
1340
1341         int compare_failed = 0;
1342
1343         if (field->in_check_mask)
1344                 compare_failed = buf_cmp_mask(captured, field->in_check_value, field->in_check_mask, num_bits);
1345         else
1346                 compare_failed = buf_cmp(captured, field->in_check_value, num_bits);
1347
1348         if (compare_failed){
1349                 /* An error handler could have caught the failing check
1350                  * only report a problem when there wasn't a handler, or if the handler
1351                  * acknowledged the error
1352                  */
1353                 LOG_WARNING("TAP %s:",
1354                                         (field->tap == NULL) ? "(unknown)" : field->tap->dotted_name );
1355                 if (compare_failed)
1356                 {
1357                         char *captured_char = buf_to_str(captured, (num_bits > 64) ? 64 : num_bits, 16);
1358                         char *in_check_value_char = buf_to_str(field->in_check_value, (num_bits > 64) ? 64 : num_bits, 16);
1359
1360                         if (field->in_check_mask)
1361                         {
1362                                 char *in_check_mask_char;
1363                                 in_check_mask_char = buf_to_str(field->in_check_mask, (num_bits > 64) ? 64 : num_bits, 16);
1364                                 LOG_WARNING("value captured during scan didn't pass the requested check:");
1365                                 LOG_WARNING("captured: 0x%s check_value: 0x%s check_mask: 0x%s",
1366                                                         captured_char, in_check_value_char, in_check_mask_char);
1367                                 free(in_check_mask_char);
1368                         }
1369                         else
1370                         {
1371                                 LOG_WARNING("value captured during scan didn't pass the requested check: captured: 0x%s check_value: 0x%s", captured_char, in_check_value_char);
1372                         }
1373
1374                         free(captured_char);
1375                         free(in_check_value_char);
1376
1377                         retval = ERROR_JTAG_QUEUE_FAILED;
1378                 }
1379
1380         }
1381         return retval;
1382 }
1383
1384 /*
1385   set up checking of this field using the in_handler. The values passed in must be valid until
1386   after jtag_execute() has completed.
1387  */
1388 void jtag_set_check_value(scan_field_t *field, u8 *value, u8 *mask, error_handler_t *in_error_handler)
1389 {
1390         if (value)
1391                 field->in_handler = jtag_check_value;
1392         else
1393                 field->in_handler = NULL;       /* No check, e.g. embeddedice uses value==NULL to indicate no check */
1394         field->in_handler_priv = NULL;
1395         field->in_check_value = value;
1396         field->in_check_mask = mask;
1397 }
1398
1399 enum scan_type jtag_scan_type(scan_command_t *cmd)
1400 {
1401         int i;
1402         int type = 0;
1403
1404         for (i = 0; i < cmd->num_fields; i++)
1405         {
1406                 if (cmd->fields[i].in_value || cmd->fields[i].in_handler)
1407                         type |= SCAN_IN;
1408                 if (cmd->fields[i].out_value)
1409                         type |= SCAN_OUT;
1410         }
1411
1412         return type;
1413 }
1414
1415 int MINIDRIVER(interface_jtag_execute_queue)(void)
1416 {
1417         int retval;
1418
1419         if (jtag==NULL)
1420         {
1421                 LOG_ERROR("No JTAG interface configured yet. Issue 'init' command in startup scripts before communicating with targets.");
1422                 return ERROR_FAIL;
1423         }
1424
1425         retval = jtag->execute_queue();
1426
1427         cmd_queue_free();
1428
1429         jtag_command_queue = NULL;
1430         last_comand_pointer = &jtag_command_queue;
1431
1432         return retval;
1433 }
1434
1435 int jtag_execute_queue(void)
1436 {
1437         int retval=interface_jtag_execute_queue();
1438         if (retval==ERROR_OK)
1439         {
1440                 retval=jtag_error;
1441         }
1442         jtag_error=ERROR_OK;
1443         return retval;
1444 }
1445
1446 int jtag_reset_callback(enum jtag_event event, void *priv)
1447 {
1448         jtag_tap_t *tap = priv;
1449
1450         LOG_DEBUG("-");
1451
1452         if (event == JTAG_TRST_ASSERTED)
1453         {
1454                 buf_set_ones(tap->cur_instr, tap->ir_length);
1455                 tap->bypass = 1;
1456         }
1457
1458         return ERROR_OK;
1459 }
1460
1461 void jtag_sleep(u32 us)
1462 {
1463         alive_sleep(us/1000);
1464 }
1465
1466 /* Try to examine chain layout according to IEEE 1149.1 §12
1467  */
1468 int jtag_examine_chain(void)
1469 {
1470         jtag_tap_t *tap;
1471         scan_field_t field;
1472         u8 idcode_buffer[JTAG_MAX_CHAIN_SIZE * 4];
1473         int i;
1474         int bit_count;
1475         int device_count = 0;
1476         u8 zero_check = 0x0;
1477         u8 one_check = 0xff;
1478
1479         field.tap = NULL;
1480         field.num_bits = sizeof(idcode_buffer) * 8;
1481         field.out_value = idcode_buffer;
1482         field.out_mask = NULL;
1483         field.in_value = idcode_buffer;
1484         field.in_check_value = NULL;
1485         field.in_check_mask = NULL;
1486         field.in_handler = NULL;
1487         field.in_handler_priv = NULL;
1488
1489         for (i = 0; i < JTAG_MAX_CHAIN_SIZE; i++)
1490         {
1491                 buf_set_u32(idcode_buffer, i * 32, 32, 0x000000FF);
1492         }
1493
1494         jtag_add_plain_dr_scan(1, &field, TAP_TLR);
1495         jtag_execute_queue();
1496
1497         for (i = 0; i < JTAG_MAX_CHAIN_SIZE * 4; i++)
1498         {
1499                 zero_check |= idcode_buffer[i];
1500                 one_check &= idcode_buffer[i];
1501         }
1502
1503         /* if there wasn't a single non-zero bit or if all bits were one, the scan isn't valid */
1504         if ((zero_check == 0x00) || (one_check == 0xff))
1505         {
1506                 LOG_ERROR("JTAG communication failure, check connection, JTAG interface, target power etc.");
1507                 return ERROR_JTAG_INIT_FAILED;
1508         }
1509
1510         // point at the 1st tap
1511         tap = jtag_NextEnabledTap(NULL);
1512         if( tap == NULL ){
1513                 LOG_ERROR("JTAG: No taps enabled?");
1514                 return ERROR_JTAG_INIT_FAILED;
1515         }
1516
1517         for (bit_count = 0; bit_count < (JTAG_MAX_CHAIN_SIZE * 32) - 31;)
1518         {
1519                 u32 idcode = buf_get_u32(idcode_buffer, bit_count, 32);
1520                 if ((idcode & 1) == 0)
1521                 {
1522                         /* LSB must not be 0, this indicates a device in bypass */
1523                         LOG_WARNING("Tap/Device does not have IDCODE");
1524                         idcode=0;
1525
1526                         bit_count += 1;
1527                 }
1528                 else
1529                 {
1530                         u32 manufacturer;
1531                         u32 part;
1532                         u32 version;
1533
1534                         if (idcode == 0x000000FF)
1535                         {
1536                                 int unexpected=0;
1537                                 /* End of chain (invalid manufacturer ID)
1538                                  *
1539                                  * The JTAG examine is the very first thing that happens
1540                                  *
1541                                  * A single JTAG device requires only 64 bits to be read back correctly.
1542                                  *
1543                                  * The code below adds a check that the rest of the data scanned (640 bits)
1544                                  * are all as expected. This helps diagnose/catch problems with the JTAG chain
1545                                  *
1546                                  * earlier and gives more helpful/explicit error messages.
1547                                  */
1548                                 for (bit_count += 32; bit_count < (JTAG_MAX_CHAIN_SIZE * 32) - 31;bit_count += 32)
1549                                 {
1550                                         idcode = buf_get_u32(idcode_buffer, bit_count, 32);
1551                                         if (unexpected||(idcode != 0x000000FF))
1552                                         {
1553                                                 LOG_WARNING("Unexpected idcode after end of chain! %d 0x%08x", bit_count, idcode);
1554                                                 unexpected = 1;
1555                                         }
1556                                 }
1557
1558                                 break;
1559                         }
1560
1561 #define EXTRACT_MFG(X)  (((X) & 0xffe) >> 1)
1562                         manufacturer = EXTRACT_MFG(idcode);
1563 #define EXTRACT_PART(X) (((X) & 0xffff000) >> 12)
1564                         part = EXTRACT_PART(idcode);
1565 #define EXTRACT_VER(X)  (((X) & 0xf0000000) >> 28)
1566                         version = EXTRACT_VER(idcode);
1567
1568                         LOG_INFO("JTAG tap: %s tap/device found: 0x%8.8x (Manufacturer: 0x%3.3x, Part: 0x%4.4x, Version: 0x%1.1x)",
1569                                          ((tap != NULL) ? (tap->dotted_name) : "(not-named)"),
1570                                 idcode, manufacturer, part, version);
1571
1572                         bit_count += 32;
1573                 }
1574                 if (tap)
1575                 {
1576                         tap->idcode = idcode;
1577                         if( tap->expected_id ){
1578                                 if( tap->idcode != tap->expected_id ){
1579                                         LOG_ERROR("ERROR: Tap: %s - Expected id: 0x%08x, Got: 0x%08x",
1580                                                           tap->dotted_name,
1581                                                           tap->expected_id,
1582                                                           idcode );
1583                                         LOG_ERROR("ERROR: expected: mfg: 0x%3.3x, part: 0x%4.4x, ver: 0x%1.1x",
1584                                                           EXTRACT_MFG( tap->expected_id ),
1585                                                           EXTRACT_PART( tap->expected_id ),
1586                                                           EXTRACT_VER( tap->expected_id ) );
1587                                         LOG_ERROR("ERROR:      got: mfg: 0x%3.3x, part: 0x%4.4x, ver: 0x%1.1x",
1588                                                           EXTRACT_MFG( tap->idcode ),
1589                                                           EXTRACT_PART( tap->idcode ),
1590                                                           EXTRACT_VER( tap->idcode ) );
1591                                 } else {
1592                                         LOG_INFO("JTAG Tap/device matched");
1593                                 }
1594                         } else {
1595 #if 0
1596                                 LOG_INFO("JTAG TAP ID: 0x%08x - Unknown - please report (A) chipname and (B) idcode to the openocd project",
1597                                                  tap->idcode);
1598 #endif
1599                         }
1600                         tap = jtag_NextEnabledTap(tap);
1601                 }
1602                 device_count++;
1603         }
1604
1605         /* see if number of discovered devices matches configuration */
1606         if (device_count != jtag_NumEnabledTaps())
1607         {
1608                 LOG_ERROR("number of discovered devices in JTAG chain (%i) doesn't match (enabled) configuration (%i), total taps: %d",
1609                                   device_count, jtag_NumEnabledTaps(), jtag_NumTotalTaps());
1610                 LOG_ERROR("check the config file and ensure proper JTAG communication (connections, speed, ...)");
1611                 return ERROR_JTAG_INIT_FAILED;
1612         }
1613
1614         return ERROR_OK;
1615 }
1616
1617 int jtag_validate_chain(void)
1618 {
1619         jtag_tap_t *tap;
1620         int total_ir_length = 0;
1621         u8 *ir_test = NULL;
1622         scan_field_t field;
1623         int chain_pos = 0;
1624
1625         tap = NULL;
1626         total_ir_length = 0;
1627         for(;;){
1628                 tap = jtag_NextEnabledTap(tap);
1629                 if( tap == NULL ){
1630                         break;
1631                 }
1632                 total_ir_length += tap->ir_length;
1633         }
1634
1635         total_ir_length += 2;
1636         ir_test = malloc(CEIL(total_ir_length, 8));
1637         buf_set_ones(ir_test, total_ir_length);
1638
1639         field.tap = NULL;
1640         field.num_bits = total_ir_length;
1641         field.out_value = ir_test;
1642         field.out_mask = NULL;
1643         field.in_value = ir_test;
1644         field.in_check_value = NULL;
1645         field.in_check_mask = NULL;
1646         field.in_handler = NULL;
1647         field.in_handler_priv = NULL;
1648
1649         jtag_add_plain_ir_scan(1, &field, TAP_TLR);
1650         jtag_execute_queue();
1651
1652         tap = NULL;
1653         chain_pos = 0;
1654         for(;;){
1655                 tap = jtag_NextEnabledTap(tap);
1656                 if( tap == NULL ){
1657                         break;
1658                 }
1659
1660
1661                 if (buf_get_u32(ir_test, chain_pos, 2) != 0x1)
1662                 {
1663                         char *cbuf = buf_to_str(ir_test, total_ir_length, 16);
1664                         LOG_ERROR("Error validating JTAG scan chain, IR mismatch, scan returned 0x%s", cbuf);
1665                         free(cbuf);
1666                         free(ir_test);
1667                         return ERROR_JTAG_INIT_FAILED;
1668                 }
1669                 chain_pos += tap->ir_length;
1670         }
1671
1672         if (buf_get_u32(ir_test, chain_pos, 2) != 0x3)
1673         {
1674                 char *cbuf = buf_to_str(ir_test, total_ir_length, 16);
1675                 LOG_ERROR("Error validating JTAG scan chain, IR mismatch, scan returned 0x%s", cbuf);
1676                 free(cbuf);
1677                 free(ir_test);
1678                 return ERROR_JTAG_INIT_FAILED;
1679         }
1680
1681         free(ir_test);
1682
1683         return ERROR_OK;
1684 }
1685
1686
1687 static int
1688 jim_newtap_cmd( Jim_GetOptInfo *goi )
1689 {
1690         jtag_tap_t *pTap;
1691         jtag_tap_t **ppTap;
1692         jim_wide w;
1693         int x;
1694         int e;
1695         int reqbits;
1696         Jim_Nvp *n;
1697         char *cp;
1698         const Jim_Nvp opts[] = {
1699 #define NTAP_OPT_IRLEN     0
1700                 { .name = "-irlen"                      ,       .value = NTAP_OPT_IRLEN },
1701 #define NTAP_OPT_IRMASK    1
1702                 { .name = "-irmask"                     ,       .value = NTAP_OPT_IRMASK },
1703 #define NTAP_OPT_IRCAPTURE 2
1704                 { .name = "-ircapture"          ,       .value = NTAP_OPT_IRCAPTURE },
1705 #define NTAP_OPT_ENABLED   3
1706                 { .name = "-enable"                     ,   .value = NTAP_OPT_ENABLED },
1707 #define NTAP_OPT_DISABLED  4
1708                 { .name = "-disable"            ,   .value = NTAP_OPT_DISABLED },
1709 #define NTAP_OPT_EXPECTED_ID 5
1710                 { .name = "-expected-id"        ,   .value = NTAP_OPT_EXPECTED_ID },
1711                 { .name = NULL                          ,   .value = -1 },
1712         };
1713
1714
1715         pTap = malloc( sizeof(jtag_tap_t) );
1716         memset( pTap, 0, sizeof(*pTap) );
1717         if( !pTap ){
1718                 Jim_SetResult_sprintf( goi->interp, "no memory");
1719                 return JIM_ERR;
1720         }
1721         //
1722         // we expect CHIP + TAP + OPTIONS
1723         //
1724         if( goi->argc < 3 ){
1725                 Jim_SetResult_sprintf(goi->interp, "Missing CHIP TAP OPTIONS ....");
1726                 return JIM_ERR;
1727         }
1728         Jim_GetOpt_String( goi, &cp, NULL );
1729         pTap->chip = strdup(cp);
1730
1731         Jim_GetOpt_String( goi, &cp, NULL );
1732         pTap->tapname = strdup(cp);
1733
1734         // name + dot + name + null
1735         x = strlen(pTap->chip) + 1 + strlen(pTap->tapname) + 1;
1736         cp = malloc( x );
1737         sprintf( cp, "%s.%s", pTap->chip, pTap->tapname );
1738         pTap->dotted_name = cp;
1739
1740         LOG_DEBUG("Creating New Tap, Chip: %s, Tap: %s, Dotted: %s, %d params",
1741                           pTap->chip, pTap->tapname, pTap->dotted_name, goi->argc);
1742
1743
1744         // default is enabled
1745         pTap->enabled = 1;
1746
1747         // deal with options
1748 #define NTREQ_IRLEN      1
1749 #define NTREQ_IRCAPTURE  2
1750 #define NTREQ_IRMASK     4
1751
1752         // clear them as we find them
1753         reqbits = (NTREQ_IRLEN | NTREQ_IRCAPTURE | NTREQ_IRMASK);
1754
1755         while( goi->argc ){
1756                 e = Jim_GetOpt_Nvp( goi, opts, &n );
1757                 if( e != JIM_OK ){
1758                         Jim_GetOpt_NvpUnknown( goi, opts, 0 );
1759                         return e;
1760                 }
1761                 LOG_DEBUG("Processing option: %s", n->name );
1762                 switch( n->value ){
1763                 case NTAP_OPT_ENABLED:
1764                         pTap->enabled = 1;
1765                         break;
1766                 case NTAP_OPT_DISABLED:
1767                         pTap->enabled = 0;
1768                         break;
1769                 case NTAP_OPT_EXPECTED_ID:
1770                         e = Jim_GetOpt_Wide( goi, &w );
1771                         pTap->expected_id = w;
1772                         break;
1773                 case NTAP_OPT_IRLEN:
1774                 case NTAP_OPT_IRMASK:
1775                 case NTAP_OPT_IRCAPTURE:
1776                         e = Jim_GetOpt_Wide( goi, &w );
1777                         if( e != JIM_OK ){
1778                                 Jim_SetResult_sprintf( goi->interp, "option: %s bad parameter", n->name );
1779                                 return e;
1780                         }
1781                         if( (w < 0) || (w > 0xffff) ){
1782                                 // wacky value
1783                                 Jim_SetResult_sprintf( goi->interp, "option: %s - wacky value: %d (0x%x)",
1784                                                                            n->name, (int)(w), (int)(w));
1785                                 return JIM_ERR;
1786                         }
1787                         switch(n->value){
1788                         case NTAP_OPT_IRLEN:
1789                                 pTap->ir_length = w;
1790                                 reqbits &= (~(NTREQ_IRLEN));
1791                                 break;
1792                         case NTAP_OPT_IRMASK:
1793                                 pTap->ir_capture_mask = w;
1794                                 reqbits &= (~(NTREQ_IRMASK));
1795                                 break;
1796                         case NTAP_OPT_IRCAPTURE:
1797                                 pTap->ir_capture_value = w;
1798                                 reqbits &= (~(NTREQ_IRCAPTURE));
1799                                 break;
1800                         }
1801                 } // switch(n->value)
1802         } // while( goi->argc )
1803
1804         // Did we get all the options?
1805         if( reqbits ){
1806                 // no
1807                 Jim_SetResult_sprintf( goi->interp,
1808                                                            "newtap: %s missing required parameters",
1809                                                            pTap->dotted_name);
1810                 // fixme: Tell user what is missing :-(
1811                 // no memory leaks pelase
1812                 free(((void *)(pTap->chip)));
1813                 free(((void *)(pTap->tapname)));
1814                 free(((void *)(pTap->dotted_name)));
1815                 free(((void *)(pTap)));
1816                 return JIM_ERR;
1817         }
1818
1819         pTap->expected      = malloc( pTap->ir_length );
1820         pTap->expected_mask = malloc( pTap->ir_length );
1821         pTap->cur_instr     = malloc( pTap->ir_length );
1822
1823         buf_set_u32( pTap->expected,
1824                                  0,
1825                                  pTap->ir_length,
1826                                  pTap->ir_capture_value );
1827         buf_set_u32( pTap->expected_mask,
1828                                  0,
1829                                  pTap->ir_length,
1830                                  pTap->ir_capture_mask );
1831         buf_set_ones( pTap->cur_instr,
1832                                   pTap->ir_length );
1833
1834         pTap->bypass = 1;
1835
1836
1837         jtag_register_event_callback(jtag_reset_callback, pTap );
1838
1839         ppTap = &(jtag_all_taps);
1840         while( (*ppTap) != NULL ){
1841                 ppTap = &((*ppTap)->next_tap);
1842         }
1843         *ppTap = pTap;
1844         {
1845                 static int n_taps = 0;
1846                 pTap->abs_chain_position = n_taps++;
1847         }
1848         LOG_DEBUG( "Created Tap: %s @ abs position %d, irlen %d, capture: 0x%x mask: 0x%x",
1849                            (*ppTap)->dotted_name,
1850                            (*ppTap)->abs_chain_position,
1851                            (*ppTap)->ir_length,
1852                            (*ppTap)->ir_capture_value,
1853                            (*ppTap)->ir_capture_mask );
1854
1855
1856         return ERROR_OK;
1857 }
1858
1859
1860 static int
1861 jim_jtag_command( Jim_Interp *interp, int argc, Jim_Obj *const *argv )
1862 {
1863         Jim_GetOptInfo goi;
1864         int e;
1865         Jim_Nvp *n;
1866         struct command_context_s *context;
1867
1868         enum {
1869                 JTAG_CMD_INTERFACE,
1870                 JTAG_CMD_INIT_RESET,
1871                 JTAG_CMD_NEWTAP,
1872                 JTAG_CMD_TAPENABLE,
1873                 JTAG_CMD_TAPDISABLE,
1874                 JTAG_CMD_TAPISENABLED
1875         };
1876
1877         const Jim_Nvp jtag_cmds[] = {
1878                 { .name = "interface"     , .value = JTAG_CMD_INTERFACE },
1879                 { .name = "arp_init-reset", .value = JTAG_CMD_INIT_RESET },
1880                 { .name = "newtap"        , .value = JTAG_CMD_NEWTAP },
1881                 { .name = "tapisenabled"     , .value = JTAG_CMD_TAPISENABLED },
1882                 { .name = "tapenable"     , .value = JTAG_CMD_TAPENABLE },
1883                 { .name = "tapdisable"    , .value = JTAG_CMD_TAPDISABLE },
1884
1885                 { .name = NULL, .value = -1 },
1886         };
1887
1888         context = Jim_GetAssocData(interp, "context");
1889         // go past the command
1890         Jim_GetOpt_Setup( &goi, interp, argc-1, argv+1 );
1891
1892         e = Jim_GetOpt_Nvp( &goi, jtag_cmds, &n );
1893         if( e != JIM_OK ){
1894                 Jim_GetOpt_NvpUnknown( &goi, jtag_cmds, 0 );
1895                 return e;
1896         }
1897                 Jim_SetEmptyResult( goi.interp );
1898         switch( n->value ){
1899         case JTAG_CMD_INTERFACE:
1900                 // return the name of the interface
1901                 // TCL code might need to know the exact type...
1902                 // FUTURE: we allow this as a means to "set" the interface.
1903                 if( goi.argc != 0 ){
1904                         Jim_WrongNumArgs( goi.interp, 1, goi.argv-1, "(no params)");
1905                         return JIM_ERR;
1906                 }
1907                 Jim_SetResultString( goi.interp, jtag_interface->name, -1 );
1908                 return JIM_OK;
1909         case JTAG_CMD_INIT_RESET:
1910                 if( goi.argc != 0 ){
1911                         Jim_WrongNumArgs( goi.interp, 1, goi.argv-1, "(no params)");
1912                         return JIM_ERR;
1913                 }
1914                 e = jtag_init_reset(context);
1915                 if( e != ERROR_OK ){
1916                         Jim_SetResult_sprintf( goi.interp, "error: %d", e);
1917                         return JIM_ERR;
1918                 }
1919                 return JIM_OK;
1920         case JTAG_CMD_NEWTAP:
1921                 return jim_newtap_cmd( &goi );
1922                 break;
1923         case JTAG_CMD_TAPISENABLED:
1924         case JTAG_CMD_TAPENABLE:
1925         case JTAG_CMD_TAPDISABLE:
1926                 if( goi.argc != 1 ){
1927                         Jim_SetResultString( goi.interp, "Too many parameters",-1 );
1928                         return JIM_ERR;
1929                 }
1930
1931                 {
1932                         jtag_tap_t *t;
1933                         t = jtag_TapByJimObj( goi.interp, goi.argv[0] );
1934                         if( t == NULL ){
1935                                 return JIM_ERR;
1936                         }
1937                         switch( n->value ){
1938                         case JTAG_CMD_TAPISENABLED:
1939                                 // below
1940                                 break;
1941                         case JTAG_CMD_TAPENABLE:
1942                                 e = 1;
1943                                 t->enabled = e;
1944                                 break;
1945                         case JTAG_CMD_TAPDISABLE:
1946                                 e = 0;
1947                                 t->enabled = e;
1948                                 break;
1949                         }
1950                         Jim_SetResult( goi.interp, Jim_NewIntObj( goi.interp, e ) );
1951                         return JIM_OK;
1952                 }
1953         }
1954
1955
1956         return JIM_ERR;
1957 }
1958
1959 int jtag_register_commands(struct command_context_s *cmd_ctx)
1960 {
1961         register_jim( cmd_ctx, "jtag", jim_jtag_command, "perform jtag tap actions");
1962
1963         register_command(cmd_ctx, NULL, "interface", handle_interface_command,
1964                 COMMAND_CONFIG, "try to configure interface");
1965         register_command(cmd_ctx, NULL, "jtag_speed", handle_jtag_speed_command,
1966                 COMMAND_ANY, "set jtag speed (if supported)");
1967         register_command(cmd_ctx, NULL, "jtag_khz", handle_jtag_khz_command,
1968                 COMMAND_ANY, "same as jtag_speed, except it takes maximum khz as arguments. 0 KHz = RTCK.");
1969         register_command(cmd_ctx, NULL, "jtag_device", handle_jtag_device_command,
1970                 COMMAND_CONFIG, "jtag_device <ir_length> <ir_expected> <ir_mask>");
1971         register_command(cmd_ctx, NULL, "reset_config", handle_reset_config_command,
1972                 COMMAND_CONFIG, NULL);
1973         register_command(cmd_ctx, NULL, "jtag_nsrst_delay", handle_jtag_nsrst_delay_command,
1974                 COMMAND_ANY, "jtag_nsrst_delay <ms> - delay after deasserting srst in ms");
1975         register_command(cmd_ctx, NULL, "jtag_ntrst_delay", handle_jtag_ntrst_delay_command,
1976                 COMMAND_ANY, "jtag_ntrst_delay <ms> - delay after deasserting trst in ms");
1977
1978         register_command(cmd_ctx, NULL, "scan_chain", handle_scan_chain_command,
1979                 COMMAND_EXEC, "print current scan chain configuration");
1980
1981         register_command(cmd_ctx, NULL, "endstate", handle_endstate_command,
1982                 COMMAND_EXEC, "finish JTAG operations in <tap_state>");
1983         register_command(cmd_ctx, NULL, "jtag_reset", handle_jtag_reset_command,
1984                 COMMAND_EXEC, "toggle reset lines <trst> <srst>");
1985         register_command(cmd_ctx, NULL, "runtest", handle_runtest_command,
1986                 COMMAND_EXEC, "move to Run-Test/Idle, and execute <num_cycles>");
1987         register_command(cmd_ctx, NULL, "irscan", handle_irscan_command,
1988                 COMMAND_EXEC, "execute IR scan <device> <instr> [dev2] [instr2] ...");
1989         register_jim(cmd_ctx, "drscan", Jim_Command_drscan, "execute DR scan <device> <num_bits> <value> <num_bits1> <value2> ...");
1990
1991         register_command(cmd_ctx, NULL, "verify_ircapture", handle_verify_ircapture_command,
1992                 COMMAND_ANY, "verify value captured during Capture-IR <enable|disable>");
1993         return ERROR_OK;
1994 }
1995
1996 int jtag_interface_init(struct command_context_s *cmd_ctx)
1997 {
1998         if (jtag)
1999                 return ERROR_OK;
2000
2001         if (!jtag_interface)
2002         {
2003                 /* nothing was previously specified by "interface" command */
2004                 LOG_ERROR("JTAG interface has to be specified, see \"interface\" command");
2005                 return ERROR_JTAG_INVALID_INTERFACE;
2006         }
2007         if(hasKHz)
2008         {
2009                 jtag_interface->khz(speed_khz, &jtag_speed);
2010                 hasKHz = 0;
2011         }
2012
2013         if (jtag_interface->init() != ERROR_OK)
2014                 return ERROR_JTAG_INIT_FAILED;
2015
2016
2017
2018         jtag = jtag_interface;
2019         return ERROR_OK;
2020 }
2021
2022 static int jtag_init_inner(struct command_context_s *cmd_ctx)
2023 {
2024         jtag_tap_t *tap;
2025         int retval;
2026
2027         LOG_DEBUG("Init JTAG chain");
2028
2029
2030         tap = jtag_NextEnabledTap(NULL);
2031         if( tap == NULL ){
2032                 LOG_ERROR("There are no enabled taps?");
2033                 return ERROR_JTAG_INIT_FAILED;
2034         }
2035
2036         jtag_add_tlr();
2037         if ((retval=jtag_execute_queue())!=ERROR_OK)
2038                 return retval;
2039
2040         /* examine chain first, as this could discover the real chain layout */
2041         if (jtag_examine_chain() != ERROR_OK)
2042         {
2043                 LOG_ERROR("trying to validate configured JTAG chain anyway...");
2044         }
2045
2046         if (jtag_validate_chain() != ERROR_OK)
2047         {
2048                 LOG_ERROR("Could not validate JTAG chain, continuing anyway...");
2049         }
2050
2051         return ERROR_OK;
2052 }
2053
2054 int jtag_init_reset(struct command_context_s *cmd_ctx)
2055 {
2056         int retval;
2057
2058         if ((retval=jtag_interface_init(cmd_ctx)) != ERROR_OK)
2059                 return retval;
2060
2061         LOG_DEBUG("Trying to bring the JTAG controller to life by asserting TRST / TLR");
2062
2063         /* Reset can happen after a power cycle.
2064          *
2065          * Ideally we would only assert TRST or run TLR before the target reset.
2066          *
2067          * However w/srst_pulls_trst, trst is asserted together with the target
2068          * reset whether we want it or not.
2069          *
2070          * NB! Some targets have JTAG circuitry disabled until a
2071          * trst & srst has been asserted.
2072          *
2073          * NB! here we assume nsrst/ntrst delay are sufficient!
2074          *
2075          * NB! order matters!!!! srst *can* disconnect JTAG circuitry
2076          *
2077          */
2078         jtag_add_reset(1, 0); /* TLR or TRST */
2079         if (jtag_reset_config & RESET_HAS_SRST)
2080         {
2081                 jtag_add_reset(1, 1);
2082                 if ((jtag_reset_config & RESET_SRST_PULLS_TRST)==0)
2083                         jtag_add_reset(0, 1);
2084         }
2085         jtag_add_reset(0, 0);
2086         if ((retval = jtag_execute_queue()) != ERROR_OK)
2087                 return retval;
2088
2089         /* Check that we can communication on the JTAG chain + eventually we want to
2090          * be able to perform enumeration only after OpenOCD has started
2091          * telnet and GDB server
2092          *
2093          * That would allow users to more easily perform any magic they need to before
2094          * reset happens.
2095          */
2096         return jtag_init_inner(cmd_ctx);
2097 }
2098
2099 int jtag_init(struct command_context_s *cmd_ctx)
2100 {
2101         int retval;
2102         if ((retval=jtag_interface_init(cmd_ctx)) != ERROR_OK)
2103                 return retval;
2104         if (jtag_init_inner(cmd_ctx)==ERROR_OK)
2105         {
2106                 return ERROR_OK;
2107         }
2108         return jtag_init_reset(cmd_ctx);
2109 }
2110
2111 static int default_khz(int khz, int *jtag_speed)
2112 {
2113         LOG_ERROR("Translation from khz to jtag_speed not implemented");
2114         return ERROR_FAIL;
2115 }
2116
2117 static int default_speed_div(int speed, int *khz)
2118 {
2119         LOG_ERROR("Translation from jtag_speed to khz not implemented");
2120         return ERROR_FAIL;
2121 }
2122
2123 static int default_power_dropout(int *dropout)
2124 {
2125         *dropout=0; /* by default we can't detect power dropout */
2126         return ERROR_OK;
2127 }
2128
2129 static int default_srst_asserted(int *srst_asserted)
2130 {
2131         *srst_asserted=0; /* by default we can't detect srst asserted */
2132         return ERROR_OK;
2133 }
2134
2135 int handle_interface_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2136 {
2137         int i;
2138         int retval;
2139
2140         /* check whether the interface is already configured */
2141         if (jtag_interface)
2142         {
2143                 LOG_WARNING("Interface already configured, ignoring");
2144                 return ERROR_OK;
2145         }
2146
2147         /* interface name is a mandatory argument */
2148         if (argc < 1 || args[0][0] == '\0')
2149         {
2150                 return ERROR_COMMAND_SYNTAX_ERROR;
2151         }
2152
2153         for (i=0; jtag_interfaces[i]; i++)
2154         {
2155                 if (strcmp(args[0], jtag_interfaces[i]->name) == 0)
2156                 {
2157                         if ((retval = jtag_interfaces[i]->register_commands(cmd_ctx)) != ERROR_OK)
2158                         {
2159                                 return retval;
2160                         }
2161
2162                         jtag_interface = jtag_interfaces[i];
2163
2164                         if (jtag_interface->khz == NULL)
2165                         {
2166                                 jtag_interface->khz = default_khz;
2167                         }
2168                         if (jtag_interface->speed_div == NULL)
2169                         {
2170                                 jtag_interface->speed_div = default_speed_div;
2171                         }
2172                         if (jtag_interface->power_dropout == NULL)
2173                         {
2174                                 jtag_interface->power_dropout = default_power_dropout;
2175                         }
2176                         if (jtag_interface->srst_asserted == NULL)
2177                         {
2178                                 jtag_interface->srst_asserted = default_srst_asserted;
2179                         }
2180
2181                         return ERROR_OK;
2182                 }
2183         }
2184
2185         /* no valid interface was found (i.e. the configuration option,
2186          * didn't match one of the compiled-in interfaces
2187          */
2188         LOG_ERROR("No valid jtag interface found (%s)", args[0]);
2189         LOG_ERROR("compiled-in jtag interfaces:");
2190         for (i = 0; jtag_interfaces[i]; i++)
2191         {
2192                 LOG_ERROR("%i: %s", i, jtag_interfaces[i]->name);
2193         }
2194
2195         return ERROR_JTAG_INVALID_INTERFACE;
2196 }
2197
2198 int handle_jtag_device_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2199 {
2200         int e;
2201         char buf[1024];
2202         Jim_Obj *newargs[ 10 ];
2203         //
2204         // CONVERT SYNTAX
2205         //
2206         //   argv[-1] = command
2207         //   argv[ 0] = ir length
2208         //   argv[ 1] = ir capture
2209         //   argv[ 2] = ir mask
2210         //   argv[ 3] = not actually used by anything but in the docs
2211
2212         if( argc < 4 ){
2213                 command_print( cmd_ctx, "OLD DEPRECATED SYNTAX: Please use the NEW syntax");
2214                 return ERROR_OK;
2215         }
2216         command_print( cmd_ctx, "OLD SYNTAX: DEPRECATED - translating to new syntax");
2217         command_print( cmd_ctx, "jtag newtap CHIP TAP -irlen %s -ircapture %s -irvalue %s",
2218                                    args[0],
2219                                    args[1],
2220                                    args[2] );
2221         command_print( cmd_ctx, "Example: STM32 has 2 taps, the cortexM3(len4) + boundryscan(len5)");
2222         command_print( cmd_ctx, "jtag newtap stm32 cortexm3  ....., thus creating the tap: \"stm32.cortexm3\"");
2223         command_print( cmd_ctx, "jtag newtap stm32 boundry  ....., and the tap: \"stm32.boundery\"");
2224         command_print( cmd_ctx, "And then refer to the taps by the dotted name.");
2225
2226
2227
2228         newargs[0] = Jim_NewStringObj( interp, "jtag", -1   );
2229         newargs[1] = Jim_NewStringObj( interp, "newtap", -1 );
2230         sprintf( buf, "chip%d", jtag_NumTotalTaps() );
2231         newargs[2] = Jim_NewStringObj( interp, buf, -1 );
2232         sprintf( buf, "tap%d", jtag_NumTotalTaps() );
2233         newargs[3] = Jim_NewStringObj( interp, buf, -1  );
2234         newargs[4] = Jim_NewStringObj( interp, "-irlen", -1  );
2235         newargs[5] = Jim_NewStringObj( interp, args[0], -1  );
2236         newargs[6] = Jim_NewStringObj( interp, "-ircapture", -1  );
2237         newargs[7] = Jim_NewStringObj( interp, args[1], -1  );
2238         newargs[8] = Jim_NewStringObj( interp, "-irmask", -1  );
2239         newargs[9] = Jim_NewStringObj( interp, args[2], -1  );
2240
2241         command_print( cmd_ctx, "NEW COMMAND:");
2242         sprintf( buf, "%s %s %s %s %s %s %s %s %s %s",
2243                          Jim_GetString( newargs[0], NULL ),
2244                          Jim_GetString( newargs[1], NULL ),
2245                          Jim_GetString( newargs[2], NULL ),
2246                          Jim_GetString( newargs[3], NULL ),
2247                          Jim_GetString( newargs[4], NULL ),
2248                          Jim_GetString( newargs[5], NULL ),
2249                          Jim_GetString( newargs[6], NULL ),
2250                          Jim_GetString( newargs[7], NULL ),
2251                          Jim_GetString( newargs[8], NULL ),
2252                          Jim_GetString( newargs[9], NULL ) );
2253
2254
2255
2256         e = jim_jtag_command( interp, 10, newargs );
2257         if( e != JIM_OK ){
2258                 command_print( cmd_ctx, "%s", Jim_GetString( Jim_GetResult(interp), NULL ) );
2259         }
2260         return e;
2261 }
2262
2263
2264 int handle_scan_chain_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2265 {
2266         jtag_tap_t *tap;
2267
2268         tap = jtag_all_taps;
2269         command_print(cmd_ctx, "     TapName            | Enabled |   IdCode      Expected    IrLen IrCap  IrMask Instr     ");
2270         command_print(cmd_ctx, "---|--------------------|---------|------------|------------|------|------|------|---------");
2271
2272         while( tap ){
2273                 u32 expected, expected_mask, cur_instr;
2274                 expected = buf_get_u32(tap->expected, 0, tap->ir_length);
2275                 expected_mask = buf_get_u32(tap->expected_mask, 0, tap->ir_length);
2276                 cur_instr = buf_get_u32(tap->cur_instr, 0, tap->ir_length);
2277                 command_print(cmd_ctx,
2278                                           "%2d | %-18s |    %c    | 0x%08x | 0x%08x | 0x%02x | 0x%02x | 0x%02x | 0x%02x",
2279                                           tap->abs_chain_position,
2280                                           tap->dotted_name,
2281                                           tap->enabled ? 'Y' : 'n',
2282                                           tap->idcode,
2283                                           tap->expected_id,
2284                                           tap->ir_length,
2285                                           expected,
2286                                           expected_mask,
2287                                           cur_instr);
2288                 tap = tap->next_tap;
2289         }
2290
2291         return ERROR_OK;
2292 }
2293
2294 int handle_reset_config_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2295 {
2296         if (argc < 1)
2297                 return ERROR_COMMAND_SYNTAX_ERROR;
2298
2299         if (argc >= 1)
2300         {
2301                 if (strcmp(args[0], "none") == 0)
2302                         jtag_reset_config = RESET_NONE;
2303                 else if (strcmp(args[0], "trst_only") == 0)
2304                         jtag_reset_config = RESET_HAS_TRST;
2305                 else if (strcmp(args[0], "srst_only") == 0)
2306                         jtag_reset_config = RESET_HAS_SRST;
2307                 else if (strcmp(args[0], "trst_and_srst") == 0)
2308                         jtag_reset_config = RESET_TRST_AND_SRST;
2309                 else
2310                 {
2311                         LOG_ERROR("(1) invalid reset_config argument (%s), defaulting to none", args[0]);
2312                         jtag_reset_config = RESET_NONE;
2313                         return ERROR_INVALID_ARGUMENTS;
2314                 }
2315         }
2316
2317         if (argc >= 2)
2318         {
2319                 if (strcmp(args[1], "separate") == 0)
2320                 {
2321                         /* seperate reset lines - default */
2322                 } else
2323                 {
2324                         if (strcmp(args[1], "srst_pulls_trst") == 0)
2325                                 jtag_reset_config |= RESET_SRST_PULLS_TRST;
2326                         else if (strcmp(args[1], "trst_pulls_srst") == 0)
2327                                 jtag_reset_config |= RESET_TRST_PULLS_SRST;
2328                         else if (strcmp(args[1], "combined") == 0)
2329                                 jtag_reset_config |= RESET_SRST_PULLS_TRST | RESET_TRST_PULLS_SRST;
2330                         else
2331                         {
2332                                 LOG_ERROR("(2) invalid reset_config argument (%s), defaulting to none", args[1]);
2333                                 jtag_reset_config = RESET_NONE;
2334                                 return ERROR_INVALID_ARGUMENTS;
2335                         }
2336                 }
2337         }
2338
2339         if (argc >= 3)
2340         {
2341                 if (strcmp(args[2], "trst_open_drain") == 0)
2342                         jtag_reset_config |= RESET_TRST_OPEN_DRAIN;
2343                 else if (strcmp(args[2], "trst_push_pull") == 0)
2344                         jtag_reset_config &= ~RESET_TRST_OPEN_DRAIN;
2345                 else
2346                 {
2347                         LOG_ERROR("(3) invalid reset_config argument (%s) defaulting to none", args[2] );
2348                         jtag_reset_config = RESET_NONE;
2349                         return ERROR_INVALID_ARGUMENTS;
2350                 }
2351         }
2352
2353         if (argc >= 4)
2354         {
2355                 if (strcmp(args[3], "srst_push_pull") == 0)
2356                         jtag_reset_config |= RESET_SRST_PUSH_PULL;
2357                 else if (strcmp(args[3], "srst_open_drain") == 0)
2358                         jtag_reset_config &= ~RESET_SRST_PUSH_PULL;
2359                 else
2360                 {
2361                         LOG_ERROR("(4) invalid reset_config argument (%s), defaulting to none", args[3]);
2362                         jtag_reset_config = RESET_NONE;
2363                         return ERROR_INVALID_ARGUMENTS;
2364                 }
2365         }
2366
2367         return ERROR_OK;
2368 }
2369
2370 int handle_jtag_nsrst_delay_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2371 {
2372         if (argc < 1)
2373         {
2374                 LOG_ERROR("jtag_nsrst_delay <ms> command takes one required argument");
2375                 exit(-1);
2376         }
2377         else
2378         {
2379                 jtag_nsrst_delay = strtoul(args[0], NULL, 0);
2380         }
2381
2382         return ERROR_OK;
2383 }
2384
2385 int handle_jtag_ntrst_delay_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2386 {
2387         if (argc < 1)
2388         {
2389                 LOG_ERROR("jtag_ntrst_delay <ms> command takes one required argument");
2390                 exit(-1);
2391         }
2392         else
2393         {
2394                 jtag_ntrst_delay = strtoul(args[0], NULL, 0);
2395         }
2396
2397         return ERROR_OK;
2398 }
2399
2400 int handle_jtag_speed_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2401 {
2402         int retval=ERROR_OK;
2403
2404         if (argc == 1)
2405         {
2406                 LOG_DEBUG("handle jtag speed");
2407
2408                 int cur_speed = 0;
2409                 cur_speed = jtag_speed = strtoul(args[0], NULL, 0);
2410
2411                 /* this command can be called during CONFIG,
2412                  * in which case jtag isn't initialized */
2413                 if (jtag)
2414                 {
2415                         retval=jtag->speed(cur_speed);
2416                 }
2417         } else if (argc == 0)
2418         {
2419         } else
2420         {
2421                 return ERROR_COMMAND_SYNTAX_ERROR;
2422         }
2423         command_print(cmd_ctx, "jtag_speed: %d", jtag_speed);
2424
2425         return retval;
2426 }
2427
2428 int handle_jtag_khz_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2429 {
2430         int retval=ERROR_OK;
2431         LOG_DEBUG("handle jtag khz");
2432
2433         if(argc == 1)
2434         {
2435                 speed_khz = strtoul(args[0], NULL, 0);
2436                 if (jtag != NULL)
2437                 {
2438                         int cur_speed = 0;
2439                         LOG_DEBUG("have interface set up");
2440                         int speed_div1;
2441                         if ((retval=jtag->khz(speed_khz, &speed_div1))!=ERROR_OK)
2442                         {
2443                                 speed_khz = 0;
2444                                 return retval;
2445                         }
2446
2447                         cur_speed = jtag_speed = speed_div1;
2448
2449                         retval=jtag->speed(cur_speed);
2450                 } else
2451                 {
2452                         hasKHz = 1;
2453                 }
2454         } else if (argc==0)
2455         {
2456         } else
2457         {
2458                 return ERROR_COMMAND_SYNTAX_ERROR;
2459         }
2460
2461         if (jtag!=NULL)
2462         {
2463                 if ((retval=jtag->speed_div(jtag_speed, &speed_khz))!=ERROR_OK)
2464                         return retval;
2465         }
2466
2467         if (speed_khz==0)
2468         {
2469                 command_print(cmd_ctx, "RCLK - adaptive");
2470         } else
2471         {
2472                 command_print(cmd_ctx, "%d kHz", speed_khz);
2473         }
2474         return retval;
2475
2476 }
2477
2478 int handle_endstate_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2479 {
2480         enum tap_state state;
2481
2482         if (argc < 1)
2483         {
2484                 return ERROR_COMMAND_SYNTAX_ERROR;
2485         }
2486         else
2487         {
2488                 for (state = 0; state < 16; state++)
2489                 {
2490                         if (strcmp(args[0], tap_state_strings[state]) == 0)
2491                         {
2492                                 jtag_add_end_state(state);
2493                                 jtag_execute_queue();
2494                         }
2495                 }
2496         }
2497         command_print(cmd_ctx, "current endstate: %s", tap_state_strings[cmd_queue_end_state]);
2498
2499         return ERROR_OK;
2500 }
2501
2502 int handle_jtag_reset_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2503 {
2504         int trst = -1;
2505         int srst = -1;
2506
2507         if (argc < 2)
2508         {
2509                 return ERROR_COMMAND_SYNTAX_ERROR;
2510         }
2511
2512         if (args[0][0] == '1')
2513                 trst = 1;
2514         else if (args[0][0] == '0')
2515                 trst = 0;
2516         else
2517         {
2518                 return ERROR_COMMAND_SYNTAX_ERROR;
2519         }
2520
2521         if (args[1][0] == '1')
2522                 srst = 1;
2523         else if (args[1][0] == '0')
2524                 srst = 0;
2525         else
2526         {
2527                 return ERROR_COMMAND_SYNTAX_ERROR;
2528         }
2529
2530         if (jtag_interface_init(cmd_ctx) != ERROR_OK)
2531                 return ERROR_JTAG_INIT_FAILED;
2532
2533         jtag_add_reset(trst, srst);
2534         jtag_execute_queue();
2535
2536         return ERROR_OK;
2537 }
2538
2539 int handle_runtest_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2540 {
2541         if (argc < 1)
2542         {
2543                 return ERROR_COMMAND_SYNTAX_ERROR;
2544         }
2545
2546         jtag_add_runtest(strtol(args[0], NULL, 0), -1);
2547         jtag_execute_queue();
2548
2549         return ERROR_OK;
2550
2551 }
2552
2553 int handle_irscan_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2554 {
2555         int i;
2556         scan_field_t *fields;
2557         jtag_tap_t *tap;
2558
2559         if ((argc < 2) || (argc % 2))
2560         {
2561                 return ERROR_COMMAND_SYNTAX_ERROR;
2562         }
2563
2564         fields = malloc(sizeof(scan_field_t) * argc / 2);
2565
2566         for (i = 0; i < argc / 2; i++)
2567         {
2568                 tap = jtag_TapByString( args[i*2] );
2569                 if (tap==NULL)
2570                 {
2571                         command_print( cmd_ctx, "Tap: %s unknown", args[i*2] );
2572                         return ERROR_FAIL;
2573                 }
2574                 int field_size = tap->ir_length;
2575                 fields[i].tap = tap;
2576                 fields[i].out_value = malloc(CEIL(field_size, 8));
2577                 buf_set_u32(fields[i].out_value, 0, field_size, strtoul(args[i*2+1], NULL, 0));
2578                 fields[i].out_mask = NULL;
2579                 fields[i].in_value = NULL;
2580                 fields[i].in_check_mask = NULL;
2581                 fields[i].in_handler = NULL;
2582                 fields[i].in_handler_priv = NULL;
2583         }
2584
2585         jtag_add_ir_scan(argc / 2, fields, -1);
2586         jtag_execute_queue();
2587
2588         for (i = 0; i < argc / 2; i++)
2589                 free(fields[i].out_value);
2590
2591         free (fields);
2592
2593         return ERROR_OK;
2594 }
2595
2596 int Jim_Command_drscan(Jim_Interp *interp, int argc, Jim_Obj *const *args)
2597 {
2598         int retval;
2599         scan_field_t *fields;
2600         int num_fields;
2601         int field_count = 0;
2602         int i, e;
2603         jtag_tap_t *tap;
2604
2605         /* args[1] = device
2606          * args[2] = num_bits
2607          * args[3] = hex string
2608          * ... repeat num bits and hex string ...
2609          */
2610         if ((argc < 4) || ((argc % 2)!=0))
2611         {
2612                 Jim_WrongNumArgs(interp, 1, args, "wrong arguments");
2613                 return JIM_ERR;
2614         }
2615
2616         for (i = 2; i < argc; i+=2)
2617         {
2618                 long bits;
2619
2620                 e = Jim_GetLong(interp, args[i], &bits);
2621                 if (e != JIM_OK)
2622                         return e;
2623         }
2624
2625         tap = jtag_TapByJimObj( interp, args[1] );
2626         if( tap == NULL ){
2627                 return JIM_ERR;
2628         }
2629
2630         num_fields=(argc-2)/2;
2631         fields = malloc(sizeof(scan_field_t) * num_fields);
2632         for (i = 2; i < argc; i+=2)
2633         {
2634                 long bits;
2635                 int len;
2636                 const char *str;
2637
2638                 Jim_GetLong(interp, args[i], &bits);
2639                 str = Jim_GetString(args[i+1], &len);
2640
2641
2642                 fields[field_count].tap = tap;
2643                 fields[field_count].num_bits = bits;
2644                 fields[field_count].out_value = malloc(CEIL(bits, 8));
2645                 str_to_buf(str, len, fields[field_count].out_value, bits, 0);
2646                 fields[field_count].out_mask = NULL;
2647                 fields[field_count].in_value = fields[field_count].out_value;
2648                 fields[field_count].in_check_mask = NULL;
2649                 fields[field_count].in_check_value = NULL;
2650                 fields[field_count].in_handler = NULL;
2651                 fields[field_count++].in_handler_priv = NULL;
2652         }
2653
2654         jtag_add_dr_scan(num_fields, fields, -1);
2655         retval = jtag_execute_queue();
2656         if (retval != ERROR_OK)
2657         {
2658                 Jim_SetResultString(interp, "drscan: jtag execute failed",-1);
2659                 return JIM_ERR;
2660         }
2661
2662         field_count=0;
2663         Jim_Obj *list = Jim_NewListObj(interp, NULL, 0);
2664         for (i = 2; i < argc; i+=2)
2665         {
2666                 long bits;
2667                 char *str;
2668
2669                 Jim_GetLong(interp, args[i], &bits);
2670                 str = buf_to_str(fields[field_count].in_value, bits, 16);
2671                 free(fields[field_count].out_value);
2672
2673                 Jim_ListAppendElement(interp, list, Jim_NewStringObj(interp, str, strlen(str)));
2674                 free(str);
2675                 field_count++;
2676         }
2677
2678         Jim_SetResult(interp, list);
2679
2680         free(fields);
2681
2682         return JIM_OK;
2683 }
2684
2685 int handle_verify_ircapture_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2686 {
2687         if (argc == 1)
2688         {
2689                 if (strcmp(args[0], "enable") == 0)
2690                 {
2691                         jtag_verify_capture_ir = 1;
2692                 }
2693                 else if (strcmp(args[0], "disable") == 0)
2694                 {
2695                         jtag_verify_capture_ir = 0;
2696                 } else
2697                 {
2698                         return ERROR_COMMAND_SYNTAX_ERROR;
2699                 }
2700         } else if (argc != 0)
2701         {
2702                 return ERROR_COMMAND_SYNTAX_ERROR;
2703         }
2704
2705         command_print(cmd_ctx, "verify Capture-IR is %s", (jtag_verify_capture_ir) ? "enabled": "disabled");
2706
2707         return ERROR_OK;
2708 }
2709
2710
2711 int jtag_power_dropout(int *dropout)
2712 {
2713         return jtag->power_dropout(dropout);
2714 }
2715
2716 int jtag_srst_asserted(int *srst_asserted)
2717 {
2718         return jtag->srst_asserted(srst_asserted);
2719 }
2720