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