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

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