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

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