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

   1 /***************************************************************************
   2  *       Copyright (C) 2009 by Simon Qian                                                                          *
   3  *       SimonQian@SimonQian.com                                                                                           *
   4  *                                                                         *
   5  *       This program is free software; you can redistribute it and/or modify  *
   6  *       it under the terms of the GNU General Public License as published by  *
   7  *       the Free Software Foundation; either version 2 of the License, or         *
   8  *       (at your option) any later version.                                                               *
   9  *                                                                                                                                                 *
  10  *       This program is distributed in the hope that it will be useful,           *
  11  *       but WITHOUT ANY WARRANTY; without even the implied warranty of            *
  12  *       MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the             *
  13  *       GNU General Public License for more details.                                              *
  14  *                                                                                                                                                 *
  15  *       You should have received a copy of the GNU General Public License         *
  16  *       along with this program; if not, write to the                                             *
  17  *       Free Software Foundation, Inc.,                                                                           *
  18  *       59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.                         *
  19  ***************************************************************************/
  20
  21
  22 /* The specification for SVF is available here:
  23  * http://www.asset-intertech.com/support/svf.pdf
  24  * Below, this document is refered to as the "SVF spec".
  25  *
  26  * The specification for XSVF is available here:
  27  * http://www.xilinx.com/support/documentation/application_notes/xapp503.pdf
  28  * Below, this document is refered to as the "XSVF spec".
  29  */
  30
  31 #ifdef HAVE_CONFIG_H
  32 #include "config.h"
  33 #endif
  34
  35 #include "svf.h"
  36
  37 #include "jtag.h"
  38 #include "command.h"
  39 #include "log.h"
  40
  41 #include <ctype.h>
  42 #include <stdlib.h>
  43 #include <unistd.h>
  44 #include <sys/types.h>
  45 #include <sys/stat.h>
  46 #include <fcntl.h>
  47 #include <string.h>
  48
  49 #include <sys/time.h>
  50 #include <time.h>
  51
  52 // SVF command
  53 typedef enum
  54 {
  55         ENDDR,
  56         ENDIR,
  57         FREQUENCY,
  58         HDR,
  59         HIR,
  60         PIO,
  61         PIOMAP,
  62         RUNTEST,
  63         SDR,
  64         SIR,
  65         STATE,
  66         TDR,
  67         TIR,
  68         TRST,
  69 }svf_command_t;
  70
  71 const char *svf_command_name[14] =
  72 {
  73         "ENDDR",
  74         "ENDIR",
  75         "FREQUENCY",
  76         "HDR",
  77         "HIR",
  78         "PIO",
  79         "PIOMAP",
  80         "RUNTEST",
  81         "SDR",
  82         "SIR",
  83         "STATE",
  84         "TDR",
  85         "TIR",
  86         "TRST"
  87 };
  88
  89 typedef enum
  90 {
  91         TRST_ON,
  92         TRST_OFF,
  93         TRST_Z,
  94         TRST_ABSENT
  95 }trst_mode_t;
  96
  97 const char *svf_trst_mode_name[4] =
  98 {
  99         "ON",
 100         "OFF",
 101         "Z",
 102         "ABSENT"
 103 };
 104
 105 char *svf_tap_state_name[16];
 106
 107 #define XXR_TDI                                         (1 << 0)
 108 #define XXR_TDO                                         (1 << 1)
 109 #define XXR_MASK                                        (1 << 2)
 110 #define XXR_SMASK                                       (1 << 3)
 111 typedef struct
 112 {
 113         int len;
 114         int data_mask;
 115         u8 *tdi;
 116         u8 *tdo;
 117         u8 *mask;
 118         u8 *smask;
 119 }svf_xxr_para_t;
 120
 121 typedef struct
 122 {
 123         float frequency;
 124         tap_state_t ir_end_state;
 125         tap_state_t dr_end_state;
 126         tap_state_t runtest_run_state;
 127         tap_state_t runtest_end_state;
 128         trst_mode_t trst_mode;
 129
 130         svf_xxr_para_t hir_para;
 131         svf_xxr_para_t hdr_para;
 132         svf_xxr_para_t tir_para;
 133         svf_xxr_para_t tdr_para;
 134         svf_xxr_para_t sir_para;
 135         svf_xxr_para_t sdr_para;
 136 }svf_para_t;
 137
 138 svf_para_t svf_para;
 139 const svf_para_t svf_para_init =
 140 {
 141 //      frequency,      ir_end_state,   dr_end_state,   runtest_run_state,      runtest_end_state,      trst_mode
 142         0,                      TAP_IDLE,               TAP_IDLE,               TAP_IDLE,                       TAP_IDLE,                       TRST_Z,
 143 //      hir_para
 144 //      {len,   data_mask,      tdi,    tdo,    mask,   smask},
 145         {0,             0,                      NULL,   NULL,   NULL,   NULL},
 146 //      hdr_para
 147 //      {len,   data_mask,      tdi,    tdo,    mask,   smask},
 148         {0,             0,                      NULL,   NULL,   NULL,   NULL},
 149 //      tir_para
 150 //      {len,   data_mask,      tdi,    tdo,    mask,   smask},
 151         {0,             0,                      NULL,   NULL,   NULL,   NULL},
 152 //      tdr_para
 153 //      {len,   data_mask,      tdi,    tdo,    mask,   smask},
 154         {0,             0,                      NULL,   NULL,   NULL,   NULL},
 155 //      sir_para
 156 //      {len,   data_mask,      tdi,    tdo,    mask,   smask},
 157         {0,             0,                      NULL,   NULL,   NULL,   NULL},
 158 //      sdr_para
 159 //      {len,   data_mask,      tdi,    tdo,    mask,   smask},
 160         {0,             0,                      NULL,   NULL,   NULL,   NULL},
 161 };
 162
 163 typedef struct
 164 {
 165         int line_num;           // used to record line number of the check operation
 166                                                 // so more information could be printed
 167         int enabled;            // check is enabled or not
 168         int buffer_offset;      // buffer_offset to buffers
 169         int bit_len;            // bit length to check
 170 }svf_check_tdo_para_t;
 171
 172 #define SVF_CHECK_TDO_PARA_SIZE 1024
 173 static svf_check_tdo_para_t *svf_check_tdo_para = NULL;
 174 static int svf_check_tdo_para_index = 0;
 175
 176 #define dimof(a)                                        (sizeof(a) / sizeof((a)[0]))
 177
 178 static int svf_read_command_from_file(int fd);
 179 static int svf_check_tdo(void);
 180 static int svf_add_check_para(u8 enabled, int buffer_offset, int bit_len);
 181 static int svf_run_command(struct command_context_s *cmd_ctx, char *cmd_str);
 182 static int handle_svf_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
 183
 184 static int svf_fd = 0;
 185 static char *svf_command_buffer = NULL;
 186 static int svf_command_buffer_size = 0;
 187 static int svf_line_number = 1;
 188
 189 static jtag_tap_t *tap = NULL;
 190
 191 #define SVF_MAX_BUFFER_SIZE_TO_COMMIT   (4 * 1024)
 192 static u8 *svf_tdi_buffer = NULL, *svf_tdo_buffer = NULL, *svf_mask_buffer = NULL;
 193 static int svf_buffer_index = 0, svf_buffer_size = 0;
 194
 195
 196 int svf_register_commands(struct command_context_s *cmd_ctx)
 197 {
 198         register_command(cmd_ctx, NULL, "svf", handle_svf_command,
 199                 COMMAND_EXEC, "run svf <file>");
 200
 201         return ERROR_OK;
 202 }
 203
 204 void svf_free_xxd_para(svf_xxr_para_t *para)
 205 {
 206         if (NULL != para)
 207         {
 208                 if (para->tdi != NULL)
 209                 {
 210                         free(para->tdi);
 211                         para->tdi = NULL;
 212                 }
 213                 if (para->tdo != NULL)
 214                 {
 215                         free(para->tdo);
 216                         para->tdo = NULL;
 217                 }
 218                 if (para->mask != NULL)
 219                 {
 220                         free(para->mask);
 221                         para->mask = NULL;
 222                 }
 223                 if (para->smask != NULL)
 224                 {
 225                         free(para->smask);
 226                         para->smask = NULL;
 227                 }
 228         }
 229 }
 230
 231 static int handle_svf_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
 232 {
 233         int command_num = 0, i;
 234         int ret = ERROR_OK;
 235
 236         if (argc < 1)
 237         {
 238                 command_print(cmd_ctx, "usage: svf <file>");
 239                 return ERROR_FAIL;
 240         }
 241
 242         if ((svf_fd = open(args[0], O_RDONLY)) < 0)
 243         {
 244                 command_print(cmd_ctx, "file \"%s\" not found", args[0]);
 245                 return ERROR_FAIL;
 246         }
 247
 248         LOG_USER("svf processing file: \"%s\"", args[0]);
 249
 250         // init
 251         svf_line_number = 1;
 252         svf_command_buffer_size = 0;
 253
 254         svf_check_tdo_para_index = 0;
 255         svf_check_tdo_para = malloc(sizeof(svf_check_tdo_para_t) * SVF_CHECK_TDO_PARA_SIZE);
 256         if (NULL == svf_check_tdo_para)
 257         {
 258                 LOG_ERROR("not enough memory");
 259                 ret = ERROR_FAIL;
 260                 goto free_all;
 261         }
 262
 263         svf_buffer_index = 0;
 264         // double the buffer size
 265         // in case current command cannot be commited, and next command is a bit scan command
 266         // here is 32K bits for this big scan command, it should be enough
 267         // buffer will be reallocated if buffer size is not enough
 268         svf_tdi_buffer = (u8 *)malloc(2 * SVF_MAX_BUFFER_SIZE_TO_COMMIT);
 269         if (NULL == svf_tdi_buffer)
 270         {
 271                 LOG_ERROR("not enough memory");
 272                 ret = ERROR_FAIL;
 273                 goto free_all;
 274         }
 275         svf_tdo_buffer = (u8 *)malloc(2 * SVF_MAX_BUFFER_SIZE_TO_COMMIT);
 276         if (NULL == svf_tdo_buffer)
 277         {
 278                 LOG_ERROR("not enough memory");
 279                 ret = ERROR_FAIL;
 280                 goto free_all;
 281         }
 282         svf_mask_buffer = (u8 *)malloc(2 * SVF_MAX_BUFFER_SIZE_TO_COMMIT);
 283         if (NULL == svf_mask_buffer)
 284         {
 285                 LOG_ERROR("not enough memory");
 286                 ret = ERROR_FAIL;
 287                 goto free_all;
 288         }
 289         svf_buffer_size = 2 * SVF_MAX_BUFFER_SIZE_TO_COMMIT;
 290
 291         memcpy(&svf_para, &svf_para_init, sizeof(svf_para));
 292         for (i = 0; i < dimof(svf_tap_state_name); i++)
 293         {
 294                 svf_tap_state_name[i] = (char *)jtag_state_name(i);
 295         }
 296         // TAP_RESET
 297         jtag_add_tlr();
 298
 299         while ( ERROR_OK == svf_read_command_from_file(svf_fd) )
 300         {
 301                 if (ERROR_OK != svf_run_command(cmd_ctx, svf_command_buffer))
 302                 {
 303                         LOG_ERROR("fail to run command at line %d", svf_line_number);
 304                         ret = ERROR_FAIL;
 305                         break;
 306                 }
 307                 command_num++;
 308         }
 309         if (ERROR_OK != jtag_execute_queue())
 310         {
 311                 ret = ERROR_FAIL;
 312         }
 313         else if (ERROR_OK != svf_check_tdo())
 314         {
 315                 ret = ERROR_FAIL;
 316         }
 317
 318 free_all:
 319
 320         close(svf_fd);
 321         svf_fd = 0;
 322
 323         // free buffers
 324         if (svf_command_buffer)
 325         {
 326                 free(svf_command_buffer);
 327                 svf_command_buffer = NULL;
 328                 svf_command_buffer_size = 0;
 329         }
 330         if (svf_check_tdo_para)
 331         {
 332                 free(svf_check_tdo_para);
 333                 svf_check_tdo_para = NULL;
 334                 svf_check_tdo_para_index = 0;
 335         }
 336         if (svf_tdi_buffer)
 337         {
 338                 free(svf_tdi_buffer);
 339                 svf_tdi_buffer = NULL;
 340         }
 341         if (svf_tdo_buffer)
 342         {
 343                 free(svf_tdo_buffer);
 344                 svf_tdo_buffer = NULL;
 345         }
 346         if (svf_mask_buffer)
 347         {
 348                 free(svf_mask_buffer);
 349                 svf_mask_buffer = NULL;
 350         }
 351         svf_buffer_index = 0;
 352         svf_buffer_size = 0;
 353
 354         svf_free_xxd_para(&svf_para.hdr_para);
 355         svf_free_xxd_para(&svf_para.hir_para);
 356         svf_free_xxd_para(&svf_para.tdr_para);
 357         svf_free_xxd_para(&svf_para.tir_para);
 358         svf_free_xxd_para(&svf_para.sdr_para);
 359         svf_free_xxd_para(&svf_para.sir_para);
 360
 361         if (ERROR_OK == ret)
 362         {
 363                 command_print(cmd_ctx, "svf file programmed successfully for %d commands", command_num);
 364         }
 365         else
 366         {
 367                 command_print(cmd_ctx, "svf file programmed failed");
 368         }
 369
 370         return ret;
 371 }
 372
 373 #define SVFP_CMD_INC_CNT                        1024
 374 static int svf_read_command_from_file(int fd)
 375 {
 376         char ch, *tmp_buffer = NULL;
 377         int cmd_pos = 0, cmd_ok = 0, slash = 0, comment = 0;
 378
 379         while (!cmd_ok && (read(fd, &ch, 1) > 0) )
 380         {
 381                 switch(ch)
 382                 {
 383                 case '!':
 384                         slash = 0;
 385                         comment = 1;
 386                         break;
 387                 case '/':
 388                         if (++slash == 2)
 389                         {
 390                                 comment = 1;
 391                         }
 392                         break;
 393                 case ';':
 394                         slash = 0;
 395                         if (!comment)
 396                         {
 397                                 cmd_ok = 1;
 398                         }
 399                         break;
 400                 case '\n':
 401                         svf_line_number++;
 402                 case '\r':
 403                         slash = 0;
 404                         comment = 0;
 405                         break;
 406                 default:
 407                         if (!comment)
 408                         {
 409                                 if (cmd_pos >= svf_command_buffer_size - 1)
 410                                 {
 411                                         tmp_buffer = (char*)malloc(svf_command_buffer_size + SVFP_CMD_INC_CNT);         // 1 more byte for '\0'
 412                                         if (NULL == tmp_buffer)
 413                                         {
 414                                                 LOG_ERROR("not enough memory");
 415                                                 return ERROR_FAIL;
 416                                         }
 417                                         if (svf_command_buffer_size > 0)
 418                                         {
 419                                                 memcpy(tmp_buffer, svf_command_buffer, svf_command_buffer_size);
 420                                         }
 421                                         if (svf_command_buffer != NULL)
 422                                         {
 423                                                 free(svf_command_buffer);
 424                                         }
 425                                         svf_command_buffer = tmp_buffer;
 426                                         svf_command_buffer_size += SVFP_CMD_INC_CNT;
 427                                         tmp_buffer = NULL;
 428                                 }
 429                                 svf_command_buffer[cmd_pos++] = (char)toupper(ch);
 430                         }
 431                         break;
 432                 }
 433         }
 434
 435         if (cmd_ok)
 436         {
 437                 svf_command_buffer[cmd_pos] = '\0';
 438                 return ERROR_OK;
 439         }
 440         else
 441         {
 442                 return ERROR_FAIL;
 443         }
 444 }
 445
 446 static int svf_parse_cmd_string(char *str, int len, char **argus, int *num_of_argu)
 447 {
 448         int pos = 0, num = 0, space_found = 1;
 449
 450         while (pos < len)
 451         {
 452                 switch(str[pos])
 453                 {
 454                 case '\n':
 455                 case '\r':
 456                 case '!':
 457                 case '/':
 458                         LOG_ERROR("fail to parse svf command");
 459                         return ERROR_FAIL;
 460                         break;
 461                 case ' ':
 462                         space_found = 1;
 463                         str[pos] = '\0';
 464                         break;
 465                 default:
 466                         if (space_found)
 467                         {
 468                                 argus[num++] = &str[pos];
 469                                 space_found = 0;
 470                         }
 471                         break;
 472                 }
 473                 pos++;
 474         }
 475
 476         *num_of_argu = num;
 477
 478         return ERROR_OK;
 479 }
 480
 481 static int svf_tap_state_is_stable(tap_state_t state)
 482 {
 483         return ((TAP_RESET == state) || (TAP_IDLE == state) || (TAP_DRPAUSE == state) || (TAP_IRPAUSE == state));
 484 }
 485
 486 static int svf_tap_state_is_valid(tap_state_t state)
 487 {
 488         return ((state >= 0) && (state < sizeof(svf_tap_state_name)));
 489 }
 490
 491 static int svf_find_string_in_array(char *str, char **strs, int num_of_element)
 492 {
 493         int i;
 494
 495         for (i = 0; i < num_of_element; i++)
 496         {
 497                 if (!strcmp(str, strs[i]))
 498                 {
 499                         return i;
 500                 }
 501         }
 502         return 0xFF;
 503 }
 504
 505 static int svf_adjust_array_length(u8 **arr, int orig_bit_len, int new_bit_len)
 506 {
 507         int new_byte_len = (new_bit_len + 7) >> 3;
 508
 509         if ((NULL == *arr) || (((orig_bit_len + 7) >> 3) < ((new_bit_len + 7) >> 3)))
 510         {
 511                 if (*arr != NULL)
 512                 {
 513                         free(*arr);
 514                         *arr = NULL;
 515                 }
 516                 *arr = (u8*)malloc(new_byte_len);
 517                 if (NULL == *arr)
 518                 {
 519                         LOG_ERROR("not enough memory");
 520                         return ERROR_FAIL;
 521                 }
 522                 memset(*arr, 0, new_byte_len);
 523         }
 524         return ERROR_OK;
 525 }
 526
 527 static int svf_copy_hexstring_to_binary(char *str, u8 **bin, int orig_bit_len, int bit_len)
 528 {
 529         int i, str_len = strlen(str), str_byte_len = (bit_len + 3) >> 2, loop_cnt;
 530         u8 ch, need_write = 1;
 531
 532         if (ERROR_OK != svf_adjust_array_length(bin, orig_bit_len, bit_len))
 533         {
 534                 LOG_ERROR("fail to adjust length of array");
 535                 return ERROR_FAIL;
 536         }
 537
 538         if (str_byte_len > str_len)
 539         {
 540                 loop_cnt = str_byte_len;
 541         }
 542         else
 543         {
 544                 loop_cnt = str_len;
 545         }
 546
 547         for (i = 0; i < loop_cnt; i++)
 548         {
 549                 if (i < str_len)
 550                 {
 551                         ch = str[str_len - i - 1];
 552                         if ((ch >= '0') && (ch <= '9'))
 553                         {
 554                                 ch = ch - '0';
 555                         }
 556                         else if ((ch >= 'A') && (ch <= 'F'))
 557                         {
 558                                 ch = ch - 'A' + 10;
 559                         }
 560                         else
 561                         {
 562                                 LOG_ERROR("invalid hex string");
 563                                 return ERROR_FAIL;
 564                         }
 565                 }
 566                 else
 567                 {
 568                         ch = 0;
 569                 }
 570
 571                 // check valid
 572                 if (i >= str_byte_len)
 573                 {
 574                         // all data written, other data should be all '0's and needn't to be written
 575                         need_write = 0;
 576                         if (ch != 0)
 577                         {
 578                                 LOG_ERROR("value execede length");
 579                                 return ERROR_FAIL;
 580                         }
 581                 }
 582                 else if (i == (str_byte_len - 1))
 583                 {
 584                         // last data byte, written if valid
 585                         if ((ch & ~((1 << (bit_len - 4 * i)) - 1)) != 0)
 586                         {
 587                                 LOG_ERROR("value execede length");
 588                                 return ERROR_FAIL;
 589                         }
 590                 }
 591
 592                 if (need_write)
 593                 {
 594                         // write bin
 595                         if (i % 2)
 596                         {
 597                                 // MSB
 598                                 (*bin)[i / 2] |= ch << 4;
 599                         }
 600                         else
 601                         {
 602                                 // LSB
 603                                 (*bin)[i / 2] = 0;
 604                                 (*bin)[i / 2] |= ch;
 605                         }
 606                 }
 607         }
 608
 609         return ERROR_OK;
 610 }
 611
 612 static int svf_check_tdo(void)
 613 {
 614         int i, j, byte_len, index;
 615
 616         for (i = 0; i < svf_check_tdo_para_index; i++)
 617         {
 618                 if (svf_check_tdo_para[i].enabled)
 619                 {
 620                         byte_len = (svf_check_tdo_para[i].bit_len + 7) >> 3;
 621                         index = svf_check_tdo_para[i].buffer_offset;
 622                         for (j = 0; j < byte_len; j++)
 623                         {
 624                                 if ((svf_tdi_buffer[index + j] & svf_mask_buffer[index + j]) != svf_tdo_buffer[index + j])
 625                                 {
 626                                         LOG_ERROR("tdo check error at line %d, read = 0x%X, want = 0x%X, mask = 0x%X",
 627                                                                 svf_check_tdo_para[i].line_num,
 628                                                                 (*(int*)(svf_tdi_buffer + index)) & ((1 << svf_check_tdo_para[i].bit_len) - 1),
 629                                                                 (*(int*)(svf_tdo_buffer + index)) & ((1 << svf_check_tdo_para[i].bit_len) - 1),
 630                                                                 (*(int*)(svf_mask_buffer + index)) & ((1 << svf_check_tdo_para[i].bit_len) - 1));
 631                                         return ERROR_FAIL;
 632                                 }
 633                         }
 634                 }
 635         }
 636         svf_check_tdo_para_index = 0;
 637
 638         return ERROR_OK;
 639 }
 640
 641 static int svf_add_check_para(u8 enabled, int buffer_offset, int bit_len)
 642 {
 643         if (svf_check_tdo_para_index >= SVF_CHECK_TDO_PARA_SIZE)
 644         {
 645                 LOG_ERROR("toooooo many operation undone");
 646                 return ERROR_FAIL;
 647         }
 648
 649         svf_check_tdo_para[svf_check_tdo_para_index].line_num = svf_line_number;
 650         svf_check_tdo_para[svf_check_tdo_para_index].bit_len = bit_len;
 651         svf_check_tdo_para[svf_check_tdo_para_index].enabled = enabled;
 652         svf_check_tdo_para[svf_check_tdo_para_index].buffer_offset = buffer_offset;
 653         svf_check_tdo_para_index++;
 654
 655         return ERROR_OK;
 656 }
 657
 658 // not good to use this
 659 extern jtag_command_t** jtag_get_last_command_p(void);
 660 extern void* cmd_queue_alloc(size_t size);
 661 extern jtag_command_t **last_comand_pointer;
 662
 663 static int svf_run_command(struct command_context_s *cmd_ctx, char *cmd_str)
 664 {
 665         char *argus[256], command;
 666         int num_of_argu = 0, i;
 667
 668         // tmp variable
 669         int i_tmp;
 670
 671         // not good to use this
 672         jtag_command_t **last_cmd;
 673
 674         // for RUNTEST
 675         int run_count;
 676         float min_time, max_time;
 677         // for XXR
 678         svf_xxr_para_t *xxr_para_tmp;
 679         u8 **pbuffer_tmp;
 680         scan_field_t field;
 681         // for STATE
 682         tap_state_t *path = NULL, state;
 683
 684         LOG_DEBUG("%s", cmd_str);
 685
 686         if (ERROR_OK != svf_parse_cmd_string(cmd_str, strlen(cmd_str), argus, &num_of_argu))
 687         {
 688                 return ERROR_FAIL;
 689         }
 690
 691         command = svf_find_string_in_array(argus[0], (char **)svf_command_name, dimof(svf_command_name));
 692         switch(command)
 693         {
 694         case ENDDR:
 695         case ENDIR:
 696                 if (num_of_argu != 2)
 697                 {
 698                         LOG_ERROR("invalid parameter of %s", argus[0]);
 699                         return ERROR_FAIL;
 700                 }
 701                 i_tmp = svf_find_string_in_array(argus[1], (char **)svf_tap_state_name, dimof(svf_tap_state_name));
 702                 if (svf_tap_state_is_stable(i_tmp))
 703                 {
 704                         if (command == ENDIR)
 705                         {
 706                                 svf_para.ir_end_state = i_tmp;
 707                                 LOG_DEBUG("\tir_end_state = %s", svf_tap_state_name[svf_para.ir_end_state]);
 708                         }
 709                         else
 710                         {
 711                                 svf_para.dr_end_state = i_tmp;
 712                                 LOG_DEBUG("\tdr_end_state = %s", svf_tap_state_name[svf_para.dr_end_state]);
 713                         }
 714                 }
 715                 else
 716                 {
 717                         LOG_ERROR("%s is not valid state", argus[1]);
 718                         return ERROR_FAIL;
 719                 }
 720                 break;
 721         case FREQUENCY:
 722                 if ((num_of_argu != 1) && (num_of_argu != 3))
 723                 {
 724                         LOG_ERROR("invalid parameter of %s", argus[0]);
 725                         return ERROR_FAIL;
 726                 }
 727                 if (1 == num_of_argu)
 728                 {
 729                         // TODO: set jtag speed to full speed
 730                         svf_para.frequency = 0;
 731                 }
 732                 else
 733                 {
 734                         if (strcmp(argus[2], "HZ"))
 735                         {
 736                                 LOG_ERROR("HZ not found in FREQUENCY command");
 737                                 return ERROR_FAIL;
 738                         }
 739                         svf_para.frequency = atof(argus[1]);
 740                         // TODO: set jtag speed to
 741                         if (svf_para.frequency > 0)
 742                         {
 743                                 command_run_linef(cmd_ctx, "jtag_khz %d", (int)svf_para.frequency / 1000);
 744                                 LOG_DEBUG("\tfrequency = %f", svf_para.frequency);
 745                         }
 746                 }
 747                 break;
 748         case HDR:
 749                 xxr_para_tmp = &svf_para.hdr_para;
 750                 goto XXR_common;
 751         case HIR:
 752                 xxr_para_tmp = &svf_para.hir_para;
 753                 goto XXR_common;
 754         case TDR:
 755                 xxr_para_tmp = &svf_para.tdr_para;
 756                 goto XXR_common;
 757         case TIR:
 758                 xxr_para_tmp = &svf_para.tir_para;
 759                 goto XXR_common;
 760         case SDR:
 761                 xxr_para_tmp = &svf_para.sdr_para;
 762                 goto XXR_common;
 763         case SIR:
 764                 xxr_para_tmp = &svf_para.sir_para;
 765                 goto XXR_common;
 766                 XXR_common:
 767                 // XXR length [TDI (tdi)] [TDO (tdo)][MASK (mask)] [SMASK (smask)]
 768                 if ((num_of_argu > 10) || (num_of_argu % 2))
 769                 {
 770                         LOG_ERROR("invalid parameter of %s", argus[0]);
 771                         return ERROR_FAIL;
 772                 }
 773                 i_tmp = xxr_para_tmp->len;
 774                 xxr_para_tmp->len = atoi(argus[1]);
 775                 LOG_DEBUG("\tlength = %d", xxr_para_tmp->len);
 776                 xxr_para_tmp->data_mask = 0;
 777                 for (i = 2; i < num_of_argu; i += 2)
 778                 {
 779                         if ((argus[i + 1][0] != '(') || (argus[i + 1][strlen(argus[i + 1]) - 1] != ')'))
 780                         {
 781                                 LOG_ERROR("data section error");
 782                                 return ERROR_FAIL;
 783                         }
 784                         argus[i + 1][strlen(argus[i + 1]) - 1] = '\0';
 785                         // TDI, TDO, MASK, SMASK
 786                         if (!strcmp(argus[i], "TDI"))
 787                         {
 788                                 // TDI
 789                                 pbuffer_tmp = &xxr_para_tmp->tdi;
 790                                 xxr_para_tmp->data_mask |= XXR_TDI;
 791                         }
 792                         else if (!strcmp(argus[i], "TDO"))
 793                         {
 794                                 // TDO
 795                                 pbuffer_tmp = &xxr_para_tmp->tdo;
 796                                 xxr_para_tmp->data_mask |= XXR_TDO;
 797                         }
 798                         else if (!strcmp(argus[i], "MASK"))
 799                         {
 800                                 // MASK
 801                                 pbuffer_tmp = &xxr_para_tmp->mask;
 802                                 xxr_para_tmp->data_mask |= XXR_MASK;
 803                         }
 804                         else if (!strcmp(argus[i], "SMASK"))
 805                         {
 806                                 // SMASK
 807                                 pbuffer_tmp = &xxr_para_tmp->smask;
 808                                 xxr_para_tmp->data_mask |= XXR_SMASK;
 809                         }
 810                         else
 811                         {
 812                                 LOG_ERROR("unknow parameter: %s", argus[i]);
 813                                 return ERROR_FAIL;
 814                         }
 815                         if (ERROR_OK != svf_copy_hexstring_to_binary(&argus[i + 1][1], pbuffer_tmp, i_tmp, xxr_para_tmp->len))
 816                         {
 817                                 LOG_ERROR("fail to parse hex value");
 818                                 return ERROR_FAIL;
 819                         }
 820                         LOG_DEBUG("\t%s = 0x%X", argus[i], (**(int**)pbuffer_tmp) & ((1 << (xxr_para_tmp->len)) - 1));
 821                 }
 822                 // If a command changes the length of the last scan of the same type and the MASK parameter is absent,
 823                 // the mask pattern used is all cares
 824                 if (!(xxr_para_tmp->data_mask & XXR_MASK) && (i_tmp != xxr_para_tmp->len))
 825                 {
 826                         // MASK not defined and length changed
 827                         if (ERROR_OK != svf_adjust_array_length(&xxr_para_tmp->mask, i_tmp, xxr_para_tmp->len))
 828                         {
 829                                 LOG_ERROR("fail to adjust length of array");
 830                                 return ERROR_FAIL;
 831                         }
 832                         buf_set_ones(xxr_para_tmp->mask, xxr_para_tmp->len);
 833                 }
 834                 // do scan if necessary
 835                 if (SDR == command)
 836                 {
 837                         // check buffer size first, reallocate if necessary
 838                         i = svf_para.hdr_para.len + svf_para.sdr_para.len + svf_para.tdr_para.len;
 839                         if ((svf_buffer_size - svf_buffer_index) < ((i + 7) >> 3))
 840                         {
 841 #if 1
 842                                 // simply print error message
 843                                 LOG_ERROR("buffer is not enough, report to author");
 844                                 return ERROR_FAIL;
 845 #else
 846                                 u8 *buffer_tmp;
 847
 848                                 // reallocate buffer
 849                                 buffer_tmp = (u8 *)malloc(svf_buffer_index + ((i + 7) >> 3));
 850                                 if (NULL == buffer_tmp)
 851                                 {
 852                                         LOG_ERROR("not enough memory");
 853                                         return ERROR_FAIL;
 854                                 }
 855                                 memcpy(buffer_tmp, svf_tdi_buffer, svf_buffer_index);
 856                                 // svf_tdi_buffer isn't NULL here
 857                                 free(svf_tdi_buffer);
 858                                 svf_tdi_buffer = buffer_tmp;
 859
 860                                 buffer_tmp = (u8 *)malloc(svf_buffer_index + ((i + 7) >> 3));
 861                                 if (NULL == buffer_tmp)
 862                                 {
 863                                         LOG_ERROR("not enough memory");
 864                                         return ERROR_FAIL;
 865                                 }
 866                                 memcpy(buffer_tmp, svf_tdo_buffer, svf_buffer_index);
 867                                 // svf_tdo_buffer isn't NULL here
 868                                 free(svf_tdo_buffer);
 869                                 svf_tdo_buffer = buffer_tmp;
 870
 871                                 buffer_tmp = (u8 *)malloc(svf_buffer_index + ((i + 7) >> 3));
 872                                 if (NULL == buffer_tmp)
 873                                 {
 874                                         LOG_ERROR("not enough memory");
 875                                         return ERROR_FAIL;
 876                                 }
 877                                 memcpy(buffer_tmp, svf_mask_buffer, svf_buffer_index);
 878                                 // svf_mask_buffer isn't NULL here
 879                                 free(svf_mask_buffer);
 880                                 svf_mask_buffer = buffer_tmp;
 881
 882                                 buffer_tmp = NULL;
 883                                 svf_buffer_size = svf_buffer_index + ((i + 7) >> 3);
 884 #endif
 885                         }
 886
 887                         // assemble dr data
 888                         i = 0;
 889                         buf_set_buf(svf_para.hdr_para.tdi, 0, &svf_tdi_buffer[svf_buffer_index], i, svf_para.hdr_para.len);
 890                         i += svf_para.hdr_para.len;
 891                         buf_set_buf(svf_para.sdr_para.tdi, 0, &svf_tdi_buffer[svf_buffer_index], i, svf_para.sdr_para.len);
 892                         i += svf_para.sdr_para.len;
 893                         buf_set_buf(svf_para.tdr_para.tdi, 0, &svf_tdi_buffer[svf_buffer_index], i, svf_para.tdr_para.len);
 894                         i += svf_para.tdr_para.len;
 895
 896                         // add check data
 897                         if (svf_para.sdr_para.data_mask & XXR_TDO)
 898                         {
 899                                 // assemble dr mask data
 900                                 i = 0;
 901                                 buf_set_buf(svf_para.hdr_para.mask, 0, &svf_mask_buffer[svf_buffer_index], i, svf_para.hdr_para.len);
 902                                 i += svf_para.hdr_para.len;
 903                                 buf_set_buf(svf_para.sdr_para.mask, 0, &svf_mask_buffer[svf_buffer_index], i, svf_para.sdr_para.len);
 904                                 i += svf_para.sdr_para.len;
 905                                 buf_set_buf(svf_para.tdr_para.mask, 0, &svf_mask_buffer[svf_buffer_index], i, svf_para.tdr_para.len);
 906                                 i += svf_para.tdr_para.len;
 907                                 // assemble dr check data
 908                                 i = 0;
 909                                 buf_set_buf(svf_para.hdr_para.tdo, 0, &svf_tdo_buffer[svf_buffer_index], i, svf_para.hdr_para.len);
 910                                 i += svf_para.hdr_para.len;
 911                                 buf_set_buf(svf_para.sdr_para.tdo, 0, &svf_tdo_buffer[svf_buffer_index], i, svf_para.sdr_para.len);
 912                                 i += svf_para.sdr_para.len;
 913                                 buf_set_buf(svf_para.tdr_para.tdo, 0, &svf_tdo_buffer[svf_buffer_index], i, svf_para.tdr_para.len);
 914                                 i += svf_para.tdr_para.len;
 915
 916                                 svf_add_check_para(1, svf_buffer_index, i);
 917                         }
 918                         else
 919                         {
 920                                 svf_add_check_para(0, svf_buffer_index, i);
 921                         }
 922                         field.tap = tap;
 923                         field.num_bits = i;
 924                         field.out_value = &svf_tdi_buffer[svf_buffer_index];
 925                         field.out_mask = NULL;
 926                         field.in_value = &svf_tdi_buffer[svf_buffer_index];
 927                         field.in_check_value = NULL;
 928                         field.in_check_mask = NULL;
 929                         field.in_handler = NULL;
 930                         field.in_handler_priv = NULL;
 931                         jtag_add_plain_dr_scan(1, &field, svf_para.dr_end_state);
 932
 933                         svf_buffer_index += (i + 7) >> 3;
 934                 }
 935                 else if (SIR == command)
 936                 {
 937                         // check buffer size first, reallocate if necessary
 938                         i = svf_para.hir_para.len + svf_para.sir_para.len + svf_para.tir_para.len;
 939                         if ((svf_buffer_size - svf_buffer_index) < ((i + 7) >> 3))
 940                         {
 941 #if 1
 942                                 // simply print error message
 943                                 LOG_ERROR("buffer is not enough, report to author");
 944                                 return ERROR_FAIL;
 945 #else
 946                                 u8 *buffer_tmp;
 947
 948                                 // reallocate buffer
 949                                 buffer_tmp = (u8 *)malloc(svf_buffer_index + ((i + 7) >> 3));
 950                                 if (NULL == buffer_tmp)
 951                                 {
 952                                         LOG_ERROR("not enough memory");
 953                                         return ERROR_FAIL;
 954                                 }
 955                                 memcpy(buffer_tmp, svf_tdi_buffer, svf_buffer_index);
 956                                 // svf_tdi_buffer isn't NULL here
 957                                 free(svf_tdi_buffer);
 958                                 svf_tdi_buffer = buffer_tmp;
 959
 960                                 buffer_tmp = (u8 *)malloc(svf_buffer_index + ((i + 7) >> 3));
 961                                 if (NULL == buffer_tmp)
 962                                 {
 963                                         LOG_ERROR("not enough memory");
 964                                         return ERROR_FAIL;
 965                                 }
 966                                 memcpy(buffer_tmp, svf_tdo_buffer, svf_buffer_index);
 967                                 // svf_tdo_buffer isn't NULL here
 968                                 free(svf_tdo_buffer);
 969                                 svf_tdo_buffer = buffer_tmp;
 970
 971                                 buffer_tmp = (u8 *)malloc(svf_buffer_index + ((i + 7) >> 3));
 972                                 if (NULL == buffer_tmp)
 973                                 {
 974                                         LOG_ERROR("not enough memory");
 975                                         return ERROR_FAIL;
 976                                 }
 977                                 memcpy(buffer_tmp, svf_mask_buffer, svf_buffer_index);
 978                                 // svf_mask_buffer isn't NULL here
 979                                 free(svf_mask_buffer);
 980                                 svf_mask_buffer = buffer_tmp;
 981
 982                                 buffer_tmp = NULL;
 983                                 svf_buffer_size = svf_buffer_index + ((i + 7) >> 3);
 984 #endif
 985                         }
 986
 987                         // assemble ir data
 988                         i = 0;
 989                         buf_set_buf(svf_para.hir_para.tdi, 0, &svf_tdi_buffer[svf_buffer_index], i, svf_para.hir_para.len);
 990                         i += svf_para.hir_para.len;
 991                         buf_set_buf(svf_para.sir_para.tdi, 0, &svf_tdi_buffer[svf_buffer_index], i, svf_para.sir_para.len);
 992                         i += svf_para.sir_para.len;
 993                         buf_set_buf(svf_para.tir_para.tdi, 0, &svf_tdi_buffer[svf_buffer_index], i, svf_para.tir_para.len);
 994                         i += svf_para.tir_para.len;
 995
 996                         // add check data
 997                         if (svf_para.sir_para.data_mask & XXR_TDO)
 998                         {
 999                                 // assemble dr mask data
1000                                 i = 0;
1001                                 buf_set_buf(svf_para.hir_para.mask, 0, &svf_mask_buffer[svf_buffer_index], i, svf_para.hir_para.len);
1002                                 i += svf_para.hir_para.len;
1003                                 buf_set_buf(svf_para.sir_para.mask, 0, &svf_mask_buffer[svf_buffer_index], i, svf_para.sir_para.len);
1004                                 i += svf_para.sir_para.len;
1005                                 buf_set_buf(svf_para.tir_para.mask, 0, &svf_mask_buffer[svf_buffer_index], i, svf_para.tir_para.len);
1006                                 i += svf_para.tir_para.len;
1007                                 // assemble dr check data
1008                                 i = 0;
1009                                 buf_set_buf(svf_para.hir_para.tdo, 0, &svf_tdo_buffer[svf_buffer_index], i, svf_para.hir_para.len);
1010                                 i += svf_para.hir_para.len;
1011                                 buf_set_buf(svf_para.sir_para.tdo, 0, &svf_tdo_buffer[svf_buffer_index], i, svf_para.sir_para.len);
1012                                 i += svf_para.sir_para.len;
1013                                 buf_set_buf(svf_para.tir_para.tdo, 0, &svf_tdo_buffer[svf_buffer_index], i, svf_para.tir_para.len);
1014                                 i += svf_para.tir_para.len;
1015
1016                                 svf_add_check_para(1, svf_buffer_index, i);
1017                         }
1018                         else
1019                         {
1020                                 svf_add_check_para(0, svf_buffer_index, i);
1021                         }
1022                         field.tap = tap;
1023                         field.num_bits = i;
1024                         field.out_value = &svf_tdi_buffer[svf_buffer_index];
1025                         field.out_mask = NULL;
1026                         field.in_value = &svf_tdi_buffer[svf_buffer_index];
1027                         field.in_check_value = NULL;
1028                         field.in_check_mask = NULL;
1029                         field.in_handler = NULL;
1030                         field.in_handler_priv = NULL;
1031                         jtag_add_plain_ir_scan(1, &field, svf_para.ir_end_state);
1032
1033                         svf_buffer_index += (i + 7) >> 3;
1034                 }
1035                 break;
1036         case PIO:
1037         case PIOMAP:
1038                 LOG_ERROR("PIO and PIOMAP are not supported");
1039                 return ERROR_FAIL;
1040                 break;
1041         case RUNTEST:
1042                 // RUNTEST [run_state] run_count run_clk [min_time SEC [MAXIMUM max_time SEC]] [ENDSTATE end_state]
1043                 // RUNTEST [run_state] min_time SEC [MAXIMUM max_time SEC] [ENDSTATE end_state]
1044                 if ((num_of_argu < 3) && (num_of_argu > 11))
1045                 {
1046                         LOG_ERROR("invalid parameter of %s", argus[0]);
1047                         return ERROR_FAIL;
1048                 }
1049                 // init
1050                 run_count = 0;
1051                 min_time = 0;
1052                 max_time = 0;
1053                 i = 1;
1054                 // run_state
1055                 i_tmp = svf_find_string_in_array(argus[i], (char **)svf_tap_state_name, dimof(svf_tap_state_name));
1056                 if (svf_tap_state_is_valid(i_tmp))
1057                 {
1058                         if (svf_tap_state_is_stable(i_tmp))
1059                         {
1060                                 svf_para.runtest_run_state = i_tmp;
1061
1062                                 // When a run_state is specified, the new  run_state becomes the default end_state
1063                                 svf_para.runtest_end_state = i_tmp;
1064                                 LOG_DEBUG("\trun_state = %s", svf_tap_state_name[svf_para.runtest_run_state]);
1065                                 i++;
1066                         }
1067                         else
1068                         {
1069                                 LOG_ERROR("%s is not valid state", svf_tap_state_name[i_tmp]);
1070                                 return ERROR_FAIL;
1071                         }
1072                 }
1073                 // run_count run_clk
1074                 if (((i + 2) <= num_of_argu) && strcmp(argus[i + 1], "SEC"))
1075                 {
1076                         if (!strcmp(argus[i + 1], "TCK"))
1077                         {
1078                                 // clock source is TCK
1079                                 run_count = atoi(argus[i]);
1080                                 LOG_DEBUG("\trun_count@TCK = %d", run_count);
1081                         }
1082                         else
1083                         {
1084                                 LOG_ERROR("%s not supported for clock", argus[i + 1]);
1085                                 return ERROR_FAIL;
1086                         }
1087                         i += 2;
1088                 }
1089                 // min_time SEC
1090                 if (((i + 2) <= num_of_argu) && !strcmp(argus[i + 1], "SEC"))
1091                 {
1092                         min_time = atof(argus[i]);
1093                         LOG_DEBUG("\tmin_time = %fs", min_time);
1094                         i += 2;
1095                 }
1096                 // MAXIMUM max_time SEC
1097                 if (((i + 3) <= num_of_argu) && !strcmp(argus[i], "MAXIMUM") && !strcmp(argus[i + 2], "SEC"))
1098                 {
1099                         max_time = atof(argus[i + 1]);
1100                         LOG_DEBUG("\tmax_time = %fs", max_time);
1101                         i += 3;
1102                 }
1103                 // ENDSTATE end_state
1104                 if (((i + 2) <= num_of_argu) && !strcmp(argus[i], "ENDSTATE"))
1105                 {
1106                         i_tmp = svf_find_string_in_array(argus[i + 1], (char **)svf_tap_state_name, dimof(svf_tap_state_name));
1107                         if (svf_tap_state_is_stable(i_tmp))
1108                         {
1109                                 svf_para.runtest_end_state = i_tmp;
1110                                 LOG_DEBUG("\tend_state = %s", svf_tap_state_name[svf_para.runtest_end_state]);
1111                         }
1112                         else
1113                         {
1114                                 LOG_ERROR("%s is not valid state", svf_tap_state_name[i_tmp]);
1115                                 return ERROR_FAIL;
1116                         }
1117                         i += 2;
1118                 }
1119                 // calculate run_count
1120                 if ((0 == run_count) && (min_time > 0))
1121                 {
1122                         run_count = min_time * svf_para.frequency;
1123                 }
1124                 // all parameter should be parsed
1125                 if (i == num_of_argu)
1126                 {
1127                         if (run_count > 0)
1128                         {
1129                                 // TODO: do runtest
1130                                 if (svf_para.runtest_run_state != TAP_IDLE)
1131                                 {
1132                                         // RUNTEST can only executed in TAP_IDLE
1133                                         LOG_ERROR("cannot runtest in %s state", svf_tap_state_name[svf_para.runtest_run_state]);
1134                                         return ERROR_FAIL;
1135                                 }
1136                                 jtag_add_runtest(run_count, svf_para.runtest_end_state);
1137                         }
1138                 }
1139                 else
1140                 {
1141                         LOG_ERROR("fail to parse parameter of RUNTEST, %d out of %d is parsed", i, num_of_argu);
1142                         return ERROR_FAIL;
1143                 }
1144                 break;
1145         case STATE:
1146                 // STATE [pathstate1 [pathstate2 ...[pathstaten]]] stable_state
1147                 if (num_of_argu < 2)
1148                 {
1149                         LOG_ERROR("invalid parameter of %s", argus[0]);
1150                         return ERROR_FAIL;
1151                 }
1152                 if (num_of_argu > 2)
1153                 {
1154                         // STATE pathstate1 ... stable_state
1155                         path = (tap_state_t *)malloc((num_of_argu - 1) * sizeof(tap_state_t));
1156                         if (NULL == path)
1157                         {
1158                                 LOG_ERROR("not enough memory");
1159                                 return ERROR_FAIL;
1160                         }
1161                         for (i = 1; i < num_of_argu; i++)
1162                         {
1163                                 path[i - 1] = svf_find_string_in_array(argus[i], (char **)svf_tap_state_name, dimof(svf_tap_state_name));
1164                                 if (!svf_tap_state_is_valid(path[i - 1]))
1165                                 {
1166                                         LOG_ERROR("%s is not valid state", svf_tap_state_name[path[i - 1]]);
1167                                         return ERROR_FAIL;
1168                                 }
1169                                 if (TAP_RESET == path[i - 1])
1170                                 {
1171                                         LOG_ERROR("TAP_RESET is not allowed in pathmove");
1172                                         return ERROR_FAIL;
1173                                 }
1174                         }
1175                         if (svf_tap_state_is_stable(path[num_of_argu - 1]))
1176                         {
1177                                 // last state MUST be stable state
1178                                 // TODO: call path_move
1179                                 jtag_add_pathmove(num_of_argu - 1, path);
1180                                 LOG_DEBUG("\tmove to %s by path_move", svf_tap_state_name[path[num_of_argu - 1]]);
1181                         }
1182                         else
1183                         {
1184                                 LOG_ERROR("%s is not valid state", svf_tap_state_name[path[num_of_argu - 1]]);
1185                                 return ERROR_FAIL;
1186                         }
1187                         if (NULL != path)
1188                         {
1189                                 free(path);
1190                                 path = NULL;
1191                         }
1192                 }
1193                 else
1194                 {
1195                         // STATE stable_state
1196                         state = svf_find_string_in_array(argus[1], (char **)svf_tap_state_name, dimof(svf_tap_state_name));
1197                         if (svf_tap_state_is_stable(state))
1198                         {
1199                                 // TODO: move to state
1200                                 last_cmd = jtag_get_last_command_p();
1201                                 *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
1202                                 last_comand_pointer = &((*last_cmd)->next);
1203                                 (*last_cmd)->next = NULL;
1204                                 (*last_cmd)->type = JTAG_STATEMOVE;
1205                                 (*last_cmd)->cmd.statemove = cmd_queue_alloc(sizeof(statemove_command_t));
1206                                 (*last_cmd)->cmd.statemove->end_state = state;
1207
1208                                 LOG_DEBUG("\tmove to %s by state_move", svf_tap_state_name[state]);
1209                         }
1210                         else
1211                         {
1212                                 LOG_ERROR("%s is not valid state", svf_tap_state_name[state]);
1213                                 return ERROR_FAIL;
1214                         }
1215                 }
1216                 break;
1217         case TRST:
1218                 // TRST trst_mode
1219                 if (num_of_argu != 2)
1220                 {
1221                         LOG_ERROR("invalid parameter of %s", argus[0]);
1222                         return ERROR_FAIL;
1223                 }
1224                 if (svf_para.trst_mode != TRST_ABSENT)
1225                 {
1226                         i_tmp = svf_find_string_in_array(argus[1], (char **)svf_trst_mode_name, dimof(svf_trst_mode_name));
1227                         switch (i_tmp)
1228                         {
1229                         case TRST_ON:
1230                                 jtag_add_reset(1, 0);
1231                                 break;
1232                         case TRST_OFF:
1233                                 jtag_add_reset(1, 1);
1234                                 break;
1235                         case TRST_Z:
1236                                 break;
1237                         case TRST_ABSENT:
1238                                 break;
1239                         default:
1240                                 LOG_ERROR("unknown TRST mode: %s", argus[1]);
1241                                 return ERROR_FAIL;
1242                         }
1243                         svf_para.trst_mode = i_tmp;
1244                         LOG_DEBUG("\ttrst_mode = %s", svf_trst_mode_name[svf_para.trst_mode]);
1245                 }
1246                 else
1247                 {
1248                         LOG_ERROR("can not accpet TRST command if trst_mode is ABSENT");
1249                         return ERROR_FAIL;
1250                 }
1251                 break;
1252         default:
1253                 LOG_ERROR("invalid svf command: %s", argus[0]);
1254                 return ERROR_FAIL;
1255                 break;
1256         }
1257
1258         if (debug_level >= LOG_LVL_DEBUG)
1259         {
1260                 // for convenient debugging, execute tap if possible
1261                 if ((svf_buffer_index > 0) && \
1262                         (((command != STATE) && (command != RUNTEST)) || \
1263                         ((command == STATE) && (num_of_argu == 2))))
1264                 {
1265                         // there is data to be executed
1266                         if (ERROR_OK != jtag_execute_queue())
1267                         {
1268                                 return ERROR_FAIL;
1269                         }
1270                         // output debug info
1271                         if ((SIR == command) || (SDR == command))
1272                         {
1273                                 LOG_DEBUG("\tTDO read = 0x%X", (*(int*)svf_tdi_buffer) & ((1 << (svf_check_tdo_para[0].bit_len)) - 1));
1274                         }
1275                         if (ERROR_OK != svf_check_tdo())
1276                         {
1277                                 return ERROR_FAIL;
1278                         }
1279
1280                         svf_buffer_index = 0;
1281                 }
1282         }
1283         else
1284         {
1285                 // for fast executing, execute tap if necessary
1286                 // half of the buffer is for the next command
1287                 if (((svf_buffer_index >= SVF_MAX_BUFFER_SIZE_TO_COMMIT) || (svf_check_tdo_para_index >= SVF_CHECK_TDO_PARA_SIZE / 2)) && \
1288                         (((command != STATE) && (command != RUNTEST)) || \
1289                         ((command == STATE) && (num_of_argu == 2))))
1290                 {
1291                         if (ERROR_OK != jtag_execute_queue())
1292                         {
1293                                 return ERROR_FAIL;
1294                         }
1295                         else if (ERROR_OK != svf_check_tdo())
1296                         {
1297                                 return ERROR_FAIL;
1298                         }
1299
1300                         svf_buffer_index = 0;
1301                 }
1302         }
1303
1304         return ERROR_OK;
1305 }