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