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