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