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

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