compile with cygwin (32-bit)
[openocd.git] / src / jtag / ft2232.c
index 0c5bfc7ddf42cc6def8c8521b02134b66ca37d37..cee55e0cf933053c05623b3d4bb8bbcc39b58af2 100644 (file)
@@ -1,4 +1,10 @@
 /***************************************************************************
+*   Copyright (C) 2009 by Øyvind Harboe                                   *
+*      Øyvind Harboe <oyvind.harboe@zylin.com>                               *
+*                                                                         *
+*   Copyright (C) 2009 by SoftPLC Corporation.  http://softplc.com        *
+*      Dick Hollenbeck <dick@softplc.com>                                    *
+*                                                                         *
 *   Copyright (C) 2004, 2006 by Dominic Rath                              *
 *   Dominic.Rath@gmx.de                                                   *
 *                                                                         *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/
 
-
 /* This code uses information contained in the MPSSE specification which was
  * found here:
  * http://www.ftdichip.com/Documents/AppNotes/AN2232C-01_MPSSE_Cmnd.pdf
  * Hereafter this is called the "MPSSE Spec".
+ *
+ * The datasheet for the ftdichip.com's FT2232D part is here:
+ * http://www.ftdichip.com/Documents/DataSheets/DS_FT2232D.pdf
  */
 
-
 #ifdef HAVE_CONFIG_H
 #include "config.h"
 #endif
 
-#include "replacements.h"
+/* project specific includes */
+#include "interface.h"
+#include "commands.h"
+#include "time_support.h"
 
 #if IS_CYGWIN == 1
-#include "windows.h"
+#include <windows.h>
 #endif
 
-/* project specific includes */
-#include "log.h"
-#include "types.h"
-#include "jtag.h"
-#include "configuration.h"
-#include "time_support.h"
+#include <assert.h>
 
-/* system includes */
-#include <string.h>
-#include <stdlib.h>
-#include <unistd.h>
+#if (BUILD_FT2232_FTD2XX == 1 && BUILD_FT2232_LIBFTDI == 1)
+#error "BUILD_FT2232_FTD2XX && BUILD_FT2232_LIBFTDI are mutually exclusive"
+#elif (BUILD_FT2232_FTD2XX != 1 && BUILD_FT2232_LIBFTDI != 1)
+#error "BUILD_FT2232_FTD2XX || BUILD_FT2232_LIBFTDI must be chosen"
+#endif
 
 /* FT2232 access library includes */
 #if BUILD_FT2232_FTD2XX == 1
 #include <ftdi.h>
 #endif
 
-static int ft2232_execute_queue(void);
-
-static int ft2232_speed(int speed);
-static int ft2232_speed_div(int speed, int* khz);
-static int ft2232_khz(int khz, int* jtag_speed);
-static int ft2232_register_commands(struct command_context_s* cmd_ctx);
-static int ft2232_init(void);
-static int ft2232_quit(void);
-
-static int ft2232_handle_device_desc_command(struct command_context_s* cmd_ctx, char* cmd, char** args, int argc);
-static int ft2232_handle_serial_command(struct command_context_s* cmd_ctx, char* cmd, char** args, int argc);
-static int ft2232_handle_layout_command(struct command_context_s* cmd_ctx, char* cmd, char** args, int argc);
-static int ft2232_handle_vid_pid_command(struct command_context_s* cmd_ctx, char* cmd, char** args, int argc);
-static int ft2232_handle_latency_command(struct command_context_s* cmd_ctx, char* cmd, char** args, int argc);
-
+/* max TCK for the high speed devices 30000 kHz */
+#define        FTDI_2232H_4232H_MAX_TCK        30000
+/* max TCK for the full speed devices 6000 kHz */
+#define        FTDI_2232C_MAX_TCK 6000
+/* this speed value tells that RTCK is requested */
+#define RTCK_SPEED -1
+
+/*
+ * On my Athlon XP 1900+ EHCI host with FT2232H JTAG dongle I get read timeout
+ * errors with a retry count of 100. Increasing it solves the problem for me.
+ *     - Dimitar
+ *
+ * FIXME There's likely an issue with the usb_read_timeout from libftdi.
+ * Fix that (libusb? kernel? libftdi? here?) and restore the retry count
+ * to something sane.
+ */
+#define LIBFTDI_READ_RETRY_COUNT               2000
+
+#ifndef BUILD_FT2232_HIGHSPEED
+ #if BUILD_FT2232_FTD2XX == 1
+       enum { FT_DEVICE_2232H = 6, FT_DEVICE_4232H };
+ #elif BUILD_FT2232_LIBFTDI == 1
+       enum { TYPE_2232H = 4, TYPE_4232H = 5 };
+ #endif
+#endif
 
 /**
- * Function ft2232_stableclocks
- * will send out \a num_cycles on the TCK line while the TAP(s)
- * are in a stable state.  Calling code must ensure that current state is
- * stable, that verification is not done in here.
- * @param num_cycles is the count of clocks cycles to send.
- * @return int - ERROR_OK or ERROR_JTAG_QUEUE_FAILED
+ * Send out \a num_cycles on the TCK line while the TAP(s) are in a
+ * stable state.  Calling code must ensure that current state is stable,
+ * that verification is not done in here.
+ *
+ * @param num_cycles The number of clocks cycles to send.
+ * @param cmd The command to send.
+ *
+ * @returns ERROR_OK on success, or ERROR_JTAG_QUEUE_FAILED on failure.
  */
-static int ft2232_stableclocks(int num_cycles, jtag_command_t* cmd);
+static int ft2232_stableclocks(int num_cycles, struct jtag_command* cmd);
 
-
-static char *        ft2232_device_desc_A = NULL;
-static char*         ft2232_device_desc = NULL;
-static char*         ft2232_serial  = NULL;
-static char*         ft2232_layout  = NULL;
-static unsigned char ft2232_latency = 2;
+static char *       ft2232_device_desc_A = NULL;
+static char*        ft2232_device_desc = NULL;
+static char*        ft2232_serial  = NULL;
+static char*        ft2232_layout  = NULL;
+static uint8_t         ft2232_latency = 2;
+static unsigned                ft2232_max_tck = FTDI_2232C_MAX_TCK;
 
 #define MAX_USB_IDS 8
 /* vid = pid = 0 marks the end of the list */
-static u16 ft2232_vid[MAX_USB_IDS + 1] = { 0x0403, 0 };
-static u16 ft2232_pid[MAX_USB_IDS + 1] = { 0x6010, 0 };
+static uint16_t ft2232_vid[MAX_USB_IDS + 1] = { 0x0403, 0 };
+static uint16_t ft2232_pid[MAX_USB_IDS + 1] = { 0x6010, 0 };
 
-typedef struct ft2232_layout_s
-{
+struct ft2232_layout {
        char* name;
        int (*init)(void);
        void (*reset)(int trst, int srst);
        void (*blink)(void);
-} ft2232_layout_t;
+};
 
 /* init procedures for supported layouts */
-static int  usbjtag_init(void);
-static int  jtagkey_init(void);
-static int  olimex_jtag_init(void);
-static int  flyswatter_init(void);
-static int  turtle_init(void);
-static int  comstick_init(void);
-static int  stm32stick_init(void);
-static int  axm0432_jtag_init(void);
+static int usbjtag_init(void);
+static int jtagkey_init(void);
+static int olimex_jtag_init(void);
+static int flyswatter_init(void);
+static int turtle_init(void);
+static int comstick_init(void);
+static int stm32stick_init(void);
+static int axm0432_jtag_init(void);
 static int sheevaplug_init(void);
 static int icebear_jtag_init(void);
+static int cortino_jtag_init(void);
+static int signalyzer_h_init(void);
+static int ktlink_init(void);
 
 /* reset procedures for supported layouts */
 static void usbjtag_reset(int trst, int srst);
@@ -127,13 +147,17 @@ static void stm32stick_reset(int trst, int srst);
 static void axm0432_jtag_reset(int trst, int srst);
 static void sheevaplug_reset(int trst, int srst);
 static void icebear_jtag_reset(int trst, int srst);
+static void signalyzer_h_reset(int trst, int srst);
+static void ktlink_reset(int trst, int srst);
 
 /* blink procedures for layouts that support a blinking led */
 static void olimex_jtag_blink(void);
 static void flyswatter_jtag_blink(void);
 static void turtle_jtag_blink(void);
+static void signalyzer_h_blink(void);
+static void ktlink_blink(void);
 
-ft2232_layout_t            ft2232_layouts[] =
+static const struct ft2232_layout  ft2232_layouts[] =
 {
        { "usbjtag",              usbjtag_init,              usbjtag_reset,      NULL                    },
        { "jtagkey",              jtagkey_init,              jtagkey_reset,      NULL                    },
@@ -141,62 +165,181 @@ ft2232_layout_t            ft2232_layouts[] =
        { "oocdlink",             jtagkey_init,              jtagkey_reset,      NULL                    },
        { "signalyzer",           usbjtag_init,              usbjtag_reset,      NULL                    },
        { "evb_lm3s811",          usbjtag_init,              usbjtag_reset,      NULL                    },
+       { "luminary_icdi",        usbjtag_init,              usbjtag_reset,      NULL                    },
        { "olimex-jtag",          olimex_jtag_init,          olimex_jtag_reset,  olimex_jtag_blink       },
        { "flyswatter",           flyswatter_init,           flyswatter_reset,   flyswatter_jtag_blink   },
        { "turtelizer2",          turtle_init,               turtle_reset,       turtle_jtag_blink       },
        { "comstick",             comstick_init,             comstick_reset,     NULL                    },
        { "stm32stick",           stm32stick_init,           stm32stick_reset,   NULL                    },
        { "axm0432_jtag",         axm0432_jtag_init,         axm0432_jtag_reset, NULL                    },
-       {"sheevaplug",            sheevaplug_init,           sheevaplug_reset,   NULL                    },
+       { "sheevaplug",           sheevaplug_init,           sheevaplug_reset,   NULL                    },
        { "icebear",              icebear_jtag_init,         icebear_jtag_reset, NULL                    },
+       { "cortino",              cortino_jtag_init,         comstick_reset, NULL                        },
+       { "signalyzer-h",         signalyzer_h_init,         signalyzer_h_reset, signalyzer_h_blink      },
+       { "ktlink",               ktlink_init,               ktlink_reset,       ktlink_blink            },
        { NULL,                   NULL,                      NULL,               NULL                    },
 };
 
-static u8                  nTRST, nTRSTnOE, nSRST, nSRSTnOE;
+static uint8_t                  nTRST, nTRSTnOE, nSRST, nSRSTnOE;
 
-static ft2232_layout_t*    layout;
-static u8                  low_output     = 0x0;
-static u8                  low_direction  = 0x0;
-static u8                  high_output    = 0x0;
-static u8                  high_direction = 0x0;
+static const struct ft2232_layout *layout;
+static uint8_t                  low_output     = 0x0;
+static uint8_t                  low_direction  = 0x0;
+static uint8_t                  high_output    = 0x0;
+static uint8_t                  high_direction = 0x0;
 
 #if BUILD_FT2232_FTD2XX == 1
-static FT_HANDLE           ftdih = NULL;
+static FT_HANDLE       ftdih = NULL;
+static FT_DEVICE       ftdi_device = 0;
 #elif BUILD_FT2232_LIBFTDI == 1
 static struct ftdi_context ftdic;
+static enum ftdi_chip_type ftdi_device;
 #endif
 
-
-static jtag_command_t* first_unsent;        /* next command that has to be sent */
+static struct jtag_command* first_unsent;        /* next command that has to be sent */
 static int             require_send;
 
-static u8*             ft2232_buffer = NULL;
+/*     http://urjtag.wiki.sourceforge.net/Cable + FT2232 says:
+
+       "There is a significant difference between libftdi and libftd2xx. The latter
+       one allows to schedule up to 64*64 bytes of result data while libftdi fails
+       with more than 4*64. As a consequence, the FT2232 driver is forced to
+       perform around 16x more USB transactions for long command streams with TDO
+       capture when running with libftdi."
+
+       No idea how we get
+       #define FT2232_BUFFER_SIZE 131072
+       a comment would have been nice.
+*/
+
+#define FT2232_BUFFER_SIZE 131072
+
+static uint8_t*             ft2232_buffer = NULL;
 static int             ft2232_buffer_size  = 0;
 static int             ft2232_read_pointer = 0;
 static int             ft2232_expect_read  = 0;
 
-#define FT2232_BUFFER_SIZE 131072
-#define BUFFER_ADD         ft2232_buffer[ft2232_buffer_size++]
-#define BUFFER_READ        ft2232_buffer[ft2232_read_pointer++]
-
-jtag_interface_t ft2232_interface =
-{
-       .name               = "ft2232",
-       .execute_queue = ft2232_execute_queue,
-       .speed     = ft2232_speed,
-       .speed_div = ft2232_speed_div,
-       .khz                = ft2232_khz,
-       .register_commands  = ft2232_register_commands,
-       .init = ft2232_init,
-       .quit = ft2232_quit,
-};
+/**
+ * Function buffer_write
+ * writes a byte into the byte buffer, "ft2232_buffer", which must be sent later.
+ * @param val is the byte to send.
+ */
+static inline void buffer_write(uint8_t val)
+{
+       assert(ft2232_buffer);
+       assert((unsigned) ft2232_buffer_size < (unsigned) FT2232_BUFFER_SIZE);
+       ft2232_buffer[ft2232_buffer_size++] = val;
+}
+
+/**
+ * Function buffer_read
+ * returns a byte from the byte buffer.
+ */
+static inline uint8_t buffer_read(void)
+{
+       assert(ft2232_buffer);
+       assert(ft2232_read_pointer < ft2232_buffer_size);
+       return ft2232_buffer[ft2232_read_pointer++];
+}
+
+/**
+ * Clocks out \a bit_count bits on the TMS line, starting with the least
+ * significant bit of tms_bits and progressing to more significant bits.
+ * Rigorous state transition logging is done here via tap_set_state().
+ *
+ * @param mpsse_cmd One of the MPSSE TMS oriented commands such as
+ *     0x4b or 0x6b.  See the MPSSE spec referenced above for their
+ *     functionality. The MPSSE command "Clock Data to TMS/CS Pin (no Read)"
+ *     is often used for this, 0x4b.
+ *
+ * @param tms_bits Holds the sequence of bits to send.
+ * @param tms_count Tells how many bits in the sequence.
+ * @param tdi_bit A single bit to pass on to TDI before the first TCK
+ *     cycle and held static for the duration of TMS clocking.
+ *
+ * See the MPSSE spec referenced above.
+ */
+static void clock_tms(uint8_t mpsse_cmd, int tms_bits, int tms_count, bool tdi_bit)
+{
+       uint8_t tms_byte;
+       int     i;
+       int     tms_ndx;                                /* bit index into tms_byte */
+
+       assert(tms_count > 0);
+
+       DEBUG_JTAG_IO("mpsse cmd=%02x, tms_bits = 0x%08x, bit_count=%d",
+                       mpsse_cmd, tms_bits, tms_count);
+
+       for (tms_byte = tms_ndx = i = 0;   i < tms_count;   ++i, tms_bits>>=1)
+       {
+               bool bit = tms_bits & 1;
+
+               if (bit)
+                       tms_byte |= (1 << tms_ndx);
+
+               /* always do state transitions in public view */
+               tap_set_state(tap_state_transition(tap_get_state(), bit));
+
+               /*      we wrote a bit to tms_byte just above, increment bit index.  if bit was zero
+                       also increment.
+               */
+               ++tms_ndx;
+
+               if (tms_ndx == 7  || i == tms_count-1)
+               {
+                       buffer_write(mpsse_cmd);
+                       buffer_write(tms_ndx - 1);
+
+                       /*      Bit 7 of the byte is passed on to TDI/DO before the first TCK/SK of
+                               TMS/CS and is held static for the duration of TMS/CS clocking.
+                       */
+                       buffer_write(tms_byte | (tdi_bit << 7));
+               }
+       }
+}
+
+/**
+ * Function get_tms_buffer_requirements
+ * returns what clock_tms() will consume if called with
+ * same \a bit_count.
+ */
+static inline int get_tms_buffer_requirements(int bit_count)
+{
+       return ((bit_count + 6)/7) * 3;
+}
+
+/**
+ * Function move_to_state
+ * moves the TAP controller from the current state to a
+ * \a goal_state through a path given by tap_get_tms_path().  State transition
+ * logging is performed by delegation to clock_tms().
+ *
+ * @param goal_state is the destination state for the move.
+ */
+static void move_to_state(tap_state_t goal_state)
+{
+       tap_state_t     start_state = tap_get_state();
+
+       /*      goal_state is 1/2 of a tuple/pair of states which allow convenient
+               lookup of the required TMS pattern to move to this state from the
+               start state.
+       */
+
+       /* do the 2 lookups */
+       int tms_bits  = tap_get_tms_path(start_state, goal_state);
+       int tms_count = tap_get_tms_path_len(start_state, goal_state);
+
+       DEBUG_JTAG_IO("start=%s goal=%s", tap_state_name(start_state), tap_state_name(goal_state));
+
+       clock_tms(0x4b,  tms_bits, tms_count, 0);
+}
 
-static int ft2232_write(u8* buf, int size, u32* bytes_written)
+static int ft2232_write(uint8_t* buf, int size, uint32_t* bytes_written)
 {
 #if BUILD_FT2232_FTD2XX == 1
        FT_STATUS status;
-       DWORD     dw_bytes_written;
-       if ( ( status = FT_Write(ftdih, buf, size, &dw_bytes_written) ) != FT_OK )
+       DWORD dw_bytes_written;
+       if ((status = FT_Write(ftdih, buf, size, &dw_bytes_written)) != FT_OK)
        {
                *bytes_written = dw_bytes_written;
                LOG_ERROR("FT_Write returned: %lu", status);
@@ -209,10 +352,10 @@ static int ft2232_write(u8* buf, int size, u32* bytes_written)
        }
 #elif BUILD_FT2232_LIBFTDI == 1
        int retval;
-       if ( ( retval = ftdi_write_data(&ftdic, buf, size) ) < 0 )
+       if ((retval = ftdi_write_data(&ftdic, buf, size)) < 0)
        {
                *bytes_written = 0;
-               LOG_ERROR( "ftdi_write_data: %s", ftdi_get_error_string(&ftdic) );
+               LOG_ERROR("ftdi_write_data: %s", ftdi_get_error_string(&ftdic));
                return ERROR_JTAG_DEVICE_ERROR;
        }
        else
@@ -223,19 +366,18 @@ static int ft2232_write(u8* buf, int size, u32* bytes_written)
 #endif
 }
 
-
-static int ft2232_read(u8* buf, u32 size, u32* bytes_read)
+static int ft2232_read(uint8_t* buf, uint32_t size, uint32_t* bytes_read)
 {
 #if BUILD_FT2232_FTD2XX == 1
-       DWORD     dw_bytes_read;
+       DWORD dw_bytes_read;
        FT_STATUS status;
-       int       timeout = 5;
+       int timeout = 5;
        *bytes_read = 0;
 
-       while ( (*bytes_read < size) && timeout-- )
+       while ((*bytes_read < size) && timeout--)
        {
-               if ( ( status = FT_Read(ftdih, buf + *bytes_read, size -
-                                         *bytes_read, &dw_bytes_read) ) != FT_OK )
+               if ((status = FT_Read(ftdih, buf + *bytes_read, size -
+                                         *bytes_read, &dw_bytes_read)) != FT_OK)
                {
                        *bytes_read = 0;
                        LOG_ERROR("FT_Read returned: %lu", status);
@@ -246,15 +388,15 @@ static int ft2232_read(u8* buf, u32 size, u32* bytes_read)
 
 #elif BUILD_FT2232_LIBFTDI == 1
        int retval;
-       int timeout = 100;
+       int timeout = LIBFTDI_READ_RETRY_COUNT;
        *bytes_read = 0;
 
-       while ( (*bytes_read < size) && timeout-- )
+       while ((*bytes_read < size) && timeout--)
        {
-               if ( ( retval = ftdi_read_data(&ftdic, buf + *bytes_read, size - *bytes_read) ) < 0 )
+               if ((retval = ftdi_read_data(&ftdic, buf + *bytes_read, size - *bytes_read)) < 0)
                {
                        *bytes_read = 0;
-                       LOG_ERROR( "ftdi_read_data: %s", ftdi_get_error_string(&ftdic) );
+                       LOG_ERROR("ftdi_read_data: %s", ftdi_get_error_string(&ftdic));
                        return ERROR_JTAG_DEVICE_ERROR;
                }
                *bytes_read += retval;
@@ -264,26 +406,96 @@ static int ft2232_read(u8* buf, u32 size, u32* bytes_read)
 
        if (*bytes_read < size)
        {
-               LOG_ERROR("couldn't read the requested number of bytes from FT2232 device (%i < %i)", *bytes_read, size);
+               LOG_ERROR("couldn't read enough bytes from "
+                               "FT2232 device (%i < %i)",
+                               (unsigned)*bytes_read,
+                               (unsigned)size);
                return ERROR_JTAG_DEVICE_ERROR;
        }
 
        return ERROR_OK;
 }
 
+static bool ft2232_device_is_highspeed(void)
+{
+#if BUILD_FT2232_FTD2XX == 1
+       return (ftdi_device == FT_DEVICE_2232H) || (ftdi_device == FT_DEVICE_4232H);
+#elif BUILD_FT2232_LIBFTDI == 1
+       return (ftdi_device == TYPE_2232H || ftdi_device == TYPE_4232H);
+#endif
+}
+
+/*
+ * Commands that only apply to the FT2232H and FT4232H devices.
+ * See chapter 6 in http://www.ftdichip.com/Documents/AppNotes/
+ * AN_108_Command_Processor_for_MPSSE_and_MCU_Host_Bus_Emulation_Modes.pdf
+ */
+
+static int ft2232h_ft4232h_adaptive_clocking(bool enable)
+{
+       uint8_t buf = enable ? 0x96 : 0x97;
+       LOG_DEBUG("%2.2x", buf);
+
+       uint32_t bytes_written;
+       int retval = ft2232_write(&buf, 1, &bytes_written);
+       if ((ERROR_OK != retval) || (bytes_written != 1))
+       {
+               LOG_ERROR("couldn't write command to %s adaptive clocking"
+                       , enable ? "enable" : "disable");
+               return retval;
+       }
+
+       return ERROR_OK;
+}
+
+/**
+ * Enable/disable the clk divide by 5 of the 60MHz master clock.
+ * This result in a JTAG clock speed range of 91.553Hz-6MHz
+ * respective 457.763Hz-30MHz.
+ */
+static int ft2232h_ft4232h_clk_divide_by_5(bool enable)
+{
+       uint32_t bytes_written;
+       uint8_t buf = enable ?  0x8b : 0x8a;
+       int retval = ft2232_write(&buf, 1, &bytes_written);
+       if ((ERROR_OK != retval) || (bytes_written != 1))
+       {
+               LOG_ERROR("couldn't write command to %s clk divide by 5"
+                       , enable ? "enable" : "disable");
+               return ERROR_JTAG_INIT_FAILED;
+       }
+       ft2232_max_tck = enable ? FTDI_2232C_MAX_TCK : FTDI_2232H_4232H_MAX_TCK;
+       LOG_INFO("max TCK change to: %u kHz", ft2232_max_tck);
+
+       return ERROR_OK;
+}
 
 static int ft2232_speed(int speed)
 {
-       u buf[3];
+       uint8_t buf[3];
        int retval;
-       u32 bytes_written;
+       uint32_t bytes_written;
+
+       retval = ERROR_OK;
+       bool enable_adaptive_clocking = (RTCK_SPEED == speed);
+       if (ft2232_device_is_highspeed())
+               retval = ft2232h_ft4232h_adaptive_clocking(enable_adaptive_clocking);
+       else if (enable_adaptive_clocking)
+       {
+               LOG_ERROR("ft2232 device %lu does not support RTCK"
+                       , (long unsigned int)ftdi_device);
+               return ERROR_FAIL;
+       }
+
+       if ((enable_adaptive_clocking) || (ERROR_OK != retval))
+               return retval;
 
-       buf[0] = 0x86;                  /* command "set divisor" */
-       buf[1] = speed & 0xff;          /* valueL (0=6MHz, 1=3MHz, 2=2.0MHz, ...*/
-       buf[2] = (speed >> 8) & 0xff;   /* valueH */
+       buf[0] = 0x86;                                  /* command "set divisor" */
+       buf[1] = speed & 0xff;                  /* valueL (0 = 6MHz, 1 = 3MHz, 2 = 2.0MHz, ...*/
+       buf[2] = (speed >> 8) & 0xff;   /* valueH */
 
        LOG_DEBUG("%2.2x %2.2x %2.2x", buf[0], buf[1], buf[2]);
-       if ( ( ( retval = ft2232_write(buf, 3, &bytes_written) ) != ERROR_OK ) || (bytes_written != 3) )
+       if (((retval = ft2232_write(buf, 3, &bytes_written)) != ERROR_OK) || (bytes_written != 3))
        {
                LOG_ERROR("couldn't set FT2232 TCK speed");
                return retval;
@@ -292,25 +504,31 @@ static int ft2232_speed(int speed)
        return ERROR_OK;
 }
 
-
 static int ft2232_speed_div(int speed, int* khz)
 {
        /* Take a look in the FT2232 manual,
         * AN2232C-01 Command Processor for
         * MPSSE and MCU Host Bus. Chapter 3.8 */
 
-       *khz = 6000 / (1 + speed);
+       *khz = (RTCK_SPEED == speed) ? 0 : ft2232_max_tck / (1 + speed);
 
        return ERROR_OK;
 }
 
-
 static int ft2232_khz(int khz, int* jtag_speed)
 {
-       if (khz==0)
+       if (khz == 0)
        {
-               LOG_ERROR("RCLK not supported");
-               return ERROR_FAIL;
+               if (ft2232_device_is_highspeed())
+               {
+                       *jtag_speed = RTCK_SPEED;
+                       return ERROR_OK;
+               }
+               else
+               {
+                       LOG_DEBUG("RCLK not supported");
+                       return ERROR_FAIL;
+               }
        }
 
        /* Take a look in the FT2232 manual,
@@ -320,9 +538,9 @@ static int ft2232_khz(int khz, int* jtag_speed)
         * We will calc here with a multiplier
         * of 10 for better rounding later. */
 
-       /* Calc speed, (6000 / khz) - 1 */
+       /* Calc speed, (ft2232_max_tck / khz) - 1 */
        /* Use 65000 for better rounding */
-       *jtag_speed = (60000 / khz) - 10;
+       *jtag_speed = ((ft2232_max_tck*10) / khz) - 10;
 
        /* Add 0.9 for rounding */
        *jtag_speed += 9;
@@ -345,36 +563,18 @@ static int ft2232_khz(int khz, int* jtag_speed)
        return ERROR_OK;
 }
 
-
-static int ft2232_register_commands(struct command_context_s* cmd_ctx)
-{
-       register_command(cmd_ctx, NULL, "ft2232_device_desc", ft2232_handle_device_desc_command,
-                       COMMAND_CONFIG, "the USB device description of the FTDI FT2232 device");
-       register_command(cmd_ctx, NULL, "ft2232_serial", ft2232_handle_serial_command,
-                       COMMAND_CONFIG, "the serial number of the FTDI FT2232 device");
-       register_command(cmd_ctx, NULL, "ft2232_layout", ft2232_handle_layout_command,
-                       COMMAND_CONFIG, "the layout of the FT2232 GPIO signals used to control output-enables and reset signals");
-       register_command(cmd_ctx, NULL, "ft2232_vid_pid", ft2232_handle_vid_pid_command,
-                       COMMAND_CONFIG, "the vendor ID and product ID of the FTDI FT2232 device");
-       register_command(cmd_ctx, NULL, "ft2232_latency", ft2232_handle_latency_command,
-                       COMMAND_CONFIG, "set the FT2232 latency timer to a new value");
-       return ERROR_OK;
-}
-
-
-void ft2232_end_state(tap_state_t state)
+static void ft2232_end_state(tap_state_t state)
 {
        if (tap_is_state_stable(state))
                tap_set_end_state(state);
        else
        {
-               LOG_ERROR("BUG: %i is not a valid end state", state);
+               LOG_ERROR("BUG: %s is not a stable end state", tap_state_name(state));
                exit(-1);
        }
 }
 
-
-static void ft2232_read_scan(enum scan_type type, u8* buffer, int scan_size)
+static void ft2232_read_scan(enum scan_type type, uint8_t* buffer, int scan_size)
 {
        int num_bytes = (scan_size + 7) / 8;
        int bits_left = scan_size;
@@ -382,26 +582,25 @@ static void ft2232_read_scan(enum scan_type type, u8* buffer, int scan_size)
 
        while (num_bytes-- > 1)
        {
-               buffer[cur_byte] = BUFFER_READ;
-               cur_byte++;
+               buffer[cur_byte++] = buffer_read();
                bits_left -= 8;
        }
 
        buffer[cur_byte] = 0x0;
 
+       /* There is one more partial byte left from the clock data in/out instructions */
        if (bits_left > 1)
        {
-               buffer[cur_byte] = BUFFER_READ >> 1;
+               buffer[cur_byte] = buffer_read() >> 1;
        }
-
-       buffer[cur_byte] = ( buffer[cur_byte] | ( (BUFFER_READ & 0x02) << 6 ) ) >> (8 - bits_left);
+       /* This shift depends on the length of the clock data to tms instruction, insterted at end of the scan, now fixed to a two step transition in ft2232_add_scan */
+       buffer[cur_byte] = (buffer[cur_byte] | (((buffer_read()) << 1) & 0x80)) >> (8 - bits_left);
 }
 
-
 static void ft2232_debug_dump_buffer(void)
 {
-       int   i;
-       char  line[256];
+       int i;
+       char line[256];
        char* line_p = line;
 
        for (i = 0; i < ft2232_buffer_size; i++)
@@ -418,16 +617,15 @@ static void ft2232_debug_dump_buffer(void)
                LOG_DEBUG("%s", line);
 }
 
-
-static int ft2232_send_and_recv(jtag_command_t* first, jtag_command_t* last)
+static int ft2232_send_and_recv(struct jtag_command* first, struct jtag_command* last)
 {
-       jtag_command_t* cmd;
-       u8*             buffer;
-       int             scan_size;
+       struct jtag_command* cmd;
+       uint8_t* buffer;
+       int scan_size;
        enum scan_type  type;
-       int             retval;
-       u32             bytes_written=0;
-       u32             bytes_read=0;
+       int retval;
+       uint32_t bytes_written = 0;
+       uint32_t bytes_read = 0;
 
 #ifdef _DEBUG_USB_IO_
        struct timeval  start, inter, inter2, end;
@@ -443,7 +641,7 @@ static int ft2232_send_and_recv(jtag_command_t* first, jtag_command_t* last)
        gettimeofday(&start, NULL);
 #endif
 
-       if ( ( retval = ft2232_write(ft2232_buffer, ft2232_buffer_size, &bytes_written) ) != ERROR_OK )
+       if ((retval = ft2232_write(ft2232_buffer, ft2232_buffer_size, &bytes_written)) != ERROR_OK)
        {
                LOG_ERROR("couldn't write MPSSE commands to FT2232");
                return retval;
@@ -455,14 +653,15 @@ static int ft2232_send_and_recv(jtag_command_t* first, jtag_command_t* last)
 
        if (ft2232_expect_read)
        {
-               int timeout = 100;
+               /* FIXME this "timeout" is never changed ... */
+               int timeout = LIBFTDI_READ_RETRY_COUNT;
                ft2232_buffer_size = 0;
 
 #ifdef _DEBUG_USB_IO_
                gettimeofday(&inter2, NULL);
 #endif
 
-               if ( ( retval = ft2232_read(ft2232_buffer, ft2232_expect_read, &bytes_read) ) != ERROR_OK )
+               if ((retval = ft2232_read(ft2232_buffer, ft2232_expect_read, &bytes_read)) != ERROR_OK)
                {
                        LOG_ERROR("couldn't read from FT2232");
                        return retval;
@@ -485,16 +684,21 @@ static int ft2232_send_and_recv(jtag_command_t* first, jtag_command_t* last)
 
                if (ft2232_expect_read != ft2232_buffer_size)
                {
-                       LOG_ERROR("ft2232_expect_read (%i) != ft2232_buffer_size (%i) (%i retries)", ft2232_expect_read,
+                       LOG_ERROR("ft2232_expect_read (%i) != "
+                                       "ft2232_buffer_size (%i) "
+                                       "(%i retries)",
+                                       ft2232_expect_read,
                                        ft2232_buffer_size,
-                                       100 - timeout);
+                                       LIBFTDI_READ_RETRY_COUNT - timeout);
                        ft2232_debug_dump_buffer();
 
                        exit(-1);
                }
 
 #ifdef _DEBUG_USB_COMMS_
-               LOG_DEBUG("read buffer (%i retries): %i bytes", 100 - timeout, ft2232_buffer_size);
+               LOG_DEBUG("read buffer (%i retries): %i bytes",
+                               LIBFTDI_READ_RETRY_COUNT - timeout,
+                               ft2232_buffer_size);
                ft2232_debug_dump_buffer();
 #endif
        }
@@ -537,78 +741,84 @@ static int ft2232_send_and_recv(jtag_command_t* first, jtag_command_t* last)
        return retval;
 }
 
-
-static void ft2232_add_pathmove(pathmove_command_t* cmd)
+/**
+ * Function ft2232_add_pathmove
+ * moves the TAP controller from the current state to a new state through the
+ * given path, where path is an array of tap_state_t's.
+ *
+ * @param path is an array of tap_stat_t which gives the states to traverse through
+ *   ending with the last state at path[num_states-1]
+ * @param num_states is the count of state steps to move through
+ */
+static void ft2232_add_pathmove(tap_state_t* path, int num_states)
 {
-       int num_states = cmd->num_states;
        int state_count = 0;
 
-       while (num_states)
-       {
-               u8  tms_byte = 0;       /* zero this on each MPSSE batch */
+       assert((unsigned) num_states <= 32u);           /* tms_bits only holds 32 bits */
 
-               int bit_count = 0;
+       DEBUG_JTAG_IO("-");
 
-               int num_states_batch = num_states > 7 ? 7 : num_states;
+       /* this loop verifies that the path is legal and logs each state in the path */
+       while (num_states)
+       {
+               unsigned char   tms_byte = 0;       /* zero this on each MPSSE batch */
+               int             bit_count = 0;
+               int             num_states_batch = num_states > 7 ? 7 : num_states;
 
                /* command "Clock Data to TMS/CS Pin (no Read)" */
-               BUFFER_ADD = 0x4b;
+               buffer_write(0x4b);
 
                /* number of states remaining */
-               BUFFER_ADD = num_states_batch - 1;
+               buffer_write(num_states_batch - 1);
 
-               while (num_states_batch--)
-               {
-                       if (tap_state_transition(tap_get_state(), false) == cmd->path[state_count])
+               while (num_states_batch--) {
+                       /* either TMS=0 or TMS=1 must work ... */
+                       if (tap_state_transition(tap_get_state(), false)
+                                               == path[state_count])
                                buf_set_u32(&tms_byte, bit_count++, 1, 0x0);
-                       else if (tap_state_transition(tap_get_state(), true) == cmd->path[state_count])
+                       else if (tap_state_transition(tap_get_state(), true)
+                                               == path[state_count])
                                buf_set_u32(&tms_byte, bit_count++, 1, 0x1);
-                       else
-                       {
-                               LOG_ERROR( "BUG: %s -> %s isn't a valid TAP transition", tap_state_name(
-                                                                tap_get_state() ), tap_state_name(cmd->path[state_count]) );
+
+                       /* ... or else the caller goofed BADLY */
+                       else {
+                               LOG_ERROR("BUG: %s -> %s isn't a valid "
+                                               "TAP state transition",
+                                       tap_state_name(tap_get_state()),
+                                       tap_state_name(path[state_count]));
                                exit(-1);
                        }
 
-                       tap_set_state(cmd->path[state_count]);
+                       tap_set_state(path[state_count]);
                        state_count++;
                        num_states--;
                }
 
-               BUFFER_ADD = tms_byte;
+               buffer_write(tms_byte);
        }
-       
        tap_set_end_state(tap_get_state());
 }
 
-
-void ft2232_add_scan(int ir_scan, enum scan_type type, u8* buffer, int scan_size)
+static void ft2232_add_scan(bool ir_scan, enum scan_type type, uint8_t* buffer, int scan_size)
 {
        int num_bytes = (scan_size + 7) / 8;
        int bits_left = scan_size;
        int cur_byte  = 0;
        int last_bit;
 
-       if ( !( ( !ir_scan && (tap_get_state() == TAP_DRSHIFT) )
-          || (    ir_scan && (tap_get_state() == TAP_IRSHIFT) ) ) )
+       if (!ir_scan)
        {
-               /* command "Clock Data to TMS/CS Pin (no Read)" */
-               BUFFER_ADD = 0x4b;
-
-               BUFFER_ADD = 0x6;       /* scan 7 bits */
-
-               /* TMS data bits */
-               if (ir_scan)
+               if (tap_get_state() != TAP_DRSHIFT)
                {
-                       BUFFER_ADD = tap_get_tms_path(tap_get_state(), TAP_IRSHIFT);
-                       tap_set_state(TAP_IRSHIFT);
+                       move_to_state(TAP_DRSHIFT);
                }
-               else
+       }
+       else
+       {
+               if (tap_get_state() != TAP_IRSHIFT)
                {
-                       BUFFER_ADD = tap_get_tms_path(tap_get_state(), TAP_DRSHIFT);
-                       tap_set_state(TAP_DRSHIFT);
+                       move_to_state(TAP_IRSHIFT);
                }
-               /* LOG_DEBUG("added TMS scan (no read)"); */
        }
 
        /* add command for complete bytes */
@@ -618,34 +828,34 @@ void ft2232_add_scan(int ir_scan, enum scan_type type, u8* buffer, int scan_size
                if (type == SCAN_IO)
                {
                        /* Clock Data Bytes In and Out LSB First */
-                       BUFFER_ADD = 0x39;
+                       buffer_write(0x39);
                        /* LOG_DEBUG("added TDI bytes (io %i)", num_bytes); */
                }
                else if (type == SCAN_OUT)
                {
                        /* Clock Data Bytes Out on -ve Clock Edge LSB First (no Read) */
-                       BUFFER_ADD = 0x19;
+                       buffer_write(0x19);
                        /* LOG_DEBUG("added TDI bytes (o)"); */
                }
                else if (type == SCAN_IN)
                {
                        /* Clock Data Bytes In on +ve Clock Edge LSB First (no Write) */
-                       BUFFER_ADD = 0x28;
+                       buffer_write(0x28);
                        /* LOG_DEBUG("added TDI bytes (i %i)", num_bytes); */
                }
 
                thisrun_bytes = (num_bytes > 65537) ? 65536 : (num_bytes - 1);
                num_bytes    -= thisrun_bytes;
-               BUFFER_ADD    = (thisrun_bytes - 1) & 0xff;
-               BUFFER_ADD    = ( (thisrun_bytes - 1) >> 8 ) & 0xff;
+
+               buffer_write((uint8_t) (thisrun_bytes - 1));
+               buffer_write((uint8_t) ((thisrun_bytes - 1) >> 8));
 
                if (type != SCAN_IN)
                {
                        /* add complete bytes */
                        while (thisrun_bytes-- > 0)
                        {
-                               BUFFER_ADD = buffer[cur_byte];
-                               cur_byte++;
+                               buffer_write(buffer[cur_byte++]);
                                bits_left -= 8;
                        }
                }
@@ -657,7 +867,7 @@ void ft2232_add_scan(int ir_scan, enum scan_type type, u8* buffer, int scan_size
 
        /* the most signifcant bit is scanned during TAP movement */
        if (type != SCAN_IN)
-               last_bit = ( buffer[cur_byte] >> (bits_left - 1) ) & 0x1;
+               last_bit = (buffer[cur_byte] >> (bits_left - 1)) & 0x1;
        else
                last_bit = 0;
 
@@ -667,83 +877,95 @@ void ft2232_add_scan(int ir_scan, enum scan_type type, u8* buffer, int scan_size
                if (type == SCAN_IO)
                {
                        /* Clock Data Bits In and Out LSB First */
-                       BUFFER_ADD = 0x3b;
+                       buffer_write(0x3b);
                        /* LOG_DEBUG("added TDI bits (io) %i", bits_left - 1); */
                }
                else if (type == SCAN_OUT)
                {
                        /* Clock Data Bits Out on -ve Clock Edge LSB First (no Read) */
-                       BUFFER_ADD = 0x1b;
+                       buffer_write(0x1b);
                        /* LOG_DEBUG("added TDI bits (o)"); */
                }
                else if (type == SCAN_IN)
                {
                        /* Clock Data Bits In on +ve Clock Edge LSB First (no Write) */
-                       BUFFER_ADD = 0x2a;
+                       buffer_write(0x2a);
                        /* LOG_DEBUG("added TDI bits (i %i)", bits_left - 1); */
                }
-               BUFFER_ADD = bits_left - 2;
+
+               buffer_write(bits_left - 2);
                if (type != SCAN_IN)
-                       BUFFER_ADD = buffer[cur_byte];
+                       buffer_write(buffer[cur_byte]);
        }
 
-       if ( (  ir_scan && (tap_get_end_state() == TAP_IRSHIFT) )
-         || ( !ir_scan && (tap_get_end_state() == TAP_DRSHIFT) ) )
+       if ((ir_scan && (tap_get_end_state() == TAP_IRSHIFT))
+         || (!ir_scan && (tap_get_end_state() == TAP_DRSHIFT)))
        {
                if (type == SCAN_IO)
                {
                        /* Clock Data Bits In and Out LSB First */
-                       BUFFER_ADD = 0x3b;
+                       buffer_write(0x3b);
                        /* LOG_DEBUG("added TDI bits (io) %i", bits_left - 1); */
                }
                else if (type == SCAN_OUT)
                {
                        /* Clock Data Bits Out on -ve Clock Edge LSB First (no Read) */
-                       BUFFER_ADD = 0x1b;
+                       buffer_write(0x1b);
                        /* LOG_DEBUG("added TDI bits (o)"); */
                }
                else if (type == SCAN_IN)
                {
                        /* Clock Data Bits In on +ve Clock Edge LSB First (no Write) */
-                       BUFFER_ADD = 0x2a;
+                       buffer_write(0x2a);
                        /* LOG_DEBUG("added TDI bits (i %i)", bits_left - 1); */
                }
-               BUFFER_ADD = 0x0;
-               BUFFER_ADD = last_bit;
+               buffer_write(0x0);
+               buffer_write(last_bit);
        }
        else
        {
+               int tms_bits;
+               int tms_count;
+               uint8_t mpsse_cmd;
+
                /* move from Shift-IR/DR to end state */
                if (type != SCAN_OUT)
                {
+                       /* We always go to the PAUSE state in two step at the end of an IN or IO scan */
+                       /* This must be coordinated with the bit shifts in ft2232_read_scan    */
+                       tms_bits  = 0x01;
+                       tms_count = 2;
                        /* Clock Data to TMS/CS Pin with Read */
-                       BUFFER_ADD = 0x6b;
-                       /* LOG_DEBUG("added TMS scan (read)"); */
+                       mpsse_cmd = 0x6b;
                }
                else
                {
+                       tms_bits  = tap_get_tms_path(tap_get_state(), tap_get_end_state());
+                       tms_count = tap_get_tms_path_len(tap_get_state(), tap_get_end_state());
                        /* Clock Data to TMS/CS Pin (no Read) */
-                       BUFFER_ADD = 0x4b;
-                       /* LOG_DEBUG("added TMS scan (no read)"); */
+                       mpsse_cmd = 0x4b;
                }
-               BUFFER_ADD = 0x6;   /* scan 7 bits */
 
-               BUFFER_ADD = tap_get_tms_path( tap_get_state(), tap_get_end_state() ) | (last_bit << 7);
-               tap_set_state( tap_get_end_state() );
+               DEBUG_JTAG_IO("finish %s", (type == SCAN_OUT) ? "without read" : "via PAUSE");
+               clock_tms(mpsse_cmd, tms_bits, tms_count, last_bit);
        }
-}
 
+       if (tap_get_state() != tap_get_end_state())
+       {
+               move_to_state(tap_get_end_state());
+       }
+}
 
-static int ft2232_large_scan(scan_command_t* cmd, enum scan_type type, u8* buffer, int scan_size)
+static int ft2232_large_scan(struct scan_command* cmd, enum scan_type type, uint8_t* buffer, int scan_size)
 {
        int num_bytes = (scan_size + 7) / 8;
        int bits_left = scan_size;
        int cur_byte  = 0;
        int last_bit;
-       u8* receive_buffer  = malloc( CEIL(scan_size, 8) );
-       u8* receive_pointer = receive_buffer;
-       u32 bytes_written;
-       u32 bytes_read;
+       uint8_t* receive_buffer  = malloc(CEIL(scan_size, 8));
+       uint8_t* receive_pointer = receive_buffer;
+       uint32_t bytes_written;
+       uint32_t bytes_read;
        int retval;
        int thisrun_read = 0;
 
@@ -755,22 +977,16 @@ static int ft2232_large_scan(scan_command_t* cmd, enum scan_type type, u8* buffe
 
        if (tap_get_state() != TAP_DRSHIFT)
        {
-               /* command "Clock Data to TMS/CS Pin (no Read)" */
-               BUFFER_ADD = 0x4b;
-
-               BUFFER_ADD = 0x6;       /* scan 7 bits */
-
-               /* TMS data bits */
-               BUFFER_ADD = tap_get_tms_path(tap_get_state(), TAP_DRSHIFT);
-               tap_set_state(TAP_DRSHIFT);
+               move_to_state(TAP_DRSHIFT);
        }
 
-       if ( ( retval = ft2232_write(ft2232_buffer, ft2232_buffer_size, &bytes_written) ) != ERROR_OK )
+       if ((retval = ft2232_write(ft2232_buffer, ft2232_buffer_size, &bytes_written)) != ERROR_OK)
        {
                LOG_ERROR("couldn't write MPSSE commands to FT2232");
                exit(-1);
        }
-       LOG_DEBUG("ft2232_buffer_size: %i, bytes_written: %i", ft2232_buffer_size, bytes_written);
+       LOG_DEBUG("ft2232_buffer_size: %i, bytes_written: %i",
+                 ft2232_buffer_size, (int)bytes_written);
        ft2232_buffer_size = 0;
 
        /* add command for complete bytes */
@@ -781,34 +997,34 @@ static int ft2232_large_scan(scan_command_t* cmd, enum scan_type type, u8* buffe
                if (type == SCAN_IO)
                {
                        /* Clock Data Bytes In and Out LSB First */
-                       BUFFER_ADD = 0x39;
+                       buffer_write(0x39);
                        /* LOG_DEBUG("added TDI bytes (io %i)", num_bytes); */
                }
                else if (type == SCAN_OUT)
                {
                        /* Clock Data Bytes Out on -ve Clock Edge LSB First (no Read) */
-                       BUFFER_ADD = 0x19;
+                       buffer_write(0x19);
                        /* LOG_DEBUG("added TDI bytes (o)"); */
                }
                else if (type == SCAN_IN)
                {
                        /* Clock Data Bytes In on +ve Clock Edge LSB First (no Write) */
-                       BUFFER_ADD = 0x28;
+                       buffer_write(0x28);
                        /* LOG_DEBUG("added TDI bytes (i %i)", num_bytes); */
                }
 
                thisrun_bytes = (num_bytes > 65537) ? 65536 : (num_bytes - 1);
                thisrun_read  = thisrun_bytes;
                num_bytes    -= thisrun_bytes;
-               BUFFER_ADD    = (thisrun_bytes - 1) & 0xff;
-               BUFFER_ADD    = ( (thisrun_bytes - 1) >> 8 ) & 0xff;
+               buffer_write((uint8_t) (thisrun_bytes - 1));
+               buffer_write((uint8_t) ((thisrun_bytes - 1) >> 8));
 
                if (type != SCAN_IN)
                {
                        /* add complete bytes */
                        while (thisrun_bytes-- > 0)
                        {
-                               BUFFER_ADD = buffer[cur_byte];
+                               buffer_write(buffer[cur_byte]);
                                cur_byte++;
                                bits_left -= 8;
                        }
@@ -818,22 +1034,26 @@ static int ft2232_large_scan(scan_command_t* cmd, enum scan_type type, u8* buffe
                        bits_left -= 8 * (thisrun_bytes);
                }
 
-               if ( ( retval = ft2232_write(ft2232_buffer, ft2232_buffer_size, &bytes_written) ) != ERROR_OK )
+               if ((retval = ft2232_write(ft2232_buffer, ft2232_buffer_size, &bytes_written)) != ERROR_OK)
                {
                        LOG_ERROR("couldn't write MPSSE commands to FT2232");
                        exit(-1);
                }
-               LOG_DEBUG("ft2232_buffer_size: %i, bytes_written: %i", ft2232_buffer_size, bytes_written);
+               LOG_DEBUG("ft2232_buffer_size: %i, bytes_written: %i",
+                         ft2232_buffer_size,
+                         (int)bytes_written);
                ft2232_buffer_size = 0;
 
                if (type != SCAN_OUT)
                {
-                       if ( ( retval = ft2232_read(receive_pointer, thisrun_read, &bytes_read) ) != ERROR_OK )
+                       if ((retval = ft2232_read(receive_pointer, thisrun_read, &bytes_read)) != ERROR_OK)
                        {
                                LOG_ERROR("couldn't read from FT2232");
                                exit(-1);
                        }
-                       LOG_DEBUG("thisrun_read: %i, bytes_read: %i", thisrun_read, bytes_read);
+                       LOG_DEBUG("thisrun_read: %i, bytes_read: %i",
+                                 thisrun_read,
+                                 (int)bytes_read);
                        receive_pointer += bytes_read;
                }
        }
@@ -842,7 +1062,7 @@ static int ft2232_large_scan(scan_command_t* cmd, enum scan_type type, u8* buffe
 
        /* the most signifcant bit is scanned during TAP movement */
        if (type != SCAN_IN)
-               last_bit = ( buffer[cur_byte] >> (bits_left - 1) ) & 0x1;
+               last_bit = (buffer[cur_byte] >> (bits_left - 1)) & 0x1;
        else
                last_bit = 0;
 
@@ -852,24 +1072,24 @@ static int ft2232_large_scan(scan_command_t* cmd, enum scan_type type, u8* buffe
                if (type == SCAN_IO)
                {
                        /* Clock Data Bits In and Out LSB First */
-                       BUFFER_ADD = 0x3b;
+                       buffer_write(0x3b);
                        /* LOG_DEBUG("added TDI bits (io) %i", bits_left - 1); */
                }
                else if (type == SCAN_OUT)
                {
                        /* Clock Data Bits Out on -ve Clock Edge LSB First (no Read) */
-                       BUFFER_ADD = 0x1b;
+                       buffer_write(0x1b);
                        /* LOG_DEBUG("added TDI bits (o)"); */
                }
                else if (type == SCAN_IN)
                {
                        /* Clock Data Bits In on +ve Clock Edge LSB First (no Write) */
-                       BUFFER_ADD = 0x2a;
+                       buffer_write(0x2a);
                        /* LOG_DEBUG("added TDI bits (i %i)", bits_left - 1); */
                }
-               BUFFER_ADD = bits_left - 2;
+               buffer_write(bits_left - 2);
                if (type != SCAN_IN)
-                       BUFFER_ADD = buffer[cur_byte];
+                       buffer_write(buffer[cur_byte]);
 
                if (type != SCAN_OUT)
                        thisrun_read += 2;
@@ -880,97 +1100,105 @@ static int ft2232_large_scan(scan_command_t* cmd, enum scan_type type, u8* buffe
                if (type == SCAN_IO)
                {
                        /* Clock Data Bits In and Out LSB First */
-                       BUFFER_ADD = 0x3b;
+                       buffer_write(0x3b);
                        /* LOG_DEBUG("added TDI bits (io) %i", bits_left - 1); */
                }
                else if (type == SCAN_OUT)
                {
                        /* Clock Data Bits Out on -ve Clock Edge LSB First (no Read) */
-                       BUFFER_ADD = 0x1b;
+                       buffer_write(0x1b);
                        /* LOG_DEBUG("added TDI bits (o)"); */
                }
                else if (type == SCAN_IN)
                {
                        /* Clock Data Bits In on +ve Clock Edge LSB First (no Write) */
-                       BUFFER_ADD = 0x2a;
+                       buffer_write(0x2a);
                        /* LOG_DEBUG("added TDI bits (i %i)", bits_left - 1); */
                }
-               BUFFER_ADD = 0x0;
-               BUFFER_ADD = last_bit;
+               buffer_write(0x0);
+               buffer_write(last_bit);
        }
        else
        {
+               int tms_bits  = tap_get_tms_path(tap_get_state(), tap_get_end_state());
+               int tms_count = tap_get_tms_path_len(tap_get_state(), tap_get_end_state());
+               uint8_t mpsse_cmd;
+
                /* move from Shift-IR/DR to end state */
                if (type != SCAN_OUT)
                {
                        /* Clock Data to TMS/CS Pin with Read */
-                       BUFFER_ADD = 0x6b;
+                       mpsse_cmd = 0x6b;
                        /* LOG_DEBUG("added TMS scan (read)"); */
                }
                else
                {
                        /* Clock Data to TMS/CS Pin (no Read) */
-                       BUFFER_ADD = 0x4b;
+                       mpsse_cmd = 0x4b;
                        /* LOG_DEBUG("added TMS scan (no read)"); */
                }
-               BUFFER_ADD = 0x6;
-               BUFFER_ADD = tap_get_tms_path( tap_get_state(), tap_get_end_state() ) | (last_bit << 7);
-               tap_set_state( tap_get_end_state() );
+
+               DEBUG_JTAG_IO("finish, %s", (type == SCAN_OUT) ? "no read" : "read");
+               clock_tms(mpsse_cmd, tms_bits, tms_count, last_bit);
        }
 
        if (type != SCAN_OUT)
                thisrun_read += 1;
 
-       if ( ( retval = ft2232_write(ft2232_buffer, ft2232_buffer_size, &bytes_written) ) != ERROR_OK )
+       if ((retval = ft2232_write(ft2232_buffer, ft2232_buffer_size, &bytes_written)) != ERROR_OK)
        {
                LOG_ERROR("couldn't write MPSSE commands to FT2232");
                exit(-1);
        }
-       LOG_DEBUG("ft2232_buffer_size: %i, bytes_written: %i", ft2232_buffer_size, bytes_written);
+       LOG_DEBUG("ft2232_buffer_size: %i, bytes_written: %i",
+                 ft2232_buffer_size,
+                 (int)bytes_written);
        ft2232_buffer_size = 0;
 
        if (type != SCAN_OUT)
        {
-               if ( ( retval = ft2232_read(receive_pointer, thisrun_read, &bytes_read) ) != ERROR_OK )
+               if ((retval = ft2232_read(receive_pointer, thisrun_read, &bytes_read)) != ERROR_OK)
                {
                        LOG_ERROR("couldn't read from FT2232");
                        exit(-1);
                }
-               LOG_DEBUG("thisrun_read: %i, bytes_read: %i", thisrun_read, bytes_read);
+               LOG_DEBUG("thisrun_read: %i, bytes_read: %i",
+                         thisrun_read,
+                         (int)bytes_read);
                receive_pointer += bytes_read;
        }
 
        return ERROR_OK;
 }
 
-
 static int ft2232_predict_scan_out(int scan_size, enum scan_type type)
 {
        int predicted_size = 3;
        int num_bytes = (scan_size - 1) / 8;
 
        if (tap_get_state() != TAP_DRSHIFT)
-               predicted_size += 3;
+               predicted_size += get_tms_buffer_requirements(tap_get_tms_path_len(tap_get_state(), TAP_DRSHIFT));
 
-       if (type == SCAN_IN)    /* only from device to host */
+       if (type == SCAN_IN)    /* only from device to host */
        {
                /* complete bytes */
                predicted_size += CEIL(num_bytes, 65536) * 3;
+
                /* remaining bits - 1 (up to 7) */
-               predicted_size += ( (scan_size - 1) % 8 ) ? 2 : 0;
+               predicted_size += ((scan_size - 1) % 8) ? 2 : 0;
        }
-       else                    /* host to device, or bidirectional */
+       else    /* host to device, or bidirectional */
        {
                /* complete bytes */
                predicted_size += num_bytes + CEIL(num_bytes, 65536) * 3;
+
                /* remaining bits -1 (up to 7) */
-               predicted_size += ( (scan_size - 1) % 8 ) ? 3 : 0;
+               predicted_size += ((scan_size - 1) % 8) ? 3 : 0;
        }
 
        return predicted_size;
 }
 
-
 static int ft2232_predict_scan_in(int scan_size, enum scan_type type)
 {
        int predicted_size = 0;
@@ -981,7 +1209,7 @@ static int ft2232_predict_scan_in(int scan_size, enum scan_type type)
                predicted_size += (CEIL(scan_size, 8) > 1) ? (CEIL(scan_size, 8) - 1) : 0;
 
                /* remaining bits - 1 */
-               predicted_size += ( (scan_size - 1) % 8 ) ? 1 : 0;
+               predicted_size += ((scan_size - 1) % 8) ? 1 : 0;
 
                /* last bit (from TMS scan) */
                predicted_size += 1;
@@ -992,48 +1220,48 @@ static int ft2232_predict_scan_in(int scan_size, enum scan_type type)
        return predicted_size;
 }
 
-
 static void usbjtag_reset(int trst, int srst)
 {
+       enum reset_types jtag_reset_config = jtag_get_reset_config();
        if (trst == 1)
        {
                if (jtag_reset_config & RESET_TRST_OPEN_DRAIN)
-                       low_direction |= nTRSTnOE;  /* switch to output pin (output is low) */
+                       low_direction |= nTRSTnOE;      /* switch to output pin (output is low) */
                else
-                       low_output &= ~nTRST;       /* switch output low */
+                       low_output &= ~nTRST;           /* switch output low */
        }
        else if (trst == 0)
        {
                if (jtag_reset_config & RESET_TRST_OPEN_DRAIN)
-                       low_direction &= ~nTRSTnOE; /* switch to input pin (high-Z + internal and external pullup) */
+                       low_direction &= ~nTRSTnOE;     /* switch to input pin (high-Z + internal and external pullup) */
                else
-                       low_output |= nTRST;        /* switch output high */
+                       low_output |= nTRST;            /* switch output high */
        }
 
        if (srst == 1)
        {
                if (jtag_reset_config & RESET_SRST_PUSH_PULL)
-                       low_output &= ~nSRST;       /* switch output low */
+                       low_output &= ~nSRST;           /* switch output low */
                else
-                       low_direction |= nSRSTnOE;  /* switch to output pin (output is low) */
+                       low_direction |= nSRSTnOE;      /* switch to output pin (output is low) */
        }
        else if (srst == 0)
        {
                if (jtag_reset_config & RESET_SRST_PUSH_PULL)
-                       low_output |= nSRST;        /* switch output high */
+                       low_output |= nSRST;            /* switch output high */
                else
-                       low_direction &= ~nSRSTnOE; /* switch to input pin (high-Z) */
+                       low_direction &= ~nSRSTnOE;     /* switch to input pin (high-Z) */
        }
 
        /* command "set data bits low byte" */
-       BUFFER_ADD = 0x80;
-       BUFFER_ADD = low_output;
-       BUFFER_ADD = low_direction;
+       buffer_write(0x80);
+       buffer_write(low_output);
+       buffer_write(low_direction);
 }
 
-
 static void jtagkey_reset(int trst, int srst)
 {
+       enum reset_types jtag_reset_config = jtag_get_reset_config();
        if (trst == 1)
        {
                if (jtag_reset_config & RESET_TRST_OPEN_DRAIN)
@@ -1065,16 +1293,16 @@ static void jtagkey_reset(int trst, int srst)
        }
 
        /* command "set data bits high byte" */
-       BUFFER_ADD = 0x82;
-       BUFFER_ADD = high_output;
-       BUFFER_ADD = high_direction;
+       buffer_write(0x82);
+       buffer_write(high_output);
+       buffer_write(high_direction);
        LOG_DEBUG("trst: %i, srst: %i, high_output: 0x%2.2x, high_direction: 0x%2.2x", trst, srst, high_output,
                        high_direction);
 }
 
-
 static void olimex_jtag_reset(int trst, int srst)
 {
+       enum reset_types jtag_reset_config = jtag_get_reset_config();
        if (trst == 1)
        {
                if (jtag_reset_config & RESET_TRST_OPEN_DRAIN)
@@ -1100,14 +1328,13 @@ static void olimex_jtag_reset(int trst, int srst)
        }
 
        /* command "set data bits high byte" */
-       BUFFER_ADD = 0x82;
-       BUFFER_ADD = high_output;
-       BUFFER_ADD = high_direction;
+       buffer_write(0x82);
+       buffer_write(high_output);
+       buffer_write(high_direction);
        LOG_DEBUG("trst: %i, srst: %i, high_output: 0x%2.2x, high_direction: 0x%2.2x", trst, srst, high_output,
                        high_direction);
 }
 
-
 static void axm0432_jtag_reset(int trst, int srst)
 {
        if (trst == 1)
@@ -1130,14 +1357,13 @@ static void axm0432_jtag_reset(int trst, int srst)
        }
 
        /* command "set data bits low byte" */
-       BUFFER_ADD = 0x82;
-       BUFFER_ADD = high_output;
-       BUFFER_ADD = high_direction;
+       buffer_write(0x82);
+       buffer_write(high_output);
+       buffer_write(high_direction);
        LOG_DEBUG("trst: %i, srst: %i, high_output: 0x%2.2x, high_direction: 0x%2.2x", trst, srst, high_output,
                        high_direction);
 }
 
-
 static void flyswatter_reset(int trst, int srst)
 {
        if (trst == 1)
@@ -1159,13 +1385,12 @@ static void flyswatter_reset(int trst, int srst)
        }
 
        /* command "set data bits low byte" */
-       BUFFER_ADD = 0x80;
-       BUFFER_ADD = low_output;
-       BUFFER_ADD = low_direction;
+       buffer_write(0x80);
+       buffer_write(low_output);
+       buffer_write(low_direction);
        LOG_DEBUG("trst: %i, srst: %i, low_output: 0x%2.2x, low_direction: 0x%2.2x", trst, srst, low_output, low_direction);
 }
 
-
 static void turtle_reset(int trst, int srst)
 {
        trst = trst;
@@ -1180,13 +1405,12 @@ static void turtle_reset(int trst, int srst)
        }
 
        /* command "set data bits low byte" */
-       BUFFER_ADD = 0x80;
-       BUFFER_ADD = low_output;
-       BUFFER_ADD = low_direction;
+       buffer_write(0x80);
+       buffer_write(low_output);
+       buffer_write(low_direction);
        LOG_DEBUG("srst: %i, low_output: 0x%2.2x, low_direction: 0x%2.2x", srst, low_output, low_direction);
 }
 
-
 static void comstick_reset(int trst, int srst)
 {
        if (trst == 1)
@@ -1208,14 +1432,13 @@ static void comstick_reset(int trst, int srst)
        }
 
        /* command "set data bits high byte" */
-       BUFFER_ADD = 0x82;
-       BUFFER_ADD = high_output;
-       BUFFER_ADD = high_direction;
+       buffer_write(0x82);
+       buffer_write(high_output);
+       buffer_write(high_direction);
        LOG_DEBUG("trst: %i, srst: %i, high_output: 0x%2.2x, high_direction: 0x%2.2x", trst, srst, high_output,
                        high_direction);
 }
 
-
 static void stm32stick_reset(int trst, int srst)
 {
        if (trst == 1)
@@ -1237,20 +1460,18 @@ static void stm32stick_reset(int trst, int srst)
        }
 
        /* command "set data bits low byte" */
-       BUFFER_ADD = 0x80;
-       BUFFER_ADD = low_output;
-       BUFFER_ADD = low_direction;
+       buffer_write(0x80);
+       buffer_write(low_output);
+       buffer_write(low_direction);
 
        /* command "set data bits high byte" */
-       BUFFER_ADD = 0x82;
-       BUFFER_ADD = high_output;
-       BUFFER_ADD = high_direction;
+       buffer_write(0x82);
+       buffer_write(high_output);
+       buffer_write(high_direction);
        LOG_DEBUG("trst: %i, srst: %i, high_output: 0x%2.2x, high_direction: 0x%2.2x", trst, srst, high_output,
                        high_direction);
 }
 
-
-
 static void sheevaplug_reset(int trst, int srst)
 {
        if (trst == 1)
@@ -1264,44 +1485,31 @@ static void sheevaplug_reset(int trst, int srst)
                high_output |= nSRSTnOE;
 
        /* command "set data bits high byte" */
-       BUFFER_ADD = 0x82;
-       BUFFER_ADD = high_output;
-       BUFFER_ADD = high_direction;
+       buffer_write(0x82);
+       buffer_write(high_output);
+       buffer_write(high_direction);
        LOG_DEBUG("trst: %i, srst: %i, high_output: 0x%2.2x, high_direction: 0x%2.2x", trst, srst, high_output, high_direction);
 }
 
-static int ft2232_execute_end_state(jtag_command_t *cmd)
-{
-       int  retval;
-       retval = ERROR_OK;
-
-       DEBUG_JTAG_IO("end_state: %i", cmd->cmd.end_state->end_state);
-
-       if (cmd->cmd.end_state->end_state != TAP_INVALID)
-               ft2232_end_state(cmd->cmd.end_state->end_state);
-
-       return retval;
-}
-
-
-static int ft2232_execute_runtest(jtag_command_t *cmd)
+static int ft2232_execute_runtest(struct jtag_command *cmd)
 {
-       int  retval;
-       int             i;
+       int retval;
+       int i;
        int predicted_size = 0;
        retval = ERROR_OK;
 
-       DEBUG_JTAG_IO("runtest %i cycles, end in %i",
+       DEBUG_JTAG_IO("runtest %i cycles, end in %s",
                        cmd->cmd.runtest->num_cycles,
-                       cmd->cmd.runtest->end_state);
+                       tap_state_name(cmd->cmd.runtest->end_state));
+
        /* only send the maximum buffer size that FT2232C can handle */
        predicted_size = 0;
        if (tap_get_state() != TAP_IDLE)
                predicted_size += 3;
        predicted_size += 3 * CEIL(cmd->cmd.runtest->num_cycles, 7);
-       if ( (cmd->cmd.runtest->end_state != TAP_INVALID) && (cmd->cmd.runtest->end_state != TAP_IDLE) )
+       if (cmd->cmd.runtest->end_state != TAP_IDLE)
                predicted_size += 3;
-       if ( (cmd->cmd.runtest->end_state == TAP_INVALID) && (tap_get_end_state() != TAP_IDLE) )
+       if (tap_get_end_state() != TAP_IDLE)
                predicted_size += 3;
        if (ft2232_buffer_size + predicted_size + 1 > FT2232_BUFFER_SIZE)
        {
@@ -1312,60 +1520,48 @@ static int ft2232_execute_runtest(jtag_command_t *cmd)
        }
        if (tap_get_state() != TAP_IDLE)
        {
-               /* command "Clock Data to TMS/CS Pin (no Read)" */
-               BUFFER_ADD = 0x4b;
-               BUFFER_ADD = 0x6;    /* scan 7 bits */
-
-               /* TMS data bits */
-               BUFFER_ADD = tap_get_tms_path(tap_get_state(), TAP_IDLE);
-               tap_set_state(TAP_IDLE);
+               move_to_state(TAP_IDLE);
                require_send = 1;
        }
        i = cmd->cmd.runtest->num_cycles;
        while (i > 0)
        {
+               /* there are no state transitions in this code, so omit state tracking */
+
                /* command "Clock Data to TMS/CS Pin (no Read)" */
-               BUFFER_ADD = 0x4b;
+               buffer_write(0x4b);
 
                /* scan 7 bits */
-               BUFFER_ADD = (i > 7) ? 6 : (i - 1);
+               buffer_write((i > 7) ? 6 : (i - 1));
 
                /* TMS data bits */
-               BUFFER_ADD = 0x0;
-               tap_set_state(TAP_IDLE);
+               buffer_write(0x0);
+
                i -= (i > 7) ? 7 : i;
                /* LOG_DEBUG("added TMS scan (no read)"); */
        }
 
-       if (cmd->cmd.runtest->end_state != TAP_INVALID)
-               ft2232_end_state(cmd->cmd.runtest->end_state);
+       ft2232_end_state(cmd->cmd.runtest->end_state);
 
-       if ( tap_get_state() != tap_get_end_state() )
+       if (tap_get_state() != tap_get_end_state())
        {
-               /* command "Clock Data to TMS/CS Pin (no Read)" */
-               BUFFER_ADD = 0x4b;
-               /* scan 7 bit */
-               BUFFER_ADD = 0x6;
-               /* TMS data bits */
-               BUFFER_ADD = tap_get_tms_path( tap_get_state(), tap_get_end_state() );
-               tap_set_state( tap_get_end_state() );
-               /* LOG_DEBUG("added TMS scan (no read)"); */
+               move_to_state(tap_get_end_state());
        }
-       require_send = 1;
-#ifdef _DEBUG_JTAG_IO_
-       LOG_DEBUG( "runtest: %i, end in %s", cmd->cmd.runtest->num_cycles, tap_state_name( tap_get_end_state() ) );
-#endif
 
+       require_send = 1;
+       DEBUG_JTAG_IO("runtest: %i, end in %s",
+                       cmd->cmd.runtest->num_cycles,
+                       tap_state_name(tap_get_end_state()));
        return retval;
 }
 
-static int ft2232_execute_statemove(jtag_command_t *cmd)
+static int ft2232_execute_statemove(struct jtag_command *cmd)
 {
-       int  retval;
-       int predicted_size = 0;
-       retval = ERROR_OK;
+       int     predicted_size = 0;
+       int     retval = ERROR_OK;
 
-       DEBUG_JTAG_IO("statemove end in %i", cmd->cmd.statemove->end_state);
+       DEBUG_JTAG_IO("statemove end in %s",
+                       tap_state_name(cmd->cmd.statemove->end_state));
 
        /* only send the maximum buffer size that FT2232C can handle */
        predicted_size = 3;
@@ -1376,66 +1572,70 @@ static int ft2232_execute_statemove(jtag_command_t *cmd)
                require_send = 0;
                first_unsent = cmd;
        }
-       if (cmd->cmd.statemove->end_state != TAP_INVALID)
-               ft2232_end_state(cmd->cmd.statemove->end_state);
+       ft2232_end_state(cmd->cmd.statemove->end_state);
 
-       /* command "Clock Data to TMS/CS Pin (no Read)" */
-       BUFFER_ADD = 0x4b;
+       /* For TAP_RESET, ignore the current recorded state.  It's often
+        * wrong at server startup, and this transation is critical whenever
+        * it's requested.
+        */
+       if (tap_get_end_state() == TAP_RESET) {
+               clock_tms(0x4b,  0xff, 5, 0);
+               require_send = 1;
 
-       BUFFER_ADD = 0x6;       /* scan 7 bits */
+       /* shortest-path move to desired end state */
+       } else if (tap_get_state() != tap_get_end_state())
+       {
+               move_to_state(tap_get_end_state());
+               require_send = 1;
+       }
 
-                       /* TMS data bits */
-       BUFFER_ADD = tap_get_tms_path( tap_get_state(), tap_get_end_state() );
-       /* LOG_DEBUG("added TMS scan (no read)"); */
-       tap_set_state( tap_get_end_state() );
-       require_send = 1;
-#ifdef _DEBUG_JTAG_IO_
-       LOG_DEBUG( "statemove: %s", tap_state_name( tap_get_end_state() ) );
-#endif
-       
        return retval;
 }
 
-static int ft2232_execute_pathmove(jtag_command_t *cmd)
+static int ft2232_execute_pathmove(struct jtag_command *cmd)
 {
-       int  retval;
-       int predicted_size = 0;
-       retval = ERROR_OK;
+       int     predicted_size = 0;
+       int     retval = ERROR_OK;
+
+       tap_state_t*     path = cmd->cmd.pathmove->path;
+       int     num_states    = cmd->cmd.pathmove->num_states;
+
+       DEBUG_JTAG_IO("pathmove: %i states, current: %s  end: %s", num_states,
+                       tap_state_name(tap_get_state()),
+                       tap_state_name(path[num_states-1]));
 
-       DEBUG_JTAG_IO("pathmove: %i states, end in %i",
-               cmd->cmd.pathmove->num_states,
-               cmd->cmd.pathmove->path[cmd->cmd.pathmove->num_states - 1]);
        /* only send the maximum buffer size that FT2232C can handle */
-       predicted_size = 3 * CEIL(cmd->cmd.pathmove->num_states, 7);
+       predicted_size = 3 * CEIL(num_states, 7);
        if (ft2232_buffer_size + predicted_size + 1 > FT2232_BUFFER_SIZE)
        {
                if (ft2232_send_and_recv(first_unsent, cmd) != ERROR_OK)
-               retval = ERROR_JTAG_QUEUE_FAILED;
+                       retval = ERROR_JTAG_QUEUE_FAILED;
+
                require_send = 0;
                first_unsent = cmd;
        }
-       ft2232_add_pathmove(cmd->cmd.pathmove);
+
+       ft2232_add_pathmove(path, num_states);
        require_send = 1;
-#ifdef _DEBUG_JTAG_IO_
-       LOG_DEBUG( "pathmove: %i states, end in %s", cmd->cmd.pathmove->num_states,
-               tap_state_name(cmd->cmd.pathmove->path[cmd->cmd.pathmove->num_states - 1]) );
-#endif
+
        return retval;
 }
 
-static int ft2232_execute_scan(jtag_command_t *cmd)
+static int ft2232_execute_scan(struct jtag_command *cmd)
 {
-       int             retval;
-       u8*             buffer;
-       int             scan_size;                  /* size of IR or DR scan */
-       enum scan_type  type;
-       int             predicted_size = 0;
-       retval = ERROR_OK;
+       uint8_t* buffer;
+       int scan_size;                          /* size of IR or DR scan */
+       int predicted_size = 0;
+       int retval = ERROR_OK;
+
+       enum scan_type  type = jtag_scan_type(cmd->cmd.scan);
+
+       DEBUG_JTAG_IO("%s type:%d", cmd->cmd.scan->ir_scan ? "IRSCAN" : "DRSCAN", type);
 
        scan_size = jtag_build_buffer(cmd->cmd.scan, &buffer);
-       type = jtag_scan_type(cmd->cmd.scan);
+
        predicted_size = ft2232_predict_scan_out(scan_size, type);
-       if ( (predicted_size + 1) > FT2232_BUFFER_SIZE )
+       if ((predicted_size + 1) > FT2232_BUFFER_SIZE)
        {
                LOG_DEBUG("oversized ft2232 scan (predicted_size > FT2232_BUFFER_SIZE)");
                /* unsent commands before this */
@@ -1444,8 +1644,7 @@ static int ft2232_execute_scan(jtag_command_t *cmd)
                                retval = ERROR_JTAG_QUEUE_FAILED;
 
                /* current command */
-               if (cmd->cmd.scan->end_state != TAP_INVALID)
-                       ft2232_end_state(cmd->cmd.scan->end_state);
+               ft2232_end_state(cmd->cmd.scan->end_state);
                ft2232_large_scan(cmd->cmd.scan, type, buffer, scan_size);
                require_send = 0;
                first_unsent = cmd->next;
@@ -1465,24 +1664,22 @@ static int ft2232_execute_scan(jtag_command_t *cmd)
        }
        ft2232_expect_read += ft2232_predict_scan_in(scan_size, type);
        /* LOG_DEBUG("new read size: %i", ft2232_expect_read); */
-       if (cmd->cmd.scan->end_state != TAP_INVALID)
-               ft2232_end_state(cmd->cmd.scan->end_state);
+       ft2232_end_state(cmd->cmd.scan->end_state);
        ft2232_add_scan(cmd->cmd.scan->ir_scan, type, buffer, scan_size);
        require_send = 1;
        if (buffer)
                free(buffer);
-#ifdef _DEBUG_JTAG_IO_
-       LOG_DEBUG( "%s scan, %i bits, end in %s", (cmd->cmd.scan->ir_scan) ? "IR" : "DR", scan_size,
-                       tap_state_name( tap_get_end_state() ) );
-#endif
+       DEBUG_JTAG_IO("%s scan, %i bits, end in %s",
+                       (cmd->cmd.scan->ir_scan) ? "IR" : "DR", scan_size,
+                       tap_state_name(tap_get_end_state()));
        return retval;
 
 }
 
-static int ft2232_execute_reset(jtag_command_t *cmd)
+static int ft2232_execute_reset(struct jtag_command *cmd)
 {
-       int             retval;
-       int             predicted_size = 0;
+       int retval;
+       int predicted_size = 0;
        retval = ERROR_OK;
 
        DEBUG_JTAG_IO("reset trst: %i srst %i",
@@ -1498,40 +1695,39 @@ static int ft2232_execute_reset(jtag_command_t *cmd)
                first_unsent = cmd;
        }
 
-       if ( (cmd->cmd.reset->trst == 1) || ( cmd->cmd.reset->srst && (jtag_reset_config & RESET_SRST_PULLS_TRST) ) )
+       if ((cmd->cmd.reset->trst == 1) || (cmd->cmd.reset->srst && (jtag_get_reset_config() & RESET_SRST_PULLS_TRST)))
        {
                tap_set_state(TAP_RESET);
        }
+
        layout->reset(cmd->cmd.reset->trst, cmd->cmd.reset->srst);
        require_send = 1;
 
-#ifdef _DEBUG_JTAG_IO_
-       LOG_DEBUG("trst: %i, srst: %i", cmd->cmd.reset->trst, cmd->cmd.reset->srst);
-#endif
+       DEBUG_JTAG_IO("trst: %i, srst: %i",
+                       cmd->cmd.reset->trst, cmd->cmd.reset->srst);
        return retval;
 }
 
-static int ft2232_execute_sleep(jtag_command_t *cmd)
+static int ft2232_execute_sleep(struct jtag_command *cmd)
 {
-       int             retval;
+       int retval;
        retval = ERROR_OK;
 
-       DEBUG_JTAG_IO("sleep %i", cmd->cmd.sleep->us);
+       DEBUG_JTAG_IO("sleep %" PRIi32, cmd->cmd.sleep->us);
 
        if (ft2232_send_and_recv(first_unsent, cmd) != ERROR_OK)
                                retval = ERROR_JTAG_QUEUE_FAILED;
        first_unsent = cmd->next;
        jtag_sleep(cmd->cmd.sleep->us);
-#ifdef _DEBUG_JTAG_IO_
-                       LOG_DEBUG( "sleep %i usec while in %s", cmd->cmd.sleep->us, tap_state_name( tap_get_state() ) );
-#endif
-
+       DEBUG_JTAG_IO("sleep %" PRIi32 " usec while in %s",
+                       cmd->cmd.sleep->us,
+                       tap_state_name(tap_get_state()));
        return retval;
 }
 
-static int ft2232_execute_stableclocks(jtag_command_t *cmd)
+static int ft2232_execute_stableclocks(struct jtag_command *cmd)
 {
-       int             retval;
+       int retval;
        retval = ERROR_OK;
 
        /* this is only allowed while in a stable state.  A check for a stable
@@ -1539,41 +1735,39 @@ static int ft2232_execute_stableclocks(jtag_command_t *cmd)
         */
        if (ft2232_stableclocks(cmd->cmd.stableclocks->num_cycles, cmd) != ERROR_OK)
                retval = ERROR_JTAG_QUEUE_FAILED;
-#ifdef _DEBUG_JTAG_IO_
-       LOG_DEBUG( "clocks %i while in %s", cmd->cmd.stableclocks->num_cycles, tap_state_name( tap_get_state() ) );
-#endif
-
+       DEBUG_JTAG_IO("clocks %i while in %s",
+                       cmd->cmd.stableclocks->num_cycles,
+                       tap_state_name(tap_get_state()));
        return retval;
 }
 
-static int ft2232_execute_command(jtag_command_t *cmd)
+static int ft2232_execute_command(struct jtag_command *cmd)
 {
-       int             retval;
+       int retval;
        retval = ERROR_OK;
 
        switch (cmd->type)
        {
-               case JTAG_END_STATE: retval = ft2232_execute_end_state(cmd); break;
-               case JTAG_RESET:         retval = ft2232_execute_reset(cmd); break;
-               case JTAG_RUNTEST:   retval = ft2232_execute_runtest(cmd); break;
-               case JTAG_STATEMOVE: retval = ft2232_execute_statemove(cmd); break;
-               case JTAG_PATHMOVE:  retval = ft2232_execute_pathmove(cmd); break;
-               case JTAG_SCAN:          retval = ft2232_execute_scan(cmd); break;
-               case JTAG_SLEEP:         retval = ft2232_execute_sleep(cmd); break;
-               case JTAG_STABLECLOCKS: retval = ft2232_execute_stableclocks(cmd); break;
-               default:
-                       LOG_ERROR("BUG: unknown JTAG command type encountered");
-                       exit(-1);       
+       case JTAG_RESET:        retval = ft2232_execute_reset(cmd); break;
+       case JTAG_RUNTEST:      retval = ft2232_execute_runtest(cmd); break;
+       case JTAG_STATEMOVE: retval = ft2232_execute_statemove(cmd); break;
+       case JTAG_PATHMOVE:     retval = ft2232_execute_pathmove(cmd); break;
+       case JTAG_SCAN:         retval = ft2232_execute_scan(cmd); break;
+       case JTAG_SLEEP:        retval = ft2232_execute_sleep(cmd); break;
+       case JTAG_STABLECLOCKS: retval = ft2232_execute_stableclocks(cmd); break;
+       default:
+               LOG_ERROR("BUG: unknown JTAG command type encountered");
+               exit(-1);
        }
        return retval;
 }
 
-static int ft2232_execute_queue()
+static int ft2232_execute_queue(void)
 {
-       jtag_command_t* cmd = jtag_command_queue;   /* currently processed command */
-       int             retval;
+       struct jtag_command* cmd = jtag_command_queue;  /* currently processed command */
+       int retval;
 
-       first_unsent = cmd;         /* next command that has to be sent */
+       first_unsent = cmd;             /* next command that has to be sent */
        require_send = 0;
 
        /* return ERROR_OK, unless ft2232_send_and_recv reports a failed check
@@ -1609,20 +1803,22 @@ static int ft2232_execute_queue()
        return retval;
 }
 
-
 #if BUILD_FT2232_FTD2XX == 1
-static int ft2232_init_ftd2xx(u16 vid, u16 pid, int more, int* try_more)
+static int ft2232_init_ftd2xx(uint16_t vid, uint16_t pid, int more, int* try_more)
 {
-       FT_STATUS status;
-       DWORD     openex_flags  = 0;
-       char*     openex_string = NULL;
-       u8        latency_timer;
+       FT_STATUS       status;
+       DWORD           deviceID;
+       char            SerialNumber[16];
+       char            Description[64];
+       DWORD   openex_flags  = 0;
+       char*   openex_string = NULL;
+       uint8_t latency_timer;
 
        LOG_DEBUG("'ft2232' interface using FTD2XX with '%s' layout (%4.4x:%4.4x)", ft2232_layout, vid, pid);
 
 #if IS_WIN32 == 0
        /* Add non-standard Vid/Pid to the linux driver */
-       if ( ( status = FT_SetVIDPID(vid, pid) ) != FT_OK )
+       if ((status = FT_SetVIDPID(vid, pid)) != FT_OK)
        {
                LOG_WARNING("couldn't add %4.4x:%4.4x", vid, pid);
        }
@@ -1653,27 +1849,27 @@ static int ft2232_init_ftd2xx(u16 vid, u16 pid, int more, int* try_more)
        }
 
        status = FT_OpenEx(openex_string, openex_flags, &ftdih);
-       if( status != FT_OK ){
-               // under Win32, the FTD2XX driver appends an "A" to the end
-               // of the description, if we tried by the desc, then
-               // try by the alternate "A" description.
-               if( openex_string == ft2232_device_desc ){
-                       // Try the alternate method.
+       if (status != FT_OK) {
+               /* under Win32, the FTD2XX driver appends an "A" to the end
+                * of the description, if we tried by the desc, then
+                * try by the alternate "A" description. */
+               if (openex_string == ft2232_device_desc) {
+                       /* Try the alternate method. */
                        openex_string = ft2232_device_desc_A;
                        status = FT_OpenEx(openex_string, openex_flags, &ftdih);
-                       if( status == FT_OK ){
-                               // yea, the "alternate" method worked!
+                       if (status == FT_OK) {
+                               /* yea, the "alternate" method worked! */
                        } else {
-                               // drat, give the user a meaningfull message.
-                               // telling the use we tried *BOTH* methods.
+                               /* drat, give the user a meaningfull message.
+                                * telling the use we tried *BOTH* methods. */
                                LOG_WARNING("Unable to open FTDI Device tried: '%s' and '%s'\n",
                                                        ft2232_device_desc,
-                                                       ft2232_device_desc_A );
+                                                       ft2232_device_desc_A);
                        }
                }
        }
 
-       if ( status != FT_OK )
+       if (status != FT_OK)
        {
                DWORD num_devices;
 
@@ -1687,8 +1883,8 @@ static int ft2232_init_ftd2xx(u16 vid, u16 pid, int more, int* try_more)
                status = FT_ListDevices(&num_devices, NULL, FT_LIST_NUMBER_ONLY);
                if (status == FT_OK)
                {
-                       char** desc_array = malloc( sizeof(char*) * (num_devices + 1) );
-                       u32 i;
+                       char** desc_array = malloc(sizeof(char*) * (num_devices + 1));
+                       uint32_t i;
 
                        for (i = 0; i < num_devices; i++)
                                desc_array[i] = malloc(64);
@@ -1701,7 +1897,7 @@ static int ft2232_init_ftd2xx(u16 vid, u16 pid, int more, int* try_more)
                        {
                                LOG_ERROR("ListDevices: %lu\n", num_devices);
                                for (i = 0; i < num_devices; i++)
-                                       LOG_ERROR("%i: \"%s\"", i, desc_array[i]);
+                                       LOG_ERROR("%" PRIu32 ": \"%s\"", i, desc_array[i]);
                        }
 
                        for (i = 0; i < num_devices; i++)
@@ -1716,13 +1912,13 @@ static int ft2232_init_ftd2xx(u16 vid, u16 pid, int more, int* try_more)
                return ERROR_JTAG_INIT_FAILED;
        }
 
-       if ( ( status = FT_SetLatencyTimer(ftdih, ft2232_latency) ) != FT_OK )
+       if ((status = FT_SetLatencyTimer(ftdih, ft2232_latency)) != FT_OK)
        {
                LOG_ERROR("unable to set latency timer: %lu", status);
                return ERROR_JTAG_INIT_FAILED;
        }
 
-       if ( ( status = FT_GetLatencyTimer(ftdih, &latency_timer) ) != FT_OK )
+       if ((status = FT_GetLatencyTimer(ftdih, &latency_timer)) != FT_OK)
        {
                LOG_ERROR("unable to get latency timer: %lu", status);
                return ERROR_JTAG_INIT_FAILED;
@@ -1732,27 +1928,44 @@ static int ft2232_init_ftd2xx(u16 vid, u16 pid, int more, int* try_more)
                LOG_DEBUG("current latency timer: %i", latency_timer);
        }
 
-       if ( ( status = FT_SetTimeouts(ftdih, 5000, 5000) ) != FT_OK )
+       if ((status = FT_SetTimeouts(ftdih, 5000, 5000)) != FT_OK)
        {
                LOG_ERROR("unable to set timeouts: %lu", status);
                return ERROR_JTAG_INIT_FAILED;
        }
 
-       if ( ( status = FT_SetBitMode(ftdih, 0x0b, 2) ) != FT_OK )
+       if ((status = FT_SetBitMode(ftdih, 0x0b, 2)) != FT_OK)
        {
                LOG_ERROR("unable to enable bit i/o mode: %lu", status);
                return ERROR_JTAG_INIT_FAILED;
        }
 
-       return ERROR_OK;
-}
-
-
-static int ft2232_purge_ftd2xx(void)
+       if ((status = FT_GetDeviceInfo(ftdih, &ftdi_device, &deviceID, SerialNumber, Description, NULL)) != FT_OK)
+       {
+               LOG_ERROR("unable to get FT_GetDeviceInfo: %lu", status);
+               return ERROR_JTAG_INIT_FAILED;
+       }
+       else
+       {
+               static const char* type_str[] =
+                       {"BM", "AM", "100AX", "UNKNOWN", "2232C", "232R", "2232H", "4232H"};
+               unsigned no_of_known_types = sizeof(type_str) / sizeof(type_str[0]) - 1;
+               unsigned type_index = ((unsigned)ftdi_device <= no_of_known_types)
+                       ? ftdi_device : FT_DEVICE_UNKNOWN;
+               LOG_INFO("device: %lu \"%s\"", ftdi_device, type_str[type_index]);
+               LOG_INFO("deviceID: %lu", deviceID);
+               LOG_INFO("SerialNumber: %s", SerialNumber);
+               LOG_INFO("Description: %s", Description);
+       }
+
+       return ERROR_OK;
+}
+
+static int ft2232_purge_ftd2xx(void)
 {
        FT_STATUS status;
 
-       if ( ( status = FT_Purge(ftdih, FT_PURGE_RX | FT_PURGE_TX) ) != FT_OK )
+       if ((status = FT_Purge(ftdih, FT_PURGE_RX | FT_PURGE_TX)) != FT_OK)
        {
                LOG_ERROR("error purging ftd2xx device: %lu", status);
                return ERROR_JTAG_INIT_FAILED;
@@ -1761,13 +1974,12 @@ static int ft2232_purge_ftd2xx(void)
        return ERROR_OK;
 }
 
-
 #endif /* BUILD_FT2232_FTD2XX == 1 */
 
 #if BUILD_FT2232_LIBFTDI == 1
-static int ft2232_init_libftdi(u16 vid, u16 pid, int more, int* try_more)
+static int ft2232_init_libftdi(uint16_t vid, uint16_t pid, int more, int* try_more)
 {
-       u8 latency_timer;
+       uint8_t latency_timer;
 
        LOG_DEBUG("'ft2232' interface using libftdi with '%s' layout (%4.4x:%4.4x)",
                        ft2232_layout, vid, pid);
@@ -1775,6 +1987,12 @@ static int ft2232_init_libftdi(u16 vid, u16 pid, int more, int* try_more)
        if (ftdi_init(&ftdic) < 0)
                return ERROR_JTAG_INIT_FAILED;
 
+       if (ftdi_set_interface(&ftdic, INTERFACE_A) < 0)
+       {
+               LOG_ERROR("unable to select FT2232 channel A: %s", ftdic.error_str);
+               return ERROR_JTAG_INIT_FAILED;
+       }
+
        /* context, vendor id, product id */
        if (ftdi_usb_open_desc(&ftdic, vid, pid, ft2232_device_desc,
                                ft2232_serial) < 0)
@@ -1788,12 +2006,7 @@ static int ft2232_init_libftdi(u16 vid, u16 pid, int more, int* try_more)
                return ERROR_JTAG_INIT_FAILED;
        }
 
-       if (ftdi_set_interface(&ftdic, INTERFACE_A) < 0)
-       {
-               LOG_ERROR("unable to select FT2232 channel A: %s", ftdic.error_str);
-               return ERROR_JTAG_INIT_FAILED;
-       }
-
+       /* There is already a reset in ftdi_usb_open_desc, this should be redundant */
        if (ftdi_usb_reset(&ftdic) < 0)
        {
                LOG_ERROR("unable to reset ftdi device");
@@ -1818,10 +2031,16 @@ static int ft2232_init_libftdi(u16 vid, u16 pid, int more, int* try_more)
 
        ftdi_set_bitmode(&ftdic, 0x0b, 2); /* ctx, JTAG I/O mask */
 
+       ftdi_device = ftdic.type;
+       static const char* type_str[] =
+               {"AM", "BM", "2232C", "R", "2232H", "4232H", "Unknown"};
+       unsigned no_of_known_types = sizeof(type_str) / sizeof(type_str[0]) - 1;
+       unsigned type_index = ((unsigned)ftdi_device < no_of_known_types)
+               ? ftdi_device : no_of_known_types;
+       LOG_DEBUG("FTDI chip type: %i \"%s\"", (int)ftdi_device, type_str[type_index]);
        return ERROR_OK;
 }
 
-
 static int ft2232_purge_libftdi(void)
 {
        if (ftdi_usb_purge_buffers(&ftdic) < 0)
@@ -1833,18 +2052,26 @@ static int ft2232_purge_libftdi(void)
        return ERROR_OK;
 }
 
-
 #endif /* BUILD_FT2232_LIBFTDI == 1 */
 
 static int ft2232_init(void)
 {
-       u8  buf[1];
+       uint8_t  buf[1];
        int retval;
-       u32 bytes_written;
-       ft2232_layout_t* cur_layout = ft2232_layouts;
+       uint32_t bytes_written;
+       const struct ft2232_layout* cur_layout = ft2232_layouts;
        int i;
 
-       if ( (ft2232_layout == NULL) || (ft2232_layout[0] == 0) )
+       if (tap_get_tms_path_len(TAP_IRPAUSE,TAP_IRPAUSE) == 7)
+       {
+               LOG_DEBUG("ft2232 interface using 7 step jtag state transitions");
+       }
+       else
+       {
+               LOG_DEBUG("ft2232 interface using shortest path jtag state transitions");
+
+       }
+       if ((ft2232_layout == NULL) || (ft2232_layout[0] == 0))
        {
                ft2232_layout = "usbjtag";
                LOG_WARNING("No ft2232 layout specified, using default 'usbjtag'");
@@ -1899,10 +2126,24 @@ static int ft2232_init(void)
        if (layout->init() != ERROR_OK)
                return ERROR_JTAG_INIT_FAILED;
 
-       ft2232_speed(jtag_speed);
+       if (ft2232_device_is_highspeed())
+       {
+#ifndef BUILD_FT2232_HIGHSPEED
+ #if BUILD_FT2232_FTD2XX == 1
+               LOG_WARNING("High Speed device found - You need a newer FTD2XX driver (version 2.04.16 or later)");
+ #elif BUILD_FT2232_LIBFTDI == 1
+               LOG_WARNING("High Speed device found - You need a newer libftdi version (0.16 or later)");
+ #endif
+#endif
+               /* make sure the legacy mode is disabled */
+               if (ft2232h_ft4232h_clk_divide_by_5(false) != ERROR_OK)
+                       return ERROR_JTAG_INIT_FAILED;
+       }
+
+       ft2232_speed(jtag_get_speed());
 
        buf[0] = 0x85; /* Disconnect TDI/DO to TDO/DI for Loopback */
-       if ( ( ( retval = ft2232_write(buf, 1, &bytes_written) ) != ERROR_OK ) || (bytes_written != 1) )
+       if (((retval = ft2232_write(buf, 1, &bytes_written)) != ERROR_OK) || (bytes_written != 1))
        {
                LOG_ERROR("couldn't write to FT2232 to disable loopback");
                return ERROR_JTAG_INIT_FAILED;
@@ -1917,11 +2158,10 @@ static int ft2232_init(void)
        return ERROR_OK;
 }
 
-
 static int usbjtag_init(void)
 {
-       u8  buf[3];
-       u32 bytes_written;
+       uint8_t  buf[3];
+       uint32_t bytes_written;
 
        low_output    = 0x08;
        low_direction = 0x0b;
@@ -1949,12 +2189,22 @@ static int usbjtag_init(void)
                low_output    = 0x88;
                low_direction = 0x8b;
        }
+       else if (strcmp(ft2232_layout, "luminary_icdi") == 0)
+       {
+               nTRST = 0x0;
+               nTRSTnOE = 0x00;
+               nSRST = 0x20;
+               nSRSTnOE = 0x20;
+               low_output    = 0x88;
+               low_direction = 0xcb;
+       }
        else
        {
                LOG_ERROR("BUG: usbjtag_init called for unknown layout '%s'", ft2232_layout);
                return ERROR_JTAG_INIT_FAILED;
        }
 
+       enum reset_types jtag_reset_config = jtag_get_reset_config();
        if (jtag_reset_config & RESET_TRST_OPEN_DRAIN)
        {
                low_direction &= ~nTRSTnOE; /* nTRST input */
@@ -1979,11 +2229,11 @@ static int usbjtag_init(void)
 
        /* initialize low byte for jtag */
        buf[0] = 0x80;          /* command "set data bits low byte" */
-       buf[1] = low_output;    /* value (TMS=1,TCK=0, TDI=0, xRST high) */
-       buf[2] = low_direction; /* dir (output=1), TCK/TDI/TMS=out, TDO=in */
+       buf[1] = low_output;    /* value (TMS = 1,TCK = 0, TDI = 0, xRST high) */
+       buf[2] = low_direction; /* dir (output = 1), TCK/TDI/TMS = out, TDO = in */
        LOG_DEBUG("%2.2x %2.2x %2.2x", buf[0], buf[1], buf[2]);
 
-       if ( ( ( ft2232_write(buf, 3, &bytes_written) ) != ERROR_OK ) || (bytes_written != 3) )
+       if (((ft2232_write(buf, 3, &bytes_written)) != ERROR_OK) || (bytes_written != 3))
        {
                LOG_ERROR("couldn't initialize FT2232 with 'USBJTAG' layout");
                return ERROR_JTAG_INIT_FAILED;
@@ -1992,22 +2242,21 @@ static int usbjtag_init(void)
        return ERROR_OK;
 }
 
-
 static int axm0432_jtag_init(void)
 {
-       u8  buf[3];
-       u32 bytes_written;
+       uint8_t  buf[3];
+       uint32_t bytes_written;
 
        low_output    = 0x08;
        low_direction = 0x2b;
 
        /* initialize low byte for jtag */
        buf[0] = 0x80;          /* command "set data bits low byte" */
-       buf[1] = low_output;    /* value (TMS=1,TCK=0, TDI=0, nOE=0) */
-       buf[2] = low_direction; /* dir (output=1), TCK/TDI/TMS=out, TDO=in, nOE=out */
+       buf[1] = low_output;    /* value (TMS = 1,TCK = 0, TDI = 0, nOE = 0) */
+       buf[2] = low_direction; /* dir (output = 1), TCK/TDI/TMS = out, TDO = in, nOE = out */
        LOG_DEBUG("%2.2x %2.2x %2.2x", buf[0], buf[1], buf[2]);
 
-       if ( ( ( ft2232_write(buf, 3, &bytes_written) ) != ERROR_OK ) || (bytes_written != 3) )
+       if (((ft2232_write(buf, 3, &bytes_written)) != ERROR_OK) || (bytes_written != 3))
        {
                LOG_ERROR("couldn't initialize FT2232 with 'JTAGkey' layout");
                return ERROR_JTAG_INIT_FAILED;
@@ -2029,6 +2278,7 @@ static int axm0432_jtag_init(void)
        high_output    = 0x0;
        high_direction = 0x0c;
 
+       enum reset_types jtag_reset_config = jtag_get_reset_config();
        if (jtag_reset_config & RESET_TRST_OPEN_DRAIN)
        {
                LOG_ERROR("can't set nTRSTOE to push-pull on the Dicarlo jtag");
@@ -2053,7 +2303,7 @@ static int axm0432_jtag_init(void)
        buf[2] = high_direction;    /* all outputs (xRST and xRSTnOE) */
        LOG_DEBUG("%2.2x %2.2x %2.2x", buf[0], buf[1], buf[2]);
 
-       if ( ( ( ft2232_write(buf, 3, &bytes_written) ) != ERROR_OK ) || (bytes_written != 3) )
+       if (((ft2232_write(buf, 3, &bytes_written)) != ERROR_OK) || (bytes_written != 3))
        {
                LOG_ERROR("couldn't initialize FT2232 with 'Dicarlo' layout");
                return ERROR_JTAG_INIT_FAILED;
@@ -2062,22 +2312,21 @@ static int axm0432_jtag_init(void)
        return ERROR_OK;
 }
 
-
 static int jtagkey_init(void)
 {
-       u8  buf[3];
-       u32 bytes_written;
+       uint8_t  buf[3];
+       uint32_t bytes_written;
 
        low_output    = 0x08;
        low_direction = 0x1b;
 
        /* initialize low byte for jtag */
        buf[0] = 0x80;          /* command "set data bits low byte" */
-       buf[1] = low_output;    /* value (TMS=1,TCK=0, TDI=0, nOE=0) */
-       buf[2] = low_direction; /* dir (output=1), TCK/TDI/TMS=out, TDO=in, nOE=out */
+       buf[1] = low_output;    /* value (TMS = 1,TCK = 0, TDI = 0, nOE = 0) */
+       buf[2] = low_direction; /* dir (output = 1), TCK/TDI/TMS = out, TDO = in, nOE = out */
        LOG_DEBUG("%2.2x %2.2x %2.2x", buf[0], buf[1], buf[2]);
 
-       if ( ( ( ft2232_write(buf, 3, &bytes_written) ) != ERROR_OK ) || (bytes_written != 3) )
+       if (((ft2232_write(buf, 3, &bytes_written)) != ERROR_OK) || (bytes_written != 3))
        {
                LOG_ERROR("couldn't initialize FT2232 with 'JTAGkey' layout");
                return ERROR_JTAG_INIT_FAILED;
@@ -2090,8 +2339,8 @@ static int jtagkey_init(void)
                nSRST    = 0x02;
                nSRSTnOE = 0x08;
        }
-       else if ( (strcmp(layout->name, "jtagkey_prototype_v1") == 0)
-                        || (strcmp(layout->name, "oocdlink") == 0) )
+       else if ((strcmp(layout->name, "jtagkey_prototype_v1") == 0)
+                        || (strcmp(layout->name, "oocdlink") == 0))
        {
                nTRST    = 0x02;
                nTRSTnOE = 0x1;
@@ -2107,6 +2356,7 @@ static int jtagkey_init(void)
        high_output    = 0x0;
        high_direction = 0x0f;
 
+       enum reset_types jtag_reset_config = jtag_get_reset_config();
        if (jtag_reset_config & RESET_TRST_OPEN_DRAIN)
        {
                high_output |= nTRSTnOE;
@@ -2135,7 +2385,7 @@ static int jtagkey_init(void)
        buf[2] = high_direction;    /* all outputs (xRST and xRSTnOE) */
        LOG_DEBUG("%2.2x %2.2x %2.2x", buf[0], buf[1], buf[2]);
 
-       if ( ( ( ft2232_write(buf, 3, &bytes_written) ) != ERROR_OK ) || (bytes_written != 3) )
+       if (((ft2232_write(buf, 3, &bytes_written)) != ERROR_OK) || (bytes_written != 3))
        {
                LOG_ERROR("couldn't initialize FT2232 with 'JTAGkey' layout");
                return ERROR_JTAG_INIT_FAILED;
@@ -2144,24 +2394,23 @@ static int jtagkey_init(void)
        return ERROR_OK;
 }
 
-
 static int olimex_jtag_init(void)
 {
-       u8  buf[3];
-       u32 bytes_written;
+       uint8_t  buf[3];
+       uint32_t bytes_written;
 
        low_output    = 0x08;
        low_direction = 0x1b;
 
        /* initialize low byte for jtag */
        buf[0] = 0x80;          /* command "set data bits low byte" */
-       buf[1] = low_output;    /* value (TMS=1,TCK=0, TDI=0, nOE=0) */
-       buf[2] = low_direction; /* dir (output=1), TCK/TDI/TMS=out, TDO=in, nOE=out */
+       buf[1] = low_output;    /* value (TMS = 1,TCK = 0, TDI = 0, nOE = 0) */
+       buf[2] = low_direction; /* dir (output = 1), TCK/TDI/TMS = out, TDO = in, nOE = out */
        LOG_DEBUG("%2.2x %2.2x %2.2x", buf[0], buf[1], buf[2]);
 
-       if ( ( ( ft2232_write(buf, 3, &bytes_written) ) != ERROR_OK ) || (bytes_written != 3) )
+       if (((ft2232_write(buf, 3, &bytes_written)) != ERROR_OK) || (bytes_written != 3))
        {
-               LOG_ERROR("couldn't initialize FT2232 with 'JTAGkey' layout");
+               LOG_ERROR("couldn't initialize FT2232 with 'Olimex' layout");
                return ERROR_JTAG_INIT_FAILED;
        }
 
@@ -2173,6 +2422,7 @@ static int olimex_jtag_init(void)
        high_output    = 0x0;
        high_direction = 0x0f;
 
+       enum reset_types jtag_reset_config = jtag_get_reset_config();
        if (jtag_reset_config & RESET_TRST_OPEN_DRAIN)
        {
                high_output |= nTRSTnOE;
@@ -2202,31 +2452,30 @@ static int olimex_jtag_init(void)
        buf[2] = high_direction;    /* all outputs (xRST and xRSTnOE) */
        LOG_DEBUG("%2.2x %2.2x %2.2x", buf[0], buf[1], buf[2]);
 
-       if ( ( ( ft2232_write(buf, 3, &bytes_written) ) != ERROR_OK ) || (bytes_written != 3) )
+       if ((ft2232_write(buf, 3, &bytes_written) != ERROR_OK) || (bytes_written != 3))
        {
-               LOG_ERROR("couldn't initialize FT2232 with 'JTAGkey' layout");
+               LOG_ERROR("couldn't initialize FT2232 with 'Olimex' layout");
                return ERROR_JTAG_INIT_FAILED;
        }
 
        return ERROR_OK;
 }
 
-
 static int flyswatter_init(void)
 {
-       u8  buf[3];
-       u32 bytes_written;
+       uint8_t  buf[3];
+       uint32_t bytes_written;
 
        low_output    = 0x18;
        low_direction = 0xfb;
 
        /* initialize low byte for jtag */
        buf[0] = 0x80;          /* command "set data bits low byte" */
-       buf[1] = low_output;    /* value (TMS=1,TCK=0, TDI=0, nOE=0) */
-       buf[2] = low_direction; /* dir (output=1), TCK/TDI/TMS=out, TDO=in, nOE[12]=out, n[ST]srst=out */
+       buf[1] = low_output;    /* value (TMS = 1,TCK = 0, TDI = 0, nOE = 0) */
+       buf[2] = low_direction; /* dir (output = 1), TCK/TDI/TMS = out, TDO = in, nOE[12]=out, n[ST]srst = out */
        LOG_DEBUG("%2.2x %2.2x %2.2x", buf[0], buf[1], buf[2]);
 
-       if ( ( ( ft2232_write(buf, 3, &bytes_written) ) != ERROR_OK ) || (bytes_written != 3) )
+       if (((ft2232_write(buf, 3, &bytes_written)) != ERROR_OK) || (bytes_written != 3))
        {
                LOG_ERROR("couldn't initialize FT2232 with 'flyswatter' layout");
                return ERROR_JTAG_INIT_FAILED;
@@ -2249,7 +2498,7 @@ static int flyswatter_init(void)
        buf[2] = high_direction;    /* all outputs (xRST and xRSTnOE) */
        LOG_DEBUG("%2.2x %2.2x %2.2x", buf[0], buf[1], buf[2]);
 
-       if ( ( ( ft2232_write(buf, 3, &bytes_written) ) != ERROR_OK ) || (bytes_written != 3) )
+       if (((ft2232_write(buf, 3, &bytes_written)) != ERROR_OK) || (bytes_written != 3))
        {
                LOG_ERROR("couldn't initialize FT2232 with 'flyswatter' layout");
                return ERROR_JTAG_INIT_FAILED;
@@ -2258,22 +2507,21 @@ static int flyswatter_init(void)
        return ERROR_OK;
 }
 
-
 static int turtle_init(void)
 {
-       u8  buf[3];
-       u32 bytes_written;
+       uint8_t  buf[3];
+       uint32_t bytes_written;
 
        low_output    = 0x08;
        low_direction = 0x5b;
 
        /* initialize low byte for jtag */
        buf[0] = 0x80;          /* command "set data bits low byte" */
-       buf[1] = low_output;    /* value (TMS=1,TCK=0, TDI=0, nOE=0) */
-       buf[2] = low_direction; /* dir (output=1), TCK/TDI/TMS=out, TDO=in, nOE=out */
+       buf[1] = low_output;    /* value (TMS = 1,TCK = 0, TDI = 0, nOE = 0) */
+       buf[2] = low_direction; /* dir (output = 1), TCK/TDI/TMS = out, TDO = in, nOE = out */
        LOG_DEBUG("%2.2x %2.2x %2.2x", buf[0], buf[1], buf[2]);
 
-       if ( ( ( ft2232_write(buf, 3, &bytes_written) ) != ERROR_OK ) || (bytes_written != 3) )
+       if (((ft2232_write(buf, 3, &bytes_written)) != ERROR_OK) || (bytes_written != 3))
        {
                LOG_ERROR("couldn't initialize FT2232 with 'turtelizer2' layout");
                return ERROR_JTAG_INIT_FAILED;
@@ -2290,7 +2538,7 @@ static int turtle_init(void)
        buf[2] = high_direction;
        LOG_DEBUG("%2.2x %2.2x %2.2x", buf[0], buf[1], buf[2]);
 
-       if ( ( ( ft2232_write(buf, 3, &bytes_written) ) != ERROR_OK ) || (bytes_written != 3) )
+       if (((ft2232_write(buf, 3, &bytes_written)) != ERROR_OK) || (bytes_written != 3))
        {
                LOG_ERROR("couldn't initialize FT2232 with 'turtelizer2' layout");
                return ERROR_JTAG_INIT_FAILED;
@@ -2299,22 +2547,21 @@ static int turtle_init(void)
        return ERROR_OK;
 }
 
-
 static int comstick_init(void)
 {
-       u8  buf[3];
-       u32 bytes_written;
+       uint8_t  buf[3];
+       uint32_t bytes_written;
 
        low_output    = 0x08;
        low_direction = 0x0b;
 
        /* initialize low byte for jtag */
        buf[0] = 0x80;          /* command "set data bits low byte" */
-       buf[1] = low_output;    /* value (TMS=1,TCK=0, TDI=0, nOE=0) */
-       buf[2] = low_direction; /* dir (output=1), TCK/TDI/TMS=out, TDO=in, nOE=out */
+       buf[1] = low_output;    /* value (TMS = 1,TCK = 0, TDI = 0, nOE = 0) */
+       buf[2] = low_direction; /* dir (output = 1), TCK/TDI/TMS = out, TDO = in, nOE = out */
        LOG_DEBUG("%2.2x %2.2x %2.2x", buf[0], buf[1], buf[2]);
 
-       if ( ( ( ft2232_write(buf, 3, &bytes_written) ) != ERROR_OK ) || (bytes_written != 3) )
+       if (((ft2232_write(buf, 3, &bytes_written)) != ERROR_OK) || (bytes_written != 3))
        {
                LOG_ERROR("couldn't initialize FT2232 with 'comstick' layout");
                return ERROR_JTAG_INIT_FAILED;
@@ -2334,7 +2581,7 @@ static int comstick_init(void)
        buf[2] = high_direction;
        LOG_DEBUG("%2.2x %2.2x %2.2x", buf[0], buf[1], buf[2]);
 
-       if ( ( ( ft2232_write(buf, 3, &bytes_written) ) != ERROR_OK ) || (bytes_written != 3) )
+       if (((ft2232_write(buf, 3, &bytes_written)) != ERROR_OK) || (bytes_written != 3))
        {
                LOG_ERROR("couldn't initialize FT2232 with 'comstick' layout");
                return ERROR_JTAG_INIT_FAILED;
@@ -2343,22 +2590,21 @@ static int comstick_init(void)
        return ERROR_OK;
 }
 
-
 static int stm32stick_init(void)
 {
-       u8  buf[3];
-       u32 bytes_written;
+       uint8_t  buf[3];
+       uint32_t bytes_written;
 
        low_output    = 0x88;
        low_direction = 0x8b;
 
        /* initialize low byte for jtag */
        buf[0] = 0x80;          /* command "set data bits low byte" */
-       buf[1] = low_output;    /* value (TMS=1,TCK=0, TDI=0, nOE=0) */
-       buf[2] = low_direction; /* dir (output=1), TCK/TDI/TMS=out, TDO=in, nOE=out */
+       buf[1] = low_output;    /* value (TMS = 1,TCK = 0, TDI = 0, nOE = 0) */
+       buf[2] = low_direction; /* dir (output = 1), TCK/TDI/TMS = out, TDO = in, nOE = out */
        LOG_DEBUG("%2.2x %2.2x %2.2x", buf[0], buf[1], buf[2]);
 
-       if ( ( ( ft2232_write(buf, 3, &bytes_written) ) != ERROR_OK ) || (bytes_written != 3) )
+       if (((ft2232_write(buf, 3, &bytes_written)) != ERROR_OK) || (bytes_written != 3))
        {
                LOG_ERROR("couldn't initialize FT2232 with 'stm32stick' layout");
                return ERROR_JTAG_INIT_FAILED;
@@ -2378,7 +2624,7 @@ static int stm32stick_init(void)
        buf[2] = high_direction;
        LOG_DEBUG("%2.2x %2.2x %2.2x", buf[0], buf[1], buf[2]);
 
-       if ( ( ( ft2232_write(buf, 3, &bytes_written) ) != ERROR_OK ) || (bytes_written != 3) )
+       if (((ft2232_write(buf, 3, &bytes_written)) != ERROR_OK) || (bytes_written != 3))
        {
                LOG_ERROR("couldn't initialize FT2232 with 'stm32stick' layout");
                return ERROR_JTAG_INIT_FAILED;
@@ -2387,24 +2633,23 @@ static int stm32stick_init(void)
        return ERROR_OK;
 }
 
-
 static int sheevaplug_init(void)
 {
-       u8 buf[3];
-       u32 bytes_written;
+       uint8_t buf[3];
+       uint32_t bytes_written;
 
        low_output = 0x08;
        low_direction = 0x1b;
 
        /* initialize low byte for jtag */
        buf[0] = 0x80; /* command "set data bits low byte" */
-       buf[1] = low_output; /* value (TMS=1,TCK=0, TDI=0, nOE=0) */
-       buf[2] = low_direction; /* dir (output=1), TCK/TDI/TMS=out, TDO=in */
+       buf[1] = low_output; /* value (TMS = 1,TCK = 0, TDI = 0, nOE = 0) */
+       buf[2] = low_direction; /* dir (output = 1), TCK/TDI/TMS = out, TDO = in */
        LOG_DEBUG("%2.2x %2.2x %2.2x", buf[0], buf[1], buf[2]);
 
        if (((ft2232_write(buf, 3, &bytes_written)) != ERROR_OK) || (bytes_written != 3))
        {
-               LOG_ERROR("couldn't initialize FT2232 with 'sheevaplug' layout"); 
+               LOG_ERROR("couldn't initialize FT2232 with 'sheevaplug' layout");
                return ERROR_JTAG_INIT_FAILED;
        }
 
@@ -2432,7 +2677,50 @@ static int sheevaplug_init(void)
 
        if (((ft2232_write(buf, 3, &bytes_written)) != ERROR_OK) || (bytes_written != 3))
        {
-               LOG_ERROR("couldn't initialize FT2232 with 'sheevaplug' layout"); 
+               LOG_ERROR("couldn't initialize FT2232 with 'sheevaplug' layout");
+               return ERROR_JTAG_INIT_FAILED;
+       }
+
+       return ERROR_OK;
+}
+
+static int cortino_jtag_init(void)
+{
+       uint8_t  buf[3];
+       uint32_t bytes_written;
+
+       low_output    = 0x08;
+       low_direction = 0x1b;
+
+       /* initialize low byte for jtag */
+       buf[0] = 0x80;          /* command "set data bits low byte" */
+       buf[1] = low_output;    /* value (TMS = 1,TCK = 0, TDI = 0, nOE = 0) */
+       buf[2] = low_direction; /* dir (output = 1), TCK/TDI/TMS = out, TDO = in, nOE = out */
+       LOG_DEBUG("%2.2x %2.2x %2.2x", buf[0], buf[1], buf[2]);
+
+       if (((ft2232_write(buf, 3, &bytes_written)) != ERROR_OK) || (bytes_written != 3))
+       {
+               LOG_ERROR("couldn't initialize FT2232 with 'cortino' layout");
+               return ERROR_JTAG_INIT_FAILED;
+       }
+
+       nTRST    = 0x01;
+       nTRSTnOE = 0x00;    /* no output enable for nTRST */
+       nSRST    = 0x02;
+       nSRSTnOE = 0x00;    /* no output enable for nSRST */
+
+       high_output    = 0x03;
+       high_direction = 0x03;
+
+       /* initialize high port */
+       buf[0] = 0x82; /* command "set data bits high byte" */
+       buf[1] = high_output;
+       buf[2] = high_direction;
+       LOG_DEBUG("%2.2x %2.2x %2.2x", buf[0], buf[1], buf[2]);
+
+       if (((ft2232_write(buf, 3, &bytes_written)) != ERROR_OK) || (bytes_written != 3))
+       {
+               LOG_ERROR("couldn't initialize FT2232 with 'stm32stick' layout");
                return ERROR_JTAG_INIT_FAILED;
        }
 
@@ -2455,12 +2743,11 @@ static void olimex_jtag_blink(void)
                high_output |= 0x08;
        }
 
-       BUFFER_ADD = 0x82;
-       BUFFER_ADD = high_output;
-       BUFFER_ADD = high_direction;
+       buffer_write(0x82);
+       buffer_write(high_output);
+       buffer_write(high_direction);
 }
 
-
 static void flyswatter_jtag_blink(void)
 {
        /*
@@ -2468,12 +2755,11 @@ static void flyswatter_jtag_blink(void)
         */
        high_output ^= 0x0c;
 
-       BUFFER_ADD = 0x82;
-       BUFFER_ADD = high_output;
-       BUFFER_ADD = high_direction;
+       buffer_write(0x82);
+       buffer_write(high_output);
+       buffer_write(high_direction);
 }
 
-
 static void turtle_jtag_blink(void)
 {
        /*
@@ -2488,12 +2774,11 @@ static void turtle_jtag_blink(void)
                high_output = 0x08;
        }
 
-       BUFFER_ADD = 0x82;
-       BUFFER_ADD = high_output;
-       BUFFER_ADD = high_direction;
+       buffer_write(0x82);
+       buffer_write(high_output);
+       buffer_write(high_direction);
 }
 
-
 static int ft2232_quit(void)
 {
 #if BUILD_FT2232_FTD2XX == 1
@@ -2501,8 +2786,6 @@ static int ft2232_quit(void)
 
        status = FT_Close(ftdih);
 #elif BUILD_FT2232_LIBFTDI == 1
-       ftdi_disable_bitbang(&ftdic);
-
        ftdi_usb_close(&ftdic);
 
        ftdi_deinit(&ftdic);
@@ -2514,31 +2797,30 @@ static int ft2232_quit(void)
        return ERROR_OK;
 }
 
-
-static int ft2232_handle_device_desc_command(struct command_context_s* cmd_ctx, char* cmd, char** args, int argc)
+COMMAND_HANDLER(ft2232_handle_device_desc_command)
 {
        char *cp;
        char buf[200];
        if (argc == 1)
        {
                ft2232_device_desc = strdup(args[0]);
-               cp = strchr( ft2232_device_desc, 0 );
-               // under Win32, the FTD2XX driver appends an "A" to the end
-               // of the description, this examines the given desc
-               // and creates the 'missing' _A or non_A variable.
-               if( (cp[-1] == 'A') && (cp[-2]==' ') ){
-                       // it was, so make this the "A" version.
+               cp = strchr(ft2232_device_desc, 0);
+               /* under Win32, the FTD2XX driver appends an "A" to the end
+                * of the description, this examines the given desc
+                * and creates the 'missing' _A or non_A variable. */
+               if ((cp[-1] == 'A') && (cp[-2]==' ')) {
+                       /* it was, so make this the "A" version. */
                        ft2232_device_desc_A = ft2232_device_desc;
-                       // and *CREATE* the non-A version.
-                       strcpy( buf, ft2232_device_desc );
-                       cp = strchr( buf, 0 );
+                       /* and *CREATE* the non-A version. */
+                       strcpy(buf, ft2232_device_desc);
+                       cp = strchr(buf, 0);
                        cp[-2] = 0;
-                       ft2232_device_desc =  strdup( buf );
+                       ft2232_device_desc =  strdup(buf);
                } else {
-                       // <space>A not defined
-                       // so create it
-                       sprintf( buf, "%s A", ft2232_device_desc );
-                       ft2232_device_desc_A = strdup( buf );
+                       /* <space > A not defined
+                        * so create it */
+                       sprintf(buf, "%s A", ft2232_device_desc);
+                       ft2232_device_desc_A = strdup(buf);
                }
        }
        else
@@ -2549,8 +2831,7 @@ static int ft2232_handle_device_desc_command(struct command_context_s* cmd_ctx,
        return ERROR_OK;
 }
 
-
-static int ft2232_handle_serial_command(struct command_context_s* cmd_ctx, char* cmd, char** args, int argc)
+COMMAND_HANDLER(ft2232_handle_serial_command)
 {
        if (argc == 1)
        {
@@ -2564,8 +2845,7 @@ static int ft2232_handle_serial_command(struct command_context_s* cmd_ctx, char*
        return ERROR_OK;
 }
 
-
-static int ft2232_handle_layout_command(struct command_context_s* cmd_ctx, char* cmd, char** args, int argc)
+COMMAND_HANDLER(ft2232_handle_layout_command)
 {
        if (argc == 0)
                return ERROR_OK;
@@ -2576,28 +2856,28 @@ static int ft2232_handle_layout_command(struct command_context_s* cmd_ctx, char*
        return ERROR_OK;
 }
 
-
-static int ft2232_handle_vid_pid_command(struct command_context_s* cmd_ctx, char* cmd, char** args, int argc)
+COMMAND_HANDLER(ft2232_handle_vid_pid_command)
 {
-       int i;
-
        if (argc > MAX_USB_IDS * 2)
        {
                LOG_WARNING("ignoring extra IDs in ft2232_vid_pid "
                                        "(maximum is %d pairs)", MAX_USB_IDS);
                argc = MAX_USB_IDS * 2;
        }
-       if ( argc < 2 || (argc & 1) )
+       if (argc < 2 || (argc & 1))
        {
                LOG_WARNING("incomplete ft2232_vid_pid configuration directive");
                if (argc < 2)
-                       return ERROR_OK;
+                       return ERROR_COMMAND_SYNTAX_ERROR;
+               /* remove the incomplete trailing id */
+               argc -= 1;
        }
 
-       for (i = 0; i + 1 < argc; i += 2)
+       unsigned i;
+       for (i = 0; i < argc; i += 2)
        {
-               ft2232_vid[i >> 1] = strtol(args[i], NULL, 0);
-               ft2232_pid[i >> 1] = strtol(args[i + 1], NULL, 0);
+               COMMAND_PARSE_NUMBER(u16, args[i], ft2232_vid[i >> 1]);
+               COMMAND_PARSE_NUMBER(u16, args[i + 1], ft2232_pid[i >> 1]);
        }
 
        /*
@@ -2609,8 +2889,7 @@ static int ft2232_handle_vid_pid_command(struct command_context_s* cmd_ctx, char
        return ERROR_OK;
 }
 
-
-static int ft2232_handle_latency_command(struct command_context_s* cmd_ctx, char* cmd, char** args, int argc)
+COMMAND_HANDLER(ft2232_handle_latency_command)
 {
        if (argc == 1)
        {
@@ -2624,13 +2903,12 @@ static int ft2232_handle_latency_command(struct command_context_s* cmd_ctx, char
        return ERROR_OK;
 }
 
-
-static int ft2232_stableclocks(int num_cycles, jtag_command_t* cmd)
+static int ft2232_stableclocks(int num_cycles, struct jtag_command* cmd)
 {
        int retval = 0;
 
        /* 7 bits of either ones or zeros. */
-       u8  tms = (tap_get_state() == TAP_RESET ? 0x7F : 0x00);
+       uint8_t  tms = (tap_get_state() == TAP_RESET ? 0x7F : 0x00);
 
        while (num_cycles > 0)
        {
@@ -2648,14 +2926,16 @@ static int ft2232_stableclocks(int num_cycles, jtag_command_t* cmd)
                        first_unsent = cmd;
                }
 
+               /* there are no state transitions in this code, so omit state tracking */
+
                /* command "Clock Data to TMS/CS Pin (no Read)" */
-               BUFFER_ADD = 0x4b;
+               buffer_write(0x4b);
 
                /* scan 7 bit */
-               BUFFER_ADD = bitcount_per_command - 1;
+               buffer_write(bitcount_per_command - 1);
 
                /* TMS data bits are either all zeros or ones to stay in the current stable state */
-               BUFFER_ADD = tms;
+               buffer_write(tms);
 
                require_send = 1;
 
@@ -2667,7 +2947,7 @@ static int ft2232_stableclocks(int num_cycles, jtag_command_t* cmd)
 
 /* ---------------------------------------------------------------------
  * Support for IceBear JTAG adapter from Section5:
- *     http://section5.ch/icebear
+ *     http://section5.ch/icebear
  *
  * Author: Sten, debian@sansys-electronic.com
  */
@@ -2692,14 +2972,15 @@ static int ft2232_stableclocks(int num_cycles, jtag_command_t* cmd)
  * ADBUS7 -   GND
  */
 static int icebear_jtag_init(void) {
-       u8  buf[3];
-       u32 bytes_written;
+       uint8_t  buf[3];
+       uint32_t bytes_written;
 
        low_direction   = 0x0b; /* output: TCK TDI TMS; input: TDO */
        low_output      = 0x08; /* high: TMS; low: TCK TDI */
        nTRST           = 0x10;
        nSRST           = 0x20;
 
+       enum reset_types jtag_reset_config = jtag_get_reset_config();
        if ((jtag_reset_config & RESET_TRST_OPEN_DRAIN) != 0) {
                low_direction   &= ~nTRST;      /* nTRST high impedance */
        }
@@ -2717,7 +2998,7 @@ static int icebear_jtag_init(void) {
        buf[2] = low_direction;
        LOG_DEBUG("%2.2x %2.2x %2.2x", buf[0], buf[1], buf[2]);
 
-       if ( ( ( ft2232_write(buf, 3, &bytes_written) ) != ERROR_OK ) || (bytes_written != 3) ) {
+       if (((ft2232_write(buf, 3, &bytes_written)) != ERROR_OK) || (bytes_written != 3)) {
                LOG_ERROR("couldn't initialize FT2232 with 'IceBear' layout (low)");
                return ERROR_JTAG_INIT_FAILED;
        }
@@ -2732,7 +3013,7 @@ static int icebear_jtag_init(void) {
        buf[2] = high_direction;    /* all outputs (xRST and xRSTnOE) */
        LOG_DEBUG("%2.2x %2.2x %2.2x", buf[0], buf[1], buf[2]);
 
-       if ( ( ( ft2232_write(buf, 3, &bytes_written) ) != ERROR_OK ) || (bytes_written != 3) ) {
+       if (((ft2232_write(buf, 3, &bytes_written)) != ERROR_OK) || (bytes_written != 3)) {
                LOG_ERROR("couldn't initialize FT2232 with 'IceBear' layout (high)");
                return ERROR_JTAG_INIT_FAILED;
        }
@@ -2747,6 +3028,7 @@ static void icebear_jtag_reset(int trst, int srst) {
                low_output      &= ~nTRST;
        }
        else if (trst == 0) {
+               enum reset_types jtag_reset_config = jtag_get_reset_config();
                if ((jtag_reset_config & RESET_TRST_OPEN_DRAIN) != 0)
                        low_direction   &= ~nTRST;
                else
@@ -2761,8 +3043,962 @@ static void icebear_jtag_reset(int trst, int srst) {
        }
 
        /* command "set data bits low byte" */
-       BUFFER_ADD = 0x80;
-       BUFFER_ADD = low_output;
-       BUFFER_ADD = low_direction;
+       buffer_write(0x80);
+       buffer_write(low_output);
+       buffer_write(low_direction);
+
        LOG_DEBUG("trst: %i, srst: %i, low_output: 0x%2.2x, low_direction: 0x%2.2x", trst, srst, low_output, low_direction);
 }
+
+/* ---------------------------------------------------------------------
+ * Support for Signalyzer H2 and Signalyzer H4
+ * JTAG adapter from Xverve Technologies Inc.
+ * http://www.signalyzer.com or http://www.xverve.com
+ *
+ * Author: Oleg Seiljus, oleg@signalyzer.com
+ */
+static unsigned char signalyzer_h_side;
+static unsigned int signalyzer_h_adapter_type;
+
+static int signalyzer_h_ctrl_write(int address, unsigned short value);
+
+#if BUILD_FT2232_FTD2XX == 1
+static int signalyzer_h_ctrl_read(int address, unsigned short *value);
+#endif
+
+#define SIGNALYZER_COMMAND_ADDR                                        128
+#define SIGNALYZER_DATA_BUFFER_ADDR                            129
+
+#define SIGNALYZER_COMMAND_VERSION                             0x41
+#define SIGNALYZER_COMMAND_RESET                               0x42
+#define SIGNALYZER_COMMAND_POWERCONTROL_GET            0x50
+#define SIGNALYZER_COMMAND_POWERCONTROL_SET            0x51
+#define SIGNALYZER_COMMAND_PWM_SET                             0x52
+#define SIGNALYZER_COMMAND_LED_SET                             0x53
+#define SIGNALYZER_COMMAND_ADC                                 0x54
+#define SIGNALYZER_COMMAND_GPIO_STATE                  0x55
+#define SIGNALYZER_COMMAND_GPIO_MODE                   0x56
+#define SIGNALYZER_COMMAND_GPIO_PORT                   0x57
+#define SIGNALYZER_COMMAND_I2C                                 0x58
+
+#define SIGNALYZER_CHAN_A                                              1
+#define SIGNALYZER_CHAN_B                                              2
+/* LEDS use channel C */
+#define SIGNALYZER_CHAN_C                                              4
+
+#define SIGNALYZER_LED_GREEN                                   1
+#define SIGNALYZER_LED_RED                                             2
+
+#define SIGNALYZER_MODULE_TYPE_EM_LT16_A               0x0301
+#define SIGNALYZER_MODULE_TYPE_EM_ARM_JTAG             0x0302
+#define SIGNALYZER_MODULE_TYPE_EM_JTAG                 0x0303
+#define SIGNALYZER_MODULE_TYPE_EM_ARM_JTAG_P   0x0304
+#define SIGNALYZER_MODULE_TYPE_EM_JTAG_P               0x0305
+
+
+static int signalyzer_h_ctrl_write(int address, unsigned short value)
+{
+#if BUILD_FT2232_FTD2XX == 1
+       return FT_WriteEE(ftdih, address, value);
+#elif BUILD_FT2232_LIBFTDI == 1
+       return 0;
+#endif
+}
+
+#if BUILD_FT2232_FTD2XX == 1
+static int signalyzer_h_ctrl_read(int address, unsigned short *value)
+{
+       return FT_ReadEE(ftdih, address, value);
+}
+#endif
+
+static int signalyzer_h_led_set(unsigned char channel, unsigned char led,
+       int on_time_ms, int off_time_ms, unsigned char cycles)
+{
+       unsigned char on_time;
+       unsigned char off_time;
+
+       if (on_time_ms < 0xFFFF)
+               on_time = (unsigned char)(on_time_ms / 62);
+       else
+               on_time = 0xFF;
+
+       off_time = (unsigned char)(off_time_ms / 62);
+
+#if BUILD_FT2232_FTD2XX == 1
+       FT_STATUS status;
+
+       if ((status = signalyzer_h_ctrl_write(SIGNALYZER_DATA_BUFFER_ADDR,
+                       ((uint32_t)(channel << 8) | led))) != FT_OK)
+       {
+               LOG_ERROR("signalyzer_h_ctrl_write  returned: %lu", status);
+               return ERROR_JTAG_DEVICE_ERROR;
+       }
+
+       if ((status = signalyzer_h_ctrl_write(
+                       (SIGNALYZER_DATA_BUFFER_ADDR + 1),
+                       ((uint32_t)(on_time << 8) | off_time))) != FT_OK)
+       {
+               LOG_ERROR("signalyzer_h_ctrl_write  returned: %lu", status);
+               return ERROR_JTAG_DEVICE_ERROR;
+       }
+
+       if ((status = signalyzer_h_ctrl_write(
+                       (SIGNALYZER_DATA_BUFFER_ADDR + 2),
+                       ((uint32_t)cycles))) != FT_OK)
+       {
+               LOG_ERROR("signalyzer_h_ctrl_write  returned: %lu", status);
+               return ERROR_JTAG_DEVICE_ERROR;
+       }
+
+       if ((status = signalyzer_h_ctrl_write(SIGNALYZER_COMMAND_ADDR,
+                       SIGNALYZER_COMMAND_LED_SET)) != FT_OK)
+       {
+               LOG_ERROR("signalyzer_h_ctrl_write  returned: %lu", status);
+               return ERROR_JTAG_DEVICE_ERROR;
+       }
+
+       return ERROR_OK;
+#elif BUILD_FT2232_LIBFTDI == 1
+       int retval;
+
+       if ((retval = signalyzer_h_ctrl_write(SIGNALYZER_DATA_BUFFER_ADDR,
+                       ((uint32_t)(channel << 8) | led))) < 0)
+       {
+               LOG_ERROR("signalyzer_h_ctrl_write returned: %s",
+                               ftdi_get_error_string(&ftdic));
+               return ERROR_JTAG_DEVICE_ERROR;
+       }
+
+       if ((retval = signalyzer_h_ctrl_write(
+                       (SIGNALYZER_DATA_BUFFER_ADDR + 1),
+                       ((uint32_t)(on_time << 8) | off_time))) < 0)
+       {
+               LOG_ERROR("signalyzer_h_ctrl_write returned: %s",
+                               ftdi_get_error_string(&ftdic));
+               return ERROR_JTAG_DEVICE_ERROR;
+       }
+
+       if ((retval = signalyzer_h_ctrl_write(
+                       (SIGNALYZER_DATA_BUFFER_ADDR + 2),
+                       (uint32_t)cycles)) < 0)
+       {
+               LOG_ERROR("signalyzer_h_ctrl_write returned: %s",
+                               ftdi_get_error_string(&ftdic));
+               return ERROR_JTAG_DEVICE_ERROR;
+       }
+
+       if ((retval = signalyzer_h_ctrl_write(SIGNALYZER_COMMAND_ADDR,
+                       SIGNALYZER_COMMAND_LED_SET)) < 0)
+       {
+               LOG_ERROR("signalyzer_h_ctrl_write returned: %s",
+                               ftdi_get_error_string(&ftdic));
+               return ERROR_JTAG_DEVICE_ERROR;
+       }
+
+       return ERROR_OK;
+#endif
+}
+
+static int signalyzer_h_init(void)
+{
+#if BUILD_FT2232_FTD2XX == 1
+       FT_STATUS status;
+       int i;
+#endif
+
+       char *end_of_desc;
+
+       uint16_t read_buf[12] = { 0 };
+       uint8_t  buf[3];
+       uint32_t bytes_written;
+
+       /* turn on center green led */
+       signalyzer_h_led_set(SIGNALYZER_CHAN_C, SIGNALYZER_LED_GREEN,
+                       0xFFFF, 0x00, 0x00);
+
+       /* determine what channel config wants to open
+        * TODO: change me... current implementation is made to work
+        * with openocd description parsing.
+        */
+       end_of_desc = strrchr(ft2232_device_desc, 0x00);
+
+       if (end_of_desc)
+       {
+               signalyzer_h_side = *(end_of_desc - 1);
+               if (signalyzer_h_side == 'B')
+                       signalyzer_h_side = SIGNALYZER_CHAN_B;
+               else
+                       signalyzer_h_side = SIGNALYZER_CHAN_A;
+       }
+       else
+       {
+               LOG_ERROR("No Channel was specified");
+               return ERROR_FAIL;
+       }
+
+       signalyzer_h_led_set(signalyzer_h_side, SIGNALYZER_LED_GREEN,
+                       1000, 1000, 0xFF);
+
+#if BUILD_FT2232_FTD2XX == 1
+       /* read signalyzer versionining information */
+       if ((status = signalyzer_h_ctrl_write(SIGNALYZER_COMMAND_ADDR,
+                       SIGNALYZER_COMMAND_VERSION)) != FT_OK)
+       {
+               LOG_ERROR("signalyzer_h_ctrl_write returned: %lu", status);
+               return ERROR_JTAG_DEVICE_ERROR;
+       }
+
+       for (i = 0; i < 10; i++)
+       {
+               if ((status = signalyzer_h_ctrl_read(
+                       (SIGNALYZER_DATA_BUFFER_ADDR + i),
+                       &read_buf[i])) != FT_OK)
+               {
+                       LOG_ERROR("signalyzer_h_ctrl_read returned: %lu",
+                                       status);
+                       return ERROR_JTAG_DEVICE_ERROR;
+               }
+       }
+
+       LOG_INFO("Signalyzer: ID info: { %.4x %.4x %.4x %.4x %.4x %.4x %.4x }",
+                       read_buf[0], read_buf[1], read_buf[2], read_buf[3],
+                       read_buf[4], read_buf[5], read_buf[6]);
+
+       /* set gpio register */
+       if ((status = signalyzer_h_ctrl_write(SIGNALYZER_DATA_BUFFER_ADDR,
+                       (uint32_t)(signalyzer_h_side << 8))) != FT_OK)
+       {
+               LOG_ERROR("signalyzer_h_ctrl_write returned: %lu", status);
+               return ERROR_JTAG_DEVICE_ERROR;
+       }
+
+       if ((status = signalyzer_h_ctrl_write(SIGNALYZER_DATA_BUFFER_ADDR + 1,
+                       0x0404)) != FT_OK)
+       {
+               LOG_ERROR("signalyzer_h_ctrl_write returned: %lu", status);
+               return ERROR_JTAG_DEVICE_ERROR;
+       }
+
+       if ((status = signalyzer_h_ctrl_write(SIGNALYZER_COMMAND_ADDR,
+                       SIGNALYZER_COMMAND_GPIO_STATE)) != FT_OK)
+       {
+               LOG_ERROR("signalyzer_h_ctrl_write returned: %lu", status);
+               return ERROR_JTAG_DEVICE_ERROR;
+       }
+
+       /* read adapter type information */
+       if ((status = signalyzer_h_ctrl_write(SIGNALYZER_DATA_BUFFER_ADDR,
+                       ((uint32_t)(signalyzer_h_side << 8) | 0x01))) != FT_OK)
+       {
+               LOG_ERROR("signalyzer_h_ctrl_write returned: %lu", status);
+               return ERROR_JTAG_DEVICE_ERROR;
+       }
+
+       if ((status = signalyzer_h_ctrl_write(
+                       (SIGNALYZER_DATA_BUFFER_ADDR + 1), 0xA000)) != FT_OK)
+       {
+               LOG_ERROR("signalyzer_h_ctrl_write returned: %lu", status);
+               return ERROR_JTAG_DEVICE_ERROR;
+       }
+
+       if ((status = signalyzer_h_ctrl_write(
+                       (SIGNALYZER_DATA_BUFFER_ADDR + 2), 0x0008)) != FT_OK)
+       {
+               LOG_ERROR("signalyzer_h_ctrl_write returned: %lu", status);
+               return ERROR_JTAG_DEVICE_ERROR;
+       }
+
+       if ((status = signalyzer_h_ctrl_write(SIGNALYZER_COMMAND_ADDR,
+                       SIGNALYZER_COMMAND_I2C)) != FT_OK)
+       {
+               LOG_ERROR("signalyzer_h_ctrl_write returned: %lu", status);
+               return ERROR_JTAG_DEVICE_ERROR;
+       }
+
+       usleep(100000);
+
+       if ((status = signalyzer_h_ctrl_read(SIGNALYZER_COMMAND_ADDR,
+                       &read_buf[0])) != FT_OK)
+       {
+               LOG_ERROR("signalyzer_h_ctrl_read returned: %lu", status);
+               return ERROR_JTAG_DEVICE_ERROR;
+       }
+
+       if (read_buf[0] != 0x0498)
+               signalyzer_h_adapter_type = 0x0000;
+       else
+       {
+               for (i = 0; i < 4; i++)
+               {
+                       if ((status = signalyzer_h_ctrl_read(
+                                       (SIGNALYZER_DATA_BUFFER_ADDR + i),
+                                       &read_buf[i])) != FT_OK)
+                       {
+                               LOG_ERROR("signalyzer_h_ctrl_read returned: %lu",
+                                       status);
+                               return ERROR_JTAG_DEVICE_ERROR;
+                       }
+               }
+
+               signalyzer_h_adapter_type = read_buf[0];
+       }
+
+#elif BUILD_FT2232_LIBFTDI == 1
+       /* currently libftdi does not allow reading individual eeprom
+        * locations, therefore adapter type cannot be detected.
+        * override with most common type
+        */
+       signalyzer_h_adapter_type = SIGNALYZER_MODULE_TYPE_EM_ARM_JTAG;
+#endif
+
+       enum reset_types jtag_reset_config = jtag_get_reset_config();
+
+       /* ADAPTOR: EM_LT16_A */
+       if (signalyzer_h_adapter_type == SIGNALYZER_MODULE_TYPE_EM_LT16_A)
+       {
+               LOG_INFO("Signalyzer: EM-LT (16-channel level translator) "
+                       "detected. (HW: %2x).", (read_buf[1] >> 8));
+
+               nTRST    = 0x10;
+               nTRSTnOE = 0x10;
+               nSRST    = 0x20;
+               nSRSTnOE = 0x20;
+
+               low_output     = 0x08;
+               low_direction  = 0x1b;
+
+               high_output    = 0x0;
+               high_direction = 0x0;
+
+               if (jtag_reset_config & RESET_TRST_OPEN_DRAIN)
+               {
+                       low_direction &= ~nTRSTnOE; /* nTRST input */
+                       low_output    &= ~nTRST;    /* nTRST = 0 */
+               }
+               else
+               {
+                       low_direction |= nTRSTnOE;  /* nTRST output */
+                       low_output    |= nTRST;     /* nTRST = 1 */
+               }
+
+               if (jtag_reset_config & RESET_SRST_PUSH_PULL)
+               {
+                       low_direction |= nSRSTnOE;  /* nSRST output */
+                       low_output    |= nSRST;     /* nSRST = 1 */
+               }
+               else
+               {
+                       low_direction &= ~nSRSTnOE; /* nSRST input */
+                       low_output    &= ~nSRST;    /* nSRST = 0 */
+               }
+
+#if BUILD_FT2232_FTD2XX == 1
+               /* enable power to the module */
+               if ((status = signalyzer_h_ctrl_write(
+                               SIGNALYZER_DATA_BUFFER_ADDR,
+                               ((uint32_t)(signalyzer_h_side << 8) | 0x01)))
+                       != FT_OK)
+               {
+                       LOG_ERROR("signalyzer_h_ctrl_write returned: %lu",
+                               status);
+                       return ERROR_JTAG_DEVICE_ERROR;
+               }
+
+               if ((status = signalyzer_h_ctrl_write(SIGNALYZER_COMMAND_ADDR,
+                               SIGNALYZER_COMMAND_POWERCONTROL_SET)) != FT_OK)
+               {
+                       LOG_ERROR("signalyzer_h_ctrl_write returned: %lu",
+                                       status);
+                       return ERROR_JTAG_DEVICE_ERROR;
+               }
+
+               /* set gpio mode register */
+               if ((status = signalyzer_h_ctrl_write(
+                               SIGNALYZER_DATA_BUFFER_ADDR,
+                               (uint32_t)(signalyzer_h_side << 8))) != FT_OK)
+               {
+                       LOG_ERROR("signalyzer_h_ctrl_write returned: %lu",
+                                       status);
+                       return ERROR_JTAG_DEVICE_ERROR;
+               }
+
+               if ((status = signalyzer_h_ctrl_write(
+                               SIGNALYZER_DATA_BUFFER_ADDR + 1, 0x0000))
+                       != FT_OK)
+               {
+                       LOG_ERROR("signalyzer_h_ctrl_write returned: %lu",
+                                       status);
+                       return ERROR_JTAG_DEVICE_ERROR;
+               }
+
+               if ((status = signalyzer_h_ctrl_write(SIGNALYZER_COMMAND_ADDR,
+                               SIGNALYZER_COMMAND_GPIO_MODE)) != FT_OK)
+               {
+                       LOG_ERROR("signalyzer_h_ctrl_write returned: %lu",
+                                       status);
+                       return ERROR_JTAG_DEVICE_ERROR;
+               }
+
+               /* set gpio register */
+               if ((status = signalyzer_h_ctrl_write(
+                               SIGNALYZER_DATA_BUFFER_ADDR,
+                               (uint32_t)(signalyzer_h_side << 8))) != FT_OK)
+               {
+                       LOG_ERROR("signalyzer_h_ctrl_write returned: %lu",
+                                       status);
+                       return ERROR_JTAG_DEVICE_ERROR;
+               }
+
+               if ((status = signalyzer_h_ctrl_write(
+                               SIGNALYZER_DATA_BUFFER_ADDR + 1, 0x4040))
+                       != FT_OK)
+               {
+                       LOG_ERROR("signalyzer_h_ctrl_write returned: %lu",
+                                       status);
+                       return ERROR_JTAG_DEVICE_ERROR;
+               }
+
+               if ((status = signalyzer_h_ctrl_write(
+                               SIGNALYZER_COMMAND_ADDR,
+                               SIGNALYZER_COMMAND_GPIO_STATE)) != FT_OK)
+               {
+                       LOG_ERROR("signalyzer_h_ctrl_write returned: %lu",
+                                       status);
+                       return ERROR_JTAG_DEVICE_ERROR;
+               }
+#endif
+       }
+
+       /* ADAPTOR: EM_ARM_JTAG, EM_ARM_JTAG_P, EM_JTAG, EM_JTAG_P */
+       else if ((signalyzer_h_adapter_type == SIGNALYZER_MODULE_TYPE_EM_ARM_JTAG) ||
+                               (signalyzer_h_adapter_type == SIGNALYZER_MODULE_TYPE_EM_ARM_JTAG_P) ||
+                               (signalyzer_h_adapter_type == SIGNALYZER_MODULE_TYPE_EM_JTAG)  ||
+                               (signalyzer_h_adapter_type == SIGNALYZER_MODULE_TYPE_EM_JTAG_P))
+       {
+               if (signalyzer_h_adapter_type
+                               == SIGNALYZER_MODULE_TYPE_EM_ARM_JTAG)
+                       LOG_INFO("Signalyzer: EM-ARM-JTAG (ARM JTAG) "
+                               "detected. (HW: %2x).", (read_buf[1] >> 8));
+               else if (signalyzer_h_adapter_type
+                               == SIGNALYZER_MODULE_TYPE_EM_ARM_JTAG_P)
+                       LOG_INFO("Signalyzer: EM-ARM-JTAG_P "
+                               "(ARM JTAG with PSU) detected. (HW: %2x).",
+                               (read_buf[1] >> 8));
+               else if (signalyzer_h_adapter_type
+                               == SIGNALYZER_MODULE_TYPE_EM_JTAG)
+                       LOG_INFO("Signalyzer: EM-JTAG (Generic JTAG) "
+                               "detected. (HW: %2x).", (read_buf[1] >> 8));
+               else if (signalyzer_h_adapter_type
+                               == SIGNALYZER_MODULE_TYPE_EM_JTAG_P)
+                       LOG_INFO("Signalyzer: EM-JTAG-P "
+                               "(Generic JTAG with PSU) detected. (HW: %2x).",
+                               (read_buf[1] >> 8));
+
+               nTRST          = 0x02;
+               nTRSTnOE       = 0x04;
+               nSRST          = 0x08;
+               nSRSTnOE       = 0x10;
+
+               low_output     = 0x08;
+               low_direction  = 0x1b;
+
+               high_output    = 0x0;
+               high_direction = 0x1f;
+
+               if (jtag_reset_config & RESET_TRST_OPEN_DRAIN)
+               {
+                       high_output |= nTRSTnOE;
+                       high_output &= ~nTRST;
+               }
+               else
+               {
+                       high_output &= ~nTRSTnOE;
+                       high_output |= nTRST;
+               }
+
+               if (jtag_reset_config & RESET_SRST_PUSH_PULL)
+               {
+                       high_output &= ~nSRSTnOE;
+                       high_output |= nSRST;
+               }
+               else
+               {
+                       high_output |= nSRSTnOE;
+                       high_output &= ~nSRST;
+               }
+
+#if BUILD_FT2232_FTD2XX == 1
+               /* enable power to the module */
+               if ((status = signalyzer_h_ctrl_write(
+                               SIGNALYZER_DATA_BUFFER_ADDR,
+                               ((uint32_t)(signalyzer_h_side << 8) | 0x01)))
+                       != FT_OK)
+               {
+                       LOG_ERROR("signalyzer_h_ctrl_write returned: %lu",
+                                       status);
+                       return ERROR_JTAG_DEVICE_ERROR;
+               }
+
+               if ((status = signalyzer_h_ctrl_write(
+                               SIGNALYZER_COMMAND_ADDR,
+                               SIGNALYZER_COMMAND_POWERCONTROL_SET)) != FT_OK)
+               {
+                       LOG_ERROR("signalyzer_h_ctrl_write returned: %lu",
+                                       status);
+                       return ERROR_JTAG_DEVICE_ERROR;
+               }
+
+               /* set gpio mode register (IO_16 and IO_17 set as analog
+                * inputs, other is gpio)
+                */
+               if ((status = signalyzer_h_ctrl_write(
+                               SIGNALYZER_DATA_BUFFER_ADDR,
+                               (uint32_t)(signalyzer_h_side << 8))) != FT_OK)
+               {
+                       LOG_ERROR("signalyzer_h_ctrl_write returned: %lu",
+                                       status);
+                       return ERROR_JTAG_DEVICE_ERROR;
+               }
+
+               if ((status = signalyzer_h_ctrl_write(
+                               SIGNALYZER_DATA_BUFFER_ADDR + 1, 0x0060))
+                       != FT_OK)
+               {
+                       LOG_ERROR("signalyzer_h_ctrl_write returned: %lu",
+                                       status);
+                       return ERROR_JTAG_DEVICE_ERROR;
+               }
+
+               if ((status = signalyzer_h_ctrl_write(
+                               SIGNALYZER_COMMAND_ADDR,
+                               SIGNALYZER_COMMAND_GPIO_MODE)) != FT_OK)
+               {
+                       LOG_ERROR("signalyzer_h_ctrl_write returned: %lu",
+                                       status);
+                       return ERROR_JTAG_DEVICE_ERROR;
+               }
+
+               /* set gpio register (all inputs, for -P modules,
+                * PSU will be turned off)
+                */
+               if ((status = signalyzer_h_ctrl_write(
+                               SIGNALYZER_DATA_BUFFER_ADDR,
+                               (uint32_t)(signalyzer_h_side << 8))) != FT_OK)
+               {
+                       LOG_ERROR("signalyzer_h_ctrl_write returned: %lu",
+                                       status);
+                       return ERROR_JTAG_DEVICE_ERROR;
+               }
+
+               if ((status = signalyzer_h_ctrl_write(
+                               SIGNALYZER_DATA_BUFFER_ADDR + 1, 0x0000))
+                       != FT_OK)
+               {
+                       LOG_ERROR("signalyzer_h_ctrl_write returned: %lu",
+                                       status);
+                       return ERROR_JTAG_DEVICE_ERROR;
+               }
+
+               if ((status = signalyzer_h_ctrl_write(
+                               SIGNALYZER_COMMAND_ADDR,
+                               SIGNALYZER_COMMAND_GPIO_STATE)) != FT_OK)
+               {
+                       LOG_ERROR("signalyzer_h_ctrl_write returned: %lu",
+                                       status);
+                       return ERROR_JTAG_DEVICE_ERROR;
+               }
+#endif
+       }
+
+       else if (signalyzer_h_adapter_type == 0x0000)
+       {
+               LOG_INFO("Signalyzer: No external modules were detected.");
+
+               nTRST    = 0x10;
+               nTRSTnOE = 0x10;
+               nSRST    = 0x20;
+               nSRSTnOE = 0x20;
+
+               low_output     = 0x08;
+               low_direction  = 0x1b;
+
+               high_output    = 0x0;
+               high_direction = 0x0;
+
+               if (jtag_reset_config & RESET_TRST_OPEN_DRAIN)
+               {
+                       low_direction &= ~nTRSTnOE; /* nTRST input */
+                       low_output    &= ~nTRST;    /* nTRST = 0 */
+               }
+               else
+               {
+                       low_direction |= nTRSTnOE;  /* nTRST output */
+                       low_output    |= nTRST;     /* nTRST = 1 */
+               }
+
+               if (jtag_reset_config & RESET_SRST_PUSH_PULL)
+               {
+                       low_direction |= nSRSTnOE;  /* nSRST output */
+                       low_output    |= nSRST;     /* nSRST = 1 */
+               }
+               else
+               {
+                       low_direction &= ~nSRSTnOE; /* nSRST input */
+                       low_output    &= ~nSRST;    /* nSRST = 0 */
+               }
+       }
+       else
+       {
+               LOG_ERROR("Unknown module type is detected: %.4x",
+                               signalyzer_h_adapter_type);
+               return ERROR_JTAG_DEVICE_ERROR;
+       }
+
+       /* initialize low byte of controller for jtag operation */
+       buf[0] = 0x80;
+       buf[1] = low_output;
+       buf[2] = low_direction;
+
+       if (((ft2232_write(buf, 3, &bytes_written)) != ERROR_OK)
+                       || (bytes_written != 3))
+       {
+               LOG_ERROR("couldn't initialize Signalyzer-H layout");
+               return ERROR_JTAG_INIT_FAILED;
+       }
+
+#if BUILD_FT2232_FTD2XX == 1
+       if (ftdi_device == FT_DEVICE_2232H)
+       {
+               /* initialize high byte of controller for jtag operation */
+               buf[0] = 0x82;
+               buf[1] = high_output;
+               buf[2] = high_direction;
+
+               if ((ft2232_write(buf, 3, &bytes_written) != ERROR_OK)
+                               || (bytes_written != 3))
+               {
+                       LOG_ERROR("couldn't initialize Signalyzer-H layout");
+                       return ERROR_JTAG_INIT_FAILED;
+               }
+       }
+#elif BUILD_FT2232_LIBFTDI == 1
+       if (ftdi_device == TYPE_2232H)
+       {
+               /* initialize high byte of controller for jtag operation */
+               buf[0] = 0x82;
+               buf[1] = high_output;
+               buf[2] = high_direction;
+
+               if ((ft2232_write(buf, 3, &bytes_written) != ERROR_OK)
+                               || (bytes_written != 3))
+               {
+                       LOG_ERROR("couldn't initialize Signalyzer-H layout");
+                       return ERROR_JTAG_INIT_FAILED;
+               }
+       }
+#endif
+       return ERROR_OK;
+}
+
+static void signalyzer_h_reset(int trst, int srst)
+{
+       enum reset_types jtag_reset_config = jtag_get_reset_config();
+
+       /* ADAPTOR: EM_LT16_A */
+       if (signalyzer_h_adapter_type == SIGNALYZER_MODULE_TYPE_EM_LT16_A)
+       {
+               if (trst == 1)
+               {
+                       if (jtag_reset_config & RESET_TRST_OPEN_DRAIN)
+                               /* switch to output pin (output is low) */
+                               low_direction |= nTRSTnOE;
+                       else
+                               /* switch output low */
+                               low_output &= ~nTRST;
+               }
+               else if (trst == 0)
+               {
+                       if (jtag_reset_config & RESET_TRST_OPEN_DRAIN)
+                               /* switch to input pin (high-Z + internal
+                                * and external pullup) */
+                               low_direction &= ~nTRSTnOE;
+                       else
+                               /* switch output high */
+                               low_output |= nTRST;
+               }
+
+               if (srst == 1)
+               {
+                       if (jtag_reset_config & RESET_SRST_PUSH_PULL)
+                               /* switch output low */
+                               low_output &= ~nSRST;
+                       else
+                               /* switch to output pin (output is low) */
+                               low_direction |= nSRSTnOE;
+               }
+               else if (srst == 0)
+               {
+                       if (jtag_reset_config & RESET_SRST_PUSH_PULL)
+                               /* switch output high */
+                               low_output |= nSRST;
+                       else
+                               /* switch to input pin (high-Z) */
+                               low_direction &= ~nSRSTnOE;
+               }
+
+               /* command "set data bits low byte" */
+               buffer_write(0x80);
+               buffer_write(low_output);
+               buffer_write(low_direction);
+               LOG_DEBUG("trst: %i, srst: %i, low_output: 0x%2.2x, "
+                               "low_direction: 0x%2.2x",
+                               trst, srst, low_output, low_direction);
+       }
+       /* ADAPTOR: EM_ARM_JTAG,  EM_ARM_JTAG_P, EM_JTAG, EM_JTAG_P */
+       else if ((signalyzer_h_adapter_type == SIGNALYZER_MODULE_TYPE_EM_ARM_JTAG) ||
+                               (signalyzer_h_adapter_type == SIGNALYZER_MODULE_TYPE_EM_ARM_JTAG_P) ||
+                               (signalyzer_h_adapter_type == SIGNALYZER_MODULE_TYPE_EM_JTAG)  ||
+                               (signalyzer_h_adapter_type == SIGNALYZER_MODULE_TYPE_EM_JTAG_P))
+       {
+               if (trst == 1)
+               {
+                       if (jtag_reset_config & RESET_TRST_OPEN_DRAIN)
+                               high_output &= ~nTRSTnOE;
+                       else
+                               high_output &= ~nTRST;
+               }
+               else if (trst == 0)
+               {
+                       if (jtag_reset_config & RESET_TRST_OPEN_DRAIN)
+                               high_output |= nTRSTnOE;
+                       else
+                               high_output |= nTRST;
+               }
+
+               if (srst == 1)
+               {
+                       if (jtag_reset_config & RESET_SRST_PUSH_PULL)
+                               high_output &= ~nSRST;
+                       else
+                               high_output &= ~nSRSTnOE;
+               }
+               else if (srst == 0)
+               {
+                       if (jtag_reset_config & RESET_SRST_PUSH_PULL)
+                               high_output |= nSRST;
+                       else
+                               high_output |= nSRSTnOE;
+               }
+
+               /* command "set data bits high byte" */
+               buffer_write(0x82);
+               buffer_write(high_output);
+               buffer_write(high_direction);
+               LOG_INFO("trst: %i, srst: %i, high_output: 0x%2.2x, "
+                               "high_direction: 0x%2.2x",
+                               trst, srst, high_output, high_direction);
+       }
+       else if (signalyzer_h_adapter_type == 0x0000)
+       {
+               if (trst == 1)
+               {
+                       if (jtag_reset_config & RESET_TRST_OPEN_DRAIN)
+                               /* switch to output pin (output is low) */
+                               low_direction |= nTRSTnOE;
+                       else
+                               /* switch output low */
+                               low_output &= ~nTRST;
+               }
+               else if (trst == 0)
+               {
+                       if (jtag_reset_config & RESET_TRST_OPEN_DRAIN)
+                               /* switch to input pin (high-Z + internal
+                                * and external pullup) */
+                               low_direction &= ~nTRSTnOE;
+                       else
+                               /* switch output high */
+                               low_output |= nTRST;
+               }
+
+               if (srst == 1)
+               {
+                       if (jtag_reset_config & RESET_SRST_PUSH_PULL)
+                               /* switch output low */
+                               low_output &= ~nSRST;
+                       else
+                               /* switch to output pin (output is low) */
+                               low_direction |= nSRSTnOE;
+               }
+               else if (srst == 0)
+               {
+                       if (jtag_reset_config & RESET_SRST_PUSH_PULL)
+                               /* switch output high */
+                               low_output |= nSRST;
+                       else
+                               /* switch to input pin (high-Z) */
+                               low_direction &= ~nSRSTnOE;
+               }
+
+               /* command "set data bits low byte" */
+               buffer_write(0x80);
+               buffer_write(low_output);
+               buffer_write(low_direction);
+               LOG_DEBUG("trst: %i, srst: %i, low_output: 0x%2.2x, "
+                               "low_direction: 0x%2.2x",
+                               trst, srst, low_output, low_direction);
+       }
+}
+
+static void signalyzer_h_blink(void)
+{
+       signalyzer_h_led_set(signalyzer_h_side, SIGNALYZER_LED_RED, 100, 0, 1);
+}
+
+/********************************************************************
+ * Support for KT-LINK
+ * JTAG adapter from KRISTECH
+ * http://www.kristech.eu
+ *******************************************************************/
+static int ktlink_init(void)
+{
+       uint8_t  buf[3];
+       uint32_t bytes_written;
+       uint8_t  swd_en = 0x20; //0x20 SWD disable, 0x00 SWD enable (ADBUS5)
+
+       low_output    = 0x08 | swd_en; // value; TMS=1,TCK=0,TDI=0,SWD=swd_en
+       low_direction = 0x3B;          // out=1; TCK/TDI/TMS=out,TDO=in,SWD=out,RTCK=in,SRSTIN=in
+
+       // initialize low port
+       buf[0] = 0x80;          // command "set data bits low byte"
+       buf[1] = low_output;
+       buf[2] = low_direction;
+       LOG_DEBUG("%2.2x %2.2x %2.2x", buf[0], buf[1], buf[2]);
+
+       if (((ft2232_write(buf, 3, &bytes_written)) != ERROR_OK) || (bytes_written != 3))
+       {
+               LOG_ERROR("couldn't initialize FT2232 with 'ktlink' layout");
+               return ERROR_JTAG_INIT_FAILED;
+       }
+
+       nTRST    = 0x01;
+       nSRST    = 0x02;
+       nTRSTnOE = 0x04;
+       nSRSTnOE = 0x08;
+
+       high_output    = 0x80; // turn LED on
+       high_direction = 0xFF; // all outputs
+
+       enum reset_types jtag_reset_config = jtag_get_reset_config();
+
+       if (jtag_reset_config & RESET_TRST_OPEN_DRAIN) {
+               high_output |= nTRSTnOE;
+               high_output &= ~nTRST;
+       } else {
+               high_output &= ~nTRSTnOE;
+               high_output |= nTRST;
+       }
+
+       if (jtag_reset_config & RESET_SRST_PUSH_PULL) {
+               high_output &= ~nSRSTnOE;
+               high_output |= nSRST;
+       } else {
+               high_output |= nSRSTnOE;
+               high_output &= ~nSRST;
+       }
+
+       // initialize high port
+       buf[0] = 0x82;              // command "set data bits high byte"
+       buf[1] = high_output;       // value
+       buf[2] = high_direction;
+       LOG_DEBUG("%2.2x %2.2x %2.2x", buf[0], buf[1], buf[2]);
+
+       if (((ft2232_write(buf, 3, &bytes_written)) != ERROR_OK) || (bytes_written != 3))
+       {
+               LOG_ERROR("couldn't initialize FT2232 with 'ktlink' layout");
+               return ERROR_JTAG_INIT_FAILED;
+       }
+
+       return ERROR_OK;
+}
+
+static void ktlink_reset(int trst, int srst)
+{
+       enum reset_types jtag_reset_config = jtag_get_reset_config();
+
+       if (trst == 1) {
+               if (jtag_reset_config & RESET_TRST_OPEN_DRAIN)
+                       high_output &= ~nTRSTnOE;
+               else
+                       high_output &= ~nTRST;
+       } else if (trst == 0) {
+               if (jtag_reset_config & RESET_TRST_OPEN_DRAIN)
+                       high_output |= nTRSTnOE;
+               else
+                       high_output |= nTRST;
+       }
+
+       if (srst == 1) {
+               if (jtag_reset_config & RESET_SRST_PUSH_PULL)
+                       high_output &= ~nSRST;
+               else
+                       high_output &= ~nSRSTnOE;
+       } else if (srst == 0) {
+               if (jtag_reset_config & RESET_SRST_PUSH_PULL)
+                       high_output |= nSRST;
+               else
+                       high_output |= nSRSTnOE;
+       }
+
+       buffer_write(0x82); // command "set data bits high byte"
+       buffer_write(high_output);
+       buffer_write(high_direction);
+       LOG_DEBUG("trst: %i, srst: %i, high_output: 0x%2.2x, high_direction: 0x%2.2x", trst, srst, high_output,high_direction);
+}
+
+static void ktlink_blink(void)
+{
+       /* LED connected to ACBUS7 */
+       if (high_output & 0x80)
+               high_output &= 0x7F;
+       else
+               high_output |= 0x80;
+
+       buffer_write(0x82);  // command "set data bits high byte"
+       buffer_write(high_output);
+       buffer_write(high_direction);
+}
+
+static int ft2232_register_commands(struct command_context* cmd_ctx)
+{
+       register_command(cmd_ctx, NULL, "ft2232_device_desc",
+                       ft2232_handle_device_desc_command, COMMAND_CONFIG,
+                       "the USB device description of the FTDI FT2232 device");
+       register_command(cmd_ctx, NULL, "ft2232_serial",
+                       ft2232_handle_serial_command, COMMAND_CONFIG,
+                       "the serial number of the FTDI FT2232 device");
+       register_command(cmd_ctx, NULL, "ft2232_layout",
+                       ft2232_handle_layout_command, COMMAND_CONFIG,
+                       "the layout of the FT2232 GPIO signals used "
+                       "to control output-enables and reset signals");
+       register_command(cmd_ctx, NULL, "ft2232_vid_pid",
+                       ft2232_handle_vid_pid_command, COMMAND_CONFIG,
+                       "the vendor ID and product ID of the FTDI FT2232 device");
+       register_command(cmd_ctx, NULL, "ft2232_latency",
+                       ft2232_handle_latency_command, COMMAND_CONFIG,
+                       "set the FT2232 latency timer to a new value");
+
+       return ERROR_OK;
+}
+
+
+struct jtag_interface ft2232_interface = {
+               .name = "ft2232",
+               .register_commands = &ft2232_register_commands,
+               .init = &ft2232_init,
+               .quit = &ft2232_quit,
+               .speed = &ft2232_speed,
+               .speed_div = &ft2232_speed_div,
+               .khz = &ft2232_khz,
+               .execute_queue = &ft2232_execute_queue,
+       };

Linking to existing account procedure

If you already have an account and want to add another login method you MUST first sign in with your existing account and then change URL to read https://review.openocd.org/login/?link to get to this page again but this time it'll work for linking. Thank you.

SSH host keys fingerprints

1024 SHA256:YKx8b7u5ZWdcbp7/4AeXNaqElP49m6QrwfXaqQGJAOk gerrit-code-review@openocd.zylin.com (DSA)
384 SHA256:jHIbSQa4REvwCFG4cq5LBlBLxmxSqelQPem/EXIrxjk gerrit-code-review@openocd.org (ECDSA)
521 SHA256:UAOPYkU9Fjtcao0Ul/Rrlnj/OsQvt+pgdYSZ4jOYdgs gerrit-code-review@openocd.org (ECDSA)
256 SHA256:A13M5QlnozFOvTllybRZH6vm7iSt0XLxbA48yfc2yfY gerrit-code-review@openocd.org (ECDSA)
256 SHA256:spYMBqEYoAOtK7yZBrcwE8ZpYt6b68Cfh9yEVetvbXg gerrit-code-review@openocd.org (ED25519)
+--[ED25519 256]--+
|=..              |
|+o..   .         |
|*.o   . .        |
|+B . . .         |
|Bo. = o S        |
|Oo.+ + =         |
|oB=.* = . o      |
| =+=.+   + E     |
|. .=o   . o      |
+----[SHA256]-----+
2048 SHA256:0Onrb7/PHjpo6iVZ7xQX2riKN83FJ3KGU0TvI0TaFG4 gerrit-code-review@openocd.zylin.com (RSA)