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