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

   1 /* SPDX-License-Identifier: GPL-2.0-or-later */
   2
   3 /***************************************************************************
   4  *    Copyright (C) 2009 by Simon Qian                                     *
   5  *    SimonQian@SimonQian.com                                              *
   6  ***************************************************************************/
   7
   8 /* The specification for SVF is available here:
   9  * http://www.asset-intertech.com/support/svf.pdf
  10  * Below, this document is referred to as the "SVF spec".
  11  *
  12  * The specification for XSVF is available here:
  13  * http://www.xilinx.com/support/documentation/application_notes/xapp503.pdf
  14  * Below, this document is referred to as the "XSVF spec".
  15  */
  16
  17 #ifdef HAVE_CONFIG_H
  18 #include "config.h"
  19 #endif
  20
  21 #include <jtag/jtag.h>
  22 #include "svf.h"
  23 #include "helper/system.h"
  24 #include <helper/time_support.h>
  25
  26 /* SVF command */
  27 enum svf_command {
  28         ENDDR,
  29         ENDIR,
  30         FREQUENCY,
  31         HDR,
  32         HIR,
  33         PIO,
  34         PIOMAP,
  35         RUNTEST,
  36         SDR,
  37         SIR,
  38         STATE,
  39         TDR,
  40         TIR,
  41         TRST,
  42 };
  43
  44 static const char *svf_command_name[14] = {
  45         "ENDDR",
  46         "ENDIR",
  47         "FREQUENCY",
  48         "HDR",
  49         "HIR",
  50         "PIO",
  51         "PIOMAP",
  52         "RUNTEST",
  53         "SDR",
  54         "SIR",
  55         "STATE",
  56         "TDR",
  57         "TIR",
  58         "TRST"
  59 };
  60
  61 enum trst_mode {
  62         TRST_ON,
  63         TRST_OFF,
  64         TRST_Z,
  65         TRST_ABSENT
  66 };
  67
  68 static const char *svf_trst_mode_name[4] = {
  69         "ON",
  70         "OFF",
  71         "Z",
  72         "ABSENT"
  73 };
  74
  75 struct svf_statemove {
  76         tap_state_t from;
  77         tap_state_t to;
  78         uint32_t num_of_moves;
  79         tap_state_t paths[8];
  80 };
  81
  82 /*
  83  * These paths are from the SVF specification for the STATE command, to be
  84  * used when the STATE command only includes the final state.  The first
  85  * element of the path is the "from" (current) state, and the last one is
  86  * the "to" (target) state.
  87  *
  88  * All specified paths are the shortest ones in the JTAG spec, and are thus
  89  * not (!!) exact matches for the paths used elsewhere in OpenOCD.  Note
  90  * that PAUSE-to-PAUSE transitions all go through UPDATE and then CAPTURE,
  91  * which has specific effects on the various registers; they are not NOPs.
  92  *
  93  * Paths to RESET are disabled here.  As elsewhere in OpenOCD, and in XSVF
  94  * and many SVF implementations, we don't want to risk missing that state.
  95  * To get to RESET, always we ignore the current state.
  96  */
  97 static const struct svf_statemove svf_statemoves[] = {
  98         /* from                 to                              num_of_moves,   paths[8] */
  99 /*      {TAP_RESET,             TAP_RESET,              1,                              {TAP_RESET}}, */
 100         {TAP_RESET,             TAP_IDLE,               2,                              {TAP_RESET, TAP_IDLE} },
 101         {TAP_RESET,             TAP_DRPAUSE,    6,                              {TAP_RESET, TAP_IDLE, TAP_DRSELECT,
 102                                                                                                                 TAP_DRCAPTURE, TAP_DREXIT1, TAP_DRPAUSE} },
 103         {TAP_RESET,             TAP_IRPAUSE,    7,                              {TAP_RESET, TAP_IDLE, TAP_DRSELECT,
 104                                                                                                                 TAP_IRSELECT, TAP_IRCAPTURE,
 105                                                                                                                 TAP_IREXIT1, TAP_IRPAUSE} },
 106
 107 /*      {TAP_IDLE,              TAP_RESET,              4,                              {TAP_IDLE,
 108  * TAP_DRSELECT, TAP_IRSELECT, TAP_RESET}}, */
 109         {TAP_IDLE,              TAP_IDLE,               1,                              {TAP_IDLE} },
 110         {TAP_IDLE,              TAP_DRPAUSE,    5,                              {TAP_IDLE, TAP_DRSELECT, TAP_DRCAPTURE,
 111                                                                                                                 TAP_DREXIT1, TAP_DRPAUSE} },
 112         {TAP_IDLE,              TAP_IRPAUSE,    6,                              {TAP_IDLE, TAP_DRSELECT, TAP_IRSELECT,
 113                                                                                                                 TAP_IRCAPTURE, TAP_IREXIT1, TAP_IRPAUSE} },
 114
 115 /*      {TAP_DRPAUSE,   TAP_RESET,              6,                              {TAP_DRPAUSE,
 116  * TAP_DREXIT2, TAP_DRUPDATE, TAP_DRSELECT, TAP_IRSELECT, TAP_RESET}}, */
 117         {TAP_DRPAUSE,   TAP_IDLE,               4,                              {TAP_DRPAUSE, TAP_DREXIT2, TAP_DRUPDATE,
 118                                                                                                                 TAP_IDLE} },
 119         {TAP_DRPAUSE,   TAP_DRPAUSE,    7,                              {TAP_DRPAUSE, TAP_DREXIT2, TAP_DRUPDATE,
 120                                                                                                                 TAP_DRSELECT, TAP_DRCAPTURE,
 121                                                                                                                 TAP_DREXIT1, TAP_DRPAUSE} },
 122         {TAP_DRPAUSE,   TAP_IRPAUSE,    8,                              {TAP_DRPAUSE, TAP_DREXIT2, TAP_DRUPDATE,
 123                                                                                                                 TAP_DRSELECT, TAP_IRSELECT,
 124                                                                                                                 TAP_IRCAPTURE, TAP_IREXIT1, TAP_IRPAUSE} },
 125
 126 /*      {TAP_IRPAUSE,   TAP_RESET,              6,                              {TAP_IRPAUSE,
 127  * TAP_IREXIT2, TAP_IRUPDATE, TAP_DRSELECT, TAP_IRSELECT, TAP_RESET}}, */
 128         {TAP_IRPAUSE,   TAP_IDLE,               4,                              {TAP_IRPAUSE, TAP_IREXIT2, TAP_IRUPDATE,
 129                                                                                                                 TAP_IDLE} },
 130         {TAP_IRPAUSE,   TAP_DRPAUSE,    7,                              {TAP_IRPAUSE, TAP_IREXIT2, TAP_IRUPDATE,
 131                                                                                                                 TAP_DRSELECT, TAP_DRCAPTURE,
 132                                                                                                                 TAP_DREXIT1, TAP_DRPAUSE} },
 133         {TAP_IRPAUSE,   TAP_IRPAUSE,    8,                              {TAP_IRPAUSE, TAP_IREXIT2, TAP_IRUPDATE,
 134                                                                                                                 TAP_DRSELECT, TAP_IRSELECT,
 135                                                                                                                 TAP_IRCAPTURE, TAP_IREXIT1, TAP_IRPAUSE} }
 136 };
 137
 138 #define XXR_TDI                         (1 << 0)
 139 #define XXR_TDO                         (1 << 1)
 140 #define XXR_MASK                        (1 << 2)
 141 #define XXR_SMASK                       (1 << 3)
 142 struct svf_xxr_para {
 143         int len;
 144         int data_mask;
 145         uint8_t *tdi;
 146         uint8_t *tdo;
 147         uint8_t *mask;
 148         uint8_t *smask;
 149 };
 150
 151 struct svf_para {
 152         float frequency;
 153         tap_state_t ir_end_state;
 154         tap_state_t dr_end_state;
 155         tap_state_t runtest_run_state;
 156         tap_state_t runtest_end_state;
 157         enum trst_mode trst_mode;
 158
 159         struct svf_xxr_para hir_para;
 160         struct svf_xxr_para hdr_para;
 161         struct svf_xxr_para tir_para;
 162         struct svf_xxr_para tdr_para;
 163         struct svf_xxr_para sir_para;
 164         struct svf_xxr_para sdr_para;
 165 };
 166
 167 static struct svf_para svf_para;
 168 static const struct svf_para svf_para_init = {
 169 /*      frequency, ir_end_state, dr_end_state, runtest_run_state, runtest_end_state, trst_mode */
 170         0,                      TAP_IDLE,               TAP_IDLE,       TAP_IDLE,               TAP_IDLE,               TRST_Z,
 171 /*      hir_para */
 172 /*      {len,   data_mask,      tdi,    tdo,    mask,   smask}, */
 173         {0,                     0,              NULL,   NULL,   NULL,   NULL},
 174 /*      hdr_para */
 175 /*      {len,   data_mask,      tdi,    tdo,    mask,   smask}, */
 176         {0,                     0,              NULL,   NULL,   NULL,   NULL},
 177 /*      tir_para */
 178 /*      {len,   data_mask,      tdi,    tdo,    mask,   smask}, */
 179         {0,                     0,              NULL,   NULL,   NULL,   NULL},
 180 /*      tdr_para */
 181 /*      {len,   data_mask,      tdi,    tdo,    mask,   smask}, */
 182         {0,                     0,              NULL,   NULL,   NULL,   NULL},
 183 /*      sir_para */
 184 /*      {len,   data_mask,      tdi,    tdo,    mask,   smask}, */
 185         {0,                     0,              NULL,   NULL,   NULL,   NULL},
 186 /*      sdr_para */
 187 /*      {len,   data_mask,      tdi,    tdo,    mask,   smask}, */
 188         {0,                     0,              NULL,   NULL,   NULL,   NULL},
 189 };
 190
 191 struct svf_check_tdo_para {
 192         int line_num;           /* used to record line number of the check operation */
 193         /* so more information could be printed */
 194         int enabled;            /* check is enabled or not */
 195         int buffer_offset;      /* buffer_offset to buffers */
 196         int bit_len;            /* bit length to check */
 197 };
 198
 199 #define SVF_CHECK_TDO_PARA_SIZE 1024
 200 static struct svf_check_tdo_para *svf_check_tdo_para;
 201 static int svf_check_tdo_para_index;
 202
 203 static int svf_read_command_from_file(FILE *fd);
 204 static int svf_check_tdo(void);
 205 static int svf_add_check_para(uint8_t enabled, int buffer_offset, int bit_len);
 206 static int svf_run_command(struct command_context *cmd_ctx, char *cmd_str);
 207 static int svf_execute_tap(void);
 208
 209 static FILE *svf_fd;
 210 static char *svf_read_line;
 211 static size_t svf_read_line_size;
 212 static char *svf_command_buffer;
 213 static size_t svf_command_buffer_size;
 214 static int svf_line_number;
 215 static int svf_getline(char **lineptr, size_t *n, FILE *stream);
 216
 217 #define SVF_MAX_BUFFER_SIZE_TO_COMMIT   (1024 * 1024)
 218 static uint8_t *svf_tdi_buffer, *svf_tdo_buffer, *svf_mask_buffer;
 219 static int svf_buffer_index, svf_buffer_size;
 220 static int svf_quiet;
 221 static int svf_nil;
 222 static int svf_ignore_error;
 223
 224 /* Targeting particular tap */
 225 static int svf_tap_is_specified;
 226 static int svf_set_padding(struct svf_xxr_para *para, int len, unsigned char tdi);
 227
 228 /* Progress Indicator */
 229 static int svf_progress_enabled;
 230 static long svf_total_lines;
 231 static int svf_percentage;
 232 static int svf_last_printed_percentage = -1;
 233
 234 /*
 235  * macro is used to print the svf hex buffer at desired debug level
 236  * DEBUG, INFO, ERROR, USER
 237  */
 238 #define SVF_BUF_LOG(_lvl, _buf, _nbits, _desc)                                                  \
 239         svf_hexbuf_print(LOG_LVL_##_lvl,  __FILE__, __LINE__, __func__, _buf, _nbits, _desc)
 240
 241 static void svf_hexbuf_print(int dbg_lvl, const char *file, unsigned line,
 242                                                          const char *function, const uint8_t *buf,
 243                                                          int bit_len, const char *desc)
 244 {
 245         int j, len = 0;
 246         int byte_len = DIV_ROUND_UP(bit_len, 8);
 247         int msbits = bit_len % 8;
 248
 249         /* allocate 2 bytes per hex digit */
 250         char *prbuf = malloc((byte_len * 2) + 2 + 1);
 251         if (!prbuf)
 252                 return;
 253
 254         /* print correct number of bytes, mask excess bits where applicable */
 255         uint8_t msb = buf[byte_len - 1] & (msbits ? (1 << msbits) - 1 : 0xff);
 256         len = sprintf(prbuf, msbits <= 4 ? "0x%01"PRIx8 : "0x%02"PRIx8, msb);
 257         for (j = byte_len - 2; j >= 0; j--)
 258                 len += sprintf(prbuf + len, "%02"PRIx8, buf[j]);
 259
 260         log_printf_lf(dbg_lvl, file, line, function, "%8s = %s", desc ? desc : " ", prbuf);
 261
 262         free(prbuf);
 263 }
 264
 265 static int svf_realloc_buffers(size_t len)
 266 {
 267         void *ptr;
 268
 269         if (svf_execute_tap() != ERROR_OK)
 270                 return ERROR_FAIL;
 271
 272         ptr = realloc(svf_tdi_buffer, len);
 273         if (!ptr)
 274                 return ERROR_FAIL;
 275         svf_tdi_buffer = ptr;
 276
 277         ptr = realloc(svf_tdo_buffer, len);
 278         if (!ptr)
 279                 return ERROR_FAIL;
 280         svf_tdo_buffer = ptr;
 281
 282         ptr = realloc(svf_mask_buffer, len);
 283         if (!ptr)
 284                 return ERROR_FAIL;
 285         svf_mask_buffer = ptr;
 286
 287         svf_buffer_size = len;
 288
 289         return ERROR_OK;
 290 }
 291
 292 static void svf_free_xxd_para(struct svf_xxr_para *para)
 293 {
 294         if (para) {
 295                 free(para->tdi);
 296                 para->tdi = NULL;
 297
 298                 free(para->tdo);
 299                 para->tdo = NULL;
 300
 301                 free(para->mask);
 302                 para->mask = NULL;
 303
 304                 free(para->smask);
 305                 para->smask = NULL;
 306         }
 307 }
 308
 309 int svf_add_statemove(tap_state_t state_to)
 310 {
 311         tap_state_t state_from = cmd_queue_cur_state;
 312         unsigned index_var;
 313
 314         /* when resetting, be paranoid and ignore current state */
 315         if (state_to == TAP_RESET) {
 316                 if (svf_nil)
 317                         return ERROR_OK;
 318
 319                 jtag_add_tlr();
 320                 return ERROR_OK;
 321         }
 322
 323         for (index_var = 0; index_var < ARRAY_SIZE(svf_statemoves); index_var++) {
 324                 if ((svf_statemoves[index_var].from == state_from)
 325                                 && (svf_statemoves[index_var].to == state_to)) {
 326                         if (svf_nil)
 327                                 continue;
 328                                                 /* recorded path includes current state ... avoid
 329                                                  *extra TCKs! */
 330                         if (svf_statemoves[index_var].num_of_moves > 1)
 331                                 jtag_add_pathmove(svf_statemoves[index_var].num_of_moves - 1,
 332                                         svf_statemoves[index_var].paths + 1);
 333                         else
 334                                 jtag_add_pathmove(svf_statemoves[index_var].num_of_moves,
 335                                         svf_statemoves[index_var].paths);
 336                         return ERROR_OK;
 337                 }
 338         }
 339         LOG_ERROR("SVF: can not move to %s", tap_state_name(state_to));
 340         return ERROR_FAIL;
 341 }
 342
 343 COMMAND_HANDLER(handle_svf_command)
 344 {
 345 #define SVF_MIN_NUM_OF_OPTIONS 1
 346 #define SVF_MAX_NUM_OF_OPTIONS 5
 347         int command_num = 0;
 348         int ret = ERROR_OK;
 349         int64_t time_measure_ms;
 350         int time_measure_s, time_measure_m;
 351
 352         /* use NULL to indicate a "plain" svf file which accounts for
 353          * any additional devices in the scan chain, otherwise the device
 354          * that should be affected
 355         */
 356         struct jtag_tap *tap = NULL;
 357
 358         if ((CMD_ARGC < SVF_MIN_NUM_OF_OPTIONS) || (CMD_ARGC > SVF_MAX_NUM_OF_OPTIONS))
 359                 return ERROR_COMMAND_SYNTAX_ERROR;
 360
 361         /* parse command line */
 362         svf_quiet = 0;
 363         svf_nil = 0;
 364         svf_progress_enabled = 0;
 365         svf_ignore_error = 0;
 366         for (unsigned int i = 0; i < CMD_ARGC; i++) {
 367                 if (strcmp(CMD_ARGV[i], "-tap") == 0) {
 368                         tap = jtag_tap_by_string(CMD_ARGV[i+1]);
 369                         if (!tap) {
 370                                 command_print(CMD, "Tap: %s unknown", CMD_ARGV[i+1]);
 371                                 return ERROR_FAIL;
 372                         }
 373                         i++;
 374                 } else if ((strcmp(CMD_ARGV[i],
 375                                 "quiet") == 0) || (strcmp(CMD_ARGV[i], "-quiet") == 0))
 376                         svf_quiet = 1;
 377                 else if ((strcmp(CMD_ARGV[i], "nil") == 0) || (strcmp(CMD_ARGV[i], "-nil") == 0))
 378                         svf_nil = 1;
 379                 else if ((strcmp(CMD_ARGV[i],
 380                                   "progress") == 0) || (strcmp(CMD_ARGV[i], "-progress") == 0))
 381                         svf_progress_enabled = 1;
 382                 else if ((strcmp(CMD_ARGV[i],
 383                                   "ignore_error") == 0) || (strcmp(CMD_ARGV[i], "-ignore_error") == 0))
 384                         svf_ignore_error = 1;
 385                 else {
 386                         svf_fd = fopen(CMD_ARGV[i], "r");
 387                         if (!svf_fd) {
 388                                 int err = errno;
 389                                 command_print(CMD, "open(\"%s\"): %s", CMD_ARGV[i], strerror(err));
 390                                 /* no need to free anything now */
 391                                 return ERROR_COMMAND_SYNTAX_ERROR;
 392                         } else
 393                                 LOG_USER("svf processing file: \"%s\"", CMD_ARGV[i]);
 394                 }
 395         }
 396
 397         if (!svf_fd)
 398                 return ERROR_COMMAND_SYNTAX_ERROR;
 399
 400         /* get time */
 401         time_measure_ms = timeval_ms();
 402
 403         /* init */
 404         svf_line_number = 0;
 405         svf_command_buffer_size = 0;
 406
 407         svf_check_tdo_para_index = 0;
 408         svf_check_tdo_para = malloc(sizeof(struct svf_check_tdo_para) * SVF_CHECK_TDO_PARA_SIZE);
 409         if (!svf_check_tdo_para) {
 410                 LOG_ERROR("not enough memory");
 411                 ret = ERROR_FAIL;
 412                 goto free_all;
 413         }
 414
 415         svf_buffer_index = 0;
 416         /* double the buffer size */
 417         /* in case current command cannot be committed, and next command is a bit scan command */
 418         /* here is 32K bits for this big scan command, it should be enough */
 419         /* buffer will be reallocated if buffer size is not enough */
 420         if (svf_realloc_buffers(2 * SVF_MAX_BUFFER_SIZE_TO_COMMIT) != ERROR_OK) {
 421                 ret = ERROR_FAIL;
 422                 goto free_all;
 423         }
 424
 425         memcpy(&svf_para, &svf_para_init, sizeof(svf_para));
 426
 427         if (!svf_nil) {
 428                 /* TAP_RESET */
 429                 jtag_add_tlr();
 430         }
 431
 432         if (tap) {
 433                 /* Tap is specified, set header/trailer paddings */
 434                 int header_ir_len = 0, header_dr_len = 0, trailer_ir_len = 0, trailer_dr_len = 0;
 435                 struct jtag_tap *check_tap;
 436
 437                 svf_tap_is_specified = 1;
 438
 439                 for (check_tap = jtag_all_taps(); check_tap; check_tap = check_tap->next_tap) {
 440                         if (check_tap->abs_chain_position < tap->abs_chain_position) {
 441                                 /* Header */
 442                                 header_ir_len += check_tap->ir_length;
 443                                 header_dr_len++;
 444                         } else if (check_tap->abs_chain_position > tap->abs_chain_position) {
 445                                 /* Trailer */
 446                                 trailer_ir_len += check_tap->ir_length;
 447                                 trailer_dr_len++;
 448                         }
 449                 }
 450
 451                 /* HDR %d TDI (0) */
 452                 if (svf_set_padding(&svf_para.hdr_para, header_dr_len, 0) != ERROR_OK) {
 453                         LOG_ERROR("failed to set data header");
 454                         return ERROR_FAIL;
 455                 }
 456
 457                 /* HIR %d TDI (0xFF) */
 458                 if (svf_set_padding(&svf_para.hir_para, header_ir_len, 0xFF) != ERROR_OK) {
 459                         LOG_ERROR("failed to set instruction header");
 460                         return ERROR_FAIL;
 461                 }
 462
 463                 /* TDR %d TDI (0) */
 464                 if (svf_set_padding(&svf_para.tdr_para, trailer_dr_len, 0) != ERROR_OK) {
 465                         LOG_ERROR("failed to set data trailer");
 466                         return ERROR_FAIL;
 467                 }
 468
 469                 /* TIR %d TDI (0xFF) */
 470                 if (svf_set_padding(&svf_para.tir_para, trailer_ir_len, 0xFF) != ERROR_OK) {
 471                         LOG_ERROR("failed to set instruction trailer");
 472                         return ERROR_FAIL;
 473                 }
 474         }
 475
 476         if (svf_progress_enabled) {
 477                 /* Count total lines in file. */
 478                 while (!feof(svf_fd)) {
 479                         svf_getline(&svf_command_buffer, &svf_command_buffer_size, svf_fd);
 480                         svf_total_lines++;
 481                 }
 482                 rewind(svf_fd);
 483         }
 484         while (svf_read_command_from_file(svf_fd) == ERROR_OK) {
 485                 /* Log Output */
 486                 if (svf_quiet) {
 487                         if (svf_progress_enabled) {
 488                                 svf_percentage = ((svf_line_number * 20) / svf_total_lines) * 5;
 489                                 if (svf_last_printed_percentage != svf_percentage) {
 490                                         LOG_USER_N("\r%d%%    ", svf_percentage);
 491                                         svf_last_printed_percentage = svf_percentage;
 492                                 }
 493                         }
 494                 } else {
 495                         if (svf_progress_enabled) {
 496                                 svf_percentage = ((svf_line_number * 20) / svf_total_lines) * 5;
 497                                 LOG_USER_N("%3d%%  %s", svf_percentage, svf_read_line);
 498                         } else
 499                                 LOG_USER_N("%s", svf_read_line);
 500                 }
 501                 /* Run Command */
 502                 if (svf_run_command(CMD_CTX, svf_command_buffer) != ERROR_OK) {
 503                         LOG_ERROR("fail to run command at line %d", svf_line_number);
 504                         ret = ERROR_FAIL;
 505                         break;
 506                 }
 507                 command_num++;
 508         }
 509
 510         if ((!svf_nil) && (jtag_execute_queue() != ERROR_OK))
 511                 ret = ERROR_FAIL;
 512         else if (svf_check_tdo() != ERROR_OK)
 513                 ret = ERROR_FAIL;
 514
 515         /* print time */
 516         time_measure_ms = timeval_ms() - time_measure_ms;
 517         time_measure_s = time_measure_ms / 1000;
 518         time_measure_ms %= 1000;
 519         time_measure_m = time_measure_s / 60;
 520         time_measure_s %= 60;
 521         if (time_measure_ms < 1000)
 522                 command_print(CMD,
 523                         "\r\nTime used: %dm%ds%" PRId64 "ms ",
 524                         time_measure_m,
 525                         time_measure_s,
 526                         time_measure_ms);
 527
 528 free_all:
 529
 530         fclose(svf_fd);
 531         svf_fd = 0;
 532
 533         /* free buffers */
 534         free(svf_command_buffer);
 535         svf_command_buffer = NULL;
 536         svf_command_buffer_size = 0;
 537
 538         free(svf_check_tdo_para);
 539         svf_check_tdo_para = NULL;
 540         svf_check_tdo_para_index = 0;
 541
 542         free(svf_tdi_buffer);
 543         svf_tdi_buffer = NULL;
 544
 545         free(svf_tdo_buffer);
 546         svf_tdo_buffer = NULL;
 547
 548         free(svf_mask_buffer);
 549         svf_mask_buffer = NULL;
 550
 551         svf_buffer_index = 0;
 552         svf_buffer_size = 0;
 553
 554         svf_free_xxd_para(&svf_para.hdr_para);
 555         svf_free_xxd_para(&svf_para.hir_para);
 556         svf_free_xxd_para(&svf_para.tdr_para);
 557         svf_free_xxd_para(&svf_para.tir_para);
 558         svf_free_xxd_para(&svf_para.sdr_para);
 559         svf_free_xxd_para(&svf_para.sir_para);
 560
 561         if (ret == ERROR_OK)
 562                 command_print(CMD,
 563                               "svf file programmed %s for %d commands with %d errors",
 564                               (svf_ignore_error > 1) ? "unsuccessfully" : "successfully",
 565                               command_num,
 566                               (svf_ignore_error > 1) ? (svf_ignore_error - 1) : 0);
 567         else
 568                 command_print(CMD, "svf file programmed failed");
 569
 570         svf_ignore_error = 0;
 571         return ret;
 572 }
 573
 574 static int svf_getline(char **lineptr, size_t *n, FILE *stream)
 575 {
 576 #define MIN_CHUNK 16    /* Buffer is increased by this size each time as required */
 577         size_t i = 0;
 578
 579         if (!*lineptr) {
 580                 *n = MIN_CHUNK;
 581                 *lineptr = malloc(*n);
 582                 if (!*lineptr)
 583                         return -1;
 584         }
 585
 586         (*lineptr)[0] = fgetc(stream);
 587         while ((*lineptr)[i] != '\n') {
 588                 (*lineptr)[++i] = fgetc(stream);
 589                 if (feof(stream)) {
 590                         (*lineptr)[0] = 0;
 591                         return -1;
 592                 }
 593                 if ((i + 2) > *n) {
 594                         *n += MIN_CHUNK;
 595                         *lineptr = realloc(*lineptr, *n);
 596                 }
 597         }
 598
 599         (*lineptr)[++i] = 0;
 600
 601         return sizeof(*lineptr);
 602 }
 603
 604 #define SVFP_CMD_INC_CNT 1024
 605 static int svf_read_command_from_file(FILE *fd)
 606 {
 607         unsigned char ch;
 608         int i = 0;
 609         size_t cmd_pos = 0;
 610         int cmd_ok = 0, slash = 0;
 611
 612         if (svf_getline(&svf_read_line, &svf_read_line_size, svf_fd) <= 0)
 613                 return ERROR_FAIL;
 614         svf_line_number++;
 615         ch = svf_read_line[0];
 616         while (!cmd_ok && (ch != 0)) {
 617                 switch (ch) {
 618                         case '!':
 619                                 slash = 0;
 620                                 if (svf_getline(&svf_read_line, &svf_read_line_size, svf_fd) <= 0)
 621                                         return ERROR_FAIL;
 622                                 svf_line_number++;
 623                                 i = -1;
 624                                 break;
 625                         case '/':
 626                                 if (++slash == 2) {
 627                                         slash = 0;
 628                                         if (svf_getline(&svf_read_line, &svf_read_line_size,
 629                                                 svf_fd) <= 0)
 630                                                 return ERROR_FAIL;
 631                                         svf_line_number++;
 632                                         i = -1;
 633                                 }
 634                                 break;
 635                         case ';':
 636                                 slash = 0;
 637                                 cmd_ok = 1;
 638                                 break;
 639                         case '\n':
 640                                 svf_line_number++;
 641                                 if (svf_getline(&svf_read_line, &svf_read_line_size, svf_fd) <= 0)
 642                                         return ERROR_FAIL;
 643                                 i = -1;
 644                                 /* fallthrough */
 645                         case '\r':
 646                                 slash = 0;
 647                                 /* Don't save '\r' and '\n' if no data is parsed */
 648                                 if (!cmd_pos)
 649                                         break;
 650                                 /* fallthrough */
 651                         default:
 652                                 /* The parsing code currently expects a space
 653                                  * before parentheses -- "TDI (123)".  Also a
 654                                  * space afterwards -- "TDI (123) TDO(456)".
 655                                  * But such spaces are optional... instead of
 656                                  * parser updates, cope with that by adding the
 657                                  * spaces as needed.
 658                                  *
 659                                  * Ensure there are 3 bytes available, for:
 660                                  *  - current character
 661                                  *  - added space.
 662                                  *  - terminating NUL ('\0')
 663                                  */
 664                                 if (cmd_pos + 3 > svf_command_buffer_size) {
 665                                         svf_command_buffer = realloc(svf_command_buffer, cmd_pos + 3);
 666                                         svf_command_buffer_size = cmd_pos + 3;
 667                                         if (!svf_command_buffer) {
 668                                                 LOG_ERROR("not enough memory");
 669                                                 return ERROR_FAIL;
 670                                         }
 671                                 }
 672
 673                                 /* insert a space before '(' */
 674                                 if ('(' == ch)
 675                                         svf_command_buffer[cmd_pos++] = ' ';
 676
 677                                 svf_command_buffer[cmd_pos++] = (char)toupper(ch);
 678
 679                                 /* insert a space after ')' */
 680                                 if (')' == ch)
 681                                         svf_command_buffer[cmd_pos++] = ' ';
 682                                 break;
 683                 }
 684                 ch = svf_read_line[++i];
 685         }
 686
 687         if (cmd_ok) {
 688                 svf_command_buffer[cmd_pos] = '\0';
 689                 return ERROR_OK;
 690         } else
 691                 return ERROR_FAIL;
 692 }
 693
 694 static int svf_parse_cmd_string(char *str, int len, char **argus, int *num_of_argu)
 695 {
 696         int pos = 0, num = 0, space_found = 1, in_bracket = 0;
 697
 698         while (pos < len) {
 699                 switch (str[pos]) {
 700                         case '!':
 701                         case '/':
 702                                 LOG_ERROR("fail to parse svf command");
 703                                 return ERROR_FAIL;
 704                         case '(':
 705                                 in_bracket = 1;
 706                                 goto parse_char;
 707                         case ')':
 708                                 in_bracket = 0;
 709                                 goto parse_char;
 710                         default:
 711 parse_char:
 712                                 if (!in_bracket && isspace((int) str[pos])) {
 713                                         space_found = 1;
 714                                         str[pos] = '\0';
 715                                 } else if (space_found) {
 716                                         argus[num++] = &str[pos];
 717                                         space_found = 0;
 718                                 }
 719                                 break;
 720                 }
 721                 pos++;
 722         }
 723
 724         if (num == 0)
 725                 return ERROR_FAIL;
 726
 727         *num_of_argu = num;
 728
 729         return ERROR_OK;
 730 }
 731
 732 bool svf_tap_state_is_stable(tap_state_t state)
 733 {
 734         return (state == TAP_RESET) || (state == TAP_IDLE)
 735                         || (state == TAP_DRPAUSE) || (state == TAP_IRPAUSE);
 736 }
 737
 738 static int svf_find_string_in_array(char *str, char **strs, int num_of_element)
 739 {
 740         int i;
 741
 742         for (i = 0; i < num_of_element; i++) {
 743                 if (!strcmp(str, strs[i]))
 744                         return i;
 745         }
 746         return 0xFF;
 747 }
 748
 749 static int svf_adjust_array_length(uint8_t **arr, int orig_bit_len, int new_bit_len)
 750 {
 751         int new_byte_len = (new_bit_len + 7) >> 3;
 752
 753         if ((!*arr) || (((orig_bit_len + 7) >> 3) < ((new_bit_len + 7) >> 3))) {
 754                 free(*arr);
 755                 *arr = calloc(1, new_byte_len);
 756                 if (!*arr) {
 757                         LOG_ERROR("not enough memory");
 758                         return ERROR_FAIL;
 759                 }
 760         }
 761         return ERROR_OK;
 762 }
 763
 764 static int svf_set_padding(struct svf_xxr_para *para, int len, unsigned char tdi)
 765 {
 766         int error = ERROR_OK;
 767         error |= svf_adjust_array_length(&para->tdi, para->len, len);
 768         memset(para->tdi, tdi, (len + 7) >> 3);
 769         error |= svf_adjust_array_length(&para->tdo, para->len, len);
 770         error |= svf_adjust_array_length(&para->mask, para->len, len);
 771         para->len = len;
 772         para->data_mask = XXR_TDI;
 773
 774         return error;
 775 }
 776
 777 static int svf_copy_hexstring_to_binary(char *str, uint8_t **bin, int orig_bit_len, int bit_len)
 778 {
 779         int i, str_len = strlen(str), str_hbyte_len = (bit_len + 3) >> 2;
 780         uint8_t ch = 0;
 781
 782         if (svf_adjust_array_length(bin, orig_bit_len, bit_len) != ERROR_OK) {
 783                 LOG_ERROR("fail to adjust length of array");
 784                 return ERROR_FAIL;
 785         }
 786
 787         /* fill from LSB (end of str) to MSB (beginning of str) */
 788         for (i = 0; i < str_hbyte_len; i++) {
 789                 ch = 0;
 790                 while (str_len > 0) {
 791                         ch = str[--str_len];
 792
 793                         /* Skip whitespace.  The SVF specification (rev E) is
 794                          * deficient in terms of basic lexical issues like
 795                          * where whitespace is allowed.  Long bitstrings may
 796                          * require line ends for correctness, since there is
 797                          * a hard limit on line length.
 798                          */
 799                         if (!isspace(ch)) {
 800                                 if ((ch >= '0') && (ch <= '9')) {
 801                                         ch = ch - '0';
 802                                         break;
 803                                 } else if ((ch >= 'A') && (ch <= 'F')) {
 804                                         ch = ch - 'A' + 10;
 805                                         break;
 806                                 } else {
 807                                         LOG_ERROR("invalid hex string");
 808                                         return ERROR_FAIL;
 809                                 }
 810                         }
 811
 812                         ch = 0;
 813                 }
 814
 815                 /* write bin */
 816                 if (i % 2) {
 817                         /* MSB */
 818                         (*bin)[i / 2] |= ch << 4;
 819                 } else {
 820                         /* LSB */
 821                         (*bin)[i / 2] = 0;
 822                         (*bin)[i / 2] |= ch;
 823                 }
 824         }
 825
 826         /* consume optional leading '0' MSBs or whitespace */
 827         while (str_len > 0 && ((str[str_len - 1] == '0')
 828                         || isspace((int) str[str_len - 1])))
 829                 str_len--;
 830
 831         /* check validity: we must have consumed everything */
 832         if (str_len > 0 || (ch & ~((2 << ((bit_len - 1) % 4)) - 1)) != 0) {
 833                 LOG_ERROR("value exceeds length");
 834                 return ERROR_FAIL;
 835         }
 836
 837         return ERROR_OK;
 838 }
 839
 840 static int svf_check_tdo(void)
 841 {
 842         int i, len, index_var;
 843
 844         for (i = 0; i < svf_check_tdo_para_index; i++) {
 845                 index_var = svf_check_tdo_para[i].buffer_offset;
 846                 len = svf_check_tdo_para[i].bit_len;
 847                 if ((svf_check_tdo_para[i].enabled)
 848                                 && buf_cmp_mask(&svf_tdi_buffer[index_var], &svf_tdo_buffer[index_var],
 849                                 &svf_mask_buffer[index_var], len)) {
 850                         LOG_ERROR("tdo check error at line %d",
 851                                 svf_check_tdo_para[i].line_num);
 852                         SVF_BUF_LOG(ERROR, &svf_tdi_buffer[index_var], len, "READ");
 853                         SVF_BUF_LOG(ERROR, &svf_tdo_buffer[index_var], len, "WANT");
 854                         SVF_BUF_LOG(ERROR, &svf_mask_buffer[index_var], len, "MASK");
 855
 856                         if (svf_ignore_error == 0)
 857                                 return ERROR_FAIL;
 858                         else
 859                                 svf_ignore_error++;
 860                 }
 861         }
 862         svf_check_tdo_para_index = 0;
 863
 864         return ERROR_OK;
 865 }
 866
 867 static int svf_add_check_para(uint8_t enabled, int buffer_offset, int bit_len)
 868 {
 869         if (svf_check_tdo_para_index >= SVF_CHECK_TDO_PARA_SIZE) {
 870                 LOG_ERROR("toooooo many operation undone");
 871                 return ERROR_FAIL;
 872         }
 873
 874         svf_check_tdo_para[svf_check_tdo_para_index].line_num = svf_line_number;
 875         svf_check_tdo_para[svf_check_tdo_para_index].bit_len = bit_len;
 876         svf_check_tdo_para[svf_check_tdo_para_index].enabled = enabled;
 877         svf_check_tdo_para[svf_check_tdo_para_index].buffer_offset = buffer_offset;
 878         svf_check_tdo_para_index++;
 879
 880         return ERROR_OK;
 881 }
 882
 883 static int svf_execute_tap(void)
 884 {
 885         if ((!svf_nil) && (jtag_execute_queue() != ERROR_OK))
 886                 return ERROR_FAIL;
 887         else if (svf_check_tdo() != ERROR_OK)
 888                 return ERROR_FAIL;
 889
 890         svf_buffer_index = 0;
 891
 892         return ERROR_OK;
 893 }
 894
 895 static int svf_run_command(struct command_context *cmd_ctx, char *cmd_str)
 896 {
 897         char *argus[256], command;
 898         int num_of_argu = 0, i;
 899
 900         /* tmp variable */
 901         int i_tmp;
 902
 903         /* for RUNTEST */
 904         int run_count;
 905         float min_time;
 906         /* for XXR */
 907         struct svf_xxr_para *xxr_para_tmp;
 908         uint8_t **pbuffer_tmp;
 909         struct scan_field field;
 910         /* for STATE */
 911         tap_state_t *path = NULL, state;
 912         /* flag padding commands skipped due to -tap command */
 913         int padding_command_skipped = 0;
 914
 915         if (svf_parse_cmd_string(cmd_str, strlen(cmd_str), argus, &num_of_argu) != ERROR_OK)
 916                 return ERROR_FAIL;
 917
 918         /* NOTE: we're a bit loose here, because we ignore case in
 919          * TAP state names (instead of insisting on uppercase).
 920          */
 921
 922         command = svf_find_string_in_array(argus[0],
 923                         (char **)svf_command_name, ARRAY_SIZE(svf_command_name));
 924         switch (command) {
 925                 case ENDDR:
 926                 case ENDIR:
 927                         if (num_of_argu != 2) {
 928                                 LOG_ERROR("invalid parameter of %s", argus[0]);
 929                                 return ERROR_FAIL;
 930                         }
 931
 932                         i_tmp = tap_state_by_name(argus[1]);
 933
 934                         if (svf_tap_state_is_stable(i_tmp)) {
 935                                 if (command == ENDIR) {
 936                                         svf_para.ir_end_state = i_tmp;
 937                                         LOG_DEBUG("\tIR end_state = %s",
 938                                                         tap_state_name(i_tmp));
 939                                 } else {
 940                                         svf_para.dr_end_state = i_tmp;
 941                                         LOG_DEBUG("\tDR end_state = %s",
 942                                                         tap_state_name(i_tmp));
 943                                 }
 944                         } else {
 945                                 LOG_ERROR("%s: %s is not a stable state",
 946                                                 argus[0], argus[1]);
 947                                 return ERROR_FAIL;
 948                         }
 949                         break;
 950                 case FREQUENCY:
 951                         if ((num_of_argu != 1) && (num_of_argu != 3)) {
 952                                 LOG_ERROR("invalid parameter of %s", argus[0]);
 953                                 return ERROR_FAIL;
 954                         }
 955                         if (num_of_argu == 1) {
 956                                 /* TODO: set jtag speed to full speed */
 957                                 svf_para.frequency = 0;
 958                         } else {
 959                                 if (strcmp(argus[2], "HZ")) {
 960                                         LOG_ERROR("HZ not found in FREQUENCY command");
 961                                         return ERROR_FAIL;
 962                                 }
 963                                 if (svf_execute_tap() != ERROR_OK)
 964                                         return ERROR_FAIL;
 965                                 svf_para.frequency = atof(argus[1]);
 966                                 /* TODO: set jtag speed to */
 967                                 if (svf_para.frequency > 0) {
 968                                         command_run_linef(cmd_ctx,
 969                                                         "adapter speed %d",
 970                                                         (int)svf_para.frequency / 1000);
 971                                         LOG_DEBUG("\tfrequency = %f", svf_para.frequency);
 972                                 }
 973                         }
 974                         break;
 975                 case HDR:
 976                         if (svf_tap_is_specified) {
 977                                 padding_command_skipped = 1;
 978                                 break;
 979                         }
 980                         xxr_para_tmp = &svf_para.hdr_para;
 981                         goto xxr_common;
 982                 case HIR:
 983                         if (svf_tap_is_specified) {
 984                                 padding_command_skipped = 1;
 985                                 break;
 986                         }
 987                         xxr_para_tmp = &svf_para.hir_para;
 988                         goto xxr_common;
 989                 case TDR:
 990                         if (svf_tap_is_specified) {
 991                                 padding_command_skipped = 1;
 992                                 break;
 993                         }
 994                         xxr_para_tmp = &svf_para.tdr_para;
 995                         goto xxr_common;
 996                 case TIR:
 997                         if (svf_tap_is_specified) {
 998                                 padding_command_skipped = 1;
 999                                 break;
1000                         }
1001                         xxr_para_tmp = &svf_para.tir_para;
1002                         goto xxr_common;
1003                 case SDR:
1004                         xxr_para_tmp = &svf_para.sdr_para;
1005                         goto xxr_common;
1006                 case SIR:
1007                         xxr_para_tmp = &svf_para.sir_para;
1008                         goto xxr_common;
1009 xxr_common:
1010                         /* XXR length [TDI (tdi)] [TDO (tdo)][MASK (mask)] [SMASK (smask)] */
1011                         if ((num_of_argu > 10) || (num_of_argu % 2)) {
1012                                 LOG_ERROR("invalid parameter of %s", argus[0]);
1013                                 return ERROR_FAIL;
1014                         }
1015                         i_tmp = xxr_para_tmp->len;
1016                         xxr_para_tmp->len = atoi(argus[1]);
1017                         /* If we are to enlarge the buffers, all parts of xxr_para_tmp
1018                          * need to be freed */
1019                         if (i_tmp < xxr_para_tmp->len) {
1020                                 free(xxr_para_tmp->tdi);
1021                                 xxr_para_tmp->tdi = NULL;
1022                                 free(xxr_para_tmp->tdo);
1023                                 xxr_para_tmp->tdo = NULL;
1024                                 free(xxr_para_tmp->mask);
1025                                 xxr_para_tmp->mask = NULL;
1026                                 free(xxr_para_tmp->smask);
1027                                 xxr_para_tmp->smask = NULL;
1028                         }
1029
1030                         LOG_DEBUG("\tlength = %d", xxr_para_tmp->len);
1031                         xxr_para_tmp->data_mask = 0;
1032                         for (i = 2; i < num_of_argu; i += 2) {
1033                                 if ((strlen(argus[i + 1]) < 3) || (argus[i + 1][0] != '(') ||
1034                                 (argus[i + 1][strlen(argus[i + 1]) - 1] != ')')) {
1035                                         LOG_ERROR("data section error");
1036                                         return ERROR_FAIL;
1037                                 }
1038                                 argus[i + 1][strlen(argus[i + 1]) - 1] = '\0';
1039                                 /* TDI, TDO, MASK, SMASK */
1040                                 if (!strcmp(argus[i], "TDI")) {
1041                                         /* TDI */
1042                                         pbuffer_tmp = &xxr_para_tmp->tdi;
1043                                         xxr_para_tmp->data_mask |= XXR_TDI;
1044                                 } else if (!strcmp(argus[i], "TDO")) {
1045                                         /* TDO */
1046                                         pbuffer_tmp = &xxr_para_tmp->tdo;
1047                                         xxr_para_tmp->data_mask |= XXR_TDO;
1048                                 } else if (!strcmp(argus[i], "MASK")) {
1049                                         /* MASK */
1050                                         pbuffer_tmp = &xxr_para_tmp->mask;
1051                                         xxr_para_tmp->data_mask |= XXR_MASK;
1052                                 } else if (!strcmp(argus[i], "SMASK")) {
1053                                         /* SMASK */
1054                                         pbuffer_tmp = &xxr_para_tmp->smask;
1055                                         xxr_para_tmp->data_mask |= XXR_SMASK;
1056                                 } else {
1057                                         LOG_ERROR("unknown parameter: %s", argus[i]);
1058                                         return ERROR_FAIL;
1059                                 }
1060                                 if (ERROR_OK !=
1061                                 svf_copy_hexstring_to_binary(&argus[i + 1][1], pbuffer_tmp, i_tmp,
1062                                         xxr_para_tmp->len)) {
1063                                         LOG_ERROR("fail to parse hex value");
1064                                         return ERROR_FAIL;
1065                                 }
1066                                 SVF_BUF_LOG(DEBUG, *pbuffer_tmp, xxr_para_tmp->len, argus[i]);
1067                         }
1068                         /* If a command changes the length of the last scan of the same type and the
1069                          * MASK parameter is absent, */
1070                         /* the mask pattern used is all cares */
1071                         if (!(xxr_para_tmp->data_mask & XXR_MASK) && (i_tmp != xxr_para_tmp->len)) {
1072                                 /* MASK not defined and length changed */
1073                                 if (ERROR_OK !=
1074                                 svf_adjust_array_length(&xxr_para_tmp->mask, i_tmp,
1075                                         xxr_para_tmp->len)) {
1076                                         LOG_ERROR("fail to adjust length of array");
1077                                         return ERROR_FAIL;
1078                                 }
1079                                 buf_set_ones(xxr_para_tmp->mask, xxr_para_tmp->len);
1080                         }
1081                         /* If TDO is absent, no comparison is needed, set the mask to 0 */
1082                         if (!(xxr_para_tmp->data_mask & XXR_TDO)) {
1083                                 if (!xxr_para_tmp->tdo) {
1084                                         if (ERROR_OK !=
1085                                         svf_adjust_array_length(&xxr_para_tmp->tdo, i_tmp,
1086                                                 xxr_para_tmp->len)) {
1087                                                 LOG_ERROR("fail to adjust length of array");
1088                                                 return ERROR_FAIL;
1089                                         }
1090                                 }
1091                                 if (!xxr_para_tmp->mask) {
1092                                         if (ERROR_OK !=
1093                                         svf_adjust_array_length(&xxr_para_tmp->mask, i_tmp,
1094                                                 xxr_para_tmp->len)) {
1095                                                 LOG_ERROR("fail to adjust length of array");
1096                                                 return ERROR_FAIL;
1097                                         }
1098                                 }
1099                                 memset(xxr_para_tmp->mask, 0, (xxr_para_tmp->len + 7) >> 3);
1100                         }
1101                         /* do scan if necessary */
1102                         if (command == SDR) {
1103                                 /* check buffer size first, reallocate if necessary */
1104                                 i = svf_para.hdr_para.len + svf_para.sdr_para.len +
1105                                                 svf_para.tdr_para.len;
1106                                 if ((svf_buffer_size - svf_buffer_index) < ((i + 7) >> 3)) {
1107                                         /* reallocate buffer */
1108                                         if (svf_realloc_buffers(svf_buffer_index + ((i + 7) >> 3)) != ERROR_OK) {
1109                                                 LOG_ERROR("not enough memory");
1110                                                 return ERROR_FAIL;
1111                                         }
1112                                 }
1113
1114                                 /* assemble dr data */
1115                                 i = 0;
1116                                 buf_set_buf(svf_para.hdr_para.tdi,
1117                                                 0,
1118                                                 &svf_tdi_buffer[svf_buffer_index],
1119                                                 i,
1120                                                 svf_para.hdr_para.len);
1121                                 i += svf_para.hdr_para.len;
1122                                 buf_set_buf(svf_para.sdr_para.tdi,
1123                                                 0,
1124                                                 &svf_tdi_buffer[svf_buffer_index],
1125                                                 i,
1126                                                 svf_para.sdr_para.len);
1127                                 i += svf_para.sdr_para.len;
1128                                 buf_set_buf(svf_para.tdr_para.tdi,
1129                                                 0,
1130                                                 &svf_tdi_buffer[svf_buffer_index],
1131                                                 i,
1132                                                 svf_para.tdr_para.len);
1133                                 i += svf_para.tdr_para.len;
1134
1135                                 /* add check data */
1136                                 if (svf_para.sdr_para.data_mask & XXR_TDO) {
1137                                         /* assemble dr mask data */
1138                                         i = 0;
1139                                         buf_set_buf(svf_para.hdr_para.mask,
1140                                                         0,
1141                                                         &svf_mask_buffer[svf_buffer_index],
1142                                                         i,
1143                                                         svf_para.hdr_para.len);
1144                                         i += svf_para.hdr_para.len;
1145                                         buf_set_buf(svf_para.sdr_para.mask,
1146                                                         0,
1147                                                         &svf_mask_buffer[svf_buffer_index],
1148                                                         i,
1149                                                         svf_para.sdr_para.len);
1150                                         i += svf_para.sdr_para.len;
1151                                         buf_set_buf(svf_para.tdr_para.mask,
1152                                                         0,
1153                                                         &svf_mask_buffer[svf_buffer_index],
1154                                                         i,
1155                                                         svf_para.tdr_para.len);
1156
1157                                         /* assemble dr check data */
1158                                         i = 0;
1159                                         buf_set_buf(svf_para.hdr_para.tdo,
1160                                                         0,
1161                                                         &svf_tdo_buffer[svf_buffer_index],
1162                                                         i,
1163                                                         svf_para.hdr_para.len);
1164                                         i += svf_para.hdr_para.len;
1165                                         buf_set_buf(svf_para.sdr_para.tdo,
1166                                                         0,
1167                                                         &svf_tdo_buffer[svf_buffer_index],
1168                                                         i,
1169                                                         svf_para.sdr_para.len);
1170                                         i += svf_para.sdr_para.len;
1171                                         buf_set_buf(svf_para.tdr_para.tdo,
1172                                                         0,
1173                                                         &svf_tdo_buffer[svf_buffer_index],
1174                                                         i,
1175                                                         svf_para.tdr_para.len);
1176                                         i += svf_para.tdr_para.len;
1177
1178                                         svf_add_check_para(1, svf_buffer_index, i);
1179                                 } else
1180                                         svf_add_check_para(0, svf_buffer_index, i);
1181                                 field.num_bits = i;
1182                                 field.out_value = &svf_tdi_buffer[svf_buffer_index];
1183                                 field.in_value = (xxr_para_tmp->data_mask & XXR_TDO) ? &svf_tdi_buffer[svf_buffer_index] : NULL;
1184                                 if (!svf_nil) {
1185                                         /* NOTE:  doesn't use SVF-specified state paths */
1186                                         jtag_add_plain_dr_scan(field.num_bits,
1187                                                         field.out_value,
1188                                                         field.in_value,
1189                                                         svf_para.dr_end_state);
1190                                 }
1191
1192                                 svf_buffer_index += (i + 7) >> 3;
1193                         } else if (command == SIR) {
1194                                 /* check buffer size first, reallocate if necessary */
1195                                 i = svf_para.hir_para.len + svf_para.sir_para.len +
1196                                                 svf_para.tir_para.len;
1197                                 if ((svf_buffer_size - svf_buffer_index) < ((i + 7) >> 3)) {
1198                                         if (svf_realloc_buffers(svf_buffer_index + ((i + 7) >> 3)) != ERROR_OK) {
1199                                                 LOG_ERROR("not enough memory");
1200                                                 return ERROR_FAIL;
1201                                         }
1202                                 }
1203
1204                                 /* assemble ir data */
1205                                 i = 0;
1206                                 buf_set_buf(svf_para.hir_para.tdi,
1207                                                 0,
1208                                                 &svf_tdi_buffer[svf_buffer_index],
1209                                                 i,
1210                                                 svf_para.hir_para.len);
1211                                 i += svf_para.hir_para.len;
1212                                 buf_set_buf(svf_para.sir_para.tdi,
1213                                                 0,
1214                                                 &svf_tdi_buffer[svf_buffer_index],
1215                                                 i,
1216                                                 svf_para.sir_para.len);
1217                                 i += svf_para.sir_para.len;
1218                                 buf_set_buf(svf_para.tir_para.tdi,
1219                                                 0,
1220                                                 &svf_tdi_buffer[svf_buffer_index],
1221                                                 i,
1222                                                 svf_para.tir_para.len);
1223                                 i += svf_para.tir_para.len;
1224
1225                                 /* add check data */
1226                                 if (svf_para.sir_para.data_mask & XXR_TDO) {
1227                                         /* assemble dr mask data */
1228                                         i = 0;
1229                                         buf_set_buf(svf_para.hir_para.mask,
1230                                                         0,
1231                                                         &svf_mask_buffer[svf_buffer_index],
1232                                                         i,
1233                                                         svf_para.hir_para.len);
1234                                         i += svf_para.hir_para.len;
1235                                         buf_set_buf(svf_para.sir_para.mask,
1236                                                         0,
1237                                                         &svf_mask_buffer[svf_buffer_index],
1238                                                         i,
1239                                                         svf_para.sir_para.len);
1240                                         i += svf_para.sir_para.len;
1241                                         buf_set_buf(svf_para.tir_para.mask,
1242                                                         0,
1243                                                         &svf_mask_buffer[svf_buffer_index],
1244                                                         i,
1245                                                         svf_para.tir_para.len);
1246
1247                                         /* assemble dr check data */
1248                                         i = 0;
1249                                         buf_set_buf(svf_para.hir_para.tdo,
1250                                                         0,
1251                                                         &svf_tdo_buffer[svf_buffer_index],
1252                                                         i,
1253                                                         svf_para.hir_para.len);
1254                                         i += svf_para.hir_para.len;
1255                                         buf_set_buf(svf_para.sir_para.tdo,
1256                                                         0,
1257                                                         &svf_tdo_buffer[svf_buffer_index],
1258                                                         i,
1259                                                         svf_para.sir_para.len);
1260                                         i += svf_para.sir_para.len;
1261                                         buf_set_buf(svf_para.tir_para.tdo,
1262                                                         0,
1263                                                         &svf_tdo_buffer[svf_buffer_index],
1264                                                         i,
1265                                                         svf_para.tir_para.len);
1266                                         i += svf_para.tir_para.len;
1267
1268                                         svf_add_check_para(1, svf_buffer_index, i);
1269                                 } else
1270                                         svf_add_check_para(0, svf_buffer_index, i);
1271                                 field.num_bits = i;
1272                                 field.out_value = &svf_tdi_buffer[svf_buffer_index];
1273                                 field.in_value = (xxr_para_tmp->data_mask & XXR_TDO) ? &svf_tdi_buffer[svf_buffer_index] : NULL;
1274                                 if (!svf_nil) {
1275                                         /* NOTE:  doesn't use SVF-specified state paths */
1276                                         jtag_add_plain_ir_scan(field.num_bits,
1277                                                         field.out_value,
1278                                                         field.in_value,
1279                                                         svf_para.ir_end_state);
1280                                 }
1281
1282                                 svf_buffer_index += (i + 7) >> 3;
1283                         }
1284                         break;
1285                 case PIO:
1286                 case PIOMAP:
1287                         LOG_ERROR("PIO and PIOMAP are not supported");
1288                         return ERROR_FAIL;
1289                 case RUNTEST:
1290                         /* RUNTEST [run_state] run_count run_clk [min_time SEC [MAXIMUM max_time
1291                          * SEC]] [ENDSTATE end_state] */
1292                         /* RUNTEST [run_state] min_time SEC [MAXIMUM max_time SEC] [ENDSTATE
1293                          * end_state] */
1294                         if ((num_of_argu < 3) || (num_of_argu > 11)) {
1295                                 LOG_ERROR("invalid parameter of %s", argus[0]);
1296                                 return ERROR_FAIL;
1297                         }
1298                         /* init */
1299                         run_count = 0;
1300                         min_time = 0;
1301                         i = 1;
1302
1303                         /* run_state */
1304                         i_tmp = tap_state_by_name(argus[i]);
1305                         if (i_tmp != TAP_INVALID) {
1306                                 if (svf_tap_state_is_stable(i_tmp)) {
1307                                         svf_para.runtest_run_state = i_tmp;
1308
1309                                         /* When a run_state is specified, the new
1310                                          * run_state becomes the default end_state.
1311                                          */
1312                                         svf_para.runtest_end_state = i_tmp;
1313                                         LOG_DEBUG("\trun_state = %s", tap_state_name(i_tmp));
1314                                         i++;
1315                                 } else {
1316                                         LOG_ERROR("%s: %s is not a stable state", argus[0], tap_state_name(i_tmp));
1317                                         return ERROR_FAIL;
1318                                 }
1319                         }
1320
1321                         /* run_count run_clk */
1322                         if (((i + 2) <= num_of_argu) && strcmp(argus[i + 1], "SEC")) {
1323                                 if (!strcmp(argus[i + 1], "TCK")) {
1324                                         /* clock source is TCK */
1325                                         run_count = atoi(argus[i]);
1326                                         LOG_DEBUG("\trun_count@TCK = %d", run_count);
1327                                 } else {
1328                                         LOG_ERROR("%s not supported for clock", argus[i + 1]);
1329                                         return ERROR_FAIL;
1330                                 }
1331                                 i += 2;
1332                         }
1333                         /* min_time SEC */
1334                         if (((i + 2) <= num_of_argu) && !strcmp(argus[i + 1], "SEC")) {
1335                                 min_time = atof(argus[i]);
1336                                 LOG_DEBUG("\tmin_time = %fs", min_time);
1337                                 i += 2;
1338                         }
1339                         /* MAXIMUM max_time SEC */
1340                         if (((i + 3) <= num_of_argu) &&
1341                         !strcmp(argus[i], "MAXIMUM") && !strcmp(argus[i + 2], "SEC")) {
1342                                 float max_time = 0;
1343                                 max_time = atof(argus[i + 1]);
1344                                 LOG_DEBUG("\tmax_time = %fs", max_time);
1345                                 i += 3;
1346                         }
1347                         /* ENDSTATE end_state */
1348                         if (((i + 2) <= num_of_argu) && !strcmp(argus[i], "ENDSTATE")) {
1349                                 i_tmp = tap_state_by_name(argus[i + 1]);
1350
1351                                 if (svf_tap_state_is_stable(i_tmp)) {
1352                                         svf_para.runtest_end_state = i_tmp;
1353                                         LOG_DEBUG("\tend_state = %s", tap_state_name(i_tmp));
1354                                 } else {
1355                                         LOG_ERROR("%s: %s is not a stable state", argus[0], tap_state_name(i_tmp));
1356                                         return ERROR_FAIL;
1357                                 }
1358                                 i += 2;
1359                         }
1360
1361                         /* all parameter should be parsed */
1362                         if (i == num_of_argu) {
1363 #if 1
1364                                 /* FIXME handle statemove failures */
1365                                 uint32_t min_usec = 1000000 * min_time;
1366
1367                                 /* enter into run_state if necessary */
1368                                 if (cmd_queue_cur_state != svf_para.runtest_run_state)
1369                                         svf_add_statemove(svf_para.runtest_run_state);
1370
1371                                 /* add clocks and/or min wait */
1372                                 if (run_count > 0) {
1373                                         if (!svf_nil)
1374                                                 jtag_add_clocks(run_count);
1375                                 }
1376
1377                                 if (min_usec > 0) {
1378                                         if (!svf_nil)
1379                                                 jtag_add_sleep(min_usec);
1380                                 }
1381
1382                                 /* move to end_state if necessary */
1383                                 if (svf_para.runtest_end_state != svf_para.runtest_run_state)
1384                                         svf_add_statemove(svf_para.runtest_end_state);
1385
1386 #else
1387                                 if (svf_para.runtest_run_state != TAP_IDLE) {
1388                                         LOG_ERROR("cannot runtest in %s state",
1389                                                         tap_state_name(svf_para.runtest_run_state));
1390                                         return ERROR_FAIL;
1391                                 }
1392
1393                                 if (!svf_nil)
1394                                         jtag_add_runtest(run_count, svf_para.runtest_end_state);
1395 #endif
1396                         } else {
1397                                 LOG_ERROR("fail to parse parameter of RUNTEST, %d out of %d is parsed",
1398                                                 i,
1399                                                 num_of_argu);
1400                                 return ERROR_FAIL;
1401                         }
1402                         break;
1403                 case STATE:
1404                         /* STATE [pathstate1 [pathstate2 ...[pathstaten]]] stable_state */
1405                         if (num_of_argu < 2) {
1406                                 LOG_ERROR("invalid parameter of %s", argus[0]);
1407                                 return ERROR_FAIL;
1408                         }
1409                         if (num_of_argu > 2) {
1410                                 /* STATE pathstate1 ... stable_state */
1411                                 path = malloc((num_of_argu - 1) * sizeof(tap_state_t));
1412                                 if (!path) {
1413                                         LOG_ERROR("not enough memory");
1414                                         return ERROR_FAIL;
1415                                 }
1416                                 num_of_argu--;  /* num of path */
1417                                 i_tmp = 1;              /* path is from parameter 1 */
1418                                 for (i = 0; i < num_of_argu; i++, i_tmp++) {
1419                                         path[i] = tap_state_by_name(argus[i_tmp]);
1420                                         if (path[i] == TAP_INVALID) {
1421                                                 LOG_ERROR("%s: %s is not a valid state", argus[0], argus[i_tmp]);
1422                                                 free(path);
1423                                                 return ERROR_FAIL;
1424                                         }
1425                                         /* OpenOCD refuses paths containing TAP_RESET */
1426                                         if (path[i] == TAP_RESET) {
1427                                                 /* FIXME last state MUST be stable! */
1428                                                 if (i > 0) {
1429                                                         if (!svf_nil)
1430                                                                 jtag_add_pathmove(i, path);
1431                                                 }
1432                                                 if (!svf_nil)
1433                                                         jtag_add_tlr();
1434                                                 num_of_argu -= i + 1;
1435                                                 i = -1;
1436                                         }
1437                                 }
1438                                 if (num_of_argu > 0) {
1439                                         /* execute last path if necessary */
1440                                         if (svf_tap_state_is_stable(path[num_of_argu - 1])) {
1441                                                 /* last state MUST be stable state */
1442                                                 if (!svf_nil)
1443                                                         jtag_add_pathmove(num_of_argu, path);
1444                                                 LOG_DEBUG("\tmove to %s by path_move",
1445                                                                 tap_state_name(path[num_of_argu - 1]));
1446                                         } else {
1447                                                 LOG_ERROR("%s: %s is not a stable state",
1448                                                                 argus[0],
1449                                                                 tap_state_name(path[num_of_argu - 1]));
1450                                                 free(path);
1451                                                 return ERROR_FAIL;
1452                                         }
1453                                 }
1454
1455                                 free(path);
1456                                 path = NULL;
1457                         } else {
1458                                 /* STATE stable_state */
1459                                 state = tap_state_by_name(argus[1]);
1460                                 if (svf_tap_state_is_stable(state)) {
1461                                         LOG_DEBUG("\tmove to %s by svf_add_statemove",
1462                                                         tap_state_name(state));
1463                                         /* FIXME handle statemove failures */
1464                                         svf_add_statemove(state);
1465                                 } else {
1466                                         LOG_ERROR("%s: %s is not a stable state",
1467                                                         argus[0], tap_state_name(state));
1468                                         return ERROR_FAIL;
1469                                 }
1470                         }
1471                         break;
1472                 case TRST:
1473                         /* TRST trst_mode */
1474                         if (num_of_argu != 2) {
1475                                 LOG_ERROR("invalid parameter of %s", argus[0]);
1476                                 return ERROR_FAIL;
1477                         }
1478                         if (svf_para.trst_mode != TRST_ABSENT) {
1479                                 if (svf_execute_tap() != ERROR_OK)
1480                                         return ERROR_FAIL;
1481                                 i_tmp = svf_find_string_in_array(argus[1],
1482                                                 (char **)svf_trst_mode_name,
1483                                                 ARRAY_SIZE(svf_trst_mode_name));
1484                                 switch (i_tmp) {
1485                                 case TRST_ON:
1486                                         if (!svf_nil)
1487                                                 jtag_add_reset(1, 0);
1488                                         break;
1489                                 case TRST_Z:
1490                                 case TRST_OFF:
1491                                         if (!svf_nil)
1492                                                 jtag_add_reset(0, 0);
1493                                         break;
1494                                 case TRST_ABSENT:
1495                                         break;
1496                                 default:
1497                                         LOG_ERROR("unknown TRST mode: %s", argus[1]);
1498                                         return ERROR_FAIL;
1499                                 }
1500                                 svf_para.trst_mode = i_tmp;
1501                                 LOG_DEBUG("\ttrst_mode = %s", svf_trst_mode_name[svf_para.trst_mode]);
1502                         } else {
1503                                 LOG_ERROR("can not accept TRST command if trst_mode is ABSENT");
1504                                 return ERROR_FAIL;
1505                         }
1506                         break;
1507                 default:
1508                         LOG_ERROR("invalid svf command: %s", argus[0]);
1509                         return ERROR_FAIL;
1510         }
1511
1512         if (!svf_quiet) {
1513                 if (padding_command_skipped)
1514                         LOG_USER("(Above Padding command skipped, as per -tap argument)");
1515         }
1516
1517         if (debug_level >= LOG_LVL_DEBUG) {
1518                 /* for convenient debugging, execute tap if possible */
1519                 if ((svf_buffer_index > 0) &&
1520                                 (((command != STATE) && (command != RUNTEST)) ||
1521                                                 ((command == STATE) && (num_of_argu == 2)))) {
1522                         if (svf_execute_tap() != ERROR_OK)
1523                                 return ERROR_FAIL;
1524
1525                         /* output debug info */
1526                         if ((command == SIR) || (command == SDR))
1527                                 SVF_BUF_LOG(DEBUG, svf_tdi_buffer, svf_check_tdo_para[0].bit_len, "TDO read");
1528                 }
1529         } else {
1530                 /* for fast executing, execute tap if necessary */
1531                 /* half of the buffer is for the next command */
1532                 if (((svf_buffer_index >= SVF_MAX_BUFFER_SIZE_TO_COMMIT) ||
1533                                 (svf_check_tdo_para_index >= SVF_CHECK_TDO_PARA_SIZE / 2)) &&
1534                                 (((command != STATE) && (command != RUNTEST)) ||
1535                                                 ((command == STATE) && (num_of_argu == 2))))
1536                         return svf_execute_tap();
1537         }
1538
1539         return ERROR_OK;
1540 }
1541
1542 static const struct command_registration svf_command_handlers[] = {
1543         {
1544                 .name = "svf",
1545                 .handler = handle_svf_command,
1546                 .mode = COMMAND_EXEC,
1547                 .help = "Runs a SVF file.",
1548                 .usage = "[-tap device.tap] <file> [quiet] [nil] [progress] [ignore_error]",
1549         },
1550         COMMAND_REGISTRATION_DONE
1551 };
1552
1553 int svf_register_commands(struct command_context *cmd_ctx)
1554 {
1555         return register_commands(cmd_ctx, NULL, svf_command_handlers);
1556 }