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

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