git.gag.com Git - fw/openocd/blob - src/jtag/drivers/buspirate.c

   1 /***************************************************************************
   2  *   Copyright (C) 2010 by Michal Demin                                    *
   3  *   based on usbprog.c and arm-jtag-ew.c                                  *
   4  *   Several fixes by R. Diez in 2013.                                     *
   5  *                                                                         *
   6  *   This program is free software; you can redistribute it and/or modify  *
   7  *   it under the terms of the GNU General Public License as published by  *
   8  *   the Free Software Foundation; either version 2 of the License, or     *
   9  *   (at your option) any later version.                                   *
  10  *                                                                         *
  11  *   This program is distributed in the hope that it will be useful,       *
  12  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  13  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  14  *   GNU General Public License for more details.                          *
  15  *                                                                         *
  16  *   You should have received a copy of the GNU General Public License     *
  17  *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
  18  ***************************************************************************/
  19
  20 #ifdef HAVE_CONFIG_H
  21 #include "config.h"
  22 #endif
  23
  24 #include <jtag/interface.h>
  25 #include <jtag/swd.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_init(void);
  36 static int buspirate_quit(void);
  37 static int buspirate_reset(int trst, int srst);
  38
  39 static void buspirate_end_state(tap_state_t state);
  40 static void buspirate_state_move(void);
  41 static void buspirate_path_move(int num_states, tap_state_t *path);
  42 static void buspirate_runtest(int num_cycles);
  43 static void buspirate_scan(bool ir_scan, enum scan_type type,
  44         uint8_t *buffer, int scan_size, struct scan_command *command);
  45 static void buspirate_stableclocks(int num_cycles);
  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_RWIRE   0x05
  54 #define CMD_ENTER_OOCD    0x06
  55 #define CMD_UART_SPEED    0x07
  56 #define CMD_JTAG_SPEED    0x08
  57 #define CMD_RAW_PERIPH    0x40
  58 #define CMD_RAW_SPEED     0x60
  59 #define CMD_RAW_MODE      0x80
  60
  61 #define CMD_TAP_SHIFT_HEADER_LEN 3
  62
  63 /* raw-wire mode configuration */
  64 #define CMD_RAW_CONFIG_HIZ 0x00
  65 #define CMD_RAW_CONFIG_3V3 0x08
  66 #define CMD_RAW_CONFIG_2W  0x00
  67 #define CMD_RAW_CONFIG_3W  0x04
  68 #define CMD_RAW_CONFIG_MSB 0x00
  69 #define CMD_RAW_CONFIG_LSB 0x02
  70
  71 /* Not all OSes have this speed defined */
  72 #if !defined(B1000000)
  73 #define  B1000000 0010010
  74 #endif
  75
  76 #define SHORT_TIMEOUT  1  /* Must be at least 1. */
  77 #define NORMAL_TIMEOUT 10
  78
  79 enum {
  80         MODE_HIZ = 0,
  81         MODE_JTAG = 1,          /* push-pull outputs */
  82         MODE_JTAG_OD = 2,       /* open-drain outputs */
  83 };
  84
  85 enum {
  86         FEATURE_LED = 0x01,
  87         FEATURE_VREG = 0x02,
  88         FEATURE_TRST = 0x04,
  89         FEATURE_SRST = 0x08,
  90         FEATURE_PULLUP = 0x10
  91 };
  92
  93 enum {
  94         ACTION_DISABLE = 0,
  95         ACTION_ENABLE = 1
  96 };
  97
  98 enum {
  99         SERIAL_NORMAL = 0,
 100         SERIAL_FAST = 1
 101 };
 102
 103 enum {
 104         SPEED_RAW_5_KHZ   = 0x0,
 105         SPEED_RAW_50_KHZ  = 0x1,
 106         SPEED_RAW_100_KHZ = 0x2,
 107         SPEED_RAW_400_KHZ = 0x3
 108 };
 109
 110 /* SWD mode specific */
 111 static bool swd_mode;
 112 static int  queued_retval;
 113 static char swd_features;
 114
 115 static int buspirate_fd = -1;
 116 static int buspirate_pinmode = MODE_JTAG_OD;
 117 static int buspirate_baudrate = SERIAL_NORMAL;
 118 static int buspirate_vreg;
 119 static int buspirate_pullup;
 120 static char *buspirate_port;
 121
 122 static enum tap_state last_tap_state = TAP_RESET;
 123
 124 /* SWD interface */
 125 static int buspirate_swd_init(void);
 126 static void buspirate_swd_read_reg(uint8_t cmd, uint32_t *value, uint32_t ap_delay_clk);
 127 static void buspirate_swd_write_reg(uint8_t cmd, uint32_t value, uint32_t ap_delay_clk);
 128 static int buspirate_swd_switch_seq(enum swd_special_seq seq);
 129 static int buspirate_swd_run_queue(void);
 130
 131 /* TAP interface */
 132 static void buspirate_tap_init(void);
 133 static int buspirate_tap_execute(void);
 134 static void buspirate_tap_append(int tms, int tdi);
 135 static void buspirate_tap_append_scan(int length, uint8_t *buffer,
 136                 struct scan_command *command);
 137 static void buspirate_tap_make_space(int scan, int bits);
 138
 139 static void buspirate_set_feature(int, char, char);
 140 static void buspirate_set_mode(int, char);
 141 static void buspirate_set_speed(int, char);
 142
 143 /* low level interface */
 144 static void buspirate_bbio_enable(int);
 145 static void buspirate_jtag_reset(int);
 146 static unsigned char buspirate_jtag_command(int, uint8_t *, int);
 147 static void buspirate_jtag_set_speed(int, char);
 148 static void buspirate_jtag_set_mode(int, char);
 149 static void buspirate_jtag_set_feature(int, char, char);
 150 static void buspirate_jtag_get_adcs(int);
 151
 152 /* low level two-wire interface */
 153 static void buspirate_swd_set_speed(int, char);
 154 static void buspirate_swd_set_feature(int, char, char);
 155 static void buspirate_swd_set_mode(int, char);
 156
 157 /* low level HW communication interface */
 158 static int buspirate_serial_open(char *port);
 159 static int buspirate_serial_setspeed(int fd, char speed, cc_t timeout);
 160 static int buspirate_serial_write(int fd, uint8_t *buf, int size);
 161 static int buspirate_serial_read(int fd, uint8_t *buf, int size);
 162 static void buspirate_serial_close(int fd);
 163 static void buspirate_print_buffer(uint8_t *buf, int size);
 164
 165 static int buspirate_execute_queue(void)
 166 {
 167         /* currently processed command */
 168         struct jtag_command *cmd = jtag_command_queue;
 169         int scan_size;
 170         enum scan_type type;
 171         uint8_t *buffer;
 172
 173         while (cmd) {
 174                 switch (cmd->type) {
 175                 case JTAG_RUNTEST:
 176                         LOG_DEBUG_IO("runtest %i cycles, end in %s",
 177                                 cmd->cmd.runtest->num_cycles,
 178                                 tap_state_name(cmd->cmd.runtest
 179                                         ->end_state));
 180                         buspirate_end_state(cmd->cmd.runtest
 181                                         ->end_state);
 182                         buspirate_runtest(cmd->cmd.runtest
 183                                         ->num_cycles);
 184                         break;
 185                 case JTAG_TLR_RESET:
 186                         LOG_DEBUG_IO("statemove end in %s",
 187                                 tap_state_name(cmd->cmd.statemove
 188                                                 ->end_state));
 189                         buspirate_end_state(cmd->cmd.statemove
 190                                         ->end_state);
 191                         buspirate_state_move();
 192                         break;
 193                 case JTAG_PATHMOVE:
 194                         LOG_DEBUG_IO("pathmove: %i states, end in %s",
 195                                 cmd->cmd.pathmove->num_states,
 196                                 tap_state_name(cmd->cmd.pathmove
 197                                         ->path[cmd->cmd.pathmove
 198                                                 ->num_states - 1]));
 199                         buspirate_path_move(cmd->cmd.pathmove
 200                                         ->num_states,
 201                                         cmd->cmd.pathmove->path);
 202                         break;
 203                 case JTAG_SCAN:
 204                         LOG_DEBUG_IO("scan end in %s",
 205                                 tap_state_name(cmd->cmd.scan
 206                                         ->end_state));
 207
 208                         buspirate_end_state(cmd->cmd.scan
 209                                         ->end_state);
 210
 211                         scan_size = jtag_build_buffer(cmd->cmd.scan,
 212                                         &buffer);
 213                         type = jtag_scan_type(cmd->cmd.scan);
 214                         buspirate_scan(cmd->cmd.scan->ir_scan, type,
 215                                 buffer, scan_size, cmd->cmd.scan);
 216
 217                         break;
 218                 case JTAG_SLEEP:
 219                         LOG_DEBUG_IO("sleep %" PRIu32, cmd->cmd.sleep->us);
 220                         buspirate_tap_execute();
 221                         jtag_sleep(cmd->cmd.sleep->us);
 222                                 break;
 223                 case JTAG_STABLECLOCKS:
 224                         LOG_DEBUG_IO("stable clock %i cycles", cmd->cmd.stableclocks->num_cycles);
 225                         buspirate_stableclocks(cmd->cmd.stableclocks->num_cycles);
 226                                 break;
 227                 default:
 228                         LOG_ERROR("BUG: unknown JTAG command type encountered");
 229                         exit(-1);
 230                 }
 231
 232                 cmd = cmd->next;
 233         }
 234
 235         return buspirate_tap_execute();
 236 }
 237
 238
 239 /* Returns true if successful, false if error. */
 240
 241 static bool read_and_discard_all_data(const int fd)
 242 {
 243         /* LOG_INFO("Discarding any stale data from a previous connection..."); */
 244
 245         bool was_msg_already_printed = false;
 246
 247         for ( ; ; ) {
 248                 uint8_t buffer[1024];  /* Any size will do, it's a trade-off between stack size and performance. */
 249
 250                 const ssize_t read_count = read(fd, buffer, sizeof(buffer));
 251
 252                 if (read_count == 0) {
 253                         /* This is the "end of file" or "connection closed at the other end" condition. */
 254                         return true;
 255                 }
 256
 257                 if (read_count > 0) {
 258                         if (!was_msg_already_printed)   {
 259                                 LOG_INFO("Some stale data from a previous connection was discarded.");
 260                                 was_msg_already_printed = true;
 261                         }
 262
 263                         continue;
 264                 }
 265
 266                 assert(read_count == -1);  /* According to the specification. */
 267
 268                 const int errno_code = errno;
 269
 270                 if (errno_code == EINTR)
 271                         continue;
 272
 273                 if (errno_code == EAGAIN ||
 274                         errno_code == EWOULDBLOCK) {
 275                         /* We know that the file descriptor has been opened with O_NONBLOCK or O_NDELAY,
 276                            and these codes mean that there is no data to read at present. */
 277                         return true;
 278                 }
 279
 280                 /* Some other error has occurred. */
 281                 return false;
 282         }
 283 }
 284
 285
 286 static int buspirate_init(void)
 287 {
 288         if (!buspirate_port) {
 289                 LOG_ERROR("You need to specify the serial port!");
 290                 return ERROR_JTAG_INIT_FAILED;
 291         }
 292
 293         buspirate_fd = buspirate_serial_open(buspirate_port);
 294         if (buspirate_fd == -1) {
 295                 LOG_ERROR("Could not open serial port");
 296                 return ERROR_JTAG_INIT_FAILED;
 297         }
 298
 299         /* The Operating System or the device itself may deliver stale data from the last connection,
 300            so discard all available bytes right after the new connection has been established.
 301            After all, we are implementing here a master/slave protocol, so the slave should have nothing
 302            to say until the master sends the first command.
 303
 304            In the past, there was a tcflush() call in buspirate_serial_setspeed(), but that
 305            was not enough. I guess you must actively read from the serial port to trigger any
 306            data collection from the device and/or lower USB layers. If you disable the serial port
 307            read timeout (if you set SHORT_TIMEOUT to 0), then the discarding does not work any more.
 308
 309            Note that we are lowering the serial port timeout for this first read operation,
 310            otherwise the normal initialisation would be delayed for too long. */
 311
 312         if (-1 == buspirate_serial_setspeed(buspirate_fd, SERIAL_NORMAL, SHORT_TIMEOUT)) {
 313                 LOG_ERROR("Error configuring the serial port.");
 314                 return ERROR_JTAG_INIT_FAILED;
 315         }
 316
 317         if (!read_and_discard_all_data(buspirate_fd)) {
 318                 LOG_ERROR("Error while attempting to discard any stale data right after establishing the connection.");
 319                 return ERROR_JTAG_INIT_FAILED;
 320         }
 321
 322         if (-1 == buspirate_serial_setspeed(buspirate_fd, SERIAL_NORMAL, NORMAL_TIMEOUT)) {
 323                 LOG_ERROR("Error configuring the serial port.");
 324                 return ERROR_JTAG_INIT_FAILED;
 325         }
 326
 327         buspirate_bbio_enable(buspirate_fd);
 328
 329         if (swd_mode || buspirate_baudrate != SERIAL_NORMAL)
 330                 buspirate_set_speed(buspirate_fd, SERIAL_FAST);
 331
 332         LOG_INFO("Buspirate %s Interface ready!", swd_mode ? "SWD" : "JTAG");
 333
 334         if (!swd_mode)
 335                 buspirate_tap_init();
 336
 337         buspirate_set_mode(buspirate_fd, buspirate_pinmode);
 338         buspirate_set_feature(buspirate_fd, FEATURE_VREG,
 339                 (buspirate_vreg == 1) ? ACTION_ENABLE : ACTION_DISABLE);
 340         buspirate_set_feature(buspirate_fd, FEATURE_PULLUP,
 341                 (buspirate_pullup == 1) ? ACTION_ENABLE : ACTION_DISABLE);
 342         buspirate_reset(0, 0);
 343
 344         return ERROR_OK;
 345 }
 346
 347 static int buspirate_quit(void)
 348 {
 349         LOG_INFO("Shutting down buspirate.");
 350         buspirate_set_mode(buspirate_fd, MODE_HIZ);
 351         buspirate_set_speed(buspirate_fd, SERIAL_NORMAL);
 352
 353         buspirate_jtag_reset(buspirate_fd);
 354
 355         buspirate_serial_close(buspirate_fd);
 356
 357         free(buspirate_port);
 358         buspirate_port = NULL;
 359         return ERROR_OK;
 360 }
 361
 362 /* openocd command interface */
 363 COMMAND_HANDLER(buspirate_handle_adc_command)
 364 {
 365         if (buspirate_fd == -1)
 366                 return ERROR_OK;
 367
 368         /* unavailable in SWD mode */
 369         if (swd_mode)
 370                 return ERROR_OK;
 371
 372         /* send the command */
 373         buspirate_jtag_get_adcs(buspirate_fd);
 374
 375         return ERROR_OK;
 376
 377 }
 378
 379 COMMAND_HANDLER(buspirate_handle_vreg_command)
 380 {
 381         if (CMD_ARGC < 1)
 382                 return ERROR_COMMAND_SYNTAX_ERROR;
 383
 384         if (atoi(CMD_ARGV[0]) == 1)
 385                 buspirate_vreg = 1;
 386         else if (atoi(CMD_ARGV[0]) == 0)
 387                 buspirate_vreg = 0;
 388         else
 389                 LOG_ERROR("usage: buspirate_vreg <1|0>");
 390
 391         return ERROR_OK;
 392
 393 }
 394
 395 COMMAND_HANDLER(buspirate_handle_pullup_command)
 396 {
 397         if (CMD_ARGC < 1)
 398                 return ERROR_COMMAND_SYNTAX_ERROR;
 399
 400         if (atoi(CMD_ARGV[0]) == 1)
 401                 buspirate_pullup = 1;
 402         else if (atoi(CMD_ARGV[0]) == 0)
 403                 buspirate_pullup = 0;
 404         else
 405                 LOG_ERROR("usage: buspirate_pullup <1|0>");
 406
 407         return ERROR_OK;
 408
 409 }
 410
 411 COMMAND_HANDLER(buspirate_handle_led_command)
 412 {
 413         if (CMD_ARGC < 1)
 414                 return ERROR_COMMAND_SYNTAX_ERROR;
 415
 416         if (atoi(CMD_ARGV[0]) == 1) {
 417                 /* enable led */
 418                 buspirate_set_feature(buspirate_fd, FEATURE_LED,
 419                                 ACTION_ENABLE);
 420         } else if (atoi(CMD_ARGV[0]) == 0) {
 421                 /* disable led */
 422                 buspirate_set_feature(buspirate_fd, FEATURE_LED,
 423                                 ACTION_DISABLE);
 424         } else {
 425                 LOG_ERROR("usage: buspirate_led <1|0>");
 426         }
 427
 428         return ERROR_OK;
 429
 430 }
 431
 432 COMMAND_HANDLER(buspirate_handle_mode_command)
 433 {
 434         if (CMD_ARGC < 1)
 435                 return ERROR_COMMAND_SYNTAX_ERROR;
 436
 437         if (CMD_ARGV[0][0] == 'n')
 438                 buspirate_pinmode = MODE_JTAG;
 439         else if (CMD_ARGV[0][0] == 'o')
 440                 buspirate_pinmode = MODE_JTAG_OD;
 441         else
 442                 LOG_ERROR("usage: buspirate_mode <normal|open-drain>");
 443
 444         return ERROR_OK;
 445
 446 }
 447
 448 COMMAND_HANDLER(buspirate_handle_speed_command)
 449 {
 450         if (CMD_ARGC < 1)
 451                 return ERROR_COMMAND_SYNTAX_ERROR;
 452
 453         if (CMD_ARGV[0][0] == 'n')
 454                 buspirate_baudrate = SERIAL_NORMAL;
 455         else if (CMD_ARGV[0][0] == 'f')
 456                 buspirate_baudrate = SERIAL_FAST;
 457         else
 458                 LOG_ERROR("usage: buspirate_speed <normal|fast>");
 459
 460         return ERROR_OK;
 461
 462 }
 463
 464 COMMAND_HANDLER(buspirate_handle_port_command)
 465 {
 466         if (CMD_ARGC < 1)
 467                 return ERROR_COMMAND_SYNTAX_ERROR;
 468
 469         if (!buspirate_port)
 470                 buspirate_port = strdup(CMD_ARGV[0]);
 471
 472         return ERROR_OK;
 473
 474 }
 475
 476 static const struct command_registration buspirate_subcommand_handlers[] = {
 477         {
 478                 .name = "adc",
 479                 .handler = &buspirate_handle_adc_command,
 480                 .mode = COMMAND_EXEC,
 481                 .help = "reads voltages on adc pins",
 482                 .usage = "",
 483         },
 484         {
 485                 .name = "vreg",
 486                 .usage = "<1|0>",
 487                 .handler = &buspirate_handle_vreg_command,
 488                 .mode = COMMAND_CONFIG,
 489                 .help = "changes the state of voltage regulators",
 490         },
 491         {
 492                 .name = "pullup",
 493                 .usage = "<1|0>",
 494                 .handler = &buspirate_handle_pullup_command,
 495                 .mode = COMMAND_CONFIG,
 496                 .help = "changes the state of pullup",
 497         },
 498         {
 499                 .name = "led",
 500                 .usage = "<1|0>",
 501                 .handler = &buspirate_handle_led_command,
 502                 .mode = COMMAND_EXEC,
 503                 .help = "changes the state of led",
 504         },
 505         {
 506                 .name = "speed",
 507                 .usage = "<normal|fast>",
 508                 .handler = &buspirate_handle_speed_command,
 509                 .mode = COMMAND_CONFIG,
 510                 .help = "speed of the interface",
 511         },
 512         {
 513                 .name = "mode",
 514                 .usage = "<normal|open-drain>",
 515                 .handler = &buspirate_handle_mode_command,
 516                 .mode = COMMAND_CONFIG,
 517                 .help = "pin mode of the interface",
 518         },
 519         {
 520                 .name = "port",
 521                 .usage = "/dev/ttyUSB0",
 522                 .handler = &buspirate_handle_port_command,
 523                 .mode = COMMAND_CONFIG,
 524                 .help = "name of the serial port to open",
 525         },
 526         COMMAND_REGISTRATION_DONE
 527 };
 528
 529 static const struct command_registration buspirate_command_handlers[] = {
 530         {
 531                 .name = "buspirate",
 532                 .mode = COMMAND_ANY,
 533                 .help = "perform buspirate management",
 534                 .chain = buspirate_subcommand_handlers,
 535                 .usage = "",
 536         },
 537         COMMAND_REGISTRATION_DONE
 538 };
 539
 540 static const struct swd_driver buspirate_swd = {
 541         .init = buspirate_swd_init,
 542         .switch_seq = buspirate_swd_switch_seq,
 543         .read_reg = buspirate_swd_read_reg,
 544         .write_reg = buspirate_swd_write_reg,
 545         .run = buspirate_swd_run_queue,
 546 };
 547
 548 static const char * const buspirate_transports[] = { "jtag", "swd", NULL };
 549
 550 static struct jtag_interface buspirate_interface = {
 551         .execute_queue = buspirate_execute_queue,
 552 };
 553
 554 struct adapter_driver buspirate_adapter_driver = {
 555         .name = "buspirate",
 556         .transports = buspirate_transports,
 557         .commands = buspirate_command_handlers,
 558
 559         .init = buspirate_init,
 560         .quit = buspirate_quit,
 561         .reset = buspirate_reset,
 562
 563         .jtag_ops = &buspirate_interface,
 564         .swd_ops = &buspirate_swd,
 565 };
 566
 567 /*************** jtag execute commands **********************/
 568 static void buspirate_end_state(tap_state_t state)
 569 {
 570         if (tap_is_state_stable(state))
 571                 tap_set_end_state(state);
 572         else {
 573                 LOG_ERROR("BUG: %i is not a valid end state", state);
 574                 exit(-1);
 575         }
 576 }
 577
 578 static void buspirate_state_move(void)
 579 {
 580         int i = 0, tms = 0;
 581         uint8_t tms_scan = tap_get_tms_path(tap_get_state(),
 582                         tap_get_end_state());
 583         int tms_count = tap_get_tms_path_len(tap_get_state(),
 584                         tap_get_end_state());
 585
 586         for (i = 0; i < tms_count; i++) {
 587                 tms = (tms_scan >> i) & 1;
 588                 buspirate_tap_append(tms, 0);
 589         }
 590
 591         tap_set_state(tap_get_end_state());
 592 }
 593
 594 static void buspirate_path_move(int num_states, tap_state_t *path)
 595 {
 596         int i;
 597
 598         for (i = 0; i < num_states; i++) {
 599                 if (tap_state_transition(tap_get_state(), false) == path[i]) {
 600                         buspirate_tap_append(0, 0);
 601                 } else if (tap_state_transition(tap_get_state(), true)
 602                                 == path[i]) {
 603                         buspirate_tap_append(1, 0);
 604                 } else {
 605                         LOG_ERROR("BUG: %s -> %s isn't a valid "
 606                                 "TAP transition",
 607                                 tap_state_name(tap_get_state()),
 608                                 tap_state_name(path[i]));
 609                         exit(-1);
 610                 }
 611
 612                 tap_set_state(path[i]);
 613         }
 614
 615         tap_set_end_state(tap_get_state());
 616 }
 617
 618 static void buspirate_runtest(int num_cycles)
 619 {
 620         int i;
 621
 622         tap_state_t saved_end_state = tap_get_end_state();
 623
 624         /* only do a state_move when we're not already in IDLE */
 625         if (tap_get_state() != TAP_IDLE) {
 626                 buspirate_end_state(TAP_IDLE);
 627                 buspirate_state_move();
 628         }
 629
 630         for (i = 0; i < num_cycles; i++)
 631                 buspirate_tap_append(0, 0);
 632
 633         LOG_DEBUG_IO("runtest: cur_state %s end_state %s",
 634                         tap_state_name(tap_get_state()),
 635                         tap_state_name(tap_get_end_state()));
 636
 637         /* finish in end_state */
 638         buspirate_end_state(saved_end_state);
 639         if (tap_get_state() != tap_get_end_state())
 640                 buspirate_state_move();
 641 }
 642
 643 static void buspirate_scan(bool ir_scan, enum scan_type type,
 644         uint8_t *buffer, int scan_size, struct scan_command *command)
 645 {
 646         tap_state_t saved_end_state;
 647
 648         buspirate_tap_make_space(1, scan_size+8);
 649         /* is 8 correct ? (2 moves = 16) */
 650
 651         saved_end_state = tap_get_end_state();
 652
 653         buspirate_end_state(ir_scan ? TAP_IRSHIFT : TAP_DRSHIFT);
 654
 655         /* Only move if we're not already there */
 656         if (tap_get_state() != tap_get_end_state())
 657                 buspirate_state_move();
 658
 659         buspirate_tap_append_scan(scan_size, buffer, command);
 660
 661         /* move to PAUSE */
 662         buspirate_tap_append(0, 0);
 663
 664         /* restore the saved state */
 665         buspirate_end_state(saved_end_state);
 666         tap_set_state(ir_scan ? TAP_IRPAUSE : TAP_DRPAUSE);
 667
 668         if (tap_get_state() != tap_get_end_state())
 669                 buspirate_state_move();
 670 }
 671
 672 static void buspirate_stableclocks(int num_cycles)
 673 {
 674         int i;
 675         int tms = (tap_get_state() == TAP_RESET ? 1 : 0);
 676
 677         buspirate_tap_make_space(0, num_cycles);
 678
 679         for (i = 0; i < num_cycles; i++)
 680                 buspirate_tap_append(tms, 0);
 681 }
 682
 683 /************************* TAP related stuff **********/
 684
 685 /* This buffer size matches the maximum CMD_TAP_SHIFT bit length in the Bus Pirate firmware,
 686    look for constant 0x2000 in OpenOCD.c . */
 687 #define BUSPIRATE_BUFFER_SIZE 1024
 688
 689 /* The old value of 32 scans was not enough to achieve near 100% utilisation ratio
 690    for the current BUSPIRATE_BUFFER_SIZE value of 1024.
 691    With 128 scans I am getting full USB 2.0 high speed packets (512 bytes long) when
 692    using the JtagDue firmware on the Arduino Due instead of the Bus Pirate, which
 693    amounts approximately to a 10% overall speed gain. Bigger packets should also
 694    benefit the Bus Pirate, but the speed difference is much smaller.
 695    Unfortunately, each 512-byte packet is followed by a 329-byte one, which is not ideal.
 696    However, increasing BUSPIRATE_BUFFER_SIZE for the benefit of the JtagDue would
 697    make it incompatible with the Bus Pirate firmware. */
 698 #define BUSPIRATE_MAX_PENDING_SCANS 128
 699
 700 static uint8_t tms_chain[BUSPIRATE_BUFFER_SIZE]; /* send */
 701 static uint8_t tdi_chain[BUSPIRATE_BUFFER_SIZE]; /* send */
 702 static int tap_chain_index;
 703
 704 struct pending_scan_result /* this was stolen from arm-jtag-ew */
 705 {
 706         int first; /* First bit position in tdo_buffer to read */
 707         int length; /* Number of bits to read */
 708         struct scan_command *command; /* Corresponding scan command */
 709         uint8_t *buffer;
 710 };
 711
 712 static struct pending_scan_result
 713 tap_pending_scans[BUSPIRATE_MAX_PENDING_SCANS];
 714 static int tap_pending_scans_num;
 715
 716 static void buspirate_tap_init(void)
 717 {
 718         tap_chain_index = 0;
 719         tap_pending_scans_num = 0;
 720 }
 721
 722 static int buspirate_tap_execute(void)
 723 {
 724         uint8_t tmp[4096];
 725         uint8_t *in_buf;
 726         int i;
 727         int fill_index = 0;
 728         int ret;
 729         int bytes_to_send;
 730
 731         if (tap_chain_index <= 0)
 732                 return ERROR_OK;
 733
 734         LOG_DEBUG("executing tap num bits = %i scans = %i",
 735                         tap_chain_index, tap_pending_scans_num);
 736
 737         bytes_to_send = DIV_ROUND_UP(tap_chain_index, 8);
 738
 739         tmp[0] = CMD_TAP_SHIFT; /* this command expects number of bits */
 740         tmp[1] = tap_chain_index >> 8;  /* high */
 741         tmp[2] = tap_chain_index;  /* low */
 742
 743         fill_index = CMD_TAP_SHIFT_HEADER_LEN;
 744         for (i = 0; i < bytes_to_send; i++) {
 745                 tmp[fill_index] = tdi_chain[i];
 746                 fill_index++;
 747                 tmp[fill_index] = tms_chain[i];
 748                 fill_index++;
 749         }
 750
 751         /* jlink.c calls the routine below, which may be useful for debugging purposes.
 752            For example, enabling this allows you to compare the log outputs from jlink.c
 753            and from this module for JTAG development or troubleshooting purposes. */
 754         if (false) {
 755                 last_tap_state = jtag_debug_state_machine(tms_chain, tdi_chain,
 756                                                                                                   tap_chain_index, last_tap_state);
 757         }
 758
 759         ret = buspirate_serial_write(buspirate_fd, tmp, CMD_TAP_SHIFT_HEADER_LEN + bytes_to_send*2);
 760         if (ret != bytes_to_send*2+CMD_TAP_SHIFT_HEADER_LEN) {
 761                 LOG_ERROR("error writing :(");
 762                 return ERROR_JTAG_DEVICE_ERROR;
 763         }
 764
 765         ret = buspirate_serial_read(buspirate_fd, tmp, bytes_to_send + CMD_TAP_SHIFT_HEADER_LEN);
 766         if (ret != bytes_to_send + CMD_TAP_SHIFT_HEADER_LEN) {
 767                 LOG_ERROR("error reading");
 768                 return ERROR_FAIL;
 769         }
 770         in_buf = (uint8_t *)(&tmp[CMD_TAP_SHIFT_HEADER_LEN]);
 771
 772         /* parse the scans */
 773         for (i = 0; i < tap_pending_scans_num; i++) {
 774                 uint8_t *buffer = tap_pending_scans[i].buffer;
 775                 int length = tap_pending_scans[i].length;
 776                 int first = tap_pending_scans[i].first;
 777                 struct scan_command *command = tap_pending_scans[i].command;
 778
 779                 /* copy bits from buffer */
 780                 buf_set_buf(in_buf, first, buffer, 0, length);
 781
 782                 /* return buffer to higher level */
 783                 if (jtag_read_buffer(buffer, command) != ERROR_OK) {
 784                         buspirate_tap_init();
 785                         return ERROR_JTAG_QUEUE_FAILED;
 786                 }
 787
 788                 free(buffer);
 789         }
 790         buspirate_tap_init();
 791         return ERROR_OK;
 792 }
 793
 794 static void buspirate_tap_make_space(int scans, int bits)
 795 {
 796         int have_scans = BUSPIRATE_MAX_PENDING_SCANS - tap_pending_scans_num;
 797         int have_bits = BUSPIRATE_BUFFER_SIZE * 8 - tap_chain_index;
 798
 799         if ((have_scans < scans) || (have_bits < bits))
 800                 buspirate_tap_execute();
 801 }
 802
 803 static void buspirate_tap_append(int tms, int tdi)
 804 {
 805         int chain_index;
 806
 807         buspirate_tap_make_space(0, 1);
 808         chain_index = tap_chain_index / 8;
 809
 810         if (chain_index < BUSPIRATE_BUFFER_SIZE) {
 811                 int bit_index = tap_chain_index % 8;
 812                 uint8_t bit = 1 << bit_index;
 813
 814                 if (bit_index == 0) {
 815                         /* Let's say that the TAP shift operation wants to shift 9 bits,
 816                            so we will be sending to the Bus Pirate a bit count of 9 but still
 817                            full 16 bits (2 bytes) of shift data.
 818                            If we don't clear all bits at this point, the last 7 bits will contain
 819                            random data from the last buffer contents, which is not pleasant to the eye.
 820                            Besides, the Bus Pirate (or some clone) may want to assert in debug builds
 821                            that, after consuming all significant data bits, the rest of them are zero.
 822                            Therefore, for aesthetic and for assert purposes, we clear all bits below. */
 823                         tms_chain[chain_index] = 0;
 824                         tdi_chain[chain_index] = 0;
 825                 }
 826
 827                 if (tms)
 828                         tms_chain[chain_index] |= bit;
 829                 else
 830                         tms_chain[chain_index] &= ~bit;
 831
 832                 if (tdi)
 833                         tdi_chain[chain_index] |= bit;
 834                 else
 835                         tdi_chain[chain_index] &= ~bit;
 836
 837                 tap_chain_index++;
 838         } else {
 839                 LOG_ERROR("tap_chain overflow, bad things will happen");
 840                 /* Exit abruptly, like jlink.c does. After a buffer overflow we don't want
 841                    to carry on, as data will be corrupt. Another option would be to return
 842                    some error code at this point. */
 843                 exit(-1);
 844         }
 845 }
 846
 847 static void buspirate_tap_append_scan(int length, uint8_t *buffer,
 848                 struct scan_command *command)
 849 {
 850         int i;
 851         tap_pending_scans[tap_pending_scans_num].length = length;
 852         tap_pending_scans[tap_pending_scans_num].buffer = buffer;
 853         tap_pending_scans[tap_pending_scans_num].command = command;
 854         tap_pending_scans[tap_pending_scans_num].first = tap_chain_index;
 855
 856         for (i = 0; i < length; i++) {
 857                 int tms = (i < length-1 ? 0 : 1);
 858                 int tdi = (buffer[i/8] >> (i%8)) & 1;
 859                 buspirate_tap_append(tms, tdi);
 860         }
 861         tap_pending_scans_num++;
 862 }
 863
 864 /*************** wrapper functions *********************/
 865
 866 /* (1) assert or (0) deassert reset lines */
 867 static int buspirate_reset(int trst, int srst)
 868 {
 869         LOG_DEBUG("trst: %i, srst: %i", trst, srst);
 870
 871         if (trst)
 872                 buspirate_set_feature(buspirate_fd, FEATURE_TRST, ACTION_DISABLE);
 873         else
 874                 buspirate_set_feature(buspirate_fd, FEATURE_TRST, ACTION_ENABLE);
 875
 876         if (srst)
 877                 buspirate_set_feature(buspirate_fd, FEATURE_SRST, ACTION_DISABLE);
 878         else
 879                 buspirate_set_feature(buspirate_fd, FEATURE_SRST, ACTION_ENABLE);
 880
 881         return ERROR_OK;
 882 }
 883
 884 static void buspirate_set_feature(int fd, char feat, char action)
 885 {
 886         if (swd_mode)
 887                 buspirate_swd_set_feature(fd, feat, action);
 888         else
 889                 buspirate_jtag_set_feature(fd, feat, action);
 890 }
 891
 892 static void buspirate_set_mode(int fd, char mode)
 893 {
 894         if (swd_mode)
 895                 buspirate_swd_set_mode(fd, mode);
 896         else
 897                 buspirate_jtag_set_mode(fd, mode);
 898 }
 899
 900 static void buspirate_set_speed(int fd, char speed)
 901 {
 902         if (swd_mode)
 903                 buspirate_swd_set_speed(fd, speed);
 904         else
 905                 buspirate_jtag_set_speed(fd, speed);
 906 }
 907
 908
 909 /*************** swd lowlevel functions ********************/
 910
 911 static void buspirate_swd_set_speed(int fd, char speed)
 912 {
 913         int  ret;
 914         uint8_t tmp[1];
 915
 916         LOG_DEBUG("Buspirate speed setting in SWD mode defaults to 400 kHz");
 917
 918         /* speed settings */
 919         tmp[0] = CMD_RAW_SPEED | SPEED_RAW_400_KHZ;
 920         buspirate_serial_write(fd, tmp, 1);
 921         ret = buspirate_serial_read(fd, tmp, 1);
 922         if (ret != 1) {
 923                 LOG_ERROR("Buspirate did not answer correctly");
 924                 exit(-1);
 925         }
 926         if (tmp[0] != 1) {
 927                 LOG_ERROR("Buspirate did not reply as expected to the speed change command");
 928                 exit(-1);
 929         }
 930 }
 931
 932 static void buspirate_swd_set_mode(int fd, char mode)
 933 {
 934         int ret;
 935         uint8_t tmp[1];
 936
 937         /* raw-wire mode configuration */
 938         if (mode == MODE_HIZ)
 939                 tmp[0] = CMD_RAW_MODE | CMD_RAW_CONFIG_LSB;
 940         else
 941                 tmp[0] = CMD_RAW_MODE | CMD_RAW_CONFIG_LSB | CMD_RAW_CONFIG_3V3;
 942
 943         buspirate_serial_write(fd, tmp, 1);
 944         ret = buspirate_serial_read(fd, tmp, 1);
 945         if (ret != 1) {
 946                 LOG_ERROR("Buspirate did not answer correctly");
 947                 exit(-1);
 948         }
 949         if (tmp[0] != 1) {
 950                 LOG_ERROR("Buspirate did not reply as expected to the configure command");
 951                 exit(-1);
 952         }
 953 }
 954
 955 static void buspirate_swd_set_feature(int fd, char feat, char action)
 956 {
 957         int  ret;
 958         uint8_t tmp[1];
 959
 960         switch (feat) {
 961                 case FEATURE_TRST:
 962                         LOG_DEBUG("Buspirate TRST feature not available in SWD mode");
 963                         return;
 964                 case FEATURE_LED:
 965                         LOG_ERROR("Buspirate LED feature not available in SWD mode");
 966                         return;
 967                 case FEATURE_SRST:
 968                         swd_features = (action == ACTION_ENABLE) ? swd_features | 0x02 : swd_features & 0x0D;
 969                         break;
 970                 case FEATURE_PULLUP:
 971                         swd_features = (action == ACTION_ENABLE) ? swd_features | 0x04 : swd_features & 0x0B;
 972                         break;
 973                 case FEATURE_VREG:
 974                         swd_features = (action == ACTION_ENABLE) ? swd_features | 0x08 : swd_features & 0x07;
 975                         break;
 976                 default:
 977                         LOG_DEBUG("Buspirate unknown feature %d", feat);
 978                         return;
 979         }
 980
 981         tmp[0] = CMD_RAW_PERIPH | swd_features;
 982         buspirate_serial_write(fd, tmp, 1);
 983         ret = buspirate_serial_read(fd, tmp, 1);
 984         if (ret != 1) {
 985                 LOG_DEBUG("Buspirate feature %d not supported in SWD mode", feat);
 986         } else if (tmp[0] != 1) {
 987                 LOG_ERROR("Buspirate did not reply as expected to the configure command");
 988                 exit(-1);
 989         }
 990 }
 991
 992 /*************** jtag lowlevel functions ********************/
 993 static void buspirate_bbio_enable(int fd)
 994 {
 995         int ret;
 996         char command;
 997         const char *mode_answers[2] = { "OCD1", "RAW1" };
 998         const char *correct_ans     = NULL;
 999         uint8_t tmp[21] = { [0 ... 20] = 0x00 };
1000         int done = 0;
1001         int cmd_sent = 0;
1002
1003         if (swd_mode) {
1004                 command     = CMD_ENTER_RWIRE;
1005                 correct_ans = mode_answers[1];
1006         } else {
1007                 command     = CMD_ENTER_OOCD;
1008                 correct_ans = mode_answers[0];
1009         }
1010
1011         LOG_DEBUG("Entering binary mode, that is %s", correct_ans);
1012         buspirate_serial_write(fd, tmp, 20);
1013         usleep(10000);
1014
1015         /* reads 1 to n "BBIO1"s and one "OCD1" or "RAW1" */
1016         while (!done) {
1017                 ret = buspirate_serial_read(fd, tmp, 4);
1018                 if (ret != 4) {
1019                         LOG_ERROR("Buspirate error. Is binary"
1020                                 "/OpenOCD support enabled?");
1021                         exit(-1);
1022                 }
1023                 if (strncmp((char *)tmp, "BBIO", 4) == 0) {
1024                         ret = buspirate_serial_read(fd, tmp, 1);
1025                         if (ret != 1) {
1026                                 LOG_ERROR("Buspirate did not answer correctly! "
1027                                         "Do you have correct firmware?");
1028                                 exit(-1);
1029                         }
1030                         if (tmp[0] != '1') {
1031                                 LOG_ERROR("Unsupported binary protocol");
1032                                 exit(-1);
1033                         }
1034                         if (cmd_sent == 0) {
1035                                 cmd_sent = 1;
1036                                 tmp[0] = command;
1037                                 ret = buspirate_serial_write(fd, tmp, 1);
1038                                 if (ret != 1) {
1039                                         LOG_ERROR("error reading");
1040                                         exit(-1);
1041                                 }
1042                         }
1043                 } else if (strncmp((char *)tmp, correct_ans, 4) == 0)
1044                         done = 1;
1045                 else {
1046                         LOG_ERROR("Buspirate did not answer correctly! "
1047                                 "Do you have correct firmware?");
1048                         exit(-1);
1049                 }
1050         }
1051
1052 }
1053
1054 static void buspirate_jtag_reset(int fd)
1055 {
1056         uint8_t tmp[5];
1057
1058         tmp[0] = 0x00; /* exit OCD1 mode */
1059         buspirate_serial_write(fd, tmp, 1);
1060         usleep(10000);
1061         /* We ignore the return value here on purpose, nothing we can do */
1062         buspirate_serial_read(fd, tmp, 5);
1063         if (strncmp((char *)tmp, "BBIO1", 5) == 0) {
1064                 tmp[0] = 0x0F; /*  reset BP */
1065                 buspirate_serial_write(fd, tmp, 1);
1066         } else
1067                 LOG_ERROR("Unable to restart buspirate!");
1068 }
1069
1070 static void buspirate_jtag_set_speed(int fd, char speed)
1071 {
1072         int ret;
1073         uint8_t tmp[2];
1074         uint8_t ack[2];
1075
1076         ack[0] = 0xAA;
1077         ack[1] = 0x55;
1078
1079         tmp[0] = CMD_UART_SPEED;
1080         tmp[1] = speed;
1081         buspirate_jtag_command(fd, tmp, 2);
1082
1083         /* here the adapter changes speed, we need follow */
1084         if (-1 == buspirate_serial_setspeed(fd, speed, NORMAL_TIMEOUT)) {
1085                 LOG_ERROR("Error configuring the serial port.");
1086                 exit(-1);
1087         }
1088
1089         buspirate_serial_write(fd, ack, 2);
1090         ret = buspirate_serial_read(fd, tmp, 2);
1091         if (ret != 2) {
1092                 LOG_ERROR("Buspirate did not ack speed change");
1093                 exit(-1);
1094         }
1095         if ((tmp[0] != CMD_UART_SPEED) || (tmp[1] != speed)) {
1096                 LOG_ERROR("Buspirate did not reply as expected to the speed change command");
1097                 exit(-1);
1098         }
1099         LOG_INFO("Buspirate switched to %s mode",
1100                 (speed == SERIAL_NORMAL) ? "normal" : "FAST");
1101 }
1102
1103
1104 static void buspirate_jtag_set_mode(int fd, char mode)
1105 {
1106         uint8_t tmp[2];
1107         tmp[0] = CMD_PORT_MODE;
1108         tmp[1] = mode;
1109         buspirate_jtag_command(fd, tmp, 2);
1110 }
1111
1112 static void buspirate_jtag_set_feature(int fd, char feat, char action)
1113 {
1114         uint8_t tmp[3];
1115         tmp[0] = CMD_FEATURE;
1116         tmp[1] = feat;   /* what */
1117         tmp[2] = action; /* action */
1118         buspirate_jtag_command(fd, tmp, 3);
1119 }
1120
1121 static void buspirate_jtag_get_adcs(int fd)
1122 {
1123         uint8_t tmp[10];
1124         uint16_t a, b, c, d;
1125         tmp[0] = CMD_READ_ADCS;
1126         buspirate_jtag_command(fd, tmp, 1);
1127         a = tmp[2] << 8 | tmp[3];
1128         b = tmp[4] << 8 | tmp[5];
1129         c = tmp[6] << 8 | tmp[7];
1130         d = tmp[8] << 8 | tmp[9];
1131
1132         LOG_INFO("ADC: ADC_Pin = %.02f VPullup = %.02f V33 = %.02f "
1133                 "V50 = %.02f",
1134                 ((float)a)/155.1515, ((float)b)/155.1515,
1135                 ((float)c)/155.1515, ((float)d)/155.1515);
1136 }
1137
1138 static unsigned char buspirate_jtag_command(int fd,
1139                 uint8_t *cmd, int cmdlen)
1140 {
1141         int res;
1142         int len = 0;
1143
1144         res = buspirate_serial_write(fd, cmd, cmdlen);
1145
1146         if ((cmd[0] == CMD_UART_SPEED)
1147                                 || (cmd[0] == CMD_PORT_MODE)
1148                                 || (cmd[0] == CMD_FEATURE)
1149                                 || (cmd[0] == CMD_JTAG_SPEED))
1150                 return 1;
1151
1152         if (res == cmdlen) {
1153                 switch (cmd[0]) {
1154                 case CMD_READ_ADCS:
1155                         len = 10; /* 2*sizeof(char)+4*sizeof(uint16_t) */
1156                         break;
1157                 case CMD_TAP_SHIFT:
1158                         len = cmdlen;
1159                         break;
1160                 default:
1161                         LOG_INFO("Wrong !");
1162                 }
1163                 res =  buspirate_serial_read(fd, cmd, len);
1164                 if (res > 0)
1165                         return (unsigned char)cmd[1];
1166                 else
1167                         return -1;
1168         } else
1169                 return -1;
1170         return 0;
1171 }
1172
1173 /* low level serial port */
1174 /* TODO add support for WIN32 and others ! */
1175 static int buspirate_serial_open(char *port)
1176 {
1177         int fd;
1178         fd = open(buspirate_port, O_RDWR | O_NOCTTY | O_NDELAY);
1179         return fd;
1180 }
1181
1182
1183 /* Returns -1 on error. */
1184
1185 static int buspirate_serial_setspeed(int fd, char speed, cc_t timeout)
1186 {
1187         struct termios t_opt;
1188         speed_t baud = (speed == SERIAL_FAST) ? B1000000 : B115200;
1189
1190         /* set the serial port parameters */
1191         fcntl(fd, F_SETFL, 0);
1192         if (tcgetattr(fd, &t_opt) != 0)
1193                 return -1;
1194
1195         if (cfsetispeed(&t_opt, baud) != 0)
1196                 return -1;
1197
1198         if (cfsetospeed(&t_opt, baud) != 0)
1199                 return -1;
1200
1201         t_opt.c_cflag |= (CLOCAL | CREAD);
1202         t_opt.c_cflag &= ~PARENB;
1203         t_opt.c_cflag &= ~CSTOPB;
1204         t_opt.c_cflag &= ~CSIZE;
1205         t_opt.c_cflag |= CS8;
1206         t_opt.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
1207
1208         /* The serial port may have been configured for human interaction with
1209            the Bus Pirate console, but OpenOCD is going to use a binary protocol,
1210            so make sure to turn off any CR/LF translation and the like. */
1211         t_opt.c_iflag &= ~(IXON | IXOFF | IXANY | INLCR | ICRNL);
1212
1213         t_opt.c_oflag &= ~OPOST;
1214         t_opt.c_cc[VMIN] = 0;
1215         t_opt.c_cc[VTIME] = timeout;
1216
1217         /* Note that, in the past, TCSANOW was used below instead of TCSADRAIN,
1218            and CMD_UART_SPEED did not work properly then, at least with
1219            the Bus Pirate v3.5 (USB). */
1220         if (tcsetattr(fd, TCSADRAIN, &t_opt) != 0) {
1221                 /* According to the Linux documentation, this is actually not enough
1222                    to detect errors, you need to call tcgetattr() and check that
1223                    all changes have been performed successfully. */
1224                 return -1;
1225         }
1226
1227         return 0;
1228 }
1229
1230 static int buspirate_serial_write(int fd, uint8_t *buf, int size)
1231 {
1232         int ret = 0;
1233
1234         ret = write(fd, buf, size);
1235
1236         LOG_DEBUG("size = %d ret = %d", size, ret);
1237         buspirate_print_buffer(buf, size);
1238
1239         if (ret != size)
1240                 LOG_ERROR("Error sending data");
1241
1242         return ret;
1243 }
1244
1245 static int buspirate_serial_read(int fd, uint8_t *buf, int size)
1246 {
1247         int len = 0;
1248         int ret = 0;
1249         int timeout = 0;
1250
1251         while (len < size) {
1252                 ret = read(fd, buf+len, size-len);
1253                 if (ret == -1)
1254                         return -1;
1255
1256                 if (ret == 0) {
1257                         timeout++;
1258
1259                         if (timeout >= 10)
1260                                 break;
1261
1262                         continue;
1263                 }
1264
1265                 len += ret;
1266         }
1267
1268         LOG_DEBUG("should have read = %d actual size = %d", size, len);
1269         buspirate_print_buffer(buf, len);
1270
1271         if (len != size)
1272                 LOG_ERROR("Error reading data");
1273
1274         return len;
1275 }
1276
1277 static void buspirate_serial_close(int fd)
1278 {
1279         close(fd);
1280 }
1281
1282 #define LINE_SIZE      81
1283 #define BYTES_PER_LINE 16
1284 static void buspirate_print_buffer(uint8_t *buf, int size)
1285 {
1286         char line[LINE_SIZE];
1287         char tmp[10];
1288         int offset = 0;
1289
1290         line[0] = 0;
1291         while (offset < size) {
1292                 snprintf(tmp, 5, "%02x ", (uint8_t)buf[offset]);
1293                 offset++;
1294
1295                 strcat(line, tmp);
1296
1297                 if (offset % BYTES_PER_LINE == 0) {
1298                         LOG_DEBUG("%s", line);
1299                         line[0] = 0;
1300                 }
1301         }
1302
1303         if (line[0] != 0)
1304                 LOG_DEBUG("%s", line);
1305 }
1306
1307 /************************* SWD related stuff **********/
1308
1309 static int buspirate_swd_init(void)
1310 {
1311         LOG_INFO("Buspirate SWD mode enabled");
1312         swd_mode = true;
1313
1314         return ERROR_OK;
1315 }
1316
1317 static int buspirate_swd_switch_seq(enum swd_special_seq seq)
1318 {
1319         const uint8_t *sequence;
1320         int sequence_len;
1321         uint32_t no_bytes, sequence_offset;
1322
1323         switch (seq) {
1324         case LINE_RESET:
1325                 LOG_DEBUG("SWD line reset");
1326                 sequence = swd_seq_line_reset;
1327                 sequence_len = DIV_ROUND_UP(swd_seq_line_reset_len, 8);
1328                 break;
1329         case JTAG_TO_SWD:
1330                 LOG_DEBUG("JTAG-to-SWD");
1331                 sequence = swd_seq_jtag_to_swd;
1332                 sequence_len = DIV_ROUND_UP(swd_seq_jtag_to_swd_len, 8);
1333                 break;
1334         case SWD_TO_JTAG:
1335                 LOG_DEBUG("SWD-to-JTAG");
1336                 sequence = swd_seq_swd_to_jtag;
1337                 sequence_len = DIV_ROUND_UP(swd_seq_swd_to_jtag_len, 8);
1338                 break;
1339         default:
1340                 LOG_ERROR("Sequence %d not supported", seq);
1341                 return ERROR_FAIL;
1342         }
1343
1344         no_bytes = sequence_len;
1345         sequence_offset = 0;
1346
1347         while (no_bytes) {
1348                 uint8_t tmp[17];
1349                 uint32_t to_send;
1350
1351                 to_send = no_bytes > 16 ? 16 : no_bytes;
1352
1353                 tmp[0] = 0x10 + ((to_send - 1) & 0x0F);
1354                 memcpy(tmp + 1, &sequence[sequence_offset], to_send);
1355
1356                 buspirate_serial_write(buspirate_fd, tmp, to_send + 1);
1357                 buspirate_serial_read(buspirate_fd, tmp, to_send + 1);
1358
1359                 no_bytes -= to_send;
1360                 sequence_offset += to_send;
1361         }
1362
1363         return ERROR_OK;
1364 }
1365
1366 static uint8_t buspirate_swd_write_header(uint8_t cmd)
1367 {
1368         uint8_t tmp[8];
1369         int  to_send;
1370
1371         tmp[0] = 0x10; /* bus pirate: send 1 byte */
1372         tmp[1] = cmd;  /* swd cmd */
1373         tmp[2] = 0x07; /* ack __x */
1374         tmp[3] = 0x07; /* ack _x_ */
1375         tmp[4] = 0x07; /* ack x__ */
1376         tmp[5] = 0x07; /* write mode trn_1 */
1377         tmp[6] = 0x07; /* write mode trn_2 */
1378
1379         to_send = ((cmd & SWD_CMD_RNW) == 0) ? 7 : 5;
1380         buspirate_serial_write(buspirate_fd, tmp, to_send);
1381
1382         /* read ack */
1383         buspirate_serial_read(buspirate_fd, tmp, 2); /* drop pirate command ret vals */
1384         buspirate_serial_read(buspirate_fd, tmp, to_send - 2); /* ack bits */
1385
1386         return tmp[2] << 2 | tmp[1] << 1 | tmp[0];
1387 }
1388
1389 static void buspirate_swd_idle_clocks(uint32_t no_bits)
1390 {
1391         uint32_t no_bytes;
1392         uint8_t tmp[20];
1393
1394         no_bytes = (no_bits + 7) / 8;
1395         memset(tmp + 1, 0x00, sizeof(tmp) - 1);
1396
1397         /* unfortunately bus pirate misbehaves when clocks are sent in parts
1398          * so we need to limit at 128 clock cycles
1399          */
1400         if (no_bytes > 16)
1401                 no_bytes = 16;
1402
1403         while (no_bytes) {
1404                 uint8_t to_send = no_bytes > 16 ? 16 : no_bytes;
1405                 tmp[0] = 0x10 + ((to_send - 1) & 0x0F);
1406
1407                 buspirate_serial_write(buspirate_fd, tmp, to_send + 1);
1408                 buspirate_serial_read(buspirate_fd, tmp, to_send + 1);
1409
1410                 no_bytes -= to_send;
1411         }
1412 }
1413
1414 static void buspirate_swd_clear_sticky_errors(void)
1415 {
1416         buspirate_swd_write_reg(swd_cmd(false,  false, DP_ABORT),
1417                 STKCMPCLR | STKERRCLR | WDERRCLR | ORUNERRCLR, 0);
1418 }
1419
1420 static void buspirate_swd_read_reg(uint8_t cmd, uint32_t *value, uint32_t ap_delay_clk)
1421 {
1422         uint8_t tmp[16];
1423
1424         LOG_DEBUG("buspirate_swd_read_reg");
1425         assert(cmd & SWD_CMD_RNW);
1426
1427         if (queued_retval != ERROR_OK) {
1428                 LOG_DEBUG("Skip buspirate_swd_read_reg because queued_retval=%d", queued_retval);
1429                 return;
1430         }
1431
1432         cmd |= SWD_CMD_START | SWD_CMD_PARK;
1433         uint8_t ack = buspirate_swd_write_header(cmd);
1434
1435         /* do a read transaction */
1436         tmp[0] = 0x06; /* 4 data bytes */
1437         tmp[1] = 0x06;
1438         tmp[2] = 0x06;
1439         tmp[3] = 0x06;
1440         tmp[4] = 0x07; /* parity bit */
1441         tmp[5] = 0x21; /* 2 turnaround clocks */
1442
1443         buspirate_serial_write(buspirate_fd, tmp, 6);
1444         buspirate_serial_read(buspirate_fd, tmp, 6);
1445
1446         /* store the data and parity */
1447         uint32_t data = (uint8_t) tmp[0];
1448         data |= (uint8_t) tmp[1] << 8;
1449         data |= (uint8_t) tmp[2] << 16;
1450         data |= (uint8_t) tmp[3] << 24;
1451         int parity = tmp[4] ? 0x01 : 0x00;
1452
1453         LOG_DEBUG("%s %s %s reg %X = %08"PRIx32,
1454                         ack == SWD_ACK_OK ? "OK" : ack == SWD_ACK_WAIT ? "WAIT" : ack == SWD_ACK_FAULT ? "FAULT" : "JUNK",
1455                         cmd & SWD_CMD_APNDP ? "AP" : "DP",
1456                         cmd & SWD_CMD_RNW ? "read" : "write",
1457                         (cmd & SWD_CMD_A32) >> 1,
1458                         data);
1459
1460         switch (ack) {
1461          case SWD_ACK_OK:
1462                 if (parity != parity_u32(data)) {
1463                         LOG_DEBUG("Read data parity mismatch %x %x", parity, parity_u32(data));
1464                         queued_retval = ERROR_FAIL;
1465                         return;
1466                 }
1467                 if (value)
1468                         *value = data;
1469                 if (cmd & SWD_CMD_APNDP)
1470                         buspirate_swd_idle_clocks(ap_delay_clk);
1471                 return;
1472          case SWD_ACK_WAIT:
1473                 LOG_DEBUG("SWD_ACK_WAIT");
1474                 buspirate_swd_clear_sticky_errors();
1475                 return;
1476          case SWD_ACK_FAULT:
1477                 LOG_DEBUG("SWD_ACK_FAULT");
1478                 queued_retval = ack;
1479                 return;
1480          default:
1481                 LOG_DEBUG("No valid acknowledge: ack=%d", ack);
1482                 queued_retval = ack;
1483                 return;
1484         }
1485 }
1486
1487 static void buspirate_swd_write_reg(uint8_t cmd, uint32_t value, uint32_t ap_delay_clk)
1488 {
1489         uint8_t tmp[16];
1490
1491         LOG_DEBUG("buspirate_swd_write_reg");
1492         assert(!(cmd & SWD_CMD_RNW));
1493
1494         if (queued_retval != ERROR_OK) {
1495                 LOG_DEBUG("Skip buspirate_swd_write_reg because queued_retval=%d", queued_retval);
1496                 return;
1497         }
1498
1499         cmd |= SWD_CMD_START | SWD_CMD_PARK;
1500         uint8_t ack = buspirate_swd_write_header(cmd);
1501
1502         /* do a write transaction */
1503         tmp[0] = 0x10 + ((4 + 1 - 1) & 0xF); /* bus pirate: send 4+1 bytes */
1504         buf_set_u32((uint8_t *) tmp + 1, 0, 32, value);
1505         /* write sequence ends with parity bit and 7 idle ticks */
1506         tmp[5] = parity_u32(value) ? 0x01 : 0x00;
1507
1508         buspirate_serial_write(buspirate_fd, tmp, 6);
1509         buspirate_serial_read(buspirate_fd, tmp, 6);
1510
1511         LOG_DEBUG("%s %s %s reg %X = %08"PRIx32,
1512                         ack == SWD_ACK_OK ? "OK" : ack == SWD_ACK_WAIT ? "WAIT" : ack == SWD_ACK_FAULT ? "FAULT" : "JUNK",
1513                         cmd & SWD_CMD_APNDP ? "AP" : "DP",
1514                         cmd & SWD_CMD_RNW ? "read" : "write",
1515                         (cmd & SWD_CMD_A32) >> 1,
1516                         value);
1517
1518         switch (ack) {
1519          case SWD_ACK_OK:
1520                 if (cmd & SWD_CMD_APNDP)
1521                         buspirate_swd_idle_clocks(ap_delay_clk);
1522                 return;
1523          case SWD_ACK_WAIT:
1524                 LOG_DEBUG("SWD_ACK_WAIT");
1525                 buspirate_swd_clear_sticky_errors();
1526                 return;
1527          case SWD_ACK_FAULT:
1528                 LOG_DEBUG("SWD_ACK_FAULT");
1529                 queued_retval = ack;
1530                 return;
1531          default:
1532                 LOG_DEBUG("No valid acknowledge: ack=%d", ack);
1533                 queued_retval = ack;
1534                 return;
1535         }
1536 }
1537
1538 static int buspirate_swd_run_queue(void)
1539 {
1540         LOG_DEBUG("buspirate_swd_run_queue");
1541         /* A transaction must be followed by another transaction or at least 8 idle cycles to
1542          * ensure that data is clocked through the AP. */
1543         buspirate_swd_idle_clocks(8);
1544
1545         int retval = queued_retval;
1546         queued_retval = ERROR_OK;
1547         LOG_DEBUG("SWD queue return value: %02x", retval);
1548         return retval;
1549 }