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