src/jtag/drivers/buspirate.c

   1 /***************************************************************************
   2  *   Copyright (C) 2010 by Michal Demin                                    *
   3  *   based on usbprog.c and arm-jtag-ew.c                                  *
   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 #ifdef HAVE_CONFIG_H
  22 #include "config.h"
  23 #endif
  24
  25 #include <jtag/interface.h>
  26 #include <jtag/commands.h>
  27
  28 #include <termios.h>
  29 #include <fcntl.h>
  30 #include <sys/ioctl.h>
  31
  32 #undef DEBUG_SERIAL
  33 /*#define DEBUG_SERIAL */
  34 static int buspirate_execute_queue(void);
  35 static int buspirate_speed(int speed);
  36 static int buspirate_khz(int khz, int *jtag_speed);
  37 static int buspirate_init(void);
  38 static int buspirate_quit(void);
  39
  40 static void buspirate_end_state(tap_state_t state);
  41 static void buspirate_state_move(void);
  42 static void buspirate_path_move(int num_states, tap_state_t *path);
  43 static void buspirate_runtest(int num_cycles);
  44 static void buspirate_scan(bool ir_scan, enum scan_type type,
  45         uint8_t *buffer, int scan_size, struct scan_command *command);
  46
  47 #define CMD_UNKNOWN       0x00
  48 #define CMD_PORT_MODE     0x01
  49 #define CMD_FEATURE       0x02
  50 #define CMD_READ_ADCS     0x03
  51 /*#define CMD_TAP_SHIFT     0x04 // old protocol */
  52 #define CMD_TAP_SHIFT     0x05
  53 #define CMD_ENTER_OOCD    0x06
  54 #define CMD_UART_SPEED    0x07
  55 #define CMD_JTAG_SPEED    0x08
  56
  57 /* Not all OSes have this speed defined */
  58 #if !defined(B1000000)
  59 #define  B1000000 0010010
  60 #endif
  61
  62 enum {
  63         MODE_HIZ = 0,
  64         MODE_JTAG = 1,          /* push-pull outputs */
  65         MODE_JTAG_OD = 2,       /* open-drain outputs */
  66 };
  67
  68 enum {
  69         FEATURE_LED = 0x01,
  70         FEATURE_VREG = 0x02,
  71         FEATURE_TRST = 0x04,
  72         FEATURE_SRST = 0x08,
  73         FEATURE_PULLUP = 0x10
  74 };
  75
  76 enum {
  77         ACTION_DISABLE = 0,
  78         ACTION_ENABLE = 1
  79 };
  80
  81 enum {
  82         SERIAL_NORMAL = 0,
  83         SERIAL_FAST = 1
  84 };
  85
  86 static int buspirate_fd = -1;
  87 static int buspirate_pinmode = MODE_JTAG_OD;
  88 static int buspirate_baudrate = SERIAL_NORMAL;
  89 static int buspirate_vreg;
  90 static int buspirate_pullup;
  91 static char *buspirate_port;
  92
  93 /* TAP interface */
  94 static void buspirate_tap_init(void);
  95 static int buspirate_tap_execute(void);
  96 static void buspirate_tap_append(int tms, int tdi);
  97 static void buspirate_tap_append_scan(int length, uint8_t *buffer,
  98                 struct scan_command *command);
  99 static void buspirate_tap_make_space(int scan, int bits);
 100
 101 static void buspirate_reset(int trst, int srst);
 102
 103 /* low level interface */
 104 static void buspirate_jtag_reset(int);
 105 static void buspirate_jtag_enable(int);
 106 static unsigned char buspirate_jtag_command(int, char *, int);
 107 static void buspirate_jtag_set_speed(int, char);
 108 static void buspirate_jtag_set_mode(int, char);
 109 static void buspirate_jtag_set_feature(int, char, char);
 110 static void buspirate_jtag_get_adcs(int);
 111
 112 /* low level HW communication interface */
 113 static int buspirate_serial_open(char *port);
 114 static int buspirate_serial_setspeed(int fd, char speed);
 115 static int buspirate_serial_write(int fd, char *buf, int size);
 116 static int buspirate_serial_read(int fd, char *buf, int size);
 117 static void buspirate_serial_close(int fd);
 118 static void buspirate_print_buffer(char *buf, int size);
 119
 120 static int buspirate_speed(int speed)
 121 {
 122         /* TODO */
 123         LOG_INFO("Want to set speed to %dkHz, but not implemented yet", speed);
 124         return ERROR_OK;
 125 }
 126
 127 static int buspirate_khz(int khz, int *jtag_speed)
 128 {
 129         *jtag_speed = khz;
 130         return ERROR_OK;
 131 }
 132
 133 static int buspirate_execute_queue(void)
 134 {
 135         /* currently processed command */
 136         struct jtag_command *cmd = jtag_command_queue;
 137         int scan_size;
 138         enum scan_type type;
 139         uint8_t *buffer;
 140
 141         while (cmd) {
 142                 switch (cmd->type) {
 143                 case JTAG_RUNTEST:
 144                         DEBUG_JTAG_IO("runtest %i cycles, end in %s",
 145                                 cmd->cmd.runtest->num_cycles,
 146                                 tap_state_name(cmd->cmd.runtest
 147                                         ->end_state));
 148                         buspirate_end_state(cmd->cmd.runtest
 149                                         ->end_state);
 150                         buspirate_runtest(cmd->cmd.runtest
 151                                         ->num_cycles);
 152                         break;
 153                 case JTAG_TLR_RESET:
 154                         DEBUG_JTAG_IO("statemove end in %s",
 155                                 tap_state_name(cmd->cmd.statemove
 156                                                 ->end_state));
 157                         buspirate_end_state(cmd->cmd.statemove
 158                                         ->end_state);
 159                         buspirate_state_move();
 160                         break;
 161                 case JTAG_PATHMOVE:
 162                         DEBUG_JTAG_IO("pathmove: %i states, end in %s",
 163                                 cmd->cmd.pathmove->num_states,
 164                                 tap_state_name(cmd->cmd.pathmove
 165                                         ->path[cmd->cmd.pathmove
 166                                                 ->num_states - 1]));
 167                         buspirate_path_move(cmd->cmd.pathmove
 168                                         ->num_states,
 169                                         cmd->cmd.pathmove->path);
 170                         break;
 171                 case JTAG_SCAN:
 172                         DEBUG_JTAG_IO("scan end in %s",
 173                                 tap_state_name(cmd->cmd.scan
 174                                         ->end_state));
 175
 176                         buspirate_end_state(cmd->cmd.scan
 177                                         ->end_state);
 178
 179                         scan_size = jtag_build_buffer(cmd->cmd.scan,
 180                                         &buffer);
 181                         type = jtag_scan_type(cmd->cmd.scan);
 182                         buspirate_scan(cmd->cmd.scan->ir_scan, type,
 183                                 buffer, scan_size, cmd->cmd.scan);
 184
 185                         break;
 186                 case JTAG_RESET:
 187                         DEBUG_JTAG_IO("reset trst: %i srst %i",
 188                                 cmd->cmd.reset->trst, cmd->cmd.reset->srst);
 189
 190                         /* flush buffers, so we can reset */
 191                         buspirate_tap_execute();
 192
 193                         if (cmd->cmd.reset->trst == 1)
 194                                 tap_set_state(TAP_RESET);
 195                         buspirate_reset(cmd->cmd.reset->trst,
 196                                         cmd->cmd.reset->srst);
 197                         break;
 198                 case JTAG_SLEEP:
 199                         DEBUG_JTAG_IO("sleep %i", cmd->cmd.sleep->us);
 200                         buspirate_tap_execute();
 201                         jtag_sleep(cmd->cmd.sleep->us);
 202                                 break;
 203                 default:
 204                         LOG_ERROR("BUG: unknown JTAG command type encountered");
 205                         exit(-1);
 206                 }
 207
 208                 cmd = cmd->next;
 209         }
 210
 211         return buspirate_tap_execute();
 212 }
 213
 214 static int buspirate_init(void)
 215 {
 216         if (buspirate_port == NULL) {
 217                 LOG_ERROR("You need to specify the serial port!");
 218                 return ERROR_JTAG_INIT_FAILED;
 219         }
 220
 221         buspirate_fd = buspirate_serial_open(buspirate_port);
 222         if (buspirate_fd == -1) {
 223                 LOG_ERROR("Could not open serial port");
 224                 return ERROR_JTAG_INIT_FAILED;
 225         }
 226
 227         buspirate_serial_setspeed(buspirate_fd, SERIAL_NORMAL);
 228
 229         buspirate_jtag_enable(buspirate_fd);
 230
 231         if (buspirate_baudrate != SERIAL_NORMAL)
 232                 buspirate_jtag_set_speed(buspirate_fd, SERIAL_FAST);
 233
 234         LOG_INFO("Buspirate Interface ready!");
 235
 236         buspirate_tap_init();
 237         buspirate_jtag_set_mode(buspirate_fd, buspirate_pinmode);
 238         buspirate_jtag_set_feature(buspirate_fd, FEATURE_VREG,
 239                 (buspirate_vreg == 1) ? ACTION_ENABLE : ACTION_DISABLE);
 240         buspirate_jtag_set_feature(buspirate_fd, FEATURE_PULLUP,
 241                 (buspirate_pullup == 1) ? ACTION_ENABLE : ACTION_DISABLE);
 242         buspirate_reset(0, 0);
 243
 244         return ERROR_OK;
 245 }
 246
 247 static int buspirate_quit(void)
 248 {
 249         LOG_INFO("Shutting down buspirate.");
 250         buspirate_jtag_set_mode(buspirate_fd, MODE_HIZ);
 251
 252         buspirate_jtag_set_speed(buspirate_fd, SERIAL_NORMAL);
 253         buspirate_jtag_reset(buspirate_fd);
 254
 255         buspirate_serial_close(buspirate_fd);
 256
 257         if (buspirate_port) {
 258                 free(buspirate_port);
 259                 buspirate_port = NULL;
 260         }
 261         return ERROR_OK;
 262 }
 263
 264 /* openocd command interface */
 265 COMMAND_HANDLER(buspirate_handle_adc_command)
 266 {
 267         if (buspirate_fd == -1)
 268                 return ERROR_OK;
 269
 270         /* send the command */
 271         buspirate_jtag_get_adcs(buspirate_fd);
 272
 273         return ERROR_OK;
 274
 275 }
 276
 277 COMMAND_HANDLER(buspirate_handle_vreg_command)
 278 {
 279         if (CMD_ARGC < 1)
 280                 return ERROR_COMMAND_SYNTAX_ERROR;
 281
 282         if (atoi(CMD_ARGV[0]) == 1)
 283                 buspirate_vreg = 1;
 284         else if (atoi(CMD_ARGV[0]) == 0)
 285                 buspirate_vreg = 0;
 286         else
 287                 LOG_ERROR("usage: buspirate_vreg <1|0>");
 288
 289         return ERROR_OK;
 290
 291 }
 292
 293 COMMAND_HANDLER(buspirate_handle_pullup_command)
 294 {
 295         if (CMD_ARGC < 1)
 296                 return ERROR_COMMAND_SYNTAX_ERROR;
 297
 298         if (atoi(CMD_ARGV[0]) == 1)
 299                 buspirate_pullup = 1;
 300         else if (atoi(CMD_ARGV[0]) == 0)
 301                 buspirate_pullup = 0;
 302         else
 303                 LOG_ERROR("usage: buspirate_pullup <1|0>");
 304
 305         return ERROR_OK;
 306
 307 }
 308
 309 COMMAND_HANDLER(buspirate_handle_led_command)
 310 {
 311         if (CMD_ARGC < 1)
 312                 return ERROR_COMMAND_SYNTAX_ERROR;
 313
 314         if (atoi(CMD_ARGV[0]) == 1) {
 315                 /* enable led */
 316                 buspirate_jtag_set_feature(buspirate_fd, FEATURE_LED,
 317                                 ACTION_ENABLE);
 318         } else if (atoi(CMD_ARGV[0]) == 0) {
 319                 /* disable led */
 320                 buspirate_jtag_set_feature(buspirate_fd, FEATURE_LED,
 321                                 ACTION_DISABLE);
 322         } else {
 323                 LOG_ERROR("usage: buspirate_led <1|0>");
 324         }
 325
 326         return ERROR_OK;
 327
 328 }
 329
 330 COMMAND_HANDLER(buspirate_handle_mode_command)
 331 {
 332         if (CMD_ARGC < 1)
 333                 return ERROR_COMMAND_SYNTAX_ERROR;
 334
 335         if (CMD_ARGV[0][0] == 'n')
 336                 buspirate_pinmode = MODE_JTAG;
 337         else if (CMD_ARGV[0][0] == 'o')
 338                 buspirate_pinmode = MODE_JTAG_OD;
 339         else
 340                 LOG_ERROR("usage: buspirate_mode <normal|open-drain>");
 341
 342         return ERROR_OK;
 343
 344 }
 345
 346 COMMAND_HANDLER(buspirate_handle_speed_command)
 347 {
 348         if (CMD_ARGC < 1)
 349                 return ERROR_COMMAND_SYNTAX_ERROR;
 350
 351         if (CMD_ARGV[0][0] == 'n')
 352                 buspirate_baudrate = SERIAL_NORMAL;
 353         else if (CMD_ARGV[0][0] == 'f')
 354                 buspirate_baudrate = SERIAL_FAST;
 355         else
 356                 LOG_ERROR("usage: buspirate_speed <normal|fast>");
 357
 358         return ERROR_OK;
 359
 360 }
 361
 362 COMMAND_HANDLER(buspirate_handle_port_command)
 363 {
 364         if (CMD_ARGC < 1)
 365                 return ERROR_COMMAND_SYNTAX_ERROR;
 366
 367         if (buspirate_port == NULL)
 368                 buspirate_port = strdup(CMD_ARGV[0]);
 369
 370         return ERROR_OK;
 371
 372 }
 373
 374 static const struct command_registration buspirate_command_handlers[] = {
 375         {
 376                 .name = "buspirate_adc",
 377                 .handler = &buspirate_handle_adc_command,
 378                 .mode = COMMAND_EXEC,
 379                 .help = "reads voltages on adc pins",
 380         },
 381         {
 382                 .name = "buspirate_vreg",
 383                 .usage = "<1|0>",
 384                 .handler = &buspirate_handle_vreg_command,
 385                 .mode = COMMAND_CONFIG,
 386                 .help = "changes the state of voltage regulators",
 387         },
 388         {
 389                 .name = "buspirate_pullup",
 390                 .usage = "<1|0>",
 391                 .handler = &buspirate_handle_pullup_command,
 392                 .mode = COMMAND_CONFIG,
 393                 .help = "changes the state of pullup",
 394         },
 395         {
 396                 .name = "buspirate_led",
 397                 .usage = "<1|0>",
 398                 .handler = &buspirate_handle_led_command,
 399                 .mode = COMMAND_EXEC,
 400                 .help = "changes the state of led",
 401         },
 402         {
 403                 .name = "buspirate_speed",
 404                 .usage = "<normal|fast>",
 405                 .handler = &buspirate_handle_speed_command,
 406                 .mode = COMMAND_CONFIG,
 407                 .help = "speed of the interface",
 408         },
 409         {
 410                 .name = "buspirate_mode",
 411                 .usage = "<normal|open-drain>",
 412                 .handler = &buspirate_handle_mode_command,
 413                 .mode = COMMAND_CONFIG,
 414                 .help = "pin mode of the interface",
 415         },
 416         {
 417                 .name = "buspirate_port",
 418                 .usage = "/dev/ttyUSB0",
 419                 .handler = &buspirate_handle_port_command,
 420                 .mode = COMMAND_CONFIG,
 421                 .help = "name of the serial port to open",
 422         },
 423         COMMAND_REGISTRATION_DONE
 424 };
 425
 426 struct jtag_interface buspirate_interface = {
 427         .name = "buspirate",
 428         .execute_queue = buspirate_execute_queue,
 429         .speed = buspirate_speed,
 430         .khz = buspirate_khz,
 431         .commands = buspirate_command_handlers,
 432         .init = buspirate_init,
 433         .quit = buspirate_quit
 434 };
 435
 436 /*************** jtag execute commands **********************/
 437 static void buspirate_end_state(tap_state_t state)
 438 {
 439         if (tap_is_state_stable(state))
 440                 tap_set_end_state(state);
 441         else {
 442                 LOG_ERROR("BUG: %i is not a valid end state", state);
 443                 exit(-1);
 444         }
 445 }
 446
 447 static void buspirate_state_move(void)
 448 {
 449         int i = 0, tms = 0;
 450         uint8_t tms_scan = tap_get_tms_path(tap_get_state(),
 451                         tap_get_end_state());
 452         int tms_count = tap_get_tms_path_len(tap_get_state(),
 453                         tap_get_end_state());
 454
 455         for (i = 0; i < tms_count; i++) {
 456                 tms = (tms_scan >> i) & 1;
 457                 buspirate_tap_append(tms, 0);
 458         }
 459
 460         tap_set_state(tap_get_end_state());
 461 }
 462
 463 static void buspirate_path_move(int num_states, tap_state_t *path)
 464 {
 465         int i;
 466
 467         for (i = 0; i < num_states; i++) {
 468                 if (tap_state_transition(tap_get_state(), false) == path[i]) {
 469                         buspirate_tap_append(0, 0);
 470                 } else if (tap_state_transition(tap_get_state(), true)
 471                                 == path[i]) {
 472                         buspirate_tap_append(1, 0);
 473                 } else {
 474                         LOG_ERROR("BUG: %s -> %s isn't a valid "
 475                                 "TAP transition",
 476                                 tap_state_name(tap_get_state()),
 477                                 tap_state_name(path[i]));
 478                         exit(-1);
 479                 }
 480
 481                 tap_set_state(path[i]);
 482         }
 483
 484         tap_set_end_state(tap_get_state());
 485 }
 486
 487 static void buspirate_runtest(int num_cycles)
 488 {
 489         int i;
 490
 491         tap_state_t saved_end_state = tap_get_end_state();
 492
 493         /* only do a state_move when we're not already in IDLE */
 494         if (tap_get_state() != TAP_IDLE) {
 495                 buspirate_end_state(TAP_IDLE);
 496                 buspirate_state_move();
 497         }
 498
 499         for (i = 0; i < num_cycles; i++)
 500                 buspirate_tap_append(0, 0);
 501
 502         DEBUG_JTAG_IO("runtest: cur_state %s end_state %s",
 503                         tap_state_name(tap_get_state()),
 504                         tap_state_name(tap_get_end_state()));
 505
 506         /* finish in end_state */
 507         buspirate_end_state(saved_end_state);
 508         if (tap_get_state() != tap_get_end_state())
 509                 buspirate_state_move();
 510 }
 511
 512 static void buspirate_scan(bool ir_scan, enum scan_type type,
 513         uint8_t *buffer, int scan_size, struct scan_command *command)
 514 {
 515         tap_state_t saved_end_state;
 516
 517         buspirate_tap_make_space(1, scan_size+8);
 518         /* is 8 correct ? (2 moves = 16) */
 519
 520         saved_end_state = tap_get_end_state();
 521
 522         buspirate_end_state(ir_scan ? TAP_IRSHIFT : TAP_DRSHIFT);
 523
 524         /* Only move if we're not already there */
 525         if (tap_get_state() != tap_get_end_state())
 526                 buspirate_state_move();
 527
 528         buspirate_tap_append_scan(scan_size, buffer, command);
 529
 530         /* move to PAUSE */
 531         buspirate_tap_append(0, 0);
 532
 533         /* restore the saved state */
 534         buspirate_end_state(saved_end_state);
 535         tap_set_state(ir_scan ? TAP_IRPAUSE : TAP_DRPAUSE);
 536
 537         if (tap_get_state() != tap_get_end_state())
 538                 buspirate_state_move();
 539 }
 540
 541
 542 /************************* TAP related stuff **********/
 543
 544 #define BUSPIRATE_BUFFER_SIZE 1024
 545 #define BUSPIRATE_MAX_PENDING_SCANS 32
 546
 547 static char tms_chain[BUSPIRATE_BUFFER_SIZE]; /* send */
 548 static char tdi_chain[BUSPIRATE_BUFFER_SIZE]; /* send */
 549 static int tap_chain_index;
 550
 551 struct pending_scan_result /* this was stolen from arm-jtag-ew */
 552 {
 553         int first; /* First bit position in tdo_buffer to read */
 554         int length; /* Number of bits to read */
 555         struct scan_command *command; /* Corresponding scan command */
 556         uint8_t *buffer;
 557 };
 558
 559 static struct pending_scan_result
 560 tap_pending_scans[BUSPIRATE_MAX_PENDING_SCANS];
 561 static int tap_pending_scans_num;
 562
 563 static void buspirate_tap_init(void)
 564 {
 565         tap_chain_index = 0;
 566         tap_pending_scans_num = 0;
 567 }
 568
 569 static int buspirate_tap_execute(void)
 570 {
 571         char tmp[4096];
 572         uint8_t *in_buf;
 573         int i;
 574         int fill_index = 0;
 575         int ret;
 576         int bytes_to_send;
 577
 578         if (tap_chain_index <= 0)
 579                 return ERROR_OK;
 580
 581         LOG_DEBUG("executing tap num bits = %i scans = %i",
 582                         tap_chain_index, tap_pending_scans_num);
 583
 584         bytes_to_send = (tap_chain_index+7) / 8;
 585
 586         tmp[0] = CMD_TAP_SHIFT; /* this command expects number of bits */
 587         tmp[1] = (char)(tap_chain_index >> 8);  /* high */
 588         tmp[2] = (char)(tap_chain_index);  /* low */
 589
 590         fill_index = 3;
 591         for (i = 0; i < bytes_to_send; i++) {
 592                 tmp[fill_index] = tdi_chain[i];
 593                 fill_index++;
 594                 tmp[fill_index] = tms_chain[i];
 595                 fill_index++;
 596         }
 597
 598         ret = buspirate_serial_write(buspirate_fd, tmp, 3 + bytes_to_send*2);
 599         if (ret != bytes_to_send*2+3) {
 600                 LOG_ERROR("error writing :(");
 601                 return ERROR_JTAG_DEVICE_ERROR;
 602         }
 603
 604         ret = buspirate_serial_read(buspirate_fd, tmp, bytes_to_send + 3);
 605         if (ret != bytes_to_send + 3) {
 606                 LOG_ERROR("error reading");
 607                 return ERROR_FAIL;
 608         }
 609         in_buf = (uint8_t *)(&tmp[3]);
 610
 611         /* parse the scans */
 612         for (i = 0; i < tap_pending_scans_num; i++) {
 613                 uint8_t *buffer = tap_pending_scans[i].buffer;
 614                 int length = tap_pending_scans[i].length;
 615                 int first = tap_pending_scans[i].first;
 616                 struct scan_command *command = tap_pending_scans[i].command;
 617
 618                 /* copy bits from buffer */
 619                 buf_set_buf(in_buf, first, buffer, 0, length);
 620
 621                 /* return buffer to higher level */
 622                 if (jtag_read_buffer(buffer, command) != ERROR_OK) {
 623                         buspirate_tap_init();
 624                         return ERROR_JTAG_QUEUE_FAILED;
 625                 }
 626
 627                 free(buffer);
 628         }
 629         tap_pending_scans_num = 0;
 630         tap_chain_index = 0;
 631         return ERROR_OK;
 632 }
 633
 634 static void buspirate_tap_make_space(int scans, int bits)
 635 {
 636         int have_scans = BUSPIRATE_MAX_PENDING_SCANS - tap_pending_scans_num;
 637         int have_bits = BUSPIRATE_BUFFER_SIZE * 8 - tap_chain_index;
 638
 639         if ((have_scans < scans) || (have_bits < bits))
 640                 buspirate_tap_execute();
 641 }
 642
 643 static void buspirate_tap_append(int tms, int tdi)
 644 {
 645         int chain_index;
 646
 647         buspirate_tap_make_space(0, 1);
 648         chain_index = tap_chain_index / 8;
 649
 650         if (chain_index < BUSPIRATE_BUFFER_SIZE) {
 651                 int bit_index = tap_chain_index % 8;
 652                 uint8_t bit = 1 << bit_index;
 653
 654                 if (tms)
 655                         tms_chain[chain_index] |= bit;
 656                 else
 657                         tms_chain[chain_index] &= ~bit;
 658
 659                 if (tdi)
 660                         tdi_chain[chain_index] |= bit;
 661                 else
 662                         tdi_chain[chain_index] &= ~bit;
 663
 664                 tap_chain_index++;
 665         } else
 666                 LOG_ERROR("tap_chain overflow, bad things will happen");
 667
 668 }
 669
 670 static void buspirate_tap_append_scan(int length, uint8_t *buffer,
 671                 struct scan_command *command)
 672 {
 673         int i;
 674         tap_pending_scans[tap_pending_scans_num].length = length;
 675         tap_pending_scans[tap_pending_scans_num].buffer = buffer;
 676         tap_pending_scans[tap_pending_scans_num].command = command;
 677         tap_pending_scans[tap_pending_scans_num].first = tap_chain_index;
 678
 679         for (i = 0; i < length; i++) {
 680                 int tms = (i < length-1 ? 0 : 1);
 681                 int tdi = (buffer[i/8] >> (i%8)) & 1;
 682                 buspirate_tap_append(tms, tdi);
 683         }
 684         tap_pending_scans_num++;
 685 }
 686
 687 /*************** jtag wrapper functions *********************/
 688
 689 /* (1) assert or (0) deassert reset lines */
 690 static void buspirate_reset(int trst, int srst)
 691 {
 692         LOG_DEBUG("trst: %i, srst: %i", trst, srst);
 693
 694         if (trst)
 695                 buspirate_jtag_set_feature(buspirate_fd,
 696                                 FEATURE_TRST, ACTION_DISABLE);
 697         else
 698                 buspirate_jtag_set_feature(buspirate_fd,
 699                                 FEATURE_TRST, ACTION_ENABLE);
 700
 701         if (srst)
 702                 buspirate_jtag_set_feature(buspirate_fd,
 703                                 FEATURE_SRST, ACTION_DISABLE);
 704         else
 705                 buspirate_jtag_set_feature(buspirate_fd,
 706                                 FEATURE_SRST, ACTION_ENABLE);
 707 }
 708
 709 /*************** jtag lowlevel functions ********************/
 710 static void buspirate_jtag_enable(int fd)
 711 {
 712         int ret;
 713         char tmp[21] = { [0 ... 20] = 0x00 };
 714         int done = 0;
 715         int cmd_sent = 0;
 716
 717         LOG_DEBUG("Entering binary mode");
 718         buspirate_serial_write(fd, tmp, 20);
 719         usleep(10000);
 720
 721         /* reads 1 to n "BBIO1"s and one "OCD1" */
 722         while (!done) {
 723                 ret = buspirate_serial_read(fd, tmp, 4);
 724                 if (ret != 4) {
 725                         LOG_ERROR("Buspirate error. Is binary"
 726                                 "/OpenOCD support enabled?");
 727                         exit(-1);
 728                 }
 729                 if (strncmp(tmp, "BBIO", 4) == 0) {
 730                         ret = buspirate_serial_read(fd, tmp, 1);
 731                         if (ret != 1) {
 732                                 LOG_ERROR("Buspirate did not answer correctly! "
 733                                         "Do you have correct firmware?");
 734                                 exit(-1);
 735                         }
 736                         if (tmp[0] != '1') {
 737                                 LOG_ERROR("Unsupported binary protocol");
 738                                 exit(-1);
 739                         }
 740                         if (cmd_sent == 0) {
 741                                 cmd_sent = 1;
 742                                 tmp[0] = CMD_ENTER_OOCD;
 743                                 ret = buspirate_serial_write(fd, tmp, 1);
 744                                 if (ret != 1) {
 745                                         LOG_ERROR("error reading");
 746                                         exit(-1);
 747                                 }
 748                         }
 749                 } else if (strncmp(tmp, "OCD1", 4) == 0)
 750                         done = 1;
 751                 else {
 752                         LOG_ERROR("Buspirate did not answer correctly! "
 753                                 "Do you have correct firmware?");
 754                         exit(-1);
 755                 }
 756         }
 757
 758 }
 759
 760 static void buspirate_jtag_reset(int fd)
 761 {
 762         char tmp[5];
 763
 764         tmp[0] = 0x00; /* exit OCD1 mode */
 765         buspirate_serial_write(fd, tmp, 1);
 766         usleep(10000);
 767         /* We ignore the return value here purposly, nothing we can do */
 768         buspirate_serial_read(fd, tmp, 5);
 769         if (strncmp(tmp, "BBIO1", 5) == 0) {
 770                 tmp[0] = 0x0F; /*  reset BP */
 771                 buspirate_serial_write(fd, tmp, 1);
 772         } else
 773                 LOG_ERROR("Unable to restart buspirate!");
 774 }
 775
 776 static void buspirate_jtag_set_speed(int fd, char speed)
 777 {
 778         int ret;
 779         char tmp[2];
 780         char ack[2];
 781
 782         ack[0] = 0xAA;
 783         ack[1] = 0x55;
 784
 785         tmp[0] = CMD_UART_SPEED;
 786         tmp[1] = speed;
 787         buspirate_jtag_command(fd, tmp, 2);
 788
 789         /* here the adapter changes speed, we need follow */
 790         buspirate_serial_setspeed(fd, speed);
 791
 792         buspirate_serial_write(fd, ack, 2);
 793         ret = buspirate_serial_read(fd, tmp, 2);
 794         if (ret != 2) {
 795                 LOG_ERROR("Buspirate did not ack speed change");
 796                 exit(-1);
 797         }
 798         if ((tmp[0] != CMD_UART_SPEED) || (tmp[1] != speed)) {
 799                 LOG_ERROR("Buspirate did not reply as expected");
 800                 exit(-1);
 801         }
 802         LOG_INFO("Buspirate switched to %s mode",
 803                 (speed == SERIAL_NORMAL) ? "normal" : "FAST");
 804 }
 805
 806
 807 static void buspirate_jtag_set_mode(int fd, char mode)
 808 {
 809         char tmp[2];
 810         tmp[0] = CMD_PORT_MODE;
 811         tmp[1] = mode;
 812         buspirate_jtag_command(fd, tmp, 2);
 813 }
 814
 815 static void buspirate_jtag_set_feature(int fd, char feat, char action)
 816 {
 817         char tmp[3];
 818         tmp[0] = CMD_FEATURE;
 819         tmp[1] = feat;   /* what */
 820         tmp[2] = action; /* action */
 821         buspirate_jtag_command(fd, tmp, 3);
 822 }
 823
 824 static void buspirate_jtag_get_adcs(int fd)
 825 {
 826         uint8_t tmp[10];
 827         uint16_t a, b, c, d;
 828         tmp[0] = CMD_READ_ADCS;
 829         buspirate_jtag_command(fd, (char *)tmp, 1);
 830         a = tmp[2] << 8 | tmp[3];
 831         b = tmp[4] << 8 | tmp[5];
 832         c = tmp[6] << 8 | tmp[7];
 833         d = tmp[8] << 8 | tmp[9];
 834
 835         LOG_INFO("ADC: ADC_Pin = %.02f VPullup = %.02f V33 = %.02f "
 836                 "V50 = %.02f",
 837                 ((float)a)/155.1515, ((float)b)/155.1515,
 838                 ((float)c)/155.1515, ((float)d)/155.1515);
 839 }
 840
 841 static unsigned char buspirate_jtag_command(int fd,
 842                 char *cmd, int cmdlen)
 843 {
 844         int res;
 845         int len = 0;
 846
 847         res = buspirate_serial_write(fd, cmd, cmdlen);
 848
 849         if ((cmd[0] == CMD_UART_SPEED)
 850                                 || (cmd[0] == CMD_PORT_MODE)
 851                                 || (cmd[0] == CMD_FEATURE)
 852                                 || (cmd[0] == CMD_JTAG_SPEED))
 853                 return 1;
 854
 855         if (res == cmdlen) {
 856                 switch (cmd[0]) {
 857                 case CMD_READ_ADCS:
 858                         len = 10; /* 2*sizeof(char)+4*sizeof(uint16_t) */
 859                         break;
 860                 case CMD_TAP_SHIFT:
 861                         len = cmdlen;
 862                         break;
 863                 default:
 864                         LOG_INFO("Wrong !");
 865                 }
 866                 res =  buspirate_serial_read(fd, cmd, len);
 867                 if (res > 0)
 868                         return (unsigned char)cmd[1];
 869                 else
 870                         return -1;
 871         } else
 872                 return -1;
 873         return 0;
 874 }
 875
 876 /* low level serial port */
 877 /* TODO add support for WIN32 and others ! */
 878 static int buspirate_serial_open(char *port)
 879 {
 880         int fd;
 881         fd = open(buspirate_port, O_RDWR | O_NOCTTY | O_NDELAY);
 882         return fd;
 883 }
 884
 885 static int buspirate_serial_setspeed(int fd, char speed)
 886 {
 887         struct termios t_opt;
 888         speed_t baud = (speed == SERIAL_FAST) ? B1000000 : B115200;
 889
 890         /* set the serial port parameters */
 891         fcntl(fd, F_SETFL, 0);
 892         tcgetattr(fd, &t_opt);
 893         cfsetispeed(&t_opt, baud);
 894         cfsetospeed(&t_opt, baud);
 895         t_opt.c_cflag |= (CLOCAL | CREAD);
 896         t_opt.c_cflag &= ~PARENB;
 897         t_opt.c_cflag &= ~CSTOPB;
 898         t_opt.c_cflag &= ~CSIZE;
 899         t_opt.c_cflag |= CS8;
 900         t_opt.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
 901         t_opt.c_iflag &= ~(IXON | IXOFF | IXANY);
 902         t_opt.c_oflag &= ~OPOST;
 903         t_opt.c_cc[VMIN] = 0;
 904         t_opt.c_cc[VTIME] = 10;
 905         tcflush(fd, TCIFLUSH);
 906         tcsetattr(fd, TCSANOW, &t_opt);
 907
 908         return 0;
 909 }
 910
 911 static int buspirate_serial_write(int fd, char *buf, int size)
 912 {
 913         int ret = 0;
 914
 915         ret = write(fd, buf, size);
 916
 917         LOG_DEBUG("size = %d ret = %d", size, ret);
 918         buspirate_print_buffer(buf, size);
 919
 920         if (ret != size)
 921                 LOG_ERROR("Error sending data");
 922
 923         return ret;
 924 }
 925
 926 static int buspirate_serial_read(int fd, char *buf, int size)
 927 {
 928         int len = 0;
 929         int ret = 0;
 930         int timeout = 0;
 931
 932         while (len < size) {
 933                 ret = read(fd, buf+len, size-len);
 934                 if (ret == -1)
 935                         return -1;
 936
 937                 if (ret == 0) {
 938                         timeout++;
 939
 940                         if (timeout >= 10)
 941                                 break;
 942
 943                         continue;
 944                 }
 945
 946                 len += ret;
 947         }
 948
 949         LOG_DEBUG("should have read = %d actual size = %d", size, len);
 950         buspirate_print_buffer(buf, len);
 951
 952         if (len != size)
 953                 LOG_ERROR("Error reading data");
 954
 955         return len;
 956 }
 957
 958 static void buspirate_serial_close(int fd)
 959 {
 960         close(fd);
 961 }
 962
 963 #define LINE_SIZE      81
 964 #define BYTES_PER_LINE 16
 965 static void buspirate_print_buffer(char *buf, int size)
 966 {
 967         char line[LINE_SIZE];
 968         char tmp[10];
 969         int offset = 0;
 970
 971         line[0] = 0;
 972         while (offset < size) {
 973                 snprintf(tmp, 5, "%02x ", (uint8_t)buf[offset]);
 974                 offset++;
 975
 976                 strcat(line, tmp);
 977
 978                 if (offset % BYTES_PER_LINE == 0) {
 979                         LOG_DEBUG("%s", line);
 980                         line[0] = 0;
 981                 }
 982         }
 983
 984         if (line[0] != 0)
 985                 LOG_DEBUG("%s", line);
 986 }