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  *   This program is free software; you can redistribute it and/or modify  *
   6  *   it under the terms of the GNU General Public License as published by  *
   7  *   the Free Software Foundation; either version 2 of the License, or     *
   8  *   (at your option) any later version.                                   *
   9  *                                                                         *
  10  *   This program is distributed in the hope that it will be useful,       *
  11  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  12  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  13  *   GNU General Public License for more details.                          *
  14  *                                                                         *
  15  *   You should have received a copy of the GNU General Public License     *
  16  *   along with this program; if not, write to the                         *
  17  *   Free Software Foundation, Inc.,                                       *
  18  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  19  ***************************************************************************/
  20 #ifdef HAVE_CONFIG_H
  21 #include "config.h"
  22 #endif
  23
  24 #include "replacements.h"
  25
  26 #include "jtag.h"
  27
  28 #include "command.h"
  29 #include "log.h"
  30 #include "interpreter.h"
  31
  32 #include "stdlib.h"
  33 #include "string.h"
  34 #include <unistd.h>
  35
  36
  37 /* note that this is not marked as static as it must be available from outside jtag.c for those
  38    that implement the jtag_xxx() minidriver layer
  39 */
  40 static int jtag_error=ERROR_OK;
  41
  42
  43 char* tap_state_strings[16] =
  44 {
  45         "tlr",
  46         "sds", "cd", "sd", "e1d", "pd", "e2d", "ud",
  47         "rti",
  48         "sis", "ci", "si", "e1i", "pi", "e2i", "ui"
  49 };
  50
  51 typedef struct cmd_queue_page_s
  52 {
  53         void *address;
  54         size_t used;
  55         struct cmd_queue_page_s *next;
  56 } cmd_queue_page_t;
  57
  58 #define CMD_QUEUE_PAGE_SIZE (1024 * 1024)
  59 static cmd_queue_page_t *cmd_queue_pages = NULL;
  60
  61 /* tap_move[i][j]: tap movement command to go from state i to state j
  62  * 0: Test-Logic-Reset
  63  * 1: Run-Test/Idle
  64  * 2: Shift-DR
  65  * 3: Pause-DR
  66  * 4: Shift-IR
  67  * 5: Pause-IR
  68  *
  69  * SD->SD and SI->SI have to be caught in interface specific code
  70  */
  71 u8 tap_move[6][6] =
  72 {
  73 /*        TLR   RTI   SD    PD    SI    PI             */
  74         {0x7f, 0x00, 0x17, 0x0a, 0x1b, 0x16},   /* TLR */
  75         {0x7f, 0x00, 0x25, 0x05, 0x2b, 0x0b},   /* RTI */
  76         {0x7f, 0x31, 0x00, 0x01, 0x0f, 0x2f},   /* SD  */
  77         {0x7f, 0x30, 0x20, 0x17, 0x1e, 0x2f},   /* PD  */
  78         {0x7f, 0x31, 0x07, 0x17, 0x00, 0x01},   /* SI  */
  79         {0x7f, 0x30, 0x1c, 0x17, 0x20, 0x2f}    /* PI  */
  80 };
  81
  82 int tap_move_map[16] = {
  83         0, -1, -1,  2, -1,  3, -1, -1,
  84         1, -1, -1,  4, -1,  5, -1, -1
  85 };
  86
  87 tap_transition_t tap_transitions[16] =
  88 {
  89         {TAP_TLR, TAP_RTI},             /* TLR */
  90         {TAP_SIS, TAP_CD},              /* SDS */
  91         {TAP_E1D, TAP_SD},              /* CD  */
  92         {TAP_E1D, TAP_SD},              /* SD  */
  93         {TAP_UD,  TAP_PD},              /* E1D */
  94         {TAP_E2D, TAP_PD},              /* PD  */
  95         {TAP_UD,  TAP_SD},              /* E2D */
  96         {TAP_SDS, TAP_RTI},             /* UD  */
  97         {TAP_SDS, TAP_RTI},             /* RTI */
  98         {TAP_TLR, TAP_CI},              /* SIS */
  99         {TAP_E1I, TAP_SI},              /* CI  */
 100         {TAP_E1I, TAP_SI},              /* SI  */
 101         {TAP_UI,  TAP_PI},              /* E1I */
 102         {TAP_E2I, TAP_PI},              /* PI  */
 103         {TAP_UI,  TAP_SI},              /* E2I */
 104         {TAP_SDS, TAP_RTI}              /* UI  */
 105 };
 106
 107 char* jtag_event_strings[] =
 108 {
 109         "SRST asserted",
 110         "TRST asserted",
 111         "SRST released",
 112         "TRST released"
 113 };
 114
 115 enum tap_state end_state = TAP_TLR;
 116 enum tap_state cur_state = TAP_TLR;
 117 int jtag_trst = 0;
 118 int jtag_srst = 0;
 119
 120 jtag_command_t *jtag_command_queue = NULL;
 121 jtag_command_t **last_comand_pointer = &jtag_command_queue;
 122 jtag_device_t *jtag_devices = NULL;
 123 int jtag_num_devices = 0;
 124 int jtag_ir_scan_size = 0;
 125 enum reset_types jtag_reset_config = RESET_NONE;
 126 enum tap_state cmd_queue_end_state = TAP_TLR;
 127 enum tap_state cmd_queue_cur_state = TAP_TLR;
 128
 129 int jtag_verify_capture_ir = 1;
 130
 131 /* how long the OpenOCD should wait before attempting JTAG communication after reset lines deasserted (in ms) */
 132 int jtag_nsrst_delay = 0; /* default to no nSRST delay */
 133 int jtag_ntrst_delay = 0; /* default to no nTRST delay */
 134
 135 /* maximum number of JTAG devices expected in the chain
 136  */
 137 #define JTAG_MAX_CHAIN_SIZE 20
 138
 139 /* callbacks to inform high-level handlers about JTAG state changes */
 140 jtag_event_callback_t *jtag_event_callbacks;
 141
 142 /* jtag interfaces (parport, FTDI-USB, TI-USB, ...)
 143  */
 144 #if BUILD_PARPORT == 1
 145         extern jtag_interface_t parport_interface;
 146 #endif
 147
 148 #if BUILD_FT2232_FTD2XX == 1
 149         extern jtag_interface_t ft2232_interface;
 150 #endif
 151
 152 #if BUILD_FT2232_LIBFTDI == 1
 153         extern jtag_interface_t ft2232_interface;
 154 #endif
 155
 156 #if BUILD_AMTJTAGACCEL == 1
 157         extern jtag_interface_t amt_jtagaccel_interface;
 158 #endif
 159
 160 #if BUILD_EP93XX == 1
 161         extern jtag_interface_t ep93xx_interface;
 162 #endif
 163
 164 #if BUILD_AT91RM9200 == 1
 165         extern jtag_interface_t at91rm9200_interface;
 166 #endif
 167
 168 #if BUILD_GW16012 == 1
 169         extern jtag_interface_t gw16012_interface;
 170 #endif
 171
 172 #if BUILD_PRESTO_LIBFTDI == 1 || BUILD_PRESTO_FTD2XX == 1
 173         extern jtag_interface_t presto_interface;
 174 #endif
 175
 176 #if BUILD_USBPROG == 1
 177         extern jtag_interface_t usbprog_interface;
 178 #endif
 179
 180 jtag_interface_t *jtag_interfaces[] = {
 181 #if BUILD_PARPORT == 1
 182         &parport_interface,
 183 #endif
 184 #if BUILD_FT2232_FTD2XX == 1
 185         &ft2232_interface,
 186 #endif
 187 #if BUILD_FT2232_LIBFTDI == 1
 188         &ft2232_interface,
 189 #endif
 190 #if BUILD_AMTJTAGACCEL == 1
 191         &amt_jtagaccel_interface,
 192 #endif
 193 #if BUILD_EP93XX == 1
 194         &ep93xx_interface,
 195 #endif
 196 #if BUILD_AT91RM9200 == 1
 197         &at91rm9200_interface,
 198 #endif
 199 #if BUILD_GW16012 == 1
 200         &gw16012_interface,
 201 #endif
 202 #if BUILD_PRESTO_LIBFTDI == 1 || BUILD_PRESTO_FTD2XX == 1
 203         &presto_interface,
 204 #endif
 205 #if BUILD_USBPROG == 1
 206         &usbprog_interface,
 207 #endif
 208         NULL,
 209 };
 210
 211 jtag_interface_t *jtag = NULL;
 212
 213 /* configuration */
 214 jtag_interface_t *jtag_interface = NULL;
 215 int jtag_speed = 0;
 216
 217
 218 /* forward declarations */
 219 void jtag_add_statemove(enum tap_state endstate);
 220 void jtag_add_pathmove(int num_states, enum tap_state *path);
 221 void jtag_add_runtest(int num_cycles, enum tap_state endstate);
 222 int jtag_add_reset(int trst, int srst);
 223 void jtag_add_end_state(enum tap_state endstate);
 224 void jtag_add_sleep(u32 us);
 225 int jtag_execute_queue(void);
 226 int jtag_cancel_queue(void);
 227
 228 /* jtag commands */
 229 int handle_interface_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
 230 int handle_jtag_speed_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
 231 int handle_jtag_device_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
 232 int handle_reset_config_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
 233 int handle_jtag_nsrst_delay_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
 234 int handle_jtag_ntrst_delay_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
 235
 236 int handle_scan_chain_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
 237
 238 int handle_endstate_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
 239 int handle_jtag_reset_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
 240 int handle_runtest_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
 241 int handle_statemove_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
 242 int handle_irscan_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
 243 int handle_drscan_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
 244
 245 int handle_verify_ircapture_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
 246
 247 int jtag_register_event_callback(int (*callback)(enum jtag_event event, void *priv), void *priv)
 248 {
 249         jtag_event_callback_t **callbacks_p = &jtag_event_callbacks;
 250
 251         if (callback == NULL)
 252         {
 253                 return ERROR_INVALID_ARGUMENTS;
 254         }
 255
 256         if (*callbacks_p)
 257         {
 258                 while ((*callbacks_p)->next)
 259                         callbacks_p = &((*callbacks_p)->next);
 260                 callbacks_p = &((*callbacks_p)->next);
 261         }
 262
 263         (*callbacks_p) = malloc(sizeof(jtag_event_callback_t));
 264         (*callbacks_p)->callback = callback;
 265         (*callbacks_p)->priv = priv;
 266         (*callbacks_p)->next = NULL;
 267
 268         return ERROR_OK;
 269 }
 270
 271 int jtag_unregister_event_callback(int (*callback)(enum jtag_event event, void *priv))
 272 {
 273         jtag_event_callback_t **callbacks_p = &jtag_event_callbacks;
 274
 275         if (callback == NULL)
 276         {
 277                 return ERROR_INVALID_ARGUMENTS;
 278         }
 279
 280         while (*callbacks_p)
 281         {
 282                 jtag_event_callback_t **next = &((*callbacks_p)->next);
 283                 if ((*callbacks_p)->callback == callback)
 284                 {
 285                         free(*callbacks_p);
 286                         *callbacks_p = *next;
 287                 }
 288                 callbacks_p = next;
 289         }
 290
 291         return ERROR_OK;
 292 }
 293
 294 int jtag_call_event_callbacks(enum jtag_event event)
 295 {
 296         jtag_event_callback_t *callback = jtag_event_callbacks;
 297
 298         DEBUG("jtag event: %s", jtag_event_strings[event]);
 299
 300         while (callback)
 301         {
 302                 callback->callback(event, callback->priv);
 303                 callback = callback->next;
 304         }
 305
 306         return ERROR_OK;
 307 }
 308
 309 /* returns a pointer to the pointer of the last command in queue
 310  * this may be a pointer to the root pointer (jtag_command_queue)
 311  * or to the next member of the last but one command
 312  */
 313 jtag_command_t** jtag_get_last_command_p(void)
 314 {
 315 /*      jtag_command_t *cmd = jtag_command_queue;
 316
 317         if (cmd)
 318                 while (cmd->next)
 319                         cmd = cmd->next;
 320         else
 321                 return &jtag_command_queue;
 322
 323         return &cmd->next;*/
 324
 325         return last_comand_pointer;
 326 }
 327
 328 /* returns a pointer to the n-th device in the scan chain */
 329 jtag_device_t* jtag_get_device(int num)
 330 {
 331         jtag_device_t *device = jtag_devices;
 332         int i = 0;
 333
 334         while (device)
 335         {
 336                 if (num == i)
 337                         return device;
 338                 device = device->next;
 339                 i++;
 340         }
 341
 342         ERROR("jtag device number %d not defined", num);
 343         exit(-1);
 344 }
 345
 346 void* cmd_queue_alloc(size_t size)
 347 {
 348         cmd_queue_page_t **p_page = &cmd_queue_pages;
 349         int offset;
 350
 351         if (*p_page)
 352         {
 353                 while ((*p_page)->next)
 354                         p_page = &((*p_page)->next);
 355                 if (CMD_QUEUE_PAGE_SIZE - (*p_page)->used < size)
 356                         p_page = &((*p_page)->next);
 357         }
 358
 359         if (!*p_page)
 360         {
 361                 *p_page = malloc(sizeof(cmd_queue_page_t));
 362                 (*p_page)->used = 0;
 363                 (*p_page)->address = malloc(CMD_QUEUE_PAGE_SIZE);
 364                 (*p_page)->next = NULL;
 365         }
 366
 367         offset = (*p_page)->used;
 368         (*p_page)->used += size;
 369
 370         u8 *t=(u8 *)((*p_page)->address);
 371         return t + offset;
 372 }
 373
 374 void cmd_queue_free()
 375 {
 376         cmd_queue_page_t *page = cmd_queue_pages;
 377
 378         while (page)
 379         {
 380                 cmd_queue_page_t *last = page;
 381                 free(page->address);
 382                 page = page->next;
 383                 free(last);
 384         }
 385
 386         cmd_queue_pages = NULL;
 387 }
 388
 389 void jtag_add_ir_scan(int num_fields, scan_field_t *fields, enum tap_state state)
 390 {
 391         if (jtag_trst == 1)
 392         {
 393                 WARNING("JTAG command queued, while TRST is low (TAP in reset)");
 394                 jtag_error=ERROR_JTAG_TRST_ASSERTED;
 395                 return;
 396         }
 397
 398         if (state != -1)
 399                 cmd_queue_end_state = state;
 400
 401         if (cmd_queue_cur_state == TAP_TLR && cmd_queue_end_state != TAP_TLR)
 402                 jtag_call_event_callbacks(JTAG_TRST_RELEASED);
 403
 404         if (cmd_queue_end_state == TAP_TLR)
 405                 jtag_call_event_callbacks(JTAG_TRST_ASSERTED);
 406
 407         cmd_queue_cur_state = cmd_queue_end_state;
 408
 409         int retval=interface_jtag_add_ir_scan(num_fields, fields, cmd_queue_end_state);
 410         if (retval!=ERROR_OK)
 411                 jtag_error=retval;
 412 }
 413
 414 int MINIDRIVER(interface_jtag_add_ir_scan)(int num_fields, scan_field_t *fields, enum tap_state state)
 415 {
 416         jtag_command_t **last_cmd;
 417         jtag_device_t *device;
 418         int i, j;
 419         int scan_size = 0;
 420
 421
 422         last_cmd = jtag_get_last_command_p();
 423
 424         /* allocate memory for a new list member */
 425         *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
 426         (*last_cmd)->next = NULL;
 427         last_comand_pointer = &((*last_cmd)->next);
 428         (*last_cmd)->type = JTAG_SCAN;
 429
 430         /* allocate memory for ir scan command */
 431         (*last_cmd)->cmd.scan = cmd_queue_alloc(sizeof(scan_command_t));
 432         (*last_cmd)->cmd.scan->ir_scan = 1;
 433         (*last_cmd)->cmd.scan->num_fields = jtag_num_devices;   /* one field per device */
 434         (*last_cmd)->cmd.scan->fields = cmd_queue_alloc(jtag_num_devices * sizeof(scan_field_t));
 435         (*last_cmd)->cmd.scan->end_state = state;
 436
 437         for (i = 0; i < jtag_num_devices; i++)
 438         {
 439                 int found = 0;
 440                 device = jtag_get_device(i);
 441                 scan_size = device->ir_length;
 442                 (*last_cmd)->cmd.scan->fields[i].device = i;
 443                 (*last_cmd)->cmd.scan->fields[i].num_bits = scan_size;
 444                 (*last_cmd)->cmd.scan->fields[i].in_value = NULL;
 445                 (*last_cmd)->cmd.scan->fields[i].in_handler = NULL;     /* disable verification by default */
 446
 447                 /* search the list */
 448                 for (j = 0; j < num_fields; j++)
 449                 {
 450                         if (i == fields[j].device)
 451                         {
 452                                 found = 1;
 453                                 (*last_cmd)->cmd.scan->fields[i].out_value = buf_cpy(fields[j].out_value, cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);
 454                                 (*last_cmd)->cmd.scan->fields[i].out_mask = buf_cpy(fields[j].out_mask, cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);
 455
 456                                 if (jtag_verify_capture_ir)
 457                                 {
 458                                         if (fields[j].in_handler==NULL)
 459                                         {
 460                                                 jtag_set_check_value((*last_cmd)->cmd.scan->fields+i, device->expected, device->expected_mask, NULL);
 461                                         } else
 462                                         {
 463                                                 (*last_cmd)->cmd.scan->fields[i].in_handler = fields[j].in_handler;
 464                                                 (*last_cmd)->cmd.scan->fields[i].in_handler_priv = fields[j].in_handler_priv;
 465                                                 (*last_cmd)->cmd.scan->fields[i].in_check_value = device->expected;
 466                                                 (*last_cmd)->cmd.scan->fields[i].in_check_mask = device->expected_mask;
 467                                         }
 468                                 }
 469
 470                                 device->bypass = 0;
 471                                 break;
 472                         }
 473                 }
 474
 475                 if (!found)
 476                 {
 477                         /* if a device isn't listed, set it to BYPASS */
 478                         (*last_cmd)->cmd.scan->fields[i].out_value = buf_set_ones(cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);
 479                         (*last_cmd)->cmd.scan->fields[i].out_mask = NULL;
 480                         device->bypass = 1;
 481
 482                 }
 483
 484                 /* update device information */
 485                 buf_cpy((*last_cmd)->cmd.scan->fields[i].out_value, jtag_get_device(i)->cur_instr, scan_size);
 486         }
 487
 488         return ERROR_OK;
 489 }
 490
 491 void jtag_add_plain_ir_scan(int num_fields, scan_field_t *fields, enum tap_state state)
 492 {
 493         if (jtag_trst == 1)
 494         {
 495                 WARNING("JTAG command queued, while TRST is low (TAP in reset)");
 496                 jtag_error=ERROR_JTAG_TRST_ASSERTED;
 497                 return;
 498         }
 499
 500         if (state != -1)
 501                 cmd_queue_end_state = state;
 502
 503         if (cmd_queue_cur_state == TAP_TLR && cmd_queue_end_state != TAP_TLR)
 504                 jtag_call_event_callbacks(JTAG_TRST_RELEASED);
 505
 506         if (cmd_queue_end_state == TAP_TLR)
 507                 jtag_call_event_callbacks(JTAG_TRST_ASSERTED);
 508
 509         cmd_queue_cur_state = cmd_queue_end_state;
 510
 511         int retval=interface_jtag_add_plain_ir_scan(num_fields, fields, cmd_queue_end_state);
 512         if (retval!=ERROR_OK)
 513                 jtag_error=retval;
 514 }
 515
 516 int MINIDRIVER(interface_jtag_add_plain_ir_scan)(int num_fields, scan_field_t *fields, enum tap_state state)
 517 {
 518         int i;
 519         jtag_command_t **last_cmd;
 520
 521         last_cmd = jtag_get_last_command_p();
 522
 523         /* allocate memory for a new list member */
 524         *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
 525         (*last_cmd)->next = NULL;
 526         last_comand_pointer = &((*last_cmd)->next);
 527         (*last_cmd)->type = JTAG_SCAN;
 528
 529         /* allocate memory for ir scan command */
 530         (*last_cmd)->cmd.scan = cmd_queue_alloc(sizeof(scan_command_t));
 531         (*last_cmd)->cmd.scan->ir_scan = 1;
 532         (*last_cmd)->cmd.scan->num_fields = num_fields;
 533         (*last_cmd)->cmd.scan->fields = cmd_queue_alloc(num_fields * sizeof(scan_field_t));
 534         (*last_cmd)->cmd.scan->end_state = state;
 535
 536         for (i = 0; i < num_fields; i++)
 537         {
 538                 int num_bits = fields[i].num_bits;
 539                 int num_bytes = CEIL(fields[i].num_bits, 8);
 540                 (*last_cmd)->cmd.scan->fields[i].device = fields[i].device;
 541                 (*last_cmd)->cmd.scan->fields[i].num_bits = num_bits;
 542                 (*last_cmd)->cmd.scan->fields[i].out_value = buf_cpy(fields[i].out_value, cmd_queue_alloc(num_bytes), num_bits);
 543                 (*last_cmd)->cmd.scan->fields[i].out_mask = buf_cpy(fields[i].out_mask, cmd_queue_alloc(num_bytes), num_bits);
 544                 (*last_cmd)->cmd.scan->fields[i].in_value = fields[i].in_value;
 545                 (*last_cmd)->cmd.scan->fields[i].in_check_value = fields[i].in_check_value;
 546                 (*last_cmd)->cmd.scan->fields[i].in_check_mask = fields[i].in_check_mask;
 547                 (*last_cmd)->cmd.scan->fields[i].in_handler = NULL;
 548                 (*last_cmd)->cmd.scan->fields[i].in_handler_priv = NULL;
 549         }
 550         return ERROR_OK;
 551 }
 552
 553 void jtag_add_dr_scan(int num_fields, scan_field_t *fields, enum tap_state state)
 554 {
 555         if (jtag_trst == 1)
 556         {
 557                 WARNING("JTAG command queued, while TRST is low (TAP in reset)");
 558                 jtag_error=ERROR_JTAG_TRST_ASSERTED;
 559                 return;
 560         }
 561
 562         if (state != -1)
 563                 cmd_queue_end_state = state;
 564
 565         if (cmd_queue_cur_state == TAP_TLR && cmd_queue_end_state != TAP_TLR)
 566                 jtag_call_event_callbacks(JTAG_TRST_RELEASED);
 567
 568         if (cmd_queue_end_state == TAP_TLR)
 569                 jtag_call_event_callbacks(JTAG_TRST_ASSERTED);
 570
 571         cmd_queue_cur_state = cmd_queue_end_state;
 572
 573         int retval=interface_jtag_add_dr_scan(num_fields, fields, cmd_queue_end_state);
 574         if (retval!=ERROR_OK)
 575                 jtag_error=retval;
 576 }
 577
 578 int MINIDRIVER(interface_jtag_add_dr_scan)(int num_fields, scan_field_t *fields, enum tap_state state)
 579 {
 580         int i, j;
 581         int bypass_devices = 0;
 582         int field_count = 0;
 583         int scan_size;
 584
 585         jtag_command_t **last_cmd = jtag_get_last_command_p();
 586         jtag_device_t *device = jtag_devices;
 587
 588         /* count devices in bypass */
 589         while (device)
 590         {
 591                 if (device->bypass)
 592                         bypass_devices++;
 593                 device = device->next;
 594         }
 595
 596         /* allocate memory for a new list member */
 597         *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
 598         last_comand_pointer = &((*last_cmd)->next);
 599         (*last_cmd)->next = NULL;
 600         (*last_cmd)->type = JTAG_SCAN;
 601
 602         /* allocate memory for dr scan command */
 603         (*last_cmd)->cmd.scan = cmd_queue_alloc(sizeof(scan_command_t));
 604         (*last_cmd)->cmd.scan->ir_scan = 0;
 605         (*last_cmd)->cmd.scan->num_fields = num_fields + bypass_devices;
 606         (*last_cmd)->cmd.scan->fields = cmd_queue_alloc((num_fields + bypass_devices) * sizeof(scan_field_t));
 607         (*last_cmd)->cmd.scan->end_state = state;
 608
 609         for (i = 0; i < jtag_num_devices; i++)
 610         {
 611                 int found = 0;
 612                 (*last_cmd)->cmd.scan->fields[field_count].device = i;
 613
 614                 for (j = 0; j < num_fields; j++)
 615                 {
 616                         if (i == fields[j].device)
 617                         {
 618                                 found = 1;
 619                                 scan_size = fields[j].num_bits;
 620                                 (*last_cmd)->cmd.scan->fields[field_count].num_bits = scan_size;
 621                                 (*last_cmd)->cmd.scan->fields[field_count].out_value = buf_cpy(fields[j].out_value, cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);
 622                                 (*last_cmd)->cmd.scan->fields[field_count].out_mask = buf_cpy(fields[j].out_mask, cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);
 623                                 (*last_cmd)->cmd.scan->fields[field_count].in_value = fields[j].in_value;
 624                                 (*last_cmd)->cmd.scan->fields[field_count].in_check_value = fields[j].in_check_value;
 625                                 (*last_cmd)->cmd.scan->fields[field_count].in_check_mask = fields[j].in_check_mask;
 626                                 (*last_cmd)->cmd.scan->fields[field_count].in_handler = fields[j].in_handler;
 627                                 (*last_cmd)->cmd.scan->fields[field_count++].in_handler_priv = fields[j].in_handler_priv;
 628                         }
 629                 }
 630                 if (!found)
 631                 {
 632 #ifdef _DEBUG_JTAG_IO_
 633                         /* if a device isn't listed, the BYPASS register should be selected */
 634                         if (!jtag_get_device(i)->bypass)
 635                         {
 636                                 ERROR("BUG: no scan data for a device not in BYPASS");
 637                                 exit(-1);
 638                         }
 639 #endif
 640                         /* program the scan field to 1 bit length, and ignore it's value */
 641                         (*last_cmd)->cmd.scan->fields[field_count].num_bits = 1;
 642                         (*last_cmd)->cmd.scan->fields[field_count].out_value = NULL;
 643                         (*last_cmd)->cmd.scan->fields[field_count].out_mask = NULL;
 644                         (*last_cmd)->cmd.scan->fields[field_count].in_value = NULL;
 645                         (*last_cmd)->cmd.scan->fields[field_count].in_check_value = NULL;
 646                         (*last_cmd)->cmd.scan->fields[field_count].in_check_mask = NULL;
 647                         (*last_cmd)->cmd.scan->fields[field_count].in_handler = NULL;
 648                         (*last_cmd)->cmd.scan->fields[field_count++].in_handler_priv = NULL;
 649                 }
 650                 else
 651                 {
 652 #ifdef _DEBUG_JTAG_IO_
 653                         /* if a device is listed, the BYPASS register must not be selected */
 654                         if (jtag_get_device(i)->bypass)
 655                         {
 656                                 ERROR("BUG: scan data for a device in BYPASS");
 657                                 exit(-1);
 658                         }
 659 #endif
 660                 }
 661         }
 662         return ERROR_OK;
 663 }
 664
 665 void MINIDRIVER(interface_jtag_add_dr_out)(int device_num,
 666                 int num_fields,
 667                 int *num_bits,
 668                 u32 *value,
 669                 enum tap_state end_state)
 670 {
 671         int i;
 672         int field_count = 0;
 673         int scan_size;
 674         int bypass_devices = 0;
 675
 676         jtag_command_t **last_cmd = jtag_get_last_command_p();
 677         jtag_device_t *device = jtag_devices;
 678         /* count devices in bypass */
 679         while (device)
 680         {
 681                 if (device->bypass)
 682                         bypass_devices++;
 683                 device = device->next;
 684         }
 685
 686         /* allocate memory for a new list member */
 687         *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
 688         last_comand_pointer = &((*last_cmd)->next);
 689         (*last_cmd)->next = NULL;
 690         (*last_cmd)->type = JTAG_SCAN;
 691
 692         /* allocate memory for dr scan command */
 693         (*last_cmd)->cmd.scan = cmd_queue_alloc(sizeof(scan_command_t));
 694         (*last_cmd)->cmd.scan->ir_scan = 0;
 695         (*last_cmd)->cmd.scan->num_fields = num_fields + bypass_devices;
 696         (*last_cmd)->cmd.scan->fields = cmd_queue_alloc((num_fields + bypass_devices) * sizeof(scan_field_t));
 697         (*last_cmd)->cmd.scan->end_state = end_state;
 698
 699         for (i = 0; i < jtag_num_devices; i++)
 700         {
 701                 (*last_cmd)->cmd.scan->fields[field_count].device = i;
 702
 703                 if (i == device_num)
 704                 {
 705                         int j;
 706 #ifdef _DEBUG_JTAG_IO_
 707                         /* if a device is listed, the BYPASS register must not be selected */
 708                         if (jtag_get_device(i)->bypass)
 709                         {
 710                                 ERROR("BUG: scan data for a device in BYPASS");
 711                                 exit(-1);
 712                         }
 713 #endif
 714                         for (j = 0; j < num_fields; j++)
 715                         {
 716                                 u8 out_value[4];
 717                                 scan_size = num_bits[j];
 718                                 buf_set_u32(out_value, 0, scan_size, value[j]);
 719                                 (*last_cmd)->cmd.scan->fields[field_count].num_bits = scan_size;
 720                                 (*last_cmd)->cmd.scan->fields[field_count].out_value = buf_cpy(out_value, cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);
 721                                 (*last_cmd)->cmd.scan->fields[field_count].out_mask = NULL;
 722                                 (*last_cmd)->cmd.scan->fields[field_count].in_value = NULL;
 723                                 (*last_cmd)->cmd.scan->fields[field_count].in_check_value = NULL;
 724                                 (*last_cmd)->cmd.scan->fields[field_count].in_check_mask = NULL;
 725                                 (*last_cmd)->cmd.scan->fields[field_count].in_handler = NULL;
 726                                 (*last_cmd)->cmd.scan->fields[field_count++].in_handler_priv = NULL;
 727                         }
 728                 } else
 729                 {
 730 #ifdef _DEBUG_JTAG_IO_
 731                         /* if a device isn't listed, the BYPASS register should be selected */
 732                         if (!jtag_get_device(i)->bypass)
 733                         {
 734                                 ERROR("BUG: no scan data for a device not in BYPASS");
 735                                 exit(-1);
 736                         }
 737 #endif
 738                         /* program the scan field to 1 bit length, and ignore it's value */
 739                         (*last_cmd)->cmd.scan->fields[field_count].num_bits = 1;
 740                         (*last_cmd)->cmd.scan->fields[field_count].out_value = NULL;
 741                         (*last_cmd)->cmd.scan->fields[field_count].out_mask = NULL;
 742                         (*last_cmd)->cmd.scan->fields[field_count].in_value = NULL;
 743                         (*last_cmd)->cmd.scan->fields[field_count].in_check_value = NULL;
 744                         (*last_cmd)->cmd.scan->fields[field_count].in_check_mask = NULL;
 745                         (*last_cmd)->cmd.scan->fields[field_count].in_handler = NULL;
 746                         (*last_cmd)->cmd.scan->fields[field_count++].in_handler_priv = NULL;
 747                 }
 748         }
 749 }
 750
 751
 752
 753
 754 void jtag_add_plain_dr_scan(int num_fields, scan_field_t *fields, enum tap_state state)
 755 {
 756         if (jtag_trst == 1)
 757         {
 758                 WARNING("JTAG command queued, while TRST is low (TAP in reset)");
 759                 jtag_error=ERROR_JTAG_TRST_ASSERTED;
 760                 return;
 761         }
 762
 763         if (state != -1)
 764                 cmd_queue_end_state = state;
 765
 766         if (cmd_queue_cur_state == TAP_TLR && cmd_queue_end_state != TAP_TLR)
 767                 jtag_call_event_callbacks(JTAG_TRST_RELEASED);
 768
 769         if (cmd_queue_end_state == TAP_TLR)
 770                 jtag_call_event_callbacks(JTAG_TRST_ASSERTED);
 771
 772         cmd_queue_cur_state = cmd_queue_end_state;
 773
 774         int retval=interface_jtag_add_plain_dr_scan(num_fields, fields, cmd_queue_end_state);
 775         if (retval!=ERROR_OK)
 776                 jtag_error=retval;
 777 }
 778
 779 int MINIDRIVER(interface_jtag_add_plain_dr_scan)(int num_fields, scan_field_t *fields, enum tap_state state)
 780 {
 781         int i;
 782         jtag_command_t **last_cmd = jtag_get_last_command_p();
 783
 784         /* allocate memory for a new list member */
 785         *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
 786         last_comand_pointer = &((*last_cmd)->next);
 787         (*last_cmd)->next = NULL;
 788         (*last_cmd)->type = JTAG_SCAN;
 789
 790         /* allocate memory for scan command */
 791         (*last_cmd)->cmd.scan = cmd_queue_alloc(sizeof(scan_command_t));
 792         (*last_cmd)->cmd.scan->ir_scan = 0;
 793         (*last_cmd)->cmd.scan->num_fields = num_fields;
 794         (*last_cmd)->cmd.scan->fields = cmd_queue_alloc(num_fields * sizeof(scan_field_t));
 795         (*last_cmd)->cmd.scan->end_state = state;
 796
 797         for (i = 0; i < num_fields; i++)
 798         {
 799                 int num_bits = fields[i].num_bits;
 800                 int num_bytes = CEIL(fields[i].num_bits, 8);
 801                 (*last_cmd)->cmd.scan->fields[i].device = fields[i].device;
 802                 (*last_cmd)->cmd.scan->fields[i].num_bits = num_bits;
 803                 (*last_cmd)->cmd.scan->fields[i].out_value = buf_cpy(fields[i].out_value, cmd_queue_alloc(num_bytes), num_bits);
 804                 (*last_cmd)->cmd.scan->fields[i].out_mask = buf_cpy(fields[i].out_mask, cmd_queue_alloc(num_bytes), num_bits);
 805                 (*last_cmd)->cmd.scan->fields[i].in_value = fields[i].in_value;
 806                 (*last_cmd)->cmd.scan->fields[i].in_check_value = fields[i].in_check_value;
 807                 (*last_cmd)->cmd.scan->fields[i].in_check_mask = fields[i].in_check_mask;
 808                 (*last_cmd)->cmd.scan->fields[i].in_handler = fields[i].in_handler;
 809                 (*last_cmd)->cmd.scan->fields[i].in_handler_priv = fields[i].in_handler_priv;
 810         }
 811
 812         return ERROR_OK;
 813 }
 814 void jtag_add_statemove(enum tap_state state)
 815 {
 816         if (jtag_trst == 1)
 817         {
 818                 WARNING("JTAG command queued, while TRST is low (TAP in reset)");
 819                 jtag_error=ERROR_JTAG_TRST_ASSERTED;
 820                 return;
 821         }
 822
 823         if (state != -1)
 824                 cmd_queue_end_state = state;
 825
 826         if (cmd_queue_cur_state == TAP_TLR && cmd_queue_end_state != TAP_TLR)
 827                 jtag_call_event_callbacks(JTAG_TRST_RELEASED);
 828
 829         if (cmd_queue_end_state == TAP_TLR)
 830                 jtag_call_event_callbacks(JTAG_TRST_ASSERTED);
 831
 832         cmd_queue_cur_state = cmd_queue_end_state;
 833
 834         int retval;
 835         retval=interface_jtag_add_statemove(cmd_queue_end_state);
 836         if (retval!=ERROR_OK)
 837                 jtag_error=retval;
 838 }
 839
 840 int MINIDRIVER(interface_jtag_add_statemove)(enum tap_state state)
 841 {
 842         jtag_command_t **last_cmd = jtag_get_last_command_p();
 843
 844         /* allocate memory for a new list member */
 845         *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
 846         last_comand_pointer = &((*last_cmd)->next);
 847         (*last_cmd)->next = NULL;
 848         (*last_cmd)->type = JTAG_STATEMOVE;
 849
 850         (*last_cmd)->cmd.statemove = cmd_queue_alloc(sizeof(statemove_command_t));
 851         (*last_cmd)->cmd.statemove->end_state = state;
 852
 853
 854         return ERROR_OK;
 855 }
 856
 857 void jtag_add_pathmove(int num_states, enum tap_state *path)
 858 {
 859         if (jtag_trst == 1)
 860         {
 861                 WARNING("JTAG command queued, while TRST is low (TAP in reset)");
 862                 jtag_error=ERROR_JTAG_TRST_ASSERTED;
 863                 return;
 864         }
 865
 866         if (cmd_queue_cur_state == TAP_TLR && cmd_queue_end_state != TAP_TLR)
 867                 jtag_call_event_callbacks(JTAG_TRST_RELEASED);
 868
 869         if (cmd_queue_end_state == TAP_TLR)
 870                 jtag_call_event_callbacks(JTAG_TRST_ASSERTED);
 871
 872         /* the last state has to be a stable state */
 873         if (tap_move_map[path[num_states - 1]] == -1)
 874         {
 875                 ERROR("TAP path doesn't finish in a stable state");
 876                 jtag_error=ERROR_JTAG_NOT_IMPLEMENTED;
 877                 return;
 878         }
 879
 880         enum tap_state cur_state=cmd_queue_cur_state;
 881         int i;
 882         for (i=0; i<num_states; i++)
 883         {
 884                 if ((tap_transitions[cur_state].low != path[i])&&
 885                                 (tap_transitions[cur_state].high != path[i]))
 886                 {
 887                         ERROR("BUG: %s -> %s isn't a valid TAP transition", tap_state_strings[cur_state], tap_state_strings[path[i]]);
 888                         exit(-1);
 889                 }
 890                 cur_state = path[i];
 891         }
 892
 893         cmd_queue_cur_state = path[num_states - 1];
 894
 895         int retval=interface_jtag_add_pathmove(num_states, path);
 896         if (retval!=ERROR_OK)
 897                 jtag_error=retval;
 898 }
 899
 900
 901 int MINIDRIVER(interface_jtag_add_pathmove)(int num_states, enum tap_state *path)
 902 {
 903         jtag_command_t **last_cmd = jtag_get_last_command_p();
 904         int i;
 905
 906         /* allocate memory for a new list member */
 907         *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
 908         last_comand_pointer = &((*last_cmd)->next);
 909         (*last_cmd)->next = NULL;
 910         (*last_cmd)->type = JTAG_PATHMOVE;
 911
 912         (*last_cmd)->cmd.pathmove = cmd_queue_alloc(sizeof(pathmove_command_t));
 913         (*last_cmd)->cmd.pathmove->num_states = num_states;
 914         (*last_cmd)->cmd.pathmove->path = cmd_queue_alloc(sizeof(enum tap_state) * num_states);
 915
 916         for (i = 0; i < num_states; i++)
 917                 (*last_cmd)->cmd.pathmove->path[i] = path[i];
 918
 919         return ERROR_OK;
 920 }
 921
 922 int MINIDRIVER(interface_jtag_add_runtest)(int num_cycles, enum tap_state state)
 923 {
 924         jtag_command_t **last_cmd = jtag_get_last_command_p();
 925
 926         /* allocate memory for a new list member */
 927         *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
 928         (*last_cmd)->next = NULL;
 929         last_comand_pointer = &((*last_cmd)->next);
 930         (*last_cmd)->type = JTAG_RUNTEST;
 931
 932         (*last_cmd)->cmd.runtest = cmd_queue_alloc(sizeof(runtest_command_t));
 933         (*last_cmd)->cmd.runtest->num_cycles = num_cycles;
 934         (*last_cmd)->cmd.runtest->end_state = state;
 935
 936         return ERROR_OK;
 937 }
 938
 939 void jtag_add_runtest(int num_cycles, enum tap_state state)
 940 {
 941         if (jtag_trst == 1)
 942         {
 943                 WARNING("JTAG command queued, while TRST is low (TAP in reset)");
 944                 jtag_error=ERROR_JTAG_TRST_ASSERTED;
 945                 return;
 946         }
 947
 948         if (state != -1)
 949                 cmd_queue_end_state = state;
 950
 951         if (cmd_queue_cur_state == TAP_TLR && cmd_queue_end_state != TAP_TLR)
 952                 jtag_call_event_callbacks(JTAG_TRST_RELEASED);
 953
 954         if (cmd_queue_end_state == TAP_TLR)
 955                 jtag_call_event_callbacks(JTAG_TRST_ASSERTED);
 956
 957         cmd_queue_cur_state = cmd_queue_end_state;
 958
 959         /* executed by sw or hw fifo */
 960         int retval=interface_jtag_add_runtest(num_cycles, cmd_queue_end_state);
 961         if (retval!=ERROR_OK)
 962                 jtag_error=retval;
 963 }
 964
 965 int jtag_add_reset(int req_trst, int req_srst)
 966 {
 967         int trst_with_tms = 0;
 968         int retval;
 969
 970         if (req_trst == -1)
 971                 req_trst = jtag_trst;
 972
 973         if (req_srst == -1)
 974                 req_srst = jtag_srst;
 975
 976         /* Make sure that jtag_reset_config allows the requested reset */
 977         /* if SRST pulls TRST, we can't fulfill srst == 1 with trst == 0 */
 978         if (((jtag_reset_config & RESET_SRST_PULLS_TRST) && (req_srst == 1)) && (req_trst == 0))
 979         {
 980                 ERROR("requested reset would assert trst");
 981                 return ERROR_JTAG_RESET_WOULD_ASSERT_TRST;
 982         }
 983
 984         /* if TRST pulls SRST, we reset with TAP T-L-R */
 985         if (((jtag_reset_config & RESET_TRST_PULLS_SRST) && (req_trst == 1)) && (req_srst == 0))
 986         {
 987                 req_trst = 0;
 988                 trst_with_tms = 1;
 989         }
 990
 991         if (req_srst && !(jtag_reset_config & RESET_HAS_SRST))
 992         {
 993                 ERROR("requested nSRST assertion, but the current configuration doesn't support this");
 994                 return ERROR_JTAG_RESET_CANT_SRST;
 995         }
 996
 997         if (req_trst && !(jtag_reset_config & RESET_HAS_TRST))
 998         {
 999                 req_trst = 0;
1000                 trst_with_tms = 1;
1001         }
1002
1003         jtag_trst = req_trst;
1004         jtag_srst = req_srst;
1005
1006         retval = interface_jtag_add_reset(req_trst, req_srst);
1007         if (retval!=ERROR_OK)
1008         {
1009                 jtag_error=retval;
1010                 return retval;
1011         }
1012
1013         if (jtag_srst)
1014         {
1015                 jtag_call_event_callbacks(JTAG_SRST_ASSERTED);
1016         }
1017         else
1018         {
1019                 jtag_call_event_callbacks(JTAG_SRST_RELEASED);
1020                 if (jtag_nsrst_delay)
1021                         jtag_add_sleep(jtag_nsrst_delay * 1000);
1022         }
1023
1024         if (trst_with_tms)
1025         {
1026                 jtag_call_event_callbacks(JTAG_TRST_ASSERTED);
1027                 jtag_add_end_state(TAP_TLR);
1028                 jtag_add_statemove(TAP_TLR);
1029                 return ERROR_OK;
1030         }
1031
1032         if (jtag_trst)
1033         {
1034                 /* we just asserted nTRST, so we're now in Test-Logic-Reset,
1035                  * and inform possible listeners about this
1036                  */
1037                 cmd_queue_cur_state = TAP_TLR;
1038                 jtag_call_event_callbacks(JTAG_TRST_ASSERTED);
1039         }
1040         else
1041         {
1042                 /* the nTRST line got deasserted, so we're still in Test-Logic-Reset,
1043                  * but we might want to add a delay to give the TAP time to settle
1044                  */
1045                 if (jtag_ntrst_delay)
1046                         jtag_add_sleep(jtag_ntrst_delay * 1000);
1047         }
1048         return ERROR_OK;
1049 }
1050
1051 int MINIDRIVER(interface_jtag_add_reset)(int req_trst, int req_srst)
1052 {
1053         jtag_command_t **last_cmd = jtag_get_last_command_p();
1054
1055         /* allocate memory for a new list member */
1056         *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
1057         (*last_cmd)->next = NULL;
1058         last_comand_pointer = &((*last_cmd)->next);
1059         (*last_cmd)->type = JTAG_RESET;
1060
1061         (*last_cmd)->cmd.reset = cmd_queue_alloc(sizeof(reset_command_t));
1062         (*last_cmd)->cmd.reset->trst = req_trst;
1063         (*last_cmd)->cmd.reset->srst = req_srst;
1064
1065
1066         return ERROR_OK;
1067 }
1068
1069 int MINIDRIVER(interface_jtag_add_end_state)(enum tap_state state)
1070 {
1071         jtag_command_t **last_cmd = jtag_get_last_command_p();
1072
1073         /* allocate memory for a new list member */
1074         *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
1075         (*last_cmd)->next = NULL;
1076         last_comand_pointer = &((*last_cmd)->next);
1077         (*last_cmd)->type = JTAG_END_STATE;
1078
1079         (*last_cmd)->cmd.end_state = cmd_queue_alloc(sizeof(end_state_command_t));
1080         (*last_cmd)->cmd.end_state->end_state = state;
1081
1082         return ERROR_OK;
1083 }
1084
1085 void jtag_add_end_state(enum tap_state state)
1086 {
1087         if (state != -1)
1088                 cmd_queue_end_state = state;
1089         int retval = interface_jtag_add_end_state(cmd_queue_end_state);
1090         if (retval!=ERROR_OK)
1091                 jtag_error=retval;
1092 }
1093
1094 int MINIDRIVER(interface_jtag_add_sleep)(u32 us)
1095 {
1096         jtag_command_t **last_cmd = jtag_get_last_command_p();
1097
1098         /* allocate memory for a new list member */
1099         *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
1100         (*last_cmd)->next = NULL;
1101         last_comand_pointer = &((*last_cmd)->next);
1102         (*last_cmd)->type = JTAG_SLEEP;
1103
1104         (*last_cmd)->cmd.sleep = cmd_queue_alloc(sizeof(sleep_command_t));
1105         (*last_cmd)->cmd.sleep->us = us;
1106
1107         return ERROR_OK;
1108 }
1109
1110 void jtag_add_sleep(u32 us)
1111 {
1112         int retval=interface_jtag_add_sleep(us);
1113         if (retval!=ERROR_OK)
1114                 jtag_error=retval;
1115         return;
1116 }
1117
1118 int jtag_scan_size(scan_command_t *cmd)
1119 {
1120         int bit_count = 0;
1121         int i;
1122
1123         /* count bits in scan command */
1124         for (i = 0; i < cmd->num_fields; i++)
1125         {
1126                 bit_count += cmd->fields[i].num_bits;
1127         }
1128
1129         return bit_count;
1130 }
1131
1132 int jtag_build_buffer(scan_command_t *cmd, u8 **buffer)
1133 {
1134         int bit_count = 0;
1135         int i;
1136
1137         bit_count = jtag_scan_size(cmd);
1138         *buffer = malloc(CEIL(bit_count, 8));
1139
1140         bit_count = 0;
1141
1142         for (i = 0; i < cmd->num_fields; i++)
1143         {
1144                 if (cmd->fields[i].out_value)
1145                 {
1146 #ifdef _DEBUG_JTAG_IO_
1147                         char* char_buf = buf_to_str(cmd->fields[i].out_value, (cmd->fields[i].num_bits > 64) ? 64 : cmd->fields[i].num_bits, 16);
1148 #endif
1149                         buf_set_buf(cmd->fields[i].out_value, 0, *buffer, bit_count, cmd->fields[i].num_bits);
1150 #ifdef _DEBUG_JTAG_IO_
1151                         DEBUG("fields[%i].out_value: 0x%s", i, char_buf);
1152                         free(char_buf);
1153 #endif
1154                 }
1155
1156                 bit_count += cmd->fields[i].num_bits;
1157         }
1158
1159         return bit_count;
1160
1161 }
1162
1163 int jtag_read_buffer(u8 *buffer, scan_command_t *cmd)
1164 {
1165         int i;
1166         int bit_count = 0;
1167         int retval;
1168
1169         /* we return ERROR_OK, unless a check fails, or a handler reports a problem */
1170         retval = ERROR_OK;
1171
1172         for (i = 0; i < cmd->num_fields; i++)
1173         {
1174                 /* if neither in_value nor in_handler
1175                  * are specified we don't have to examine this field
1176                  */
1177                 if (cmd->fields[i].in_value || cmd->fields[i].in_handler)
1178                 {
1179                         int num_bits = cmd->fields[i].num_bits;
1180                         u8 *captured = buf_set_buf(buffer, bit_count, malloc(CEIL(num_bits, 8)), 0, num_bits);
1181
1182 #ifdef _DEBUG_JTAG_IO_
1183                         char *char_buf;
1184
1185                         char_buf = buf_to_str(captured, (num_bits > 64) ? 64 : num_bits, 16);
1186                         DEBUG("fields[%i].in_value: 0x%s", i, char_buf);
1187                         free(char_buf);
1188 #endif
1189
1190                         if (cmd->fields[i].in_value)
1191                         {
1192                                 buf_cpy(captured, cmd->fields[i].in_value, num_bits);
1193
1194                                 if (cmd->fields[i].in_handler)
1195                                 {
1196                                         if (cmd->fields[i].in_handler(cmd->fields[i].in_value, cmd->fields[i].in_handler_priv, cmd->fields+i) != ERROR_OK)
1197                                         {
1198                                                 WARNING("in_handler reported a failed check");
1199                                                 retval = ERROR_JTAG_QUEUE_FAILED;
1200                                         }
1201                                 }
1202                         }
1203
1204                         /* no in_value specified, but a handler takes care of the scanned data */
1205                         if (cmd->fields[i].in_handler && (!cmd->fields[i].in_value))
1206                         {
1207                                 if (cmd->fields[i].in_handler(captured, cmd->fields[i].in_handler_priv, cmd->fields+i) != ERROR_OK)
1208                                 {
1209                                         /* We're going to call the error:handler later, but if the in_handler
1210                                          * reported an error we report this failure upstream
1211                                          */
1212                                         WARNING("in_handler reported a failed check");
1213                                         retval = ERROR_JTAG_QUEUE_FAILED;
1214                                 }
1215                         }
1216
1217                         free(captured);
1218                 }
1219                 bit_count += cmd->fields[i].num_bits;
1220         }
1221
1222         return retval;
1223 }
1224
1225 int jtag_check_value(u8 *captured, void *priv, scan_field_t *field)
1226 {
1227         int retval = ERROR_OK;
1228         int num_bits = field->num_bits;
1229
1230         int compare_failed = 0;
1231
1232         if (field->in_check_mask)
1233                 compare_failed = buf_cmp_mask(captured, field->in_check_value, field->in_check_mask, num_bits);
1234         else
1235                 compare_failed = buf_cmp(captured, field->in_check_value, num_bits);
1236
1237         if (compare_failed)
1238                 {
1239                 /* An error handler could have caught the failing check
1240                  * only report a problem when there wasn't a handler, or if the handler
1241                  * acknowledged the error
1242                  */
1243                 if (compare_failed)
1244                 {
1245                         char *captured_char = buf_to_str(captured, (num_bits > 64) ? 64 : num_bits, 16);
1246                         char *in_check_value_char = buf_to_str(field->in_check_value, (num_bits > 64) ? 64 : num_bits, 16);
1247
1248                         if (field->in_check_mask)
1249                         {
1250                                 char *in_check_mask_char;
1251                                 in_check_mask_char = buf_to_str(field->in_check_mask, (num_bits > 64) ? 64 : num_bits, 16);
1252                                 WARNING("value captured during scan didn't pass the requested check: captured: 0x%s check_value: 0x%s check_mask: 0x%s", captured_char, in_check_value_char, in_check_mask_char);
1253                                 free(in_check_mask_char);
1254                         }
1255                         else
1256                         {
1257                                 WARNING("value captured during scan didn't pass the requested check: captured: 0x%s check_value: 0x%s", captured_char, in_check_value_char);
1258                         }
1259
1260                         free(captured_char);
1261                         free(in_check_value_char);
1262
1263                         retval = ERROR_JTAG_QUEUE_FAILED;
1264                 }
1265
1266         }
1267         return retval;
1268 }
1269
1270 /*
1271   set up checking of this field using the in_handler. The values passed in must be valid until
1272   after jtag_execute() has completed.
1273  */
1274 void jtag_set_check_value(scan_field_t *field, u8 *value, u8 *mask, error_handler_t *in_error_handler)
1275 {
1276         if (value)
1277                 field->in_handler = jtag_check_value;
1278         else
1279                 field->in_handler = NULL;       /* No check, e.g. embeddedice uses value==NULL to indicate no check */
1280         field->in_handler_priv = NULL;  /* this will be filled in at the invocation site to point to the field duplicate */
1281         field->in_check_value = value;
1282         field->in_check_mask = mask;
1283 }
1284
1285 enum scan_type jtag_scan_type(scan_command_t *cmd)
1286 {
1287         int i;
1288         int type = 0;
1289
1290         for (i = 0; i < cmd->num_fields; i++)
1291         {
1292                 if (cmd->fields[i].in_value || cmd->fields[i].in_handler)
1293                         type |= SCAN_IN;
1294                 if (cmd->fields[i].out_value)
1295                         type |= SCAN_OUT;
1296         }
1297
1298         return type;
1299 }
1300
1301 int MINIDRIVER(interface_jtag_execute_queue)(void)
1302 {
1303         int retval;
1304
1305         retval = jtag->execute_queue();
1306
1307         cmd_queue_free();
1308
1309         jtag_command_queue = NULL;
1310         last_comand_pointer = &jtag_command_queue;
1311
1312         return retval;
1313 }
1314
1315 int jtag_execute_queue(void)
1316 {
1317         int retval=interface_jtag_execute_queue();
1318         if (retval==ERROR_OK)
1319         {
1320                 retval=jtag_error;
1321         }
1322         jtag_error=ERROR_OK;
1323         return retval;
1324 }
1325
1326 int jtag_reset_callback(enum jtag_event event, void *priv)
1327 {
1328         jtag_device_t *device = priv;
1329
1330         DEBUG("-");
1331
1332         if (event == JTAG_TRST_ASSERTED)
1333         {
1334                 buf_set_ones(device->cur_instr, device->ir_length);
1335                 device->bypass = 1;
1336         }
1337
1338         return ERROR_OK;
1339 }
1340
1341 void jtag_sleep(u32 us)
1342 {
1343         usleep(us);
1344 }
1345
1346 /* Try to examine chain layout according to IEEE 1149.1 §12
1347  */
1348 int jtag_examine_chain()
1349 {
1350         jtag_device_t *device = jtag_devices;
1351         scan_field_t field;
1352         u8 idcode_buffer[JTAG_MAX_CHAIN_SIZE * 4];
1353         int i;
1354         int bit_count;
1355         int device_count = 0;
1356         u8 zero_check = 0x0;
1357         u8 one_check = 0xff;
1358
1359         field.device = 0;
1360         field.num_bits = sizeof(idcode_buffer) * 8;
1361         field.out_value = idcode_buffer;
1362         field.out_mask = NULL;
1363         field.in_value = idcode_buffer;
1364         field.in_check_value = NULL;
1365         field.in_check_mask = NULL;
1366         field.in_handler = NULL;
1367         field.in_handler_priv = NULL;
1368
1369         for (i = 0; i < JTAG_MAX_CHAIN_SIZE; i++)
1370         {
1371                 buf_set_u32(idcode_buffer, i * 32, 32, 0x000000FF);
1372         }
1373
1374         jtag_add_plain_dr_scan(1, &field, TAP_TLR);
1375         jtag_execute_queue();
1376
1377         for (i = 0; i < JTAG_MAX_CHAIN_SIZE * 4; i++)
1378         {
1379                 zero_check |= idcode_buffer[i];
1380                 one_check &= idcode_buffer[i];
1381         }
1382
1383         /* if there wasn't a single non-zero bit or if all bits were one, the scan isn't valid */
1384         if ((zero_check == 0x00) || (one_check == 0xff))
1385         {
1386                 ERROR("JTAG communication failure, check connection, JTAG interface, target power etc.");
1387                 return ERROR_JTAG_INIT_FAILED;
1388         }
1389
1390         for (bit_count = 0; bit_count < (JTAG_MAX_CHAIN_SIZE * 32) - 31;)
1391         {
1392                 u32 idcode = buf_get_u32(idcode_buffer, bit_count, 32);
1393                 if ((idcode & 1) == 0)
1394                 {
1395                         /* LSB must not be 0, this indicates a device in bypass */
1396                         device_count++;
1397
1398                         bit_count += 1;
1399                 }
1400                 else
1401                 {
1402                         u32 manufacturer;
1403                         u32 part;
1404                         u32 version;
1405
1406                         if (idcode == 0x000000FF)
1407                         {
1408                                 /* End of chain (invalid manufacturer ID) */
1409                                 break;
1410                         }
1411
1412                         if (device)
1413                         {
1414                                 device->idcode = idcode;
1415                                 device = device->next;
1416                         }
1417                         device_count++;
1418
1419                         manufacturer = (idcode & 0xffe) >> 1;
1420                         part = (idcode & 0xffff000) >> 12;
1421                         version = (idcode & 0xf0000000) >> 28;
1422
1423                         INFO("JTAG device found: 0x%8.8x (Manufacturer: 0x%3.3x, Part: 0x%4.4x, Version: 0x%1.1x)",
1424                                 idcode, manufacturer, part, version);
1425
1426                         bit_count += 32;
1427                 }
1428         }
1429
1430         /* see if number of discovered devices matches configuration */
1431         if (device_count != jtag_num_devices)
1432         {
1433                 ERROR("number of discovered devices in JTAG chain (%i) doesn't match configuration (%i)",
1434                                 device_count, jtag_num_devices);
1435                 ERROR("check the config file and ensure proper JTAG communication (connections, speed, ...)");
1436                 return ERROR_JTAG_INIT_FAILED;
1437         }
1438
1439         return ERROR_OK;
1440 }
1441
1442 int jtag_validate_chain()
1443 {
1444         jtag_device_t *device = jtag_devices;
1445         int total_ir_length = 0;
1446         u8 *ir_test = NULL;
1447         scan_field_t field;
1448         int chain_pos = 0;
1449
1450         while (device)
1451         {
1452                 total_ir_length += device->ir_length;
1453                 device = device->next;
1454         }
1455
1456         total_ir_length += 2;
1457         ir_test = malloc(CEIL(total_ir_length, 8));
1458         buf_set_ones(ir_test, total_ir_length);
1459
1460         field.device = 0;
1461         field.num_bits = total_ir_length;
1462         field.out_value = ir_test;
1463         field.out_mask = NULL;
1464         field.in_value = ir_test;
1465         field.in_check_value = NULL;
1466         field.in_check_mask = NULL;
1467         field.in_handler = NULL;
1468         field.in_handler_priv = NULL;
1469
1470         jtag_add_plain_ir_scan(1, &field, TAP_TLR);
1471         jtag_execute_queue();
1472
1473         device = jtag_devices;
1474         while (device)
1475         {
1476                 if (buf_get_u32(ir_test, chain_pos, 2) != 0x1)
1477                 {
1478                         char *cbuf = buf_to_str(ir_test, total_ir_length, 16);
1479                         ERROR("Error validating JTAG scan chain, IR mismatch, scan returned 0x%s", cbuf);
1480                         free(cbuf);
1481                         free(ir_test);
1482                         return ERROR_JTAG_INIT_FAILED;
1483                 }
1484                 chain_pos += device->ir_length;
1485                 device = device->next;
1486         }
1487
1488         if (buf_get_u32(ir_test, chain_pos, 2) != 0x3)
1489         {
1490                 char *cbuf = buf_to_str(ir_test, total_ir_length, 16);
1491                 ERROR("Error validating JTAG scan chain, IR mismatch, scan returned 0x%s", cbuf);
1492                 free(cbuf);
1493                 free(ir_test);
1494                 return ERROR_JTAG_INIT_FAILED;
1495         }
1496
1497         free(ir_test);
1498
1499         return ERROR_OK;
1500 }
1501
1502 int jtag_register_commands(struct command_context_s *cmd_ctx)
1503 {
1504         register_command(cmd_ctx, NULL, "interface", handle_interface_command,
1505                 COMMAND_CONFIG, NULL);
1506         register_command(cmd_ctx, NULL, "jtag_speed", handle_jtag_speed_command,
1507                 COMMAND_ANY, "set jtag speed (if supported) <speed>");
1508         register_command(cmd_ctx, NULL, "jtag_device", handle_jtag_device_command,
1509                 COMMAND_CONFIG, "jtag_device <ir_length> <ir_expected> <ir_mask>");
1510         register_command(cmd_ctx, NULL, "reset_config", handle_reset_config_command,
1511                 COMMAND_CONFIG, NULL);
1512         register_command(cmd_ctx, NULL, "jtag_nsrst_delay", handle_jtag_nsrst_delay_command,
1513                 COMMAND_CONFIG, NULL);
1514         register_command(cmd_ctx, NULL, "jtag_ntrst_delay", handle_jtag_ntrst_delay_command,
1515                 COMMAND_CONFIG, NULL);
1516
1517         register_command(cmd_ctx, NULL, "scan_chain", handle_scan_chain_command,
1518                 COMMAND_EXEC, "print current scan chain configuration");
1519
1520         register_command(cmd_ctx, NULL, "endstate", handle_endstate_command,
1521                 COMMAND_EXEC, "finish JTAG operations in <tap_state>");
1522         register_command(cmd_ctx, NULL, "jtag_reset", handle_jtag_reset_command,
1523                 COMMAND_EXEC, "toggle reset lines <trst> <srst>");
1524         register_command(cmd_ctx, NULL, "runtest", handle_runtest_command,
1525                 COMMAND_EXEC, "move to Run-Test/Idle, and execute <num_cycles>");
1526         register_command(cmd_ctx, NULL, "statemove", handle_statemove_command,
1527                 COMMAND_EXEC, "move to current endstate or [tap_state]");
1528         register_command(cmd_ctx, NULL, "irscan", handle_irscan_command,
1529                 COMMAND_EXEC, "execute IR scan <device> <instr> [dev2] [instr2] ...");
1530         register_command(cmd_ctx, NULL, "drscan", handle_drscan_command,
1531                 COMMAND_EXEC, "execute DR scan <device> <var> [dev2] [var2] ...");
1532
1533         register_command(cmd_ctx, NULL, "verify_ircapture", handle_verify_ircapture_command,
1534                 COMMAND_ANY, "verify value captured during Capture-IR <enable|disable>");
1535         return ERROR_OK;
1536 }
1537
1538 int jtag_interface_init(struct command_context_s *cmd_ctx)
1539 {
1540         if (!jtag_interface)
1541         {
1542                 /* nothing was previously specified by "interface" command */
1543                 ERROR("JTAG interface has to be specified, see \"interface\" command");
1544                 return ERROR_JTAG_INVALID_INTERFACE;
1545         }
1546
1547         if (jtag_interface->init() != ERROR_OK)
1548                 return ERROR_JTAG_INIT_FAILED;
1549
1550         jtag = jtag_interface;
1551         return ERROR_OK;
1552 }
1553
1554 int jtag_init(struct command_context_s *cmd_ctx)
1555 {
1556         int validate_tries = 0;
1557         jtag_device_t *device;
1558
1559         DEBUG("-");
1560
1561         if (!jtag && jtag_interface_init(cmd_ctx) != ERROR_OK)
1562                 return ERROR_JTAG_INIT_FAILED;
1563
1564         device = jtag_devices;
1565         jtag_ir_scan_size = 0;
1566         jtag_num_devices = 0;
1567         while (device != NULL)
1568         {
1569                 jtag_ir_scan_size += device->ir_length;
1570                 jtag_num_devices++;
1571                 device = device->next;
1572         }
1573
1574         jtag_add_statemove(TAP_TLR);
1575         jtag_execute_queue();
1576
1577         /* examine chain first, as this could discover the real chain layout */
1578         if (jtag_examine_chain() != ERROR_OK)
1579         {
1580                 ERROR("trying to validate configured JTAG chain anyway...");
1581         }
1582
1583         while (jtag_validate_chain() != ERROR_OK)
1584         {
1585                 validate_tries++;
1586                 if (validate_tries > 5)
1587                 {
1588                         ERROR("Could not validate JTAG chain, exit");
1589                         return ERROR_JTAG_INVALID_INTERFACE;
1590                 }
1591                 usleep(10000);
1592         }
1593
1594         return ERROR_OK;
1595 }
1596
1597
1598 int handle_interface_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1599 {
1600         int i;
1601
1602         /* check whether the interface is already configured */
1603         if (jtag_interface)
1604         {
1605                 WARNING("Interface already configured, ignoring");
1606                 return ERROR_OK;
1607         }
1608
1609         /* interface name is a mandatory argument */
1610         if (argc < 1 || args[0][0] == '\0')
1611         {
1612                 return ERROR_COMMAND_SYNTAX_ERROR;
1613         }
1614
1615         for (i=0; jtag_interfaces[i]; i++)
1616         {
1617                 if (strcmp(args[0], jtag_interfaces[i]->name) == 0)
1618                 {
1619                         if (jtag_interfaces[i]->register_commands(cmd_ctx) != ERROR_OK)
1620                                 exit(-1);
1621
1622                         jtag_interface = jtag_interfaces[i];
1623                         return ERROR_OK;
1624                 }
1625         }
1626
1627         /* no valid interface was found (i.e. the configuration option,
1628          * didn't match one of the compiled-in interfaces
1629          */
1630         ERROR("No valid jtag interface found (%s)", args[0]);
1631         ERROR("compiled-in jtag interfaces:");
1632         for (i = 0; jtag_interfaces[i]; i++)
1633         {
1634                 ERROR("%i: %s", i, jtag_interfaces[i]->name);
1635         }
1636
1637         return ERROR_JTAG_INVALID_INTERFACE;
1638 }
1639
1640 int handle_jtag_device_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1641 {
1642         jtag_device_t **last_device_p = &jtag_devices;
1643
1644         if (*last_device_p)
1645         {
1646                 while ((*last_device_p)->next)
1647                         last_device_p = &((*last_device_p)->next);
1648                 last_device_p = &((*last_device_p)->next);
1649         }
1650
1651         if (argc < 3)
1652                 return ERROR_OK;
1653
1654         *last_device_p = malloc(sizeof(jtag_device_t));
1655         (*last_device_p)->ir_length = strtoul(args[0], NULL, 0);
1656
1657         (*last_device_p)->expected = malloc((*last_device_p)->ir_length);
1658         buf_set_u32((*last_device_p)->expected, 0, (*last_device_p)->ir_length, strtoul(args[1], NULL, 0));
1659         (*last_device_p)->expected_mask = malloc((*last_device_p)->ir_length);
1660         buf_set_u32((*last_device_p)->expected_mask, 0, (*last_device_p)->ir_length, strtoul(args[2], NULL, 0));
1661
1662         (*last_device_p)->cur_instr = malloc((*last_device_p)->ir_length);
1663         (*last_device_p)->bypass = 1;
1664         buf_set_ones((*last_device_p)->cur_instr, (*last_device_p)->ir_length);
1665
1666         (*last_device_p)->next = NULL;
1667
1668         jtag_register_event_callback(jtag_reset_callback, (*last_device_p));
1669
1670         jtag_num_devices++;
1671
1672         return ERROR_OK;
1673 }
1674
1675 int handle_scan_chain_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1676 {
1677         jtag_device_t *device = jtag_devices;
1678         int device_count = 0;
1679
1680         while (device)
1681         {
1682                 u32 expected, expected_mask, cur_instr;
1683                 expected = buf_get_u32(device->expected, 0, device->ir_length);
1684                 expected_mask = buf_get_u32(device->expected_mask, 0, device->ir_length);
1685                 cur_instr = buf_get_u32(device->cur_instr, 0, device->ir_length);
1686                 command_print(cmd_ctx, "%i: idcode: 0x%8.8x ir length %i, ir capture 0x%x, ir mask 0x%x, current instruction 0x%x", device_count, device->idcode, device->ir_length, expected, expected_mask, cur_instr);
1687                 device = device->next;
1688                 device_count++;
1689         }
1690
1691         return ERROR_OK;
1692 }
1693
1694 int handle_reset_config_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1695 {
1696         if (argc >= 1)
1697         {
1698                 if (strcmp(args[0], "none") == 0)
1699                         jtag_reset_config = RESET_NONE;
1700                 else if (strcmp(args[0], "trst_only") == 0)
1701                         jtag_reset_config = RESET_HAS_TRST;
1702                 else if (strcmp(args[0], "srst_only") == 0)
1703                         jtag_reset_config = RESET_HAS_SRST;
1704                 else if (strcmp(args[0], "trst_and_srst") == 0)
1705                         jtag_reset_config = RESET_TRST_AND_SRST;
1706                 else
1707                 {
1708                         ERROR("invalid reset_config argument, defaulting to none");
1709                         jtag_reset_config = RESET_NONE;
1710                         return ERROR_INVALID_ARGUMENTS;
1711                 }
1712         }
1713
1714         if (argc >= 2)
1715         {
1716                 if (strcmp(args[1], "srst_pulls_trst") == 0)
1717                         jtag_reset_config |= RESET_SRST_PULLS_TRST;
1718                 else if (strcmp(args[1], "trst_pulls_srst") == 0)
1719                         jtag_reset_config |= RESET_TRST_PULLS_SRST;
1720                 else if (strcmp(args[1], "combined") == 0)
1721                         jtag_reset_config |= RESET_SRST_PULLS_TRST | RESET_TRST_PULLS_SRST;
1722                 else if (strcmp(args[1], "separate") == 0)
1723                         jtag_reset_config &= ~(RESET_SRST_PULLS_TRST | RESET_TRST_PULLS_SRST);
1724                 else
1725                 {
1726                         ERROR("invalid reset_config argument, defaulting to none");
1727                         jtag_reset_config = RESET_NONE;
1728                         return ERROR_INVALID_ARGUMENTS;
1729                 }
1730         }
1731
1732         if (argc >= 3)
1733         {
1734                 if (strcmp(args[2], "trst_open_drain") == 0)
1735                         jtag_reset_config |= RESET_TRST_OPEN_DRAIN;
1736                 else if (strcmp(args[2], "trst_push_pull") == 0)
1737                         jtag_reset_config &= ~RESET_TRST_OPEN_DRAIN;
1738                 else
1739                 {
1740                         ERROR("invalid reset_config argument, defaulting to none");
1741                         jtag_reset_config = RESET_NONE;
1742                         return ERROR_INVALID_ARGUMENTS;
1743                 }
1744         }
1745
1746         if (argc >= 4)
1747         {
1748                 if (strcmp(args[3], "srst_push_pull") == 0)
1749                         jtag_reset_config |= RESET_SRST_PUSH_PULL;
1750                 else if (strcmp(args[3], "srst_open_drain") == 0)
1751                         jtag_reset_config &= ~RESET_SRST_PUSH_PULL;
1752                 else
1753                 {
1754                         ERROR("invalid reset_config argument, defaulting to none");
1755                         jtag_reset_config = RESET_NONE;
1756                         return ERROR_INVALID_ARGUMENTS;
1757                 }
1758         }
1759
1760         return ERROR_OK;
1761 }
1762
1763 int handle_jtag_nsrst_delay_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1764 {
1765         if (argc < 1)
1766         {
1767                 ERROR("jtag_nsrst_delay <ms> command takes one required argument");
1768                 exit(-1);
1769         }
1770         else
1771         {
1772                 jtag_nsrst_delay = strtoul(args[0], NULL, 0);
1773         }
1774
1775         return ERROR_OK;
1776 }
1777
1778 int handle_jtag_ntrst_delay_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1779 {
1780         if (argc < 1)
1781         {
1782                 ERROR("jtag_ntrst_delay <ms> command takes one required argument");
1783                 exit(-1);
1784         }
1785         else
1786         {
1787                 jtag_ntrst_delay = strtoul(args[0], NULL, 0);
1788         }
1789
1790         return ERROR_OK;
1791 }
1792
1793 int handle_jtag_speed_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1794 {
1795         if (argc == 0)
1796                 command_print(cmd_ctx, "jtag_speed: %i", jtag_speed);
1797
1798         if (argc > 0)
1799         {
1800                 jtag_speed = strtoul(args[0], NULL, 0);
1801                 /* this command can be called during CONFIG,
1802                  * in which case jtag isn't initialized */
1803                 if (jtag)
1804                         jtag->speed(jtag_speed);
1805         }
1806
1807         return ERROR_OK;
1808 }
1809
1810 int handle_endstate_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1811 {
1812         enum tap_state state;
1813
1814         if (argc < 1)
1815         {
1816                 return ERROR_COMMAND_SYNTAX_ERROR;
1817         }
1818         else
1819         {
1820                 for (state = 0; state < 16; state++)
1821                 {
1822                         if (strcmp(args[0], tap_state_strings[state]) == 0)
1823                         {
1824                                 jtag_add_end_state(state);
1825                                 jtag_execute_queue();
1826                         }
1827                 }
1828         }
1829         command_print(cmd_ctx, "current endstate: %s", tap_state_strings[end_state]);
1830
1831         return ERROR_OK;
1832 }
1833
1834 int handle_jtag_reset_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1835 {
1836         int trst = -1;
1837         int srst = -1;
1838
1839         if (argc < 2)
1840         {
1841                 return ERROR_COMMAND_SYNTAX_ERROR;
1842         }
1843
1844         if (args[0][0] == '1')
1845                 trst = 1;
1846         else if (args[0][0] == '0')
1847                 trst = 0;
1848         else
1849         {
1850                 return ERROR_COMMAND_SYNTAX_ERROR;
1851         }
1852
1853         if (args[1][0] == '1')
1854                 srst = 1;
1855         else if (args[1][0] == '0')
1856                 srst = 0;
1857         else
1858         {
1859                 return ERROR_COMMAND_SYNTAX_ERROR;
1860         }
1861
1862         if (!jtag && jtag_interface_init(cmd_ctx) != ERROR_OK)
1863                 return ERROR_JTAG_INIT_FAILED;
1864
1865         jtag_add_reset(trst, srst);
1866         jtag_execute_queue();
1867
1868         return ERROR_OK;
1869 }
1870
1871 int handle_runtest_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1872 {
1873         if (argc < 1)
1874         {
1875                 return ERROR_COMMAND_SYNTAX_ERROR;
1876         }
1877
1878         jtag_add_runtest(strtol(args[0], NULL, 0), -1);
1879         jtag_execute_queue();
1880
1881         return ERROR_OK;
1882
1883 }
1884
1885 int handle_statemove_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1886 {
1887         enum tap_state state;
1888
1889         state = -1;
1890         if (argc == 1)
1891         {
1892                 for (state = 0; state < 16; state++)
1893                 {
1894                         if (strcmp(args[0], tap_state_strings[state]) == 0)
1895                         {
1896                                 break;
1897                         }
1898                 }
1899         }
1900
1901         jtag_add_statemove(state);
1902         jtag_execute_queue();
1903
1904         return ERROR_OK;
1905
1906 }
1907
1908 int handle_irscan_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1909 {
1910         int i;
1911         scan_field_t *fields;
1912
1913         if ((argc < 2) || (argc % 2))
1914         {
1915                 return ERROR_COMMAND_SYNTAX_ERROR;
1916         }
1917
1918         fields = malloc(sizeof(scan_field_t) * argc / 2);
1919
1920         for (i = 0; i < argc / 2; i++)
1921         {
1922                 int device = strtoul(args[i*2], NULL, 0);
1923                 int field_size = jtag_get_device(device)->ir_length;
1924                 fields[i].device = device;
1925                 fields[i].out_value = malloc(CEIL(field_size, 8));
1926                 buf_set_u32(fields[i].out_value, 0, field_size, strtoul(args[i*2+1], NULL, 0));
1927                 fields[i].out_mask = NULL;
1928                 fields[i].in_value = NULL;
1929                 fields[i].in_check_mask = NULL;
1930                 fields[i].in_handler = NULL;
1931                 fields[i].in_handler_priv = NULL;
1932         }
1933
1934         jtag_add_ir_scan(argc / 2, fields, -1);
1935         jtag_execute_queue();
1936
1937         for (i = 0; i < argc / 2; i++)
1938                 free(fields[i].out_value);
1939
1940         free (fields);
1941
1942         return ERROR_OK;
1943 }
1944
1945 int handle_drscan_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1946 {
1947         scan_field_t *fields;
1948         int num_fields = 0;
1949         int field_count = 0;
1950         var_t *var;
1951         int i, j;
1952
1953         if ((argc < 2) || (argc % 2))
1954         {
1955                 return ERROR_COMMAND_SYNTAX_ERROR;
1956         }
1957
1958         for (i = 0; i < argc; i+=2)
1959         {
1960                 var = get_var_by_namenum(args[i+1]);
1961                 if (var)
1962                 {
1963                         num_fields += var->num_fields;
1964                 }
1965                 else
1966                 {
1967                         command_print(cmd_ctx, "variable %s doesn't exist", args[i+1]);
1968                         return ERROR_OK;
1969                 }
1970         }
1971
1972         fields = malloc(sizeof(scan_field_t) * num_fields);
1973
1974         for (i = 0; i < argc; i+=2)
1975         {
1976                 var = get_var_by_namenum(args[i+1]);
1977
1978                 for (j = 0; j < var->num_fields; j++)
1979                 {
1980                         fields[field_count].device = strtol(args[i], NULL, 0);
1981                         fields[field_count].num_bits = var->fields[j].num_bits;
1982                         fields[field_count].out_value = malloc(CEIL(var->fields[j].num_bits, 8));
1983                         buf_set_u32(fields[field_count].out_value, 0, var->fields[j].num_bits, var->fields[j].value);
1984                         fields[field_count].out_mask = NULL;
1985                         fields[field_count].in_value = fields[field_count].out_value;
1986                         fields[field_count].in_check_mask = NULL;
1987                         fields[field_count].in_check_value = NULL;
1988                         fields[field_count].in_handler = field_le_to_host;
1989                         fields[field_count++].in_handler_priv = &(var->fields[j]);
1990                 }
1991         }
1992
1993         jtag_add_dr_scan(num_fields, fields, -1);
1994         jtag_execute_queue();
1995
1996         for (i = 0; i < argc / 2; i++)
1997                 free(fields[i].out_value);
1998
1999         free(fields);
2000
2001         return ERROR_OK;
2002 }
2003
2004 int handle_verify_ircapture_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2005 {
2006         if (argc == 1)
2007         {
2008                 if (strcmp(args[0], "enable") == 0)
2009                 {
2010                         jtag_verify_capture_ir = 1;
2011                 }
2012                 else if (strcmp(args[0], "disable") == 0)
2013                 {
2014                         jtag_verify_capture_ir = 0;
2015                 } else
2016                 {
2017                         return ERROR_COMMAND_SYNTAX_ERROR;
2018                 }
2019         } else if (argc != 0)
2020         {
2021                 return ERROR_COMMAND_SYNTAX_ERROR;
2022         }
2023
2024         command_print(cmd_ctx, "verify Capture-IR is %s", (jtag_verify_capture_ir) ? "enabled": "disabled");
2025
2026         return ERROR_OK;
2027 }