- convert all files to unix line-ending

git-svn-id: svn://svn.berlios.de/openocd/trunk@347 b42882b7-edfa-0310-969c-e2dbd0fdcd60

5 files changed, 5264 insertions, 5264 deletions

diff --git a/src/jtag/bitq.c b/src/jtag/bitq.c
index 4e53bc55..7c47b8e8 100644
--- a/src/jtag/bitq.c
+++ b/src/jtag/bitq.c
@@ -1,366 +1,366 @@
-/***************************************************************************

- *   Copyright (C) 2007 by Pavel Chromy                                    *

- *   chromy@asix.cz                                                        *

- *                                                                         *

- *   This program is free software; you can redistribute it and/or modify  *

- *   it under the terms of the GNU General Public License as published by  *

- *   the Free Software Foundation; either version 2 of the License, or     *

- *   (at your option) any later version.                                   *

- *                                                                         *

- *   This program is distributed in the hope that it will be useful,       *

- *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *

- *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *

- *   GNU General Public License for more details.                          *

- *                                                                         *

- *   You should have received a copy of the GNU General Public License     *

- *   along with this program; if not, write to the                         *

- *   Free Software Foundation, Inc.,                                       *

- *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *

- ***************************************************************************/

-#ifdef HAVE_CONFIG_H

-#include "config.h"

-#endif

-

-#include "bitq.h"

-

-/* project specific includes */

-#include "log.h"

-#include "types.h"

-#include "jtag.h"

-#include "configuration.h"

-

-/* system includes */

-#include <string.h>

-#include <stdlib.h>

-#include <unistd.h>

-

-#include <sys/time.h>

-#include <time.h>

-

-

-bitq_interface_t *bitq_interface; /* low level bit queue interface */

-

-bitq_state_t bitq_in_state; /* state of input queue */

-

-u8 *bitq_in_buffer; /* buffer dynamically reallocated as needed */

-unsigned long bitq_in_bufsize=32; /* min. buffer size */

-

-

-/*

- * input queue processing does not use jtag_read_buffer() to avoid unnecessary overhead

- * also the buffer for incomming data is reallocated only if necessary

- * no parameters, makes use of stored state information

- */

-void bitq_in_proc(void)

-{

-	/* static information preserved between calls to increase performance */

-	static u8 *in_buff; /* pointer to buffer for scanned data */

-	static int in_idx; /* index of byte being scanned */

-	static u8 in_mask; /* mask of next bit to be scanned */

-

-	scan_field_t *field;

-	int tdo;

-

-	/* loop through the queue */

-	while (bitq_in_state.cmd) {

-		/* only JTAG_SCAN command may return data */

-		if (bitq_in_state.cmd->type==JTAG_SCAN) {

-			/* loop through the fields */

-			while (bitq_in_state.field_idx<bitq_in_state.cmd->cmd.scan->num_fields) {

-

-				field=&bitq_in_state.cmd->cmd.scan->fields[bitq_in_state.field_idx];

-				if ( field->in_value || field->in_handler) {

-

-					if (bitq_in_state.bit_pos==0) {

-						/* initialize field scanning */

-						in_mask=0x01;

-						in_idx=0;

-						if (field->in_value) in_buff=field->in_value;

-						else {

-							/* buffer reallocation needed? */

-							if (field->num_bits>bitq_in_bufsize*8) {

-								/* buffer previously allocated? */

-								if (bitq_in_buffer!=NULL) {

-									/* free it */

-									free(bitq_in_buffer);

-									bitq_in_buffer=NULL;

-								}

-								/* double the buffer size until it fits */

-								while (field->num_bits>bitq_in_bufsize*8) bitq_in_bufsize*=2;

-							}

-							/* if necessary, allocate buffer and check for malloc error */

-							if (bitq_in_buffer==NULL && (bitq_in_buffer=malloc(bitq_in_bufsize))==NULL) {

-								ERROR("malloc error");

-								exit(-1);

-							}

-							in_buff=(void *)bitq_in_buffer;

-						}

-					}

-

-					/* field scanning */

-					while (bitq_in_state.bit_pos<field->num_bits) {

-						if ((tdo=bitq_interface->in())<0) {

-#ifdef _DEBUG_JTAG_IO_

-							DEBUG("bitq in EOF");

-#endif

-							return;

-						}

-						if (in_mask==0x01) in_buff[in_idx]=0;

-						if (tdo) in_buff[in_idx]|=in_mask;

-						if (in_mask==0x80) {

-							in_mask=0x01;

-							in_idx++;

-						}

-						else in_mask<<=1;

-						bitq_in_state.bit_pos++;

-					}

-

-

-					if (field->in_handler && bitq_in_state.status==ERROR_OK) {

-						bitq_in_state.status=(*field->in_handler)(in_buff, field->in_handler_priv, field);

-					}

-

-				}

-

-				bitq_in_state.field_idx++; /* advance to next field */

-				bitq_in_state.bit_pos=0; /* start next field from the first bit */

-			}

-

-		}

-		bitq_in_state.cmd=bitq_in_state.cmd->next; /* advance to next command */

-		bitq_in_state.field_idx=0; /* preselect first field */

-	}

-}

-

-

-

-void bitq_io(int tms, int tdi, int tdo_req)

-{

-	bitq_interface->out(tms, tdi, tdo_req);

-	/* check and process the input queue */

-	if (bitq_interface->in_rdy()) bitq_in_proc();

-}

-

-

-void bitq_end_state(enum tap_state state)

-{

-	if (state==-1) return;

-	if (tap_move_map[state]==-1) {

-		ERROR("BUG: %i is not a valid end state", state);

-		exit(-1);

-	}

-	end_state = state;

-}

-

-

-void bitq_state_move(enum tap_state new_state)

-{

-	int i=0;

-	u8 tms_scan;

-

-	if (tap_move_map[cur_state]==-1 || tap_move_map[new_state]==-1) {

-		ERROR("TAP move from or to unstable state");

-		exit(-1);

-	}

-

-	tms_scan=TAP_MOVE(cur_state, new_state);

-

-	for (i=0; i<7; i++) {

-		bitq_io(tms_scan&1, 0, 0);

-		tms_scan>>=1;

-	}

-

-	cur_state = new_state;

-}

-

-

-void bitq_path_move(pathmove_command_t *cmd)

-{

-	int i;

-

-	for (i=0; i<=cmd->num_states; i++) {

-		if (tap_transitions[cur_state].low == cmd->path[i]) bitq_io(0, 0, 0);

-		else if (tap_transitions[cur_state].high == cmd->path[i]) bitq_io(1, 0, 0);

-		else {

-			ERROR("BUG: %s -> %s isn't a valid TAP transition", tap_state_strings[cur_state], tap_state_strings[cmd->path[i]]);

-			exit(-1);

-		}

-

-		cur_state = cmd->path[i];

-	}

-

-	end_state = cur_state;

-}

-

-

-void bitq_runtest(int num_cycles)

-{

-	int i;

-

-	/* only do a state_move when we're not already in RTI */

-	if (cur_state != TAP_RTI) bitq_state_move(TAP_RTI);

-

-	/* execute num_cycles */

-	for (i = 0; i < num_cycles; i++)

-		bitq_io(0, 0, 0);

-

-	/* finish in end_state */

-	if (cur_state != end_state) bitq_state_move(end_state);

-}

-

-

-void bitq_scan_field(scan_field_t *field, int pause)

-{

-	int bit_cnt;

-	int tdo_req;

-

-	u8 *out_ptr;

-	u8 out_mask;

-

-	if ( field->in_value || field->in_handler) tdo_req=1;

-	else tdo_req=0;

-

-	if (field->out_value==NULL) {

-		/* just send zeros and request data from TDO */

-		for (bit_cnt=field->num_bits; bit_cnt>1; bit_cnt--)

-			bitq_io(0, 0, tdo_req);

-		bitq_io(pause, 0, tdo_req);

-	}

-	else {

-		/* send data, and optionally request TDO */

-		out_mask=0x01;

-		out_ptr=field->out_value;

-		for (bit_cnt=field->num_bits; bit_cnt>1; bit_cnt--) {

-			bitq_io(0, ((*out_ptr)&out_mask)!=0, tdo_req);

-			if (out_mask==0x80) {

-				out_mask=0x01;

-				out_ptr++;

-			}

-			else out_mask<<=1;

-		}

-		bitq_io(pause, ((*out_ptr)&out_mask)!=0, tdo_req);

-	}

-

-	if (pause) {

-		bitq_io(0,0,0);

-		if (cur_state==TAP_SI) cur_state=TAP_PI;

-		else if (cur_state==TAP_SD) cur_state=TAP_PD;

-	}

-}

-

-

-void bitq_scan(scan_command_t *cmd)

-{

-	int i;

-

-	if (cmd->ir_scan) bitq_state_move(TAP_SI);

-	else bitq_state_move(TAP_SD);

-

-	for (i=0; i < cmd->num_fields-1; i++)

-		bitq_scan_field(&cmd->fields[i], 0);

-	bitq_scan_field(&cmd->fields[i], 1);

-}

-

-

-int bitq_execute_queue(void)

-{

-	jtag_command_t *cmd = jtag_command_queue; /* currently processed command */

-

-	bitq_in_state.cmd = jtag_command_queue;

-	bitq_in_state.field_idx = 0;

-	bitq_in_state.bit_pos = 0;

-	bitq_in_state.status = ERROR_OK;

-

-	while (cmd) {

-

-		switch (cmd->type) {

-

-			case JTAG_END_STATE:

-#ifdef _DEBUG_JTAG_IO_

-				DEBUG("end_state: %i", cmd->cmd.end_state->end_state);

-#endif

-				bitq_end_state(cmd->cmd.end_state->end_state);

-				break;

-

-			case JTAG_RESET:

-#ifdef _DEBUG_JTAG_IO_

-				DEBUG("reset trst: %i srst %i", cmd->cmd.reset->trst, cmd->cmd.reset->srst);

-#endif

-				bitq_interface->reset(cmd->cmd.reset->trst, cmd->cmd.reset->srst);

-				if (bitq_interface->in_rdy()) bitq_in_proc();

-				break;

-

-			case JTAG_RUNTEST:

-#ifdef _DEBUG_JTAG_IO_

-				DEBUG("runtest %i cycles, end in %i", cmd->cmd.runtest->num_cycles, cmd->cmd.runtest->end_state);

-#endif

-				bitq_end_state(cmd->cmd.runtest->end_state);

-				bitq_runtest(cmd->cmd.runtest->num_cycles);

-				break;

-

-			case JTAG_STATEMOVE:

-#ifdef _DEBUG_JTAG_IO_

-				DEBUG("statemove end in %i", cmd->cmd.statemove->end_state);

-#endif

-				bitq_end_state(cmd->cmd.statemove->end_state);

-				bitq_state_move(end_state); /* uncoditional TAP move */

-				break;

-

-			case JTAG_PATHMOVE:

-#ifdef _DEBUG_JTAG_IO_

-				DEBUG("pathmove: %i states, end in %i", cmd->cmd.pathmove->num_states, cmd->cmd.pathmove->path[cmd->cmd.pathmove->num_states - 1]);

-#endif

-				bitq_path_move(cmd->cmd.pathmove);

-				break;

-

-			case JTAG_SCAN:

-#ifdef _DEBUG_JTAG_IO_

-				DEBUG("scan end in %i", cmd->cmd.scan->end_state);

-				if (cmd->cmd.scan->ir_scan) DEBUG("scan ir");

-				else DEBUG("scan dr");

-#endif

-				bitq_end_state(cmd->cmd.scan->end_state);

-				bitq_scan(cmd->cmd.scan);

-				if (cur_state != end_state) bitq_state_move(end_state);

-				break;

-

-			case JTAG_SLEEP:

-#ifdef _DEBUG_JTAG_IO_

-				DEBUG("sleep %i", cmd->cmd.sleep->us);

-#endif

-				bitq_interface->sleep(cmd->cmd.sleep->us);

-				if (bitq_interface->in_rdy()) bitq_in_proc();

-				break;

-

-			default:

-				ERROR("BUG: unknown JTAG command type encountered");

-				exit(-1);

-		}

-

-		cmd = cmd->next;

-	}

-

-	bitq_interface->flush();

-	bitq_in_proc();

-

-	if (bitq_in_state.cmd) {

-		ERROR("missing data from bitq interface");

-		return ERROR_JTAG_QUEUE_FAILED;

-	}

-	if (bitq_interface->in()>=0) {

-		ERROR("extra data from bitq interface");

-		return ERROR_JTAG_QUEUE_FAILED;

-	}

-

-	return bitq_in_state.status;

-}

-

-

-void bitq_cleanup(void)

-{

-	if (bitq_in_buffer!=NULL)

-	{

-		free(bitq_in_buffer);

-		bitq_in_buffer=NULL;

-	}

-}

+/***************************************************************************
+ *   Copyright (C) 2007 by Pavel Chromy                                    *
+ *   chromy@asix.cz                                                        *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "bitq.h"
+
+/* project specific includes */
+#include "log.h"
+#include "types.h"
+#include "jtag.h"
+#include "configuration.h"
+
+/* system includes */
+#include <string.h>
+#include <stdlib.h>
+#include <unistd.h>
+
+#include <sys/time.h>
+#include <time.h>
+
+
+bitq_interface_t *bitq_interface; /* low level bit queue interface */
+
+bitq_state_t bitq_in_state; /* state of input queue */
+
+u8 *bitq_in_buffer; /* buffer dynamically reallocated as needed */
+unsigned long bitq_in_bufsize=32; /* min. buffer size */
+
+
+/*
+ * input queue processing does not use jtag_read_buffer() to avoid unnecessary overhead
+ * also the buffer for incomming data is reallocated only if necessary
+ * no parameters, makes use of stored state information
+ */
+void bitq_in_proc(void)
+{
+	/* static information preserved between calls to increase performance */
+	static u8 *in_buff; /* pointer to buffer for scanned data */
+	static int in_idx; /* index of byte being scanned */
+	static u8 in_mask; /* mask of next bit to be scanned */
+
+	scan_field_t *field;
+	int tdo;
+
+	/* loop through the queue */
+	while (bitq_in_state.cmd) {
+		/* only JTAG_SCAN command may return data */
+		if (bitq_in_state.cmd->type==JTAG_SCAN) {
+			/* loop through the fields */
+			while (bitq_in_state.field_idx<bitq_in_state.cmd->cmd.scan->num_fields) {
+
+				field=&bitq_in_state.cmd->cmd.scan->fields[bitq_in_state.field_idx];
+				if ( field->in_value || field->in_handler) {
+
+					if (bitq_in_state.bit_pos==0) {
+						/* initialize field scanning */
+						in_mask=0x01;
+						in_idx=0;
+						if (field->in_value) in_buff=field->in_value;
+						else {
+							/* buffer reallocation needed? */
+							if (field->num_bits>bitq_in_bufsize*8) {
+								/* buffer previously allocated? */
+								if (bitq_in_buffer!=NULL) {
+									/* free it */
+									free(bitq_in_buffer);
+									bitq_in_buffer=NULL;
+								}
+								/* double the buffer size until it fits */
+								while (field->num_bits>bitq_in_bufsize*8) bitq_in_bufsize*=2;
+							}
+							/* if necessary, allocate buffer and check for malloc error */
+							if (bitq_in_buffer==NULL && (bitq_in_buffer=malloc(bitq_in_bufsize))==NULL) {
+								ERROR("malloc error");
+								exit(-1);
+							}
+							in_buff=(void *)bitq_in_buffer;
+						}
+					}
+
+					/* field scanning */
+					while (bitq_in_state.bit_pos<field->num_bits) {
+						if ((tdo=bitq_interface->in())<0) {
+#ifdef _DEBUG_JTAG_IO_
+							DEBUG("bitq in EOF");
+#endif
+							return;
+						}
+						if (in_mask==0x01) in_buff[in_idx]=0;
+						if (tdo) in_buff[in_idx]|=in_mask;
+						if (in_mask==0x80) {
+							in_mask=0x01;
+							in_idx++;
+						}
+						else in_mask<<=1;
+						bitq_in_state.bit_pos++;
+					}
+
+
+					if (field->in_handler && bitq_in_state.status==ERROR_OK) {
+						bitq_in_state.status=(*field->in_handler)(in_buff, field->in_handler_priv, field);
+					}
+
+				}
+
+				bitq_in_state.field_idx++; /* advance to next field */
+				bitq_in_state.bit_pos=0; /* start next field from the first bit */
+			}
+
+		}
+		bitq_in_state.cmd=bitq_in_state.cmd->next; /* advance to next command */
+		bitq_in_state.field_idx=0; /* preselect first field */
+	}
+}
+
+
+
+void bitq_io(int tms, int tdi, int tdo_req)
+{
+	bitq_interface->out(tms, tdi, tdo_req);
+	/* check and process the input queue */
+	if (bitq_interface->in_rdy()) bitq_in_proc();
+}
+
+
+void bitq_end_state(enum tap_state state)
+{
+	if (state==-1) return;
+	if (tap_move_map[state]==-1) {
+		ERROR("BUG: %i is not a valid end state", state);
+		exit(-1);
+	}
+	end_state = state;
+}
+
+
+void bitq_state_move(enum tap_state new_state)
+{
+	int i=0;
+	u8 tms_scan;
+
+	if (tap_move_map[cur_state]==-1 || tap_move_map[new_state]==-1) {
+		ERROR("TAP move from or to unstable state");
+		exit(-1);
+	}
+
+	tms_scan=TAP_MOVE(cur_state, new_state);
+
+	for (i=0; i<7; i++) {
+		bitq_io(tms_scan&1, 0, 0);
+		tms_scan>>=1;
+	}
+
+	cur_state = new_state;
+}
+
+
+void bitq_path_move(pathmove_command_t *cmd)
+{
+	int i;
+
+	for (i=0; i<=cmd->num_states; i++) {
+		if (tap_transitions[cur_state].low == cmd->path[i]) bitq_io(0, 0, 0);
+		else if (tap_transitions[cur_state].high == cmd->path[i]) bitq_io(1, 0, 0);
+		else {
+			ERROR("BUG: %s -> %s isn't a valid TAP transition", tap_state_strings[cur_state], tap_state_strings[cmd->path[i]]);
+			exit(-1);
+		}
+
+		cur_state = cmd->path[i];
+	}
+
+	end_state = cur_state;
+}
+
+
+void bitq_runtest(int num_cycles)
+{
+	int i;
+
+	/* only do a state_move when we're not already in RTI */
+	if (cur_state != TAP_RTI) bitq_state_move(TAP_RTI);
+
+	/* execute num_cycles */
+	for (i = 0; i < num_cycles; i++)
+		bitq_io(0, 0, 0);
+
+	/* finish in end_state */
+	if (cur_state != end_state) bitq_state_move(end_state);
+}
+
+
+void bitq_scan_field(scan_field_t *field, int pause)
+{
+	int bit_cnt;
+	int tdo_req;
+
+	u8 *out_ptr;
+	u8 out_mask;
+
+	if ( field->in_value || field->in_handler) tdo_req=1;
+	else tdo_req=0;
+
+	if (field->out_value==NULL) {
+		/* just send zeros and request data from TDO */
+		for (bit_cnt=field->num_bits; bit_cnt>1; bit_cnt--)
+			bitq_io(0, 0, tdo_req);
+		bitq_io(pause, 0, tdo_req);
+	}
+	else {
+		/* send data, and optionally request TDO */
+		out_mask=0x01;
+		out_ptr=field->out_value;
+		for (bit_cnt=field->num_bits; bit_cnt>1; bit_cnt--) {
+			bitq_io(0, ((*out_ptr)&out_mask)!=0, tdo_req);
+			if (out_mask==0x80) {
+				out_mask=0x01;
+				out_ptr++;
+			}
+			else out_mask<<=1;
+		}
+		bitq_io(pause, ((*out_ptr)&out_mask)!=0, tdo_req);
+	}
+
+	if (pause) {
+		bitq_io(0,0,0);
+		if (cur_state==TAP_SI) cur_state=TAP_PI;
+		else if (cur_state==TAP_SD) cur_state=TAP_PD;
+	}
+}
+
+
+void bitq_scan(scan_command_t *cmd)
+{
+	int i;
+
+	if (cmd->ir_scan) bitq_state_move(TAP_SI);
+	else bitq_state_move(TAP_SD);
+
+	for (i=0; i < cmd->num_fields-1; i++)
+		bitq_scan_field(&cmd->fields[i], 0);
+	bitq_scan_field(&cmd->fields[i], 1);
+}
+
+
+int bitq_execute_queue(void)
+{
+	jtag_command_t *cmd = jtag_command_queue; /* currently processed command */
+
+	bitq_in_state.cmd = jtag_command_queue;
+	bitq_in_state.field_idx = 0;
+	bitq_in_state.bit_pos = 0;
+	bitq_in_state.status = ERROR_OK;
+
+	while (cmd) {
+
+		switch (cmd->type) {
+
+			case JTAG_END_STATE:
+#ifdef _DEBUG_JTAG_IO_
+				DEBUG("end_state: %i", cmd->cmd.end_state->end_state);
+#endif
+				bitq_end_state(cmd->cmd.end_state->end_state);
+				break;
+
+			case JTAG_RESET:
+#ifdef _DEBUG_JTAG_IO_
+				DEBUG("reset trst: %i srst %i", cmd->cmd.reset->trst, cmd->cmd.reset->srst);
+#endif
+				bitq_interface->reset(cmd->cmd.reset->trst, cmd->cmd.reset->srst);
+				if (bitq_interface->in_rdy()) bitq_in_proc();
+				break;
+
+			case JTAG_RUNTEST:
+#ifdef _DEBUG_JTAG_IO_
+				DEBUG("runtest %i cycles, end in %i", cmd->cmd.runtest->num_cycles, cmd->cmd.runtest->end_state);
+#endif
+				bitq_end_state(cmd->cmd.runtest->end_state);
+				bitq_runtest(cmd->cmd.runtest->num_cycles);
+				break;
+
+			case JTAG_STATEMOVE:
+#ifdef _DEBUG_JTAG_IO_
+				DEBUG("statemove end in %i", cmd->cmd.statemove->end_state);
+#endif
+				bitq_end_state(cmd->cmd.statemove->end_state);
+				bitq_state_move(end_state); /* uncoditional TAP move */
+				break;
+
+			case JTAG_PATHMOVE:
+#ifdef _DEBUG_JTAG_IO_
+				DEBUG("pathmove: %i states, end in %i", cmd->cmd.pathmove->num_states, cmd->cmd.pathmove->path[cmd->cmd.pathmove->num_states - 1]);
+#endif
+				bitq_path_move(cmd->cmd.pathmove);
+				break;
+
+			case JTAG_SCAN:
+#ifdef _DEBUG_JTAG_IO_
+				DEBUG("scan end in %i", cmd->cmd.scan->end_state);
+				if (cmd->cmd.scan->ir_scan) DEBUG("scan ir");
+				else DEBUG("scan dr");
+#endif
+				bitq_end_state(cmd->cmd.scan->end_state);
+				bitq_scan(cmd->cmd.scan);
+				if (cur_state != end_state) bitq_state_move(end_state);
+				break;
+
+			case JTAG_SLEEP:
+#ifdef _DEBUG_JTAG_IO_
+				DEBUG("sleep %i", cmd->cmd.sleep->us);
+#endif
+				bitq_interface->sleep(cmd->cmd.sleep->us);
+				if (bitq_interface->in_rdy()) bitq_in_proc();
+				break;
+
+			default:
+				ERROR("BUG: unknown JTAG command type encountered");
+				exit(-1);
+		}
+
+		cmd = cmd->next;
+	}
+
+	bitq_interface->flush();
+	bitq_in_proc();
+
+	if (bitq_in_state.cmd) {
+		ERROR("missing data from bitq interface");
+		return ERROR_JTAG_QUEUE_FAILED;
+	}
+	if (bitq_interface->in()>=0) {
+		ERROR("extra data from bitq interface");
+		return ERROR_JTAG_QUEUE_FAILED;
+	}
+
+	return bitq_in_state.status;
+}
+
+
+void bitq_cleanup(void)
+{
+	if (bitq_in_buffer!=NULL)
+	{
+		free(bitq_in_buffer);
+		bitq_in_buffer=NULL;
+	}
+}
diff --git a/src/jtag/ep93xx.c b/src/jtag/ep93xx.c
index ebedfdfe..aa62ad5b 100644
--- a/src/jtag/ep93xx.c
+++ b/src/jtag/ep93xx.c
@@ -1,237 +1,237 @@
-/***************************************************************************

- *   Copyright (C) 2005 by Dominic Rath                                    *

- *   Dominic.Rath@gmx.de                                                   *

- *                                                                         *

- *   This program is free software; you can redistribute it and/or modify  *

- *   it under the terms of the GNU General Public License as published by  *

- *   the Free Software Foundation; either version 2 of the License, or     *

- *   (at your option) any later version.                                   *

- *                                                                         *

- *   This program is distributed in the hope that it will be useful,       *

- *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *

- *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *

- *   GNU General Public License for more details.                          *

- *                                                                         *

- *   You should have received a copy of the GNU General Public License     *

- *   along with this program; if not, write to the                         *

- *   Free Software Foundation, Inc.,                                       *

- *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *

- ***************************************************************************/

-#ifdef HAVE_CONFIG_H

-#include "config.h"

-#endif

-

-#include "log.h"

-#include "jtag.h"

-#include "bitbang.h"

-

-#define TDO_BIT		1

-#define TDI_BIT		2

-#define TCK_BIT		4

-#define TMS_BIT		8

-#define TRST_BIT	16

-#define SRST_BIT	32

-#define VCC_BIT		64

-

-/* system includes */

-#include <string.h>

-#include <stdlib.h>

-#include <stdio.h>

-#include <sys/mman.h>

-#include <unistd.h>

-#include <fcntl.h>

-

-static u8 output_value = 0x0;

-static int dev_mem_fd;

-static void *gpio_controller;

-static volatile u8 *gpio_data_register;

-static volatile u8 *gpio_data_direction_register;

-

-/* low level command set

- */

-int ep93xx_read(void);

-void ep93xx_write(int tck, int tms, int tdi);

-void ep93xx_reset(int trst, int srst);

-

-int ep93xx_speed(int speed);

-int ep93xx_register_commands(struct command_context_s *cmd_ctx);

-int ep93xx_init(void);

-int ep93xx_quit(void);

-

-struct timespec ep93xx_zzzz;

-

-jtag_interface_t ep93xx_interface = 

-{

-	.name = "ep93xx",

-	

-	.execute_queue = bitbang_execute_queue,

-

-	.speed = ep93xx_speed,	

-	.register_commands = ep93xx_register_commands,

-	.init = ep93xx_init,

-	.quit = ep93xx_quit,

-};

-

-bitbang_interface_t ep93xx_bitbang =

-{

-	.read = ep93xx_read,

-	.write = ep93xx_write,

-	.reset = ep93xx_reset,

-	.blink = 0;

-};

-

-int ep93xx_read(void)

-{

-	return !!(*gpio_data_register & TDO_BIT);

-}

-

-void ep93xx_write(int tck, int tms, int tdi)

-{

-	if (tck)

-		output_value |= TCK_BIT;

-	else

-		output_value &= TCK_BIT;

-	

-	if (tms)

-		output_value |= TMS_BIT;

-	else

-		output_value &= TMS_BIT;

-	

-	if (tdi)

-		output_value |= TDI_BIT;

-	else

-		output_value &= TDI_BIT;

-

-	*gpio_data_register = output_value;

-	nanosleep(ep93xx_zzzz);

-}

-

-/* (1) assert or (0) deassert reset lines */

-void ep93xx_reset(int trst, int srst)

-{

-	if (trst == 0)

-		output_value |= TRST_BIT;

-	else if (trst == 1)

-		output_value &= TRST_BIT;

-

-	if (srst == 0)

-		output_value |= SRST_BIT;

-	else if (srst == 1)

-		output_value &= SRST_BIT;

-	

-	*gpio_data_register = output_value;

-	nanosleep(ep93xx_zzzz);

-}

-

-int ep93xx_speed(int speed)

-{

-	

-	return ERROR_OK;

-}

-

-int ep93xx_register_commands(struct command_context_s *cmd_ctx)

-{

-

-	return ERROR_OK;

-}

-

-static int set_gonk_mode(void)

-{

-	void *syscon;

-	u32 devicecfg;

-

-	syscon = mmap(NULL, 4096, PROT_READ | PROT_WRITE,

-			MAP_SHARED, dev_mem_fd, 0x80930000);

-	if (syscon == MAP_FAILED) {

-		perror("mmap");

-		return ERROR_JTAG_INIT_FAILED;

-	}

-

-	devicecfg = *((volatile int *)(syscon + 0x80));

-	*((volatile int *)(syscon + 0xc0)) = 0xaa;

-	*((volatile int *)(syscon + 0x80)) = devicecfg | 0x08000000;

-

-	munmap(syscon, 4096);

-

-	return ERROR_OK;

-}

-

-int ep93xx_init(void)

-{

-	int ret;

-

-	bitbang_interface = &ep93xx_bitbang;	

-

-	ep93xx_zzzz.tv_sec = 0;

-	ep93xx_zzzz.tv_nsec = 10000000;

-

-	dev_mem_fd = open("/dev/mem", O_RDWR | O_SYNC);

-	if (dev_mem_fd < 0) {

-		perror("open");

-		return ERROR_JTAG_INIT_FAILED;

-	}

-

-	gpio_controller = mmap(NULL, 4096, PROT_READ | PROT_WRITE,

-				MAP_SHARED, dev_mem_fd, 0x80840000);

-	if (gpio_controller == MAP_FAILED) {

-		perror("mmap");

-		close(dev_mem_fd);

-		return ERROR_JTAG_INIT_FAILED;

-	}

-

-	ret = set_gonk_mode();

-	if (ret != ERROR_OK) {

-		munmap(gpio_controller, 4096);

-		close(dev_mem_fd);

-		return ret;

-	}

-

-#if 0

-	/* Use GPIO port A.  */

-	gpio_data_register = gpio_controller + 0x00;

-	gpio_data_direction_register = gpio_controller + 0x10;

-

-

-	/* Use GPIO port B.  */

-	gpio_data_register = gpio_controller + 0x04;

-	gpio_data_direction_register = gpio_controller + 0x14;

-

-	/* Use GPIO port C.  */

-	gpio_data_register = gpio_controller + 0x08;

-	gpio_data_direction_register = gpio_controller + 0x18;

-

-	/* Use GPIO port D.  */

-	gpio_data_register = gpio_controller + 0x0c;

-	gpio_data_direction_register = gpio_controller + 0x1c;

-#endif

-

-	/* Use GPIO port C.  */

-	gpio_data_register = gpio_controller + 0x08;

-	gpio_data_direction_register = gpio_controller + 0x18;

-

-	INFO("gpio_data_register      = %p\n", gpio_data_register);

-	INFO("gpio_data_direction_reg = %p\n", gpio_data_direction_register); 

-	/*

-	 * Configure bit 0 (TDO) as an input, and bits 1-5 (TDI, TCK

-	 * TMS, TRST, SRST) as outputs.  Drive TDI and TCK low, and

-	 * TMS/TRST/SRST high.

-	 */

-	output_value = TMS_BIT | TRST_BIT | SRST_BIT | VCC_BIT;

-	*gpio_data_register = output_value;

-	nanosleep(ep93xx_zzzz);

-

-	/*

-	 * Configure the direction register.  1 = output, 0 = input.

-	 */

-	*gpio_data_direction_register =

-		TDI_BIT | TCK_BIT | TMS_BIT | TRST_BIT | SRST_BIT | VCC_BIT;

-

-	nanosleep(ep93xx_zzzz);

-	return ERROR_OK;

-}

-

-int ep93xx_quit(void)

-{

-

-	return ERROR_OK;

-}

+/***************************************************************************
+ *   Copyright (C) 2005 by Dominic Rath                                    *
+ *   Dominic.Rath@gmx.de                                                   *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "log.h"
+#include "jtag.h"
+#include "bitbang.h"
+
+#define TDO_BIT		1
+#define TDI_BIT		2
+#define TCK_BIT		4
+#define TMS_BIT		8
+#define TRST_BIT	16
+#define SRST_BIT	32
+#define VCC_BIT		64
+
+/* system includes */
+#include <string.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <sys/mman.h>
+#include <unistd.h>
+#include <fcntl.h>
+
+static u8 output_value = 0x0;
+static int dev_mem_fd;
+static void *gpio_controller;
+static volatile u8 *gpio_data_register;
+static volatile u8 *gpio_data_direction_register;
+
+/* low level command set
+ */
+int ep93xx_read(void);
+void ep93xx_write(int tck, int tms, int tdi);
+void ep93xx_reset(int trst, int srst);
+
+int ep93xx_speed(int speed);
+int ep93xx_register_commands(struct command_context_s *cmd_ctx);
+int ep93xx_init(void);
+int ep93xx_quit(void);
+
+struct timespec ep93xx_zzzz;
+
+jtag_interface_t ep93xx_interface = 
+{
+	.name = "ep93xx",
+	
+	.execute_queue = bitbang_execute_queue,
+
+	.speed = ep93xx_speed,	
+	.register_commands = ep93xx_register_commands,
+	.init = ep93xx_init,
+	.quit = ep93xx_quit,
+};
+
+bitbang_interface_t ep93xx_bitbang =
+{
+	.read = ep93xx_read,
+	.write = ep93xx_write,
+	.reset = ep93xx_reset,
+	.blink = 0;
+};
+
+int ep93xx_read(void)
+{
+	return !!(*gpio_data_register & TDO_BIT);
+}
+
+void ep93xx_write(int tck, int tms, int tdi)
+{
+	if (tck)
+		output_value |= TCK_BIT;
+	else
+		output_value &= TCK_BIT;
+	
+	if (tms)
+		output_value |= TMS_BIT;
+	else
+		output_value &= TMS_BIT;
+	
+	if (tdi)
+		output_value |= TDI_BIT;
+	else
+		output_value &= TDI_BIT;
+
+	*gpio_data_register = output_value;
+	nanosleep(ep93xx_zzzz);
+}
+
+/* (1) assert or (0) deassert reset lines */
+void ep93xx_reset(int trst, int srst)
+{
+	if (trst == 0)
+		output_value |= TRST_BIT;
+	else if (trst == 1)
+		output_value &= TRST_BIT;
+
+	if (srst == 0)
+		output_value |= SRST_BIT;
+	else if (srst == 1)
+		output_value &= SRST_BIT;
+	
+	*gpio_data_register = output_value;
+	nanosleep(ep93xx_zzzz);
+}
+
+int ep93xx_speed(int speed)
+{
+	
+	return ERROR_OK;
+}
+
+int ep93xx_register_commands(struct command_context_s *cmd_ctx)
+{
+
+	return ERROR_OK;
+}
+
+static int set_gonk_mode(void)
+{
+	void *syscon;
+	u32 devicecfg;
+
+	syscon = mmap(NULL, 4096, PROT_READ | PROT_WRITE,
+			MAP_SHARED, dev_mem_fd, 0x80930000);
+	if (syscon == MAP_FAILED) {
+		perror("mmap");
+		return ERROR_JTAG_INIT_FAILED;
+	}
+
+	devicecfg = *((volatile int *)(syscon + 0x80));
+	*((volatile int *)(syscon + 0xc0)) = 0xaa;
+	*((volatile int *)(syscon + 0x80)) = devicecfg | 0x08000000;
+
+	munmap(syscon, 4096);
+
+	return ERROR_OK;
+}
+
+int ep93xx_init(void)
+{
+	int ret;
+
+	bitbang_interface = &ep93xx_bitbang;	
+
+	ep93xx_zzzz.tv_sec = 0;
+	ep93xx_zzzz.tv_nsec = 10000000;
+
+	dev_mem_fd = open("/dev/mem", O_RDWR | O_SYNC);
+	if (dev_mem_fd < 0) {
+		perror("open");
+		return ERROR_JTAG_INIT_FAILED;
+	}
+
+	gpio_controller = mmap(NULL, 4096, PROT_READ | PROT_WRITE,
+				MAP_SHARED, dev_mem_fd, 0x80840000);
+	if (gpio_controller == MAP_FAILED) {
+		perror("mmap");
+		close(dev_mem_fd);
+		return ERROR_JTAG_INIT_FAILED;
+	}
+
+	ret = set_gonk_mode();
+	if (ret != ERROR_OK) {
+		munmap(gpio_controller, 4096);
+		close(dev_mem_fd);
+		return ret;
+	}
+
+#if 0
+	/* Use GPIO port A.  */
+	gpio_data_register = gpio_controller + 0x00;
+	gpio_data_direction_register = gpio_controller + 0x10;
+
+
+	/* Use GPIO port B.  */
+	gpio_data_register = gpio_controller + 0x04;
+	gpio_data_direction_register = gpio_controller + 0x14;
+
+	/* Use GPIO port C.  */
+	gpio_data_register = gpio_controller + 0x08;
+	gpio_data_direction_register = gpio_controller + 0x18;
+
+	/* Use GPIO port D.  */
+	gpio_data_register = gpio_controller + 0x0c;
+	gpio_data_direction_register = gpio_controller + 0x1c;
+#endif
+
+	/* Use GPIO port C.  */
+	gpio_data_register = gpio_controller + 0x08;
+	gpio_data_direction_register = gpio_controller + 0x18;
+
+	INFO("gpio_data_register      = %p\n", gpio_data_register);
+	INFO("gpio_data_direction_reg = %p\n", gpio_data_direction_register); 
+	/*
+	 * Configure bit 0 (TDO) as an input, and bits 1-5 (TDI, TCK
+	 * TMS, TRST, SRST) as outputs.  Drive TDI and TCK low, and
+	 * TMS/TRST/SRST high.
+	 */
+	output_value = TMS_BIT | TRST_BIT | SRST_BIT | VCC_BIT;
+	*gpio_data_register = output_value;
+	nanosleep(ep93xx_zzzz);
+
+	/*
+	 * Configure the direction register.  1 = output, 0 = input.
+	 */
+	*gpio_data_direction_register =
+		TDI_BIT | TCK_BIT | TMS_BIT | TRST_BIT | SRST_BIT | VCC_BIT;
+
+	nanosleep(ep93xx_zzzz);
+	return ERROR_OK;
+}
+
+int ep93xx_quit(void)
+{
+
+	return ERROR_OK;
+}
diff --git a/src/jtag/ft2232.c b/src/jtag/ft2232.c
index 011eead1..d5c4af25 100644
--- a/src/jtag/ft2232.c
+++ b/src/jtag/ft2232.c
@@ -1,2139 +1,2139 @@
-/***************************************************************************

- *   Copyright (C) 2004, 2006 by Dominic Rath                              *

- *   Dominic.Rath@gmx.de                                                   *

- *                                                                         *

- *   This program is free software; you can redistribute it and/or modify  *

- *   it under the terms of the GNU General Public License as published by  *

- *   the Free Software Foundation; either version 2 of the License, or     *

- *   (at your option) any later version.                                   *

- *                                                                         *

- *   This program is distributed in the hope that it will be useful,       *

- *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *

- *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *

- *   GNU General Public License for more details.                          *

- *                                                                         *

- *   You should have received a copy of the GNU General Public License     *

- *   along with this program; if not, write to the                         *

- *   Free Software Foundation, Inc.,                                       *

- *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *

- ***************************************************************************/

-#ifdef HAVE_CONFIG_H

-#include "config.h"

-#endif

-

-#if IS_CYGWIN == 1

-#include "windows.h"

-#undef ERROR

-#endif

-

-#include "replacements.h"

-

-/* project specific includes */

-#include "log.h"

-#include "types.h"

-#include "jtag.h"

-#include "configuration.h"

-#include "time_support.h"

-

-/* system includes */

-#include <string.h>

-#include <stdlib.h>

-#include <unistd.h>

-

-/* FT2232 access library includes */

-#if BUILD_FT2232_FTD2XX == 1

-#include <ftd2xx.h>

-#elif BUILD_FT2232_LIBFTDI == 1

-#include <ftdi.h>

-#endif

-

-#include <sys/time.h>

-#include <time.h>

-

-/* enable this to debug io latency

- */

-#if 0

-#define _DEBUG_USB_IO_

-#endif

-

-/* enable this to debug communication

- */

-#if 0

-#define _DEBUG_USB_COMMS_

-#endif

-

-int ft2232_execute_queue(void);

-

-int ft2232_speed(int speed);

-int ft2232_register_commands(struct command_context_s *cmd_ctx);

-int ft2232_init(void);

-int ft2232_quit(void);

-

-int ft2232_handle_device_desc_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

-int ft2232_handle_serial_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

-int ft2232_handle_layout_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

-int ft2232_handle_vid_pid_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

-int ft2232_handle_latency_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

-

-char *ft2232_device_desc = NULL;

-char *ft2232_serial = NULL;

-char *ft2232_layout = NULL;

-unsigned char ft2232_latency = 2;

-

-#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 };

-

-typedef struct ft2232_layout_s

-{

-	char* name;

-	int(*init)(void);

-	void(*reset)(int trst, int srst);

-	void(*blink)(void);

-} ft2232_layout_t;

-

-/* init procedures for supported layouts */

-int usbjtag_init(void);

-int jtagkey_init(void);

-int olimex_jtag_init(void);

-int flyswatter_init(void);

-int turtle_init(void);

-int comstick_init(void);

-int stm32stick_init(void);

-

-/* reset procedures for supported layouts */

-void usbjtag_reset(int trst, int srst);

-void jtagkey_reset(int trst, int srst);

-void olimex_jtag_reset(int trst, int srst);

-void flyswatter_reset(int trst, int srst);

-void turtle_reset(int trst, int srst);

-void comstick_reset(int trst, int srst);

-void stm32stick_reset(int trst, int srst);

-

-/* blink procedures for layouts that support a blinking led */

-void olimex_jtag_blink(void);

-void turtle_jtag_blink(void);

-

-ft2232_layout_t ft2232_layouts[] =

-{

-	{"usbjtag", usbjtag_init, usbjtag_reset, NULL},

-	{"jtagkey", jtagkey_init, jtagkey_reset, NULL},

-	{"jtagkey_prototype_v1", jtagkey_init, jtagkey_reset, NULL},

-	{"oocdlink", jtagkey_init, jtagkey_reset, NULL},

-	{"signalyzer", usbjtag_init, usbjtag_reset, NULL},

-	{"evb_lm3s811", usbjtag_init, usbjtag_reset, NULL},

-	{"olimex-jtag", olimex_jtag_init, olimex_jtag_reset, olimex_jtag_blink},

-	{"flyswatter", flyswatter_init, flyswatter_reset, NULL},

-	{"turtelizer2", turtle_init, turtle_reset, turtle_jtag_blink},

-	{"comstick", comstick_init, comstick_reset, NULL},

-	{"stm32stick", stm32stick_init, stm32stick_reset, NULL},

-	{NULL, NULL, NULL},

-};

-

-static u8 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;

-

-#if BUILD_FT2232_FTD2XX == 1

-static FT_HANDLE ftdih = NULL;

-#elif BUILD_FT2232_LIBFTDI == 1

-static struct ftdi_context ftdic;

-#endif

-

-static u8 *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,

-	.register_commands = ft2232_register_commands,

-	.init = ft2232_init,

-	.quit = ft2232_quit,

-};

-

-int ft2232_write(u8 *buf, int size, u32* 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)

-	{

-		*bytes_written = dw_bytes_written;

-		ERROR("FT_Write returned: %lu", status);

-		return ERROR_JTAG_DEVICE_ERROR;

-	}

-	else

-	{

-		*bytes_written = dw_bytes_written;

-		return ERROR_OK;	

-	}

-#elif BUILD_FT2232_LIBFTDI == 1

-	int retval;

-	if ((retval = ftdi_write_data(&ftdic, buf, size)) < 0)

-	{

-		*bytes_written = 0;

-		ERROR("ftdi_write_data: %s", ftdi_get_error_string(&ftdic));

-		return ERROR_JTAG_DEVICE_ERROR;

-	}

-	else

-	{

-		*bytes_written = retval;

-		return ERROR_OK;	

-	}

-#endif

-}

-

-int ft2232_read(u8* buf, int size, u32* bytes_read)

-{

-#if BUILD_FT2232_FTD2XX == 1

-	DWORD dw_bytes_read;

-	FT_STATUS status;

-	int timeout = 5;

-	*bytes_read = 0;

-

-	while ((*bytes_read < size) && timeout--)

-	{

-		if ((status = FT_Read(ftdih, buf + *bytes_read, size - 

-			*bytes_read, &dw_bytes_read)) != FT_OK)		

-		{

-			*bytes_read = 0; 

-			ERROR("FT_Read returned: %lu", status);

-			return ERROR_JTAG_DEVICE_ERROR;

-		}

-		*bytes_read += dw_bytes_read; 

-	}

-#elif BUILD_FT2232_LIBFTDI == 1

-	int retval;

-	int timeout = 100;

-	*bytes_read = 0;

-	

-	while ((*bytes_read < size) && timeout--)

-	{

-		if ((retval = ftdi_read_data(&ftdic, buf + *bytes_read, size - *bytes_read)) < 0)

-		{

-			*bytes_read = 0;

-			ERROR("ftdi_read_data: %s", ftdi_get_error_string(&ftdic));

-			return ERROR_JTAG_DEVICE_ERROR;

-		}

-		*bytes_read += retval;

-	}

-#endif

-

-	if (*bytes_read < size)

-	{

-		ERROR("couldn't read the requested number of bytes from FT2232 device (%i < %i)", *bytes_read, size);

-		return ERROR_JTAG_DEVICE_ERROR;

-	}

-	

-	return ERROR_OK;

-}

-

-int ft2232_speed(int speed)

-{

-	u8 buf[3];

-	int retval;

-	u32 bytes_written;

-

-	buf[0] = 0x86; /* command "set divisor" */

-	buf[1] = speed & 0xff; /* valueL (0=6MHz, 1=3MHz, 2=2.0MHz, ...*/

-	buf[2] = (speed >> 8) & 0xff; /* valueH */

-	

-	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))

-	{

-		ERROR("couldn't set FT2232 TCK speed");

-		return retval;

-	}

-

-	jtag_speed = speed;

-	

-	return ERROR_OK;

-}

-

-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, NULL);

-	register_command(cmd_ctx, NULL, "ft2232_serial", ft2232_handle_serial_command,

-		COMMAND_CONFIG, NULL);

-	register_command(cmd_ctx, NULL, "ft2232_layout", ft2232_handle_layout_command,

-		COMMAND_CONFIG, NULL);

-	register_command(cmd_ctx, NULL, "ft2232_vid_pid", ft2232_handle_vid_pid_command,

-					 COMMAND_CONFIG, NULL);

-	register_command(cmd_ctx, NULL, "ft2232_latency", ft2232_handle_latency_command,

-					 COMMAND_CONFIG, NULL);

-	return ERROR_OK;

-}

-

-void ft2232_end_state(enum tap_state state)

-{

-	if (tap_move_map[state] != -1)

-		end_state = state;

-	else

-	{

-		ERROR("BUG: %i is not a valid end state", state);

-		exit(-1);

-	}

-}

-

-void ft2232_read_scan(enum scan_type type, u8* buffer, int scan_size)

-{

-	int num_bytes = ((scan_size + 7) / 8);

-	int bits_left = scan_size;

-	int cur_byte = 0;

-

-	while(num_bytes-- > 1)

-	{

-		buffer[cur_byte] = BUFFER_READ;

-		cur_byte++;

-		bits_left -= 8;

-	}

-

-	buffer[cur_byte] = 0x0;

-

-	if (bits_left > 1)

-	{

-		buffer[cur_byte] = BUFFER_READ >> 1;

-	}

-

-	buffer[cur_byte] = (buffer[cur_byte] | ((BUFFER_READ & 0x02) << 6)) >> (8 - bits_left);

-

-}

-

-void ft2232_debug_dump_buffer(void)

-{

-	int i;

-	char line[256];

-	char *line_p = line;

-	

-	for (i = 0; i < ft2232_buffer_size; i++)

-	{

-		line_p += snprintf(line_p, 256 - (line_p - line), "%2.2x ", ft2232_buffer[i]);

-		if (i % 16 == 15)

-		{

-			DEBUG("%s", line);

-			line_p = line;

-		}

-	}

-	

-	if (line_p != line)

-		DEBUG("%s", line);

-}

-

-int ft2232_send_and_recv(jtag_command_t *first, jtag_command_t *last)

-{

-	jtag_command_t *cmd;

-	u8 *buffer;

-	int scan_size;

-	enum scan_type type;

-	int retval;

-	u32 bytes_written;

-	u32 bytes_read;

-	

-#ifdef _DEBUG_USB_IO_

-	struct timeval start, inter, inter2, end;

-	struct timeval d_inter, d_inter2, d_end;

-#endif

-

-#ifdef _DEBUG_USB_COMMS_

-	DEBUG("write buffer (size %i):", ft2232_buffer_size);

-	ft2232_debug_dump_buffer();

-#endif

-

-#ifdef _DEBUG_USB_IO_

-	gettimeofday(&start, NULL);	

-#endif

-

-	if ((retval = ft2232_write(ft2232_buffer, ft2232_buffer_size, &bytes_written)) != ERROR_OK)

-	{

-		ERROR("couldn't write MPSSE commands to FT2232");

-		exit(-1);

-	}

-	

-#ifdef _DEBUG_USB_IO_

-	gettimeofday(&inter, NULL);	

-#endif

-	

-	if (ft2232_expect_read)

-	{

-		int timeout = 100;

-		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)

-		{

-			ERROR("couldn't read from FT2232");

-			exit(-1);

-		}

-		

-#ifdef _DEBUG_USB_IO_

-		gettimeofday(&end, NULL);	

-

-		timeval_subtract(&d_inter, &inter, &start);

-		timeval_subtract(&d_inter2, &inter2, &start);

-		timeval_subtract(&d_end, &end, &start);

-

-		INFO("inter: %i.%i, inter2: %i.%i end: %i.%i", d_inter.tv_sec, d_inter.tv_usec, d_inter2.tv_sec, d_inter2.tv_usec, d_end.tv_sec, d_end.tv_usec);

-#endif

-	

-		

-		ft2232_buffer_size = bytes_read;

-		

-		if (ft2232_expect_read != ft2232_buffer_size)

-		{

-			ERROR("ft2232_expect_read (%i) != ft2232_buffer_size (%i) (%i retries)", ft2232_expect_read, ft2232_buffer_size, 100 - timeout);

-			ft2232_debug_dump_buffer();	

-

-			exit(-1);

-		}

-

-#ifdef _DEBUG_USB_COMMS_

-		DEBUG("read buffer (%i retries): %i bytes", 100 - timeout, ft2232_buffer_size);

-		ft2232_debug_dump_buffer();

-#endif

-	}

-

-	ft2232_expect_read = 0;

-	ft2232_read_pointer = 0;

-	

-	/* return ERROR_OK, unless a jtag_read_buffer returns a failed check

-	 * that wasn't handled by a caller-provided error handler

-	 */ 

-	retval = ERROR_OK;

-	

-	cmd = first;

-	while (cmd != last)

-	{

-		switch (cmd->type)

-		{

-			case JTAG_SCAN:

-				type = jtag_scan_type(cmd->cmd.scan);

-				if (type != SCAN_OUT)

-				{

-					scan_size = jtag_scan_size(cmd->cmd.scan);

-					buffer = calloc(CEIL(scan_size, 8), 1);

-					ft2232_read_scan(type, buffer, scan_size);

-					if (jtag_read_buffer(buffer, cmd->cmd.scan) != ERROR_OK)

-						retval = ERROR_JTAG_QUEUE_FAILED;

-					free(buffer);

-				}

-				break;

-			default:

-				break;

-		}

-		cmd = cmd->next;

-	}

-	

-	ft2232_buffer_size = 0;

-

-	return retval;

-}

-

-void ft2232_add_pathmove(pathmove_command_t *cmd)

-{

-	int num_states = cmd->num_states;

-	u8 tms_byte;

-	int state_count;

-

-	state_count = 0;

-	while (num_states)

-	{

-		tms_byte = 0x0;

-		int bit_count = 0;

-		

-		/* command "Clock Data to TMS/CS Pin (no Read)" */

-		BUFFER_ADD = 0x4b;

-		/* number of states remaining */

-		BUFFER_ADD = (num_states % 7) - 1;

-		

-		while (num_states % 7)

-		{

-			if (tap_transitions[cur_state].low == cmd->path[state_count])

-				buf_set_u32(&tms_byte, bit_count++, 1, 0x0);

-			else if (tap_transitions[cur_state].high == cmd->path[state_count])

-				buf_set_u32(&tms_byte, bit_count++, 1, 0x1);

-			else

-			{

-				ERROR("BUG: %s -> %s isn't a valid TAP transition", tap_state_strings[cur_state], tap_state_strings[cmd->path[state_count]]);

-				exit(-1);

-			}

-

-			cur_state = cmd->path[state_count];

-			state_count++;

-			num_states--;

-		}

-		

-		BUFFER_ADD = tms_byte;

-	}

-	

-	end_state = cur_state;

-}

-

-void ft2232_add_scan(int ir_scan, enum scan_type type, u8 *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 && (cur_state == TAP_SD)) || (ir_scan && (cur_state == TAP_SI))))

-	{

-		/* command "Clock Data to TMS/CS Pin (no Read)" */

-		BUFFER_ADD = 0x4b;

-		/* scan 7 bit */

-		BUFFER_ADD = 0x6;

-		/* TMS data bits */

-		if (ir_scan)

-		{

-			BUFFER_ADD = TAP_MOVE(cur_state, TAP_SI);

-			cur_state = TAP_SI;

-		}

-		else

-		{

-			BUFFER_ADD = TAP_MOVE(cur_state, TAP_SD);

-			cur_state = TAP_SD;

-		}

-		//DEBUG("added TMS scan (no read)");

-	}

-	

-	/* add command for complete bytes */

-	while (num_bytes > 1)

-	{

-		int thisrun_bytes;

-		if (type == SCAN_IO)

-		{

-			/* Clock Data Bytes In and Out LSB First */

-			BUFFER_ADD = 0x39;

-			//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;

-			//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;

-			//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;

-		if (type != SCAN_IN)

-		{

-			/* add complete bytes */

-			while(thisrun_bytes-- > 0)

-			{

-				BUFFER_ADD = buffer[cur_byte];

-				cur_byte++;

-				bits_left -= 8;

-			}

-		}

-		else /* (type == SCAN_IN) */

-		{

-			bits_left -= 8 * (thisrun_bytes);

-		}

-	}

-	

-	/* the most signifcant bit is scanned during TAP movement */

-	if (type != SCAN_IN)

-		last_bit = (buffer[cur_byte] >> (bits_left - 1)) & 0x1;

-	else

-		last_bit = 0;

-

-	/* process remaining bits but the last one */

-	if (bits_left > 1)

-	{

-		if (type == SCAN_IO)

-		{

-			/* Clock Data Bits In and Out LSB First */

-			BUFFER_ADD = 0x3b;

-			//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;

-			//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;

-			//DEBUG("added TDI bits (i %i)", bits_left - 1);

-		}

-		BUFFER_ADD = bits_left - 2;

-		if (type != SCAN_IN)

-			BUFFER_ADD = buffer[cur_byte];

-	}

-

-	if ((ir_scan && (end_state == TAP_SI)) ||

-		(!ir_scan && (end_state == TAP_SD)))

-	{

-		if (type == SCAN_IO)

-		{

-			/* Clock Data Bits In and Out LSB First */

-			BUFFER_ADD = 0x3b;

-			//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;

-			//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;

-			//DEBUG("added TDI bits (i %i)", bits_left - 1);

-		}

-		BUFFER_ADD = 0x0;

-		BUFFER_ADD = last_bit;

-	}

-	else

-	{

-		/* move from Shift-IR/DR to end state */

-		if (type != SCAN_OUT)

-		{

-			/* Clock Data to TMS/CS Pin with Read */

-			BUFFER_ADD = 0x6b;

-			//DEBUG("added TMS scan (read)");

-		}

-		else

-		{

-			/* Clock Data to TMS/CS Pin (no Read) */

-			BUFFER_ADD = 0x4b;

-			//DEBUG("added TMS scan (no read)");

-		}

-		BUFFER_ADD = 0x6;

-		BUFFER_ADD = TAP_MOVE(cur_state, end_state) | (last_bit << 7);

-		cur_state = end_state;

-	}

-}

-

-int ft2232_large_scan(scan_command_t *cmd, enum scan_type type, u8 *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;

-	int retval;

-	int thisrun_read = 0;

-	

-	if (cmd->ir_scan)

-	{

-		ERROR("BUG: large IR scans are not supported");

-		exit(-1);

-	}

-

-	if (cur_state != TAP_SD)

-	{

-		/* command "Clock Data to TMS/CS Pin (no Read)" */

-		BUFFER_ADD = 0x4b;

-		/* scan 7 bit */

-		BUFFER_ADD = 0x6;

-		/* TMS data bits */

-		BUFFER_ADD = TAP_MOVE(cur_state, TAP_SD);

-		cur_state = TAP_SD;

-	}

-	

-	if ((retval = ft2232_write(ft2232_buffer, ft2232_buffer_size, &bytes_written)) != ERROR_OK)

-	{

-		ERROR("couldn't write MPSSE commands to FT2232");

-		exit(-1);

-	}

-	DEBUG("ft2232_buffer_size: %i, bytes_written: %i", ft2232_buffer_size, bytes_written);

-	ft2232_buffer_size = 0;

-	

-	/* add command for complete bytes */

-	while (num_bytes > 1)

-	{

-		int thisrun_bytes;

-		

-		if (type == SCAN_IO)

-		{

-			/* Clock Data Bytes In and Out LSB First */

-			BUFFER_ADD = 0x39;

-			//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;

-			//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;

-			//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;

-		if (type != SCAN_IN)

-		{

-			/* add complete bytes */

-			while(thisrun_bytes-- > 0)

-			{

-				BUFFER_ADD = buffer[cur_byte];

-				cur_byte++;

-				bits_left -= 8;

-			}

-		}

-		else /* (type == SCAN_IN) */

-		{

-			bits_left -= 8 * (thisrun_bytes);

-		}

-

-		if ((retval = ft2232_write(ft2232_buffer, ft2232_buffer_size, &bytes_written)) != ERROR_OK)

-		{

-			ERROR("couldn't write MPSSE commands to FT2232");

-			exit(-1);

-		}

-		DEBUG("ft2232_buffer_size: %i, bytes_written: %i", ft2232_buffer_size, bytes_written);

-		ft2232_buffer_size = 0;

-		

-		if (type != SCAN_OUT)

-		{

-			if ((retval = ft2232_read(receive_pointer, thisrun_read, &bytes_read)) != ERROR_OK)

-			{

-				ERROR("couldn't read from FT2232");

-				exit(-1);

-			}

-			DEBUG("thisrun_read: %i, bytes_read: %i", thisrun_read, bytes_read);

-			receive_pointer += bytes_read;

-		}

-	}

-	

-	thisrun_read = 0;

-	

-	/* the most signifcant bit is scanned during TAP movement */

-	if (type != SCAN_IN)

-		last_bit = (buffer[cur_byte] >> (bits_left - 1)) & 0x1;

-	else

-		last_bit = 0;

-

-	/* process remaining bits but the last one */

-	if (bits_left > 1)

-	{

-		if (type == SCAN_IO)

-		{

-			/* Clock Data Bits In and Out LSB First */

-			BUFFER_ADD = 0x3b;

-			//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;

-			//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;

-			//DEBUG("added TDI bits (i %i)", bits_left - 1);

-		}

-		BUFFER_ADD = bits_left - 2;

-		if (type != SCAN_IN)

-			BUFFER_ADD = buffer[cur_byte];

-			

-		if (type != SCAN_OUT)

-			thisrun_read += 2;

-	}

-

-	if (end_state == TAP_SD)

-	{

-		if (type == SCAN_IO)

-		{

-			/* Clock Data Bits In and Out LSB First */

-			BUFFER_ADD = 0x3b;

-			//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;

-			//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;

-			//DEBUG("added TDI bits (i %i)", bits_left - 1);

-		}

-		BUFFER_ADD = 0x0;

-		BUFFER_ADD = last_bit;

-	}

-	else

-	{

-		/* move from Shift-IR/DR to end state */

-		if (type != SCAN_OUT)

-		{

-			/* Clock Data to TMS/CS Pin with Read */

-			BUFFER_ADD = 0x6b;

-			//DEBUG("added TMS scan (read)");

-		}

-		else

-		{

-			/* Clock Data to TMS/CS Pin (no Read) */

-			BUFFER_ADD = 0x4b;

-			//DEBUG("added TMS scan (no read)");

-		}

-		BUFFER_ADD = 0x6;

-		BUFFER_ADD = TAP_MOVE(cur_state, end_state) | (last_bit << 7);

-		cur_state = end_state;

-	}

-	

-	if (type != SCAN_OUT)

-		thisrun_read += 1;

-	

-	if ((retval = ft2232_write(ft2232_buffer, ft2232_buffer_size, &bytes_written)) != ERROR_OK)

-	{

-		ERROR("couldn't write MPSSE commands to FT2232");

-		exit(-1);

-	}

-	DEBUG("ft2232_buffer_size: %i, bytes_written: %i", ft2232_buffer_size, bytes_written);

-	ft2232_buffer_size = 0;

-	

-	if (type != SCAN_OUT)

-	{

-		if ((retval = ft2232_read(receive_pointer, thisrun_read, &bytes_read)) != ERROR_OK)

-		{

-			ERROR("couldn't read from FT2232");

-			exit(-1);

-		}

-		DEBUG("thisrun_read: %i, bytes_read: %i", thisrun_read, bytes_read);

-		receive_pointer += bytes_read;

-	}

-	

-	return ERROR_OK;

-}

-

-int ft2232_predict_scan_out(int scan_size, enum scan_type type)

-{

-	int predicted_size = 3;

-	int num_bytes = (scan_size - 1) / 8;

-	

-	if (cur_state != TAP_SD)

-		predicted_size += 3;

-	

-	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;

-	}

-	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;

-	}

-

-	return predicted_size;

-}

-

-int ft2232_predict_scan_in(int scan_size, enum scan_type type)

-{

-	int predicted_size = 0;

-	

-	if (type != SCAN_OUT)

-	{

-		/* complete bytes */

-		predicted_size += (CEIL(scan_size, 8) > 1) ? (CEIL(scan_size, 8) - 1) : 0;

-		/* remaining bits - 1 */

-		predicted_size += ((scan_size - 1) % 8) ? 1 : 0;

-		/* last bit (from TMS scan) */

-		predicted_size += 1;

-	}

-	

-	//DEBUG("scan_size: %i, predicted_size: %i", scan_size, predicted_size);

-

-	return predicted_size;

-}

-

-void usbjtag_reset(int trst, int srst)

-{

-	if (trst == 1)

-	{

-		cur_state = TAP_TLR;

-		if (jtag_reset_config & RESET_TRST_OPEN_DRAIN)

-			low_direction |= nTRSTnOE;	/* switch to output pin (output is low) */

-		else

-			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) */

-		else

-			low_output |= nTRST; /* switch output high */

-	}

-

-	if (srst == 1)

-	{

-		if (jtag_reset_config & RESET_SRST_PUSH_PULL)

-			low_output &= ~nSRST;	/* switch output low */

-		else

-			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 */

-		else

-			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;

-

-}

-

-void jtagkey_reset(int trst, int srst)

-{

-	if (trst == 1)

-	{

-		cur_state = TAP_TLR;

-		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_ADD = 0x82;

-	BUFFER_ADD = high_output;

-	BUFFER_ADD = high_direction;

-	DEBUG("trst: %i, srst: %i, high_output: 0x%2.2x, high_direction: 0x%2.2x", trst, srst, high_output, high_direction);

-}

-

-void olimex_jtag_reset(int trst, int srst)

-{

-	if (trst == 1)

-	{

-		cur_state = TAP_TLR;

-		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)

-    {

-        high_output |= nSRST;

-    }

-    else if (srst == 0)

-    {

-        high_output &= ~nSRST;

-    }

-

-    /* command "set data bits high byte" */

-    BUFFER_ADD = 0x82;

-    BUFFER_ADD = high_output;

-    BUFFER_ADD = high_direction;

-    DEBUG("trst: %i, srst: %i, high_output: 0x%2.2x, high_direction: 0x%2.2x", trst, srst, high_output, high_direction);

-}

-

-void flyswatter_reset(int trst, int srst)

-{

-	if (trst == 1)

-	{

-		cur_state = TAP_TLR;

-		low_output &= ~nTRST;

-	}

-	else if (trst == 0)

-	{

-		low_output |= nTRST;

-	}

-

-    if (srst == 1)

-    {

-        low_output |= nSRST;

-    }

-    else if (srst == 0)

-    {

-        low_output &= ~nSRST;

-    }

-

-    /* command "set data bits low byte" */

-    BUFFER_ADD = 0x80;

-    BUFFER_ADD = low_output;

-    BUFFER_ADD = low_direction;

-    DEBUG("trst: %i, srst: %i, low_output: 0x%2.2x, low_direction: 0x%2.2x", trst, srst, low_output, low_direction);

-}

-

-void turtle_reset(int trst, int srst)

-{

-	trst = trst;

-	

-	if (srst == 1)

-	{

-		low_output |= nSRST;

-	}

-	else if (srst == 0)

-	{

-		low_output &= ~nSRST;

-	}

-	

-	/* command "set data bits low byte" */

-	BUFFER_ADD = 0x80;

-	BUFFER_ADD = low_output;

-	BUFFER_ADD = low_direction;

-	DEBUG("srst: %i, low_output: 0x%2.2x, low_direction: 0x%2.2x", srst, low_output, low_direction);

-}

-

-void comstick_reset(int trst, int srst)

-{

-	if (trst == 1)

-	{

-		cur_state = TAP_TLR;

-		high_output &= ~nTRST;

-	}

-	else if (trst == 0)

-	{

-		high_output |= nTRST;

-	}

-

-    if (srst == 1)

-    {

-        high_output &= ~nSRST;

-    }

-    else if (srst == 0)

-    {

-        high_output |= nSRST;

-    }

-	

-	/* command "set data bits high byte" */

-	BUFFER_ADD = 0x82;

-	BUFFER_ADD = high_output;

-	BUFFER_ADD = high_direction;

-	DEBUG("trst: %i, srst: %i, high_output: 0x%2.2x, high_direction: 0x%2.2x", trst, srst, high_output, high_direction);

-}

-

-void stm32stick_reset(int trst, int srst)

-{

-	if (trst == 1)

-	{

-		cur_state = TAP_TLR;

-		high_output &= ~nTRST;

-	}

-	else if (trst == 0)

-	{

-		high_output |= nTRST;

-	}

-

-    if (srst == 1)

-    {

-        low_output &= ~nSRST;

-    }

-    else if (srst == 0)

-    {

-        low_output |= nSRST;

-    }

-	

-	/* command "set data bits low byte" */

-	BUFFER_ADD = 0x80;

-	BUFFER_ADD = low_output;

-	BUFFER_ADD = low_direction;

-	

-	/* command "set data bits high byte" */

-	BUFFER_ADD = 0x82;

-	BUFFER_ADD = high_output;

-	BUFFER_ADD = high_direction;

-	DEBUG("trst: %i, srst: %i, high_output: 0x%2.2x, high_direction: 0x%2.2x", trst, srst, high_output, high_direction);

-}

-

-int ft2232_execute_queue()

-{

-	jtag_command_t *cmd = jtag_command_queue; /* currently processed command */

-	jtag_command_t *first_unsent = cmd;	/* next command that has to be sent */

-	u8 *buffer;

-	int scan_size;	/* size of IR or DR scan */

-	enum scan_type type;

-	int i;

-	int predicted_size = 0;

-	int require_send = 0;

-	int retval;

-	

-	/* return ERROR_OK, unless ft2232_send_and_recv reports a failed check

-	 * that wasn't handled by a caller-provided error handler

-	 */ 

-	retval = ERROR_OK;

-

-	ft2232_buffer_size = 0;

-	ft2232_expect_read = 0;

-	

-	/* blink, if the current layout has that feature */

-	if (layout->blink)

-		layout->blink();

-

-	while (cmd)

-	{

-		switch(cmd->type)

-		{

-			case JTAG_END_STATE:

-				if (cmd->cmd.end_state->end_state != -1)

-					ft2232_end_state(cmd->cmd.end_state->end_state);

-				break;

-			case JTAG_RESET:

-				/* only send the maximum buffer size that FT2232C can handle */

-				predicted_size = 3;

-				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;

-					require_send = 0;

-					first_unsent = cmd;

-				}

-

-				layout->reset(cmd->cmd.reset->trst, cmd->cmd.reset->srst);

-				require_send = 1;

-				

-#ifdef _DEBUG_JTAG_IO_				

-				DEBUG("trst: %i, srst: %i", cmd->cmd.reset->trst, cmd->cmd.reset->srst);

-#endif

-				break;

-			case JTAG_RUNTEST:

-				/* only send the maximum buffer size that FT2232C can handle */

-				predicted_size = 0;

-				if (cur_state != TAP_RTI)

-					predicted_size += 3;

-				predicted_size += 3 * CEIL(cmd->cmd.runtest->num_cycles, 7);

-				if ((cmd->cmd.runtest->end_state != -1) && (cmd->cmd.runtest->end_state != TAP_RTI))

-					predicted_size += 3;

-				if ((cmd->cmd.runtest->end_state == -1) && (end_state != TAP_RTI))

-					predicted_size += 3;

-				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;

-					require_send = 0;

-					first_unsent = cmd;

-				}

-				if (cur_state != TAP_RTI)

-				{

-					/* command "Clock Data to TMS/CS Pin (no Read)" */

-					BUFFER_ADD = 0x4b;

-					/* scan 7 bit */

-					BUFFER_ADD = 0x6;

-					/* TMS data bits */

-					BUFFER_ADD = TAP_MOVE(cur_state, TAP_RTI);

-					cur_state = TAP_RTI;

-					require_send = 1;

-				}

-				i = cmd->cmd.runtest->num_cycles;

-				while (i > 0)

-				{

-					/* command "Clock Data to TMS/CS Pin (no Read)" */

-					BUFFER_ADD = 0x4b;

-					/* scan 7 bit */

-					BUFFER_ADD = (i > 7) ? 6 : (i - 1);

-					/* TMS data bits */

-					BUFFER_ADD = 0x0;

-					cur_state = TAP_RTI;

-					i -= (i > 7) ? 7 : i;

-					//DEBUG("added TMS scan (no read)");

-				}

-				if (cmd->cmd.runtest->end_state != -1)

-					ft2232_end_state(cmd->cmd.runtest->end_state);

-				if (cur_state != 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_MOVE(cur_state, end_state);

-					cur_state = end_state;

-					//DEBUG("added TMS scan (no read)");

-				}

-				require_send = 1;

-#ifdef _DEBUG_JTAG_IO_				

-				DEBUG("runtest: %i, end in %i", cmd->cmd.runtest->num_cycles, end_state);

-#endif

-				break;

-			case JTAG_STATEMOVE:

-				/* only send the maximum buffer size that FT2232C can handle */

-				predicted_size = 3;

-				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;

-					require_send = 0;

-					first_unsent = cmd;

-				}

-				if (cmd->cmd.statemove->end_state != -1)

-					ft2232_end_state(cmd->cmd.statemove->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_MOVE(cur_state, end_state);

-				//DEBUG("added TMS scan (no read)");

-				cur_state = end_state;

-				require_send = 1;

-#ifdef _DEBUG_JTAG_IO_				

-				DEBUG("statemove: %i", end_state);

-#endif

-				break;

-			case JTAG_PATHMOVE:

-				/* only send the maximum buffer size that FT2232C can handle */

-				predicted_size = 3 * CEIL(cmd->cmd.pathmove->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;

-					require_send = 0;

-					first_unsent = cmd;

-				}

-				ft2232_add_pathmove(cmd->cmd.pathmove);

-				require_send = 1;

-#ifdef _DEBUG_JTAG_IO_				

-				DEBUG("pathmove: %i states, end in %i", cmd->cmd.pathmove->num_states, cmd->cmd.pathmove->path[cmd->cmd.pathmove->num_states - 1]);

-#endif

-				break;

-			case JTAG_SCAN:

-				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)

-				{

-					DEBUG("oversized ft2232 scan (predicted_size > FT2232_BUFFER_SIZE)");

-					/* unsent commands before this */

-					if (first_unsent != cmd)

-						if (ft2232_send_and_recv(first_unsent, cmd) != ERROR_OK)

-							retval = ERROR_JTAG_QUEUE_FAILED;

-					

-					/* current command */

-					if (cmd->cmd.scan->end_state != -1)

-						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;

-					if (buffer)

-						free(buffer);

-					break;

-				}

-				else if (ft2232_buffer_size + predicted_size + 1 > FT2232_BUFFER_SIZE)

-				{

-					DEBUG("ft2232 buffer size reached, sending queued commands (first_unsent: %p, cmd: %p)", first_unsent, cmd);

-					if (ft2232_send_and_recv(first_unsent, cmd) != ERROR_OK)

-						retval = ERROR_JTAG_QUEUE_FAILED;

-					require_send = 0;

-					first_unsent = cmd;

-				}

-				ft2232_expect_read += ft2232_predict_scan_in(scan_size, type);

-				//DEBUG("new read size: %i", ft2232_expect_read);

-				if (cmd->cmd.scan->end_state != -1)

-					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_				

-				DEBUG("%s scan, %i bit, end in %i", (cmd->cmd.scan->ir_scan) ? "IR" : "DR", scan_size, end_state);

-#endif

-				break;

-			case JTAG_SLEEP:

-				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_				

-				DEBUG("sleep %i usec", cmd->cmd.sleep->us);

-#endif

-				break;

-			default:

-				ERROR("BUG: unknown JTAG command type encountered");

-				exit(-1);

-		}

-		cmd = cmd->next;

-	}

-

-	if (require_send > 0)

-		if (ft2232_send_and_recv(first_unsent, cmd) != ERROR_OK)

-			retval = ERROR_JTAG_QUEUE_FAILED;

-

-	return retval;

-}

-

-#if BUILD_FT2232_FTD2XX == 1

-static int ft2232_init_ftd2xx(u16 vid, u16 pid, int more, int *try_more)

-{

-	FT_STATUS status;

-	DWORD openex_flags = 0;

-	char *openex_string = NULL;

-	u8 latency_timer;

-

-	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)

-	{

-		WARNING("couldn't add %4.4x:%4.4x",

-		    vid, pid);

-	}

-#endif

-

-	if (ft2232_device_desc && ft2232_serial)

-	{

-		WARNING("can't open by device description and serial number, giving precedence to serial");

-		ft2232_device_desc = NULL;

-	}

-	

-	if (ft2232_device_desc)

-	{

-		openex_string = ft2232_device_desc;

-		openex_flags = FT_OPEN_BY_DESCRIPTION;

-	}

-	else if (ft2232_serial)

-	{

-		openex_string = ft2232_serial;

-		openex_flags = FT_OPEN_BY_SERIAL_NUMBER;

-	}

-	else

-	{

-		ERROR("neither device description nor serial number specified");

-		ERROR("please add \"ft2232_device_desc <string>\" or \"ft2232_serial <string>\" to your .cfg file");

-		

-		return ERROR_JTAG_INIT_FAILED;	

-	}

-

-	if ((status = FT_OpenEx(openex_string, openex_flags, &ftdih)) != FT_OK)

-	{

-		DWORD num_devices;

-		

-		if (more) {

-			WARNING("unable to open ftdi device (trying more): %lu",

-			    status);

-			*try_more = 1;

-			return ERROR_JTAG_INIT_FAILED;

-		}

-		ERROR("unable to open ftdi device: %lu", status);

-		status = FT_ListDevices(&num_devices, NULL, FT_LIST_NUMBER_ONLY);

-		if (status == FT_OK)

-		{

-			char **desc_array = malloc(sizeof(char*) * (num_devices + 1));

-			int i;

-

-			for (i = 0; i < num_devices; i++)

-				desc_array[i] = malloc(64);

-			desc_array[num_devices] = NULL;

-

-			status = FT_ListDevices(desc_array, &num_devices, FT_LIST_ALL | openex_flags);

-

-			if (status == FT_OK)

-			{

-				ERROR("ListDevices: %lu\n", num_devices);

-				for (i = 0; i < num_devices; i++)

-					ERROR("%i: %s", i, desc_array[i]);

-			}

-			

-			for (i = 0; i < num_devices; i++)

-				free(desc_array[i]);

-			free(desc_array);

-		}

-		else

-		{

-			ERROR("ListDevices: NONE\n");

-		}

-		return ERROR_JTAG_INIT_FAILED;

-	}

-

-	if ((status = FT_SetLatencyTimer(ftdih, ft2232_latency)) != FT_OK)

-	{

-		ERROR("unable to set latency timer: %lu", status);

-		return ERROR_JTAG_INIT_FAILED;

-	}

-	

-	if ((status = FT_GetLatencyTimer(ftdih, &latency_timer)) != FT_OK)

-	{

-		ERROR("unable to get latency timer: %lu", status);

-		return ERROR_JTAG_INIT_FAILED;

-	}

-	else

-	{

-		DEBUG("current latency timer: %i", latency_timer);

-	}

-	

-	if ((status = FT_SetTimeouts(ftdih, 5000, 5000)) != FT_OK)

-	{

-		ERROR("unable to set timeouts: %lu", status);

-		return ERROR_JTAG_INIT_FAILED;

-	}

-

-	if ((status = FT_SetBitMode(ftdih, 0x0b, 2)) != FT_OK)

-	{

-		ERROR("unable to enable bit i/o mode: %lu", status);

-		return ERROR_JTAG_INIT_FAILED;

-	}

-

-	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)

-	{

-		ERROR("error purging ftd2xx device: %lu", status);

-		return ERROR_JTAG_INIT_FAILED;

-	}

-

-	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)

-{

-	u8 latency_timer;

-

-	DEBUG("'ft2232' interface using libftdi with '%s' layout (%4.4x:%4.4x)",

-	    ft2232_layout, vid, pid);

-

-	if (ftdi_init(&ftdic) < 0)

-		return ERROR_JTAG_INIT_FAILED;

-

-	/* context, vendor id, product id */

-	if (ftdi_usb_open_desc(&ftdic, vid, pid, ft2232_device_desc,

-	    ft2232_serial) < 0) {

-		if (more)

-			WARNING("unable to open ftdi device (trying more): %s",

-			     ftdic.error_str);

-		else

-			ERROR("unable to open ftdi device: %s", ftdic.error_str);

-		*try_more = 1;

-		return ERROR_JTAG_INIT_FAILED;

-	}

-

-	if (ftdi_set_interface(&ftdic, INTERFACE_A) < 0)

-	{

-		ERROR("unable to select FT2232 channel A: %s", ftdic.error_str);

-		return ERROR_JTAG_INIT_FAILED;

-	}

-

-	if (ftdi_usb_reset(&ftdic) < 0)

-	{

-		ERROR("unable to reset ftdi device");

-		return ERROR_JTAG_INIT_FAILED;

-	}

-

-	if (ftdi_set_latency_timer(&ftdic, ft2232_latency) < 0)

-	{

-		ERROR("unable to set latency timer");

-		return ERROR_JTAG_INIT_FAILED;

-	}

-	

-	if (ftdi_get_latency_timer(&ftdic, &latency_timer) < 0)

-	{

-		ERROR("unable to get latency timer");

-		return ERROR_JTAG_INIT_FAILED;

-	}

-	else

-	{

-		DEBUG("current latency timer: %i", latency_timer);

-	}

-

-	ftdi_set_bitmode(&ftdic, 0x0b, 2); /* ctx, JTAG I/O mask */

-

-	return ERROR_OK;

-}

-

-static int ft2232_purge_libftdi(void)

-{

-	if (ftdi_usb_purge_buffers(&ftdic) < 0)

-	{

-		ERROR("ftdi_purge_buffers: %s", ftdic.error_str);

-		return ERROR_JTAG_INIT_FAILED;

-	}

-

-	return ERROR_OK;

-}

-#endif /* BUILD_FT2232_LIBFTDI == 1 */

-

-int ft2232_init(void)

-{

-	u8 buf[1];

-	int retval;

-	u32 bytes_written;

-	ft2232_layout_t *cur_layout = ft2232_layouts;

-	int i;

-	

-	if ((ft2232_layout == NULL) || (ft2232_layout[0] == 0))

-	{

-		ft2232_layout = "usbjtag";

-		WARNING("No ft2232 layout specified, using default 'usbjtag'");

-	}

-	

-	while (cur_layout->name)

-	{

-		if (strcmp(cur_layout->name, ft2232_layout) == 0)

-		{

-			layout = cur_layout;

-			break;

-		}

-		cur_layout++;

-	}

-

-	if (!layout)

-	{

-		ERROR("No matching layout found for %s", ft2232_layout);

-		return ERROR_JTAG_INIT_FAILED;

-	}

-	

-	for (i = 0; 1; i++) {

-		/*

-		 * "more indicates that there are more IDs to try, so we should

-		 * not print an error for an ID mismatch (but for anything

-		 * else, we should).

-		 *

-		 * try_more indicates that the error code returned indicates an

-		 * ID mismatch (and nothing else) and that we should proceeed

-		 * with the next ID pair.

-		 */

-		int more = ft2232_vid[i+1] || ft2232_pid[i+1];

-		int try_more = 0;

-

-#if BUILD_FT2232_FTD2XX == 1

-		retval = ft2232_init_ftd2xx(ft2232_vid[i], ft2232_pid[i],

-		    more, &try_more);

-#elif BUILD_FT2232_LIBFTDI == 1

-		retval = ft2232_init_libftdi(ft2232_vid[i], ft2232_pid[i],

-		    more, &try_more);

-#endif	

-		if (retval >= 0)

-			break;

-		if (!more || !try_more)

-			return retval;

-	}

-

-	ft2232_buffer_size = 0;

-	ft2232_buffer = malloc(FT2232_BUFFER_SIZE);

-

-	if (layout->init() != ERROR_OK)

-		return ERROR_JTAG_INIT_FAILED;

-

-	ft2232_speed(jtag_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))

-	{

-		ERROR("couldn't write to FT2232 to disable loopback");

-		return ERROR_JTAG_INIT_FAILED;

-	}

-

-#if BUILD_FT2232_FTD2XX == 1

-	return ft2232_purge_ftd2xx();

-#elif BUILD_FT2232_LIBFTDI == 1

-	return ft2232_purge_libftdi();

-#endif	

-

-	return ERROR_OK;

-}

-

-int usbjtag_init(void)

-{

-	u8 buf[3];

-	u32 bytes_written;

-	

-	low_output = 0x08;

-	low_direction = 0x0b;

-	

-	if (strcmp(ft2232_layout, "usbjtag") == 0)

-	{

-		nTRST = 0x10;

-		nTRSTnOE = 0x10;

-		nSRST = 0x40;

-		nSRSTnOE = 0x40;

-	}

-	else if (strcmp(ft2232_layout, "signalyzer") == 0)

-	{

-		nTRST = 0x10;

-		nTRSTnOE = 0x10;

-		nSRST = 0x20;

-		nSRSTnOE = 0x20;

-	}

-	else if (strcmp(ft2232_layout, "evb_lm3s811") == 0)

-	{

-		nTRST = 0x0;

-		nTRSTnOE = 0x00;

-		nSRST = 0x20;

-		nSRSTnOE = 0x20;

-		low_output = 0x88;

-		low_direction = 0x8b;

-	}

-	else

-	{

-		ERROR("BUG: usbjtag_init called for unknown layout '%s'", ft2232_layout);

-		return ERROR_JTAG_INIT_FAILED;	

-	}

-	

-	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 */

-	}

-	

-	/* 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 */

-	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))

-	{

-		ERROR("couldn't initialize FT2232 with 'USBJTAG' layout"); 

-		return ERROR_JTAG_INIT_FAILED;

-	}

-

-	return ERROR_OK;

-}

-

-int jtagkey_init(void)

-{

-	u8 buf[3];

-	u32 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 */

-	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))

-	{

-		ERROR("couldn't initialize FT2232 with 'JTAGkey' layout"); 

-		return ERROR_JTAG_INIT_FAILED;

-	}

-	

-	if (strcmp(layout->name, "jtagkey") == 0)

-	{

-		nTRST = 0x01;

-		nTRSTnOE = 0x4;

-		nSRST = 0x02;

-		nSRSTnOE = 0x08;

-	}

-	else if ((strcmp(layout->name, "jtagkey_prototype_v1") == 0) ||

-		(strcmp(layout->name, "oocdlink") == 0))

-	{

-		nTRST = 0x02;

-		nTRSTnOE = 0x1;

-		nSRST = 0x08;

-		nSRSTnOE = 0x04;

-	}

-	else

-	{

-		ERROR("BUG: jtagkey_init called for non jtagkey layout");

-		exit(-1);

-	}

-	

-	high_output = 0x0;

-	high_direction = 0x0f;

-

-	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;   /* all outputs (xRST and xRSTnOE) */

-	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))

-	{

-		ERROR("couldn't initialize FT2232 with 'JTAGkey' layout"); 

-		return ERROR_JTAG_INIT_FAILED;

-	}

-	

-	return ERROR_OK;

-}

-

-int olimex_jtag_init(void)

-{

-	u8 buf[3];

-	u32 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 */

-	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))

-	{

-		ERROR("couldn't initialize FT2232 with 'JTAGkey' layout"); 

-		return ERROR_JTAG_INIT_FAILED;

-	}

-	

-	nTRST = 0x01;

-	nTRSTnOE = 0x4;

-	nSRST = 0x02;

-	nSRSTnOE = 0x00; /* no output enable for nSRST */

-

-	high_output = 0x0;

-	high_direction = 0x0f;

-

-	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)

-	{

-		ERROR("can't set nSRST to push-pull on the Olimex ARM-USB-OCD");

-	}

-	else

-	{

-		high_output &= ~nSRST;

-	}

-	

-	/* turn red LED on */

-	high_output |= 0x08;

-	

-	/* initialize high port */

-	buf[0] = 0x82; /* command "set data bits high byte" */

-	buf[1] = high_output; /* value */

-	buf[2] = high_direction;   /* all outputs (xRST and xRSTnOE) */

-	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))

-	{

-		ERROR("couldn't initialize FT2232 with 'JTAGkey' layout"); 

-		return ERROR_JTAG_INIT_FAILED;

-	}

-	

-	return ERROR_OK;

-}

-

-int flyswatter_init(void)

-{

-	u8 buf[3];

-	u32 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 */

-	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))

-	{

-		ERROR("couldn't initialize FT2232 with 'flyswatter' layout"); 

-		return ERROR_JTAG_INIT_FAILED;

-	}

-	

-	nTRST = 0x10;

-	nTRSTnOE = 0x0; /* not output enable for nTRST */

-	nSRST = 0x20;

-	nSRSTnOE = 0x00; /* no output enable for nSRST */

-

-	high_output = 0x00;

-	high_direction = 0x0c;

-

-	/* turn red LED1 on, LED2 off */

-	high_output |= 0x08;

-	

-	/* initialize high port */

-	buf[0] = 0x82; /* command "set data bits high byte" */

-	buf[1] = high_output; /* value */

-	buf[2] = high_direction;   /* all outputs (xRST and xRSTnOE) */

-	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))

-	{

-		ERROR("couldn't initialize FT2232 with 'flyswatter' layout"); 

-		return ERROR_JTAG_INIT_FAILED;

-	}

-	

-	return ERROR_OK;

-}

-

-int turtle_init(void)

-{

-	u8 buf[3];

-	u32 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 */

-	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))

-	{

-		ERROR("couldn't initialize FT2232 with 'turtelizer2' layout"); 

-		return ERROR_JTAG_INIT_FAILED;

-	}

-	

-	nSRST = 0x40;

-  	

-	high_output = 0x00;

-	high_direction = 0x0C;

-	

-	/* initialize high port */

-	buf[0] = 0x82; /* command "set data bits high byte" */

-	buf[1] = high_output;

-	buf[2] = high_direction;

-	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))

-	{

-		ERROR("couldn't initialize FT2232 with 'turtelizer2' layout"); 

-		return ERROR_JTAG_INIT_FAILED;

-	}

-	

-	return ERROR_OK;

-}

-

-int comstick_init(void)

-{

-	u8 buf[3];

-	u32 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 */

-	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))

-	{

-		ERROR("couldn't initialize FT2232 with 'comstick' 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;

-	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))

-	{

-		ERROR("couldn't initialize FT2232 with 'comstick' layout"); 

-		return ERROR_JTAG_INIT_FAILED;

-	}

-	

-	return ERROR_OK;

-}

-

-int stm32stick_init(void)

-{

-	u8 buf[3];

-	u32 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 */

-	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))

-	{

-		ERROR("couldn't initialize FT2232 with 'stm32stick' layout"); 

-		return ERROR_JTAG_INIT_FAILED;

-	}

-		

-	nTRST = 0x01;

-	nTRSTnOE = 0x00; /* no output enable for nTRST */

-	nSRST = 0x80;

-	nSRSTnOE = 0x00; /* no output enable for nSRST */

-  	

-	high_output = 0x01;

-	high_direction = 0x03;

-	

-	/* initialize high port */

-	buf[0] = 0x82; /* command "set data bits high byte" */

-	buf[1] = high_output;

-	buf[2] = high_direction;

-	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))

-	{

-		ERROR("couldn't initialize FT2232 with 'stm32stick' layout"); 

-		return ERROR_JTAG_INIT_FAILED;

-	}

-	

-	return ERROR_OK;

-}

-

-void olimex_jtag_blink(void)

-{

-	/* Olimex ARM-USB-OCD has a LED connected to ACBUS3

-	 * ACBUS3 is bit 3 of the GPIOH port

-	 */

-	if (high_output & 0x08)

-	{

-		/* set port pin high */

-		high_output &= 0x07;

-	}

-	else

-	{

-		/* set port pin low */

-		high_output |= 0x08;

-	}

-	

-	BUFFER_ADD = 0x82;

-	BUFFER_ADD = high_output;

-	BUFFER_ADD = high_direction;

-}

-

-void turtle_jtag_blink(void)

-{

-	/* 

-   * Turtelizer2 has two LEDs connected to ACBUS2 and ACBUS3

-	 */

-	if (high_output & 0x08)

-	{

-		high_output = 0x04;

-	}

-	else

-	{

-		high_output = 0x08;

-	}

-	

-	BUFFER_ADD = 0x82;

-	BUFFER_ADD = high_output;

-	BUFFER_ADD = high_direction;

-}

-

-

-int ft2232_quit(void)

-{

-#if BUILD_FT2232_FTD2XX == 1

-	FT_STATUS status;

-

-	status = FT_Close(ftdih);

-#elif BUILD_FT2232_LIBFTDI == 1

-	ftdi_disable_bitbang(&ftdic);

-	

-	ftdi_usb_close(&ftdic);

-	

-	ftdi_deinit(&ftdic);

-#endif

-

-	free(ft2232_buffer);

-	ft2232_buffer = NULL;

-

-	return ERROR_OK;

-}

-

-int ft2232_handle_device_desc_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

-{

-	if (argc == 1)

-	{

-		ft2232_device_desc = strdup(args[0]);

-	}

-	else

-	{

-		ERROR("expected exactly one argument to ft2232_device_desc <description>");

-	}

-	

-	return ERROR_OK;

-}

-

-int ft2232_handle_serial_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

-{

-	if (argc == 1)

-	{

-		ft2232_serial = strdup(args[0]);

-	}

-	else

-	{

-		ERROR("expected exactly one argument to ft2232_serial <serial-number>");

-	}

-	

-	return ERROR_OK;

-}

-

-int ft2232_handle_layout_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

-{

-	if (argc == 0)

-		return ERROR_OK;

-

-	ft2232_layout = malloc(strlen(args[0]) + 1);

-	strcpy(ft2232_layout, args[0]);

-

-	return ERROR_OK;

-}

-

-int ft2232_handle_vid_pid_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

-{

-	int i;

-

-	if (argc > MAX_USB_IDS*2) {

-		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))

-	{

-		WARNING("incomplete ft2232_vid_pid configuration directive");

-		if (argc < 2)

-			return ERROR_OK;

-	}

-

-	for (i = 0; i+1 < argc; i += 2) {

-		ft2232_vid[i >> 1] = strtol(args[i], NULL, 0);

-		ft2232_pid[i >> 1] = strtol(args[i+1], NULL, 0);

-	}

-	/*

-	 * Explicitly terminate, in case there are multiples instances of

-	 * ft2232_vid_pid.

-	 */

-	ft2232_vid[i >> 1] = ft2232_pid[i >> 1] = 0;

-

-	return ERROR_OK;

-}

-

-int ft2232_handle_latency_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

-{

-	if (argc == 1)

-	{

-		ft2232_latency = atoi(args[0]);

-	}

-	else

-	{

-		ERROR("expected exactly one argument to ft2232_latency <ms>");

-	}

-	

-	return ERROR_OK;

-}

-

-

+/***************************************************************************
+ *   Copyright (C) 2004, 2006 by Dominic Rath                              *
+ *   Dominic.Rath@gmx.de                                                   *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#if IS_CYGWIN == 1
+#include "windows.h"
+#undef ERROR
+#endif
+
+#include "replacements.h"
+
+/* project specific includes */
+#include "log.h"
+#include "types.h"
+#include "jtag.h"
+#include "configuration.h"
+#include "time_support.h"
+
+/* system includes */
+#include <string.h>
+#include <stdlib.h>
+#include <unistd.h>
+
+/* FT2232 access library includes */
+#if BUILD_FT2232_FTD2XX == 1
+#include <ftd2xx.h>
+#elif BUILD_FT2232_LIBFTDI == 1
+#include <ftdi.h>
+#endif
+
+#include <sys/time.h>
+#include <time.h>
+
+/* enable this to debug io latency
+ */
+#if 0
+#define _DEBUG_USB_IO_
+#endif
+
+/* enable this to debug communication
+ */
+#if 0
+#define _DEBUG_USB_COMMS_
+#endif
+
+int ft2232_execute_queue(void);
+
+int ft2232_speed(int speed);
+int ft2232_register_commands(struct command_context_s *cmd_ctx);
+int ft2232_init(void);
+int ft2232_quit(void);
+
+int ft2232_handle_device_desc_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int ft2232_handle_serial_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int ft2232_handle_layout_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int ft2232_handle_vid_pid_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int ft2232_handle_latency_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+
+char *ft2232_device_desc = NULL;
+char *ft2232_serial = NULL;
+char *ft2232_layout = NULL;
+unsigned char ft2232_latency = 2;
+
+#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 };
+
+typedef struct ft2232_layout_s
+{
+	char* name;
+	int(*init)(void);
+	void(*reset)(int trst, int srst);
+	void(*blink)(void);
+} ft2232_layout_t;
+
+/* init procedures for supported layouts */
+int usbjtag_init(void);
+int jtagkey_init(void);
+int olimex_jtag_init(void);
+int flyswatter_init(void);
+int turtle_init(void);
+int comstick_init(void);
+int stm32stick_init(void);
+
+/* reset procedures for supported layouts */
+void usbjtag_reset(int trst, int srst);
+void jtagkey_reset(int trst, int srst);
+void olimex_jtag_reset(int trst, int srst);
+void flyswatter_reset(int trst, int srst);
+void turtle_reset(int trst, int srst);
+void comstick_reset(int trst, int srst);
+void stm32stick_reset(int trst, int srst);
+
+/* blink procedures for layouts that support a blinking led */
+void olimex_jtag_blink(void);
+void turtle_jtag_blink(void);
+
+ft2232_layout_t ft2232_layouts[] =
+{
+	{"usbjtag", usbjtag_init, usbjtag_reset, NULL},
+	{"jtagkey", jtagkey_init, jtagkey_reset, NULL},
+	{"jtagkey_prototype_v1", jtagkey_init, jtagkey_reset, NULL},
+	{"oocdlink", jtagkey_init, jtagkey_reset, NULL},
+	{"signalyzer", usbjtag_init, usbjtag_reset, NULL},
+	{"evb_lm3s811", usbjtag_init, usbjtag_reset, NULL},
+	{"olimex-jtag", olimex_jtag_init, olimex_jtag_reset, olimex_jtag_blink},
+	{"flyswatter", flyswatter_init, flyswatter_reset, NULL},
+	{"turtelizer2", turtle_init, turtle_reset, turtle_jtag_blink},
+	{"comstick", comstick_init, comstick_reset, NULL},
+	{"stm32stick", stm32stick_init, stm32stick_reset, NULL},
+	{NULL, NULL, NULL},
+};
+
+static u8 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;
+
+#if BUILD_FT2232_FTD2XX == 1
+static FT_HANDLE ftdih = NULL;
+#elif BUILD_FT2232_LIBFTDI == 1
+static struct ftdi_context ftdic;
+#endif
+
+static u8 *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,
+	.register_commands = ft2232_register_commands,
+	.init = ft2232_init,
+	.quit = ft2232_quit,
+};
+
+int ft2232_write(u8 *buf, int size, u32* 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)
+	{
+		*bytes_written = dw_bytes_written;
+		ERROR("FT_Write returned: %lu", status);
+		return ERROR_JTAG_DEVICE_ERROR;
+	}
+	else
+	{
+		*bytes_written = dw_bytes_written;
+		return ERROR_OK;	
+	}
+#elif BUILD_FT2232_LIBFTDI == 1
+	int retval;
+	if ((retval = ftdi_write_data(&ftdic, buf, size)) < 0)
+	{
+		*bytes_written = 0;
+		ERROR("ftdi_write_data: %s", ftdi_get_error_string(&ftdic));
+		return ERROR_JTAG_DEVICE_ERROR;
+	}
+	else
+	{
+		*bytes_written = retval;
+		return ERROR_OK;	
+	}
+#endif
+}
+
+int ft2232_read(u8* buf, int size, u32* bytes_read)
+{
+#if BUILD_FT2232_FTD2XX == 1
+	DWORD dw_bytes_read;
+	FT_STATUS status;
+	int timeout = 5;
+	*bytes_read = 0;
+
+	while ((*bytes_read < size) && timeout--)
+	{
+		if ((status = FT_Read(ftdih, buf + *bytes_read, size - 
+			*bytes_read, &dw_bytes_read)) != FT_OK)		
+		{
+			*bytes_read = 0; 
+			ERROR("FT_Read returned: %lu", status);
+			return ERROR_JTAG_DEVICE_ERROR;
+		}
+		*bytes_read += dw_bytes_read; 
+	}
+#elif BUILD_FT2232_LIBFTDI == 1
+	int retval;
+	int timeout = 100;
+	*bytes_read = 0;
+	
+	while ((*bytes_read < size) && timeout--)
+	{
+		if ((retval = ftdi_read_data(&ftdic, buf + *bytes_read, size - *bytes_read)) < 0)
+		{
+			*bytes_read = 0;
+			ERROR("ftdi_read_data: %s", ftdi_get_error_string(&ftdic));
+			return ERROR_JTAG_DEVICE_ERROR;
+		}
+		*bytes_read += retval;
+	}
+#endif
+
+	if (*bytes_read < size)
+	{
+		ERROR("couldn't read the requested number of bytes from FT2232 device (%i < %i)", *bytes_read, size);
+		return ERROR_JTAG_DEVICE_ERROR;
+	}
+	
+	return ERROR_OK;
+}
+
+int ft2232_speed(int speed)
+{
+	u8 buf[3];
+	int retval;
+	u32 bytes_written;
+
+	buf[0] = 0x86; /* command "set divisor" */
+	buf[1] = speed & 0xff; /* valueL (0=6MHz, 1=3MHz, 2=2.0MHz, ...*/
+	buf[2] = (speed >> 8) & 0xff; /* valueH */
+	
+	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))
+	{
+		ERROR("couldn't set FT2232 TCK speed");
+		return retval;
+	}
+
+	jtag_speed = speed;
+	
+	return ERROR_OK;
+}
+
+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, NULL);
+	register_command(cmd_ctx, NULL, "ft2232_serial", ft2232_handle_serial_command,
+		COMMAND_CONFIG, NULL);
+	register_command(cmd_ctx, NULL, "ft2232_layout", ft2232_handle_layout_command,
+		COMMAND_CONFIG, NULL);
+	register_command(cmd_ctx, NULL, "ft2232_vid_pid", ft2232_handle_vid_pid_command,
+					 COMMAND_CONFIG, NULL);
+	register_command(cmd_ctx, NULL, "ft2232_latency", ft2232_handle_latency_command,
+					 COMMAND_CONFIG, NULL);
+	return ERROR_OK;
+}
+
+void ft2232_end_state(enum tap_state state)
+{
+	if (tap_move_map[state] != -1)
+		end_state = state;
+	else
+	{
+		ERROR("BUG: %i is not a valid end state", state);
+		exit(-1);
+	}
+}
+
+void ft2232_read_scan(enum scan_type type, u8* buffer, int scan_size)
+{
+	int num_bytes = ((scan_size + 7) / 8);
+	int bits_left = scan_size;
+	int cur_byte = 0;
+
+	while(num_bytes-- > 1)
+	{
+		buffer[cur_byte] = BUFFER_READ;
+		cur_byte++;
+		bits_left -= 8;
+	}
+
+	buffer[cur_byte] = 0x0;
+
+	if (bits_left > 1)
+	{
+		buffer[cur_byte] = BUFFER_READ >> 1;
+	}
+
+	buffer[cur_byte] = (buffer[cur_byte] | ((BUFFER_READ & 0x02) << 6)) >> (8 - bits_left);
+
+}
+
+void ft2232_debug_dump_buffer(void)
+{
+	int i;
+	char line[256];
+	char *line_p = line;
+	
+	for (i = 0; i < ft2232_buffer_size; i++)
+	{
+		line_p += snprintf(line_p, 256 - (line_p - line), "%2.2x ", ft2232_buffer[i]);
+		if (i % 16 == 15)
+		{
+			DEBUG("%s", line);
+			line_p = line;
+		}
+	}
+	
+	if (line_p != line)
+		DEBUG("%s", line);
+}
+
+int ft2232_send_and_recv(jtag_command_t *first, jtag_command_t *last)
+{
+	jtag_command_t *cmd;
+	u8 *buffer;
+	int scan_size;
+	enum scan_type type;
+	int retval;
+	u32 bytes_written;
+	u32 bytes_read;
+	
+#ifdef _DEBUG_USB_IO_
+	struct timeval start, inter, inter2, end;
+	struct timeval d_inter, d_inter2, d_end;
+#endif
+
+#ifdef _DEBUG_USB_COMMS_
+	DEBUG("write buffer (size %i):", ft2232_buffer_size);
+	ft2232_debug_dump_buffer();
+#endif
+
+#ifdef _DEBUG_USB_IO_
+	gettimeofday(&start, NULL);	
+#endif
+
+	if ((retval = ft2232_write(ft2232_buffer, ft2232_buffer_size, &bytes_written)) != ERROR_OK)
+	{
+		ERROR("couldn't write MPSSE commands to FT2232");
+		exit(-1);
+	}
+	
+#ifdef _DEBUG_USB_IO_
+	gettimeofday(&inter, NULL);	
+#endif
+	
+	if (ft2232_expect_read)
+	{
+		int timeout = 100;
+		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)
+		{
+			ERROR("couldn't read from FT2232");
+			exit(-1);
+		}
+		
+#ifdef _DEBUG_USB_IO_
+		gettimeofday(&end, NULL);	
+
+		timeval_subtract(&d_inter, &inter, &start);
+		timeval_subtract(&d_inter2, &inter2, &start);
+		timeval_subtract(&d_end, &end, &start);
+
+		INFO("inter: %i.%i, inter2: %i.%i end: %i.%i", d_inter.tv_sec, d_inter.tv_usec, d_inter2.tv_sec, d_inter2.tv_usec, d_end.tv_sec, d_end.tv_usec);
+#endif
+	
+		
+		ft2232_buffer_size = bytes_read;
+		
+		if (ft2232_expect_read != ft2232_buffer_size)
+		{
+			ERROR("ft2232_expect_read (%i) != ft2232_buffer_size (%i) (%i retries)", ft2232_expect_read, ft2232_buffer_size, 100 - timeout);
+			ft2232_debug_dump_buffer();	
+
+			exit(-1);
+		}
+
+#ifdef _DEBUG_USB_COMMS_
+		DEBUG("read buffer (%i retries): %i bytes", 100 - timeout, ft2232_buffer_size);
+		ft2232_debug_dump_buffer();
+#endif
+	}
+
+	ft2232_expect_read = 0;
+	ft2232_read_pointer = 0;
+	
+	/* return ERROR_OK, unless a jtag_read_buffer returns a failed check
+	 * that wasn't handled by a caller-provided error handler
+	 */ 
+	retval = ERROR_OK;
+	
+	cmd = first;
+	while (cmd != last)
+	{
+		switch (cmd->type)
+		{
+			case JTAG_SCAN:
+				type = jtag_scan_type(cmd->cmd.scan);
+				if (type != SCAN_OUT)
+				{
+					scan_size = jtag_scan_size(cmd->cmd.scan);
+					buffer = calloc(CEIL(scan_size, 8), 1);
+					ft2232_read_scan(type, buffer, scan_size);
+					if (jtag_read_buffer(buffer, cmd->cmd.scan) != ERROR_OK)
+						retval = ERROR_JTAG_QUEUE_FAILED;
+					free(buffer);
+				}
+				break;
+			default:
+				break;
+		}
+		cmd = cmd->next;
+	}
+	
+	ft2232_buffer_size = 0;
+
+	return retval;
+}
+
+void ft2232_add_pathmove(pathmove_command_t *cmd)
+{
+	int num_states = cmd->num_states;
+	u8 tms_byte;
+	int state_count;
+
+	state_count = 0;
+	while (num_states)
+	{
+		tms_byte = 0x0;
+		int bit_count = 0;
+		
+		/* command "Clock Data to TMS/CS Pin (no Read)" */
+		BUFFER_ADD = 0x4b;
+		/* number of states remaining */
+		BUFFER_ADD = (num_states % 7) - 1;
+		
+		while (num_states % 7)
+		{
+			if (tap_transitions[cur_state].low == cmd->path[state_count])
+				buf_set_u32(&tms_byte, bit_count++, 1, 0x0);
+			else if (tap_transitions[cur_state].high == cmd->path[state_count])
+				buf_set_u32(&tms_byte, bit_count++, 1, 0x1);
+			else
+			{
+				ERROR("BUG: %s -> %s isn't a valid TAP transition", tap_state_strings[cur_state], tap_state_strings[cmd->path[state_count]]);
+				exit(-1);
+			}
+
+			cur_state = cmd->path[state_count];
+			state_count++;
+			num_states--;
+		}
+		
+		BUFFER_ADD = tms_byte;
+	}
+	
+	end_state = cur_state;
+}
+
+void ft2232_add_scan(int ir_scan, enum scan_type type, u8 *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 && (cur_state == TAP_SD)) || (ir_scan && (cur_state == TAP_SI))))
+	{
+		/* command "Clock Data to TMS/CS Pin (no Read)" */
+		BUFFER_ADD = 0x4b;
+		/* scan 7 bit */
+		BUFFER_ADD = 0x6;
+		/* TMS data bits */
+		if (ir_scan)
+		{
+			BUFFER_ADD = TAP_MOVE(cur_state, TAP_SI);
+			cur_state = TAP_SI;
+		}
+		else
+		{
+			BUFFER_ADD = TAP_MOVE(cur_state, TAP_SD);
+			cur_state = TAP_SD;
+		}
+		//DEBUG("added TMS scan (no read)");
+	}
+	
+	/* add command for complete bytes */
+	while (num_bytes > 1)
+	{
+		int thisrun_bytes;
+		if (type == SCAN_IO)
+		{
+			/* Clock Data Bytes In and Out LSB First */
+			BUFFER_ADD = 0x39;
+			//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;
+			//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;
+			//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;
+		if (type != SCAN_IN)
+		{
+			/* add complete bytes */
+			while(thisrun_bytes-- > 0)
+			{
+				BUFFER_ADD = buffer[cur_byte];
+				cur_byte++;
+				bits_left -= 8;
+			}
+		}
+		else /* (type == SCAN_IN) */
+		{
+			bits_left -= 8 * (thisrun_bytes);
+		}
+	}
+	
+	/* the most signifcant bit is scanned during TAP movement */
+	if (type != SCAN_IN)
+		last_bit = (buffer[cur_byte] >> (bits_left - 1)) & 0x1;
+	else
+		last_bit = 0;
+
+	/* process remaining bits but the last one */
+	if (bits_left > 1)
+	{
+		if (type == SCAN_IO)
+		{
+			/* Clock Data Bits In and Out LSB First */
+			BUFFER_ADD = 0x3b;
+			//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;
+			//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;
+			//DEBUG("added TDI bits (i %i)", bits_left - 1);
+		}
+		BUFFER_ADD = bits_left - 2;
+		if (type != SCAN_IN)
+			BUFFER_ADD = buffer[cur_byte];
+	}
+
+	if ((ir_scan && (end_state == TAP_SI)) ||
+		(!ir_scan && (end_state == TAP_SD)))
+	{
+		if (type == SCAN_IO)
+		{
+			/* Clock Data Bits In and Out LSB First */
+			BUFFER_ADD = 0x3b;
+			//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;
+			//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;
+			//DEBUG("added TDI bits (i %i)", bits_left - 1);
+		}
+		BUFFER_ADD = 0x0;
+		BUFFER_ADD = last_bit;
+	}
+	else
+	{
+		/* move from Shift-IR/DR to end state */
+		if (type != SCAN_OUT)
+		{
+			/* Clock Data to TMS/CS Pin with Read */
+			BUFFER_ADD = 0x6b;
+			//DEBUG("added TMS scan (read)");
+		}
+		else
+		{
+			/* Clock Data to TMS/CS Pin (no Read) */
+			BUFFER_ADD = 0x4b;
+			//DEBUG("added TMS scan (no read)");
+		}
+		BUFFER_ADD = 0x6;
+		BUFFER_ADD = TAP_MOVE(cur_state, end_state) | (last_bit << 7);
+		cur_state = end_state;
+	}
+}
+
+int ft2232_large_scan(scan_command_t *cmd, enum scan_type type, u8 *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;
+	int retval;
+	int thisrun_read = 0;
+	
+	if (cmd->ir_scan)
+	{
+		ERROR("BUG: large IR scans are not supported");
+		exit(-1);
+	}
+
+	if (cur_state != TAP_SD)
+	{
+		/* command "Clock Data to TMS/CS Pin (no Read)" */
+		BUFFER_ADD = 0x4b;
+		/* scan 7 bit */
+		BUFFER_ADD = 0x6;
+		/* TMS data bits */
+		BUFFER_ADD = TAP_MOVE(cur_state, TAP_SD);
+		cur_state = TAP_SD;
+	}
+	
+	if ((retval = ft2232_write(ft2232_buffer, ft2232_buffer_size, &bytes_written)) != ERROR_OK)
+	{
+		ERROR("couldn't write MPSSE commands to FT2232");
+		exit(-1);
+	}
+	DEBUG("ft2232_buffer_size: %i, bytes_written: %i", ft2232_buffer_size, bytes_written);
+	ft2232_buffer_size = 0;
+	
+	/* add command for complete bytes */
+	while (num_bytes > 1)
+	{
+		int thisrun_bytes;
+		
+		if (type == SCAN_IO)
+		{
+			/* Clock Data Bytes In and Out LSB First */
+			BUFFER_ADD = 0x39;
+			//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;
+			//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;
+			//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;
+		if (type != SCAN_IN)
+		{
+			/* add complete bytes */
+			while(thisrun_bytes-- > 0)
+			{
+				BUFFER_ADD = buffer[cur_byte];
+				cur_byte++;
+				bits_left -= 8;
+			}
+		}
+		else /* (type == SCAN_IN) */
+		{
+			bits_left -= 8 * (thisrun_bytes);
+		}
+
+		if ((retval = ft2232_write(ft2232_buffer, ft2232_buffer_size, &bytes_written)) != ERROR_OK)
+		{
+			ERROR("couldn't write MPSSE commands to FT2232");
+			exit(-1);
+		}
+		DEBUG("ft2232_buffer_size: %i, bytes_written: %i", ft2232_buffer_size, bytes_written);
+		ft2232_buffer_size = 0;
+		
+		if (type != SCAN_OUT)
+		{
+			if ((retval = ft2232_read(receive_pointer, thisrun_read, &bytes_read)) != ERROR_OK)
+			{
+				ERROR("couldn't read from FT2232");
+				exit(-1);
+			}
+			DEBUG("thisrun_read: %i, bytes_read: %i", thisrun_read, bytes_read);
+			receive_pointer += bytes_read;
+		}
+	}
+	
+	thisrun_read = 0;
+	
+	/* the most signifcant bit is scanned during TAP movement */
+	if (type != SCAN_IN)
+		last_bit = (buffer[cur_byte] >> (bits_left - 1)) & 0x1;
+	else
+		last_bit = 0;
+
+	/* process remaining bits but the last one */
+	if (bits_left > 1)
+	{
+		if (type == SCAN_IO)
+		{
+			/* Clock Data Bits In and Out LSB First */
+			BUFFER_ADD = 0x3b;
+			//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;
+			//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;
+			//DEBUG("added TDI bits (i %i)", bits_left - 1);
+		}
+		BUFFER_ADD = bits_left - 2;
+		if (type != SCAN_IN)
+			BUFFER_ADD = buffer[cur_byte];
+			
+		if (type != SCAN_OUT)
+			thisrun_read += 2;
+	}
+
+	if (end_state == TAP_SD)
+	{
+		if (type == SCAN_IO)
+		{
+			/* Clock Data Bits In and Out LSB First */
+			BUFFER_ADD = 0x3b;
+			//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;
+			//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;
+			//DEBUG("added TDI bits (i %i)", bits_left - 1);
+		}
+		BUFFER_ADD = 0x0;
+		BUFFER_ADD = last_bit;
+	}
+	else
+	{
+		/* move from Shift-IR/DR to end state */
+		if (type != SCAN_OUT)
+		{
+			/* Clock Data to TMS/CS Pin with Read */
+			BUFFER_ADD = 0x6b;
+			//DEBUG("added TMS scan (read)");
+		}
+		else
+		{
+			/* Clock Data to TMS/CS Pin (no Read) */
+			BUFFER_ADD = 0x4b;
+			//DEBUG("added TMS scan (no read)");
+		}
+		BUFFER_ADD = 0x6;
+		BUFFER_ADD = TAP_MOVE(cur_state, end_state) | (last_bit << 7);
+		cur_state = end_state;
+	}
+	
+	if (type != SCAN_OUT)
+		thisrun_read += 1;
+	
+	if ((retval = ft2232_write(ft2232_buffer, ft2232_buffer_size, &bytes_written)) != ERROR_OK)
+	{
+		ERROR("couldn't write MPSSE commands to FT2232");
+		exit(-1);
+	}
+	DEBUG("ft2232_buffer_size: %i, bytes_written: %i", ft2232_buffer_size, bytes_written);
+	ft2232_buffer_size = 0;
+	
+	if (type != SCAN_OUT)
+	{
+		if ((retval = ft2232_read(receive_pointer, thisrun_read, &bytes_read)) != ERROR_OK)
+		{
+			ERROR("couldn't read from FT2232");
+			exit(-1);
+		}
+		DEBUG("thisrun_read: %i, bytes_read: %i", thisrun_read, bytes_read);
+		receive_pointer += bytes_read;
+	}
+	
+	return ERROR_OK;
+}
+
+int ft2232_predict_scan_out(int scan_size, enum scan_type type)
+{
+	int predicted_size = 3;
+	int num_bytes = (scan_size - 1) / 8;
+	
+	if (cur_state != TAP_SD)
+		predicted_size += 3;
+	
+	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;
+	}
+	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;
+	}
+
+	return predicted_size;
+}
+
+int ft2232_predict_scan_in(int scan_size, enum scan_type type)
+{
+	int predicted_size = 0;
+	
+	if (type != SCAN_OUT)
+	{
+		/* complete bytes */
+		predicted_size += (CEIL(scan_size, 8) > 1) ? (CEIL(scan_size, 8) - 1) : 0;
+		/* remaining bits - 1 */
+		predicted_size += ((scan_size - 1) % 8) ? 1 : 0;
+		/* last bit (from TMS scan) */
+		predicted_size += 1;
+	}
+	
+	//DEBUG("scan_size: %i, predicted_size: %i", scan_size, predicted_size);
+
+	return predicted_size;
+}
+
+void usbjtag_reset(int trst, int srst)
+{
+	if (trst == 1)
+	{
+		cur_state = TAP_TLR;
+		if (jtag_reset_config & RESET_TRST_OPEN_DRAIN)
+			low_direction |= nTRSTnOE;	/* switch to output pin (output is low) */
+		else
+			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) */
+		else
+			low_output |= nTRST; /* switch output high */
+	}
+
+	if (srst == 1)
+	{
+		if (jtag_reset_config & RESET_SRST_PUSH_PULL)
+			low_output &= ~nSRST;	/* switch output low */
+		else
+			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 */
+		else
+			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;
+
+}
+
+void jtagkey_reset(int trst, int srst)
+{
+	if (trst == 1)
+	{
+		cur_state = TAP_TLR;
+		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_ADD = 0x82;
+	BUFFER_ADD = high_output;
+	BUFFER_ADD = high_direction;
+	DEBUG("trst: %i, srst: %i, high_output: 0x%2.2x, high_direction: 0x%2.2x", trst, srst, high_output, high_direction);
+}
+
+void olimex_jtag_reset(int trst, int srst)
+{
+	if (trst == 1)
+	{
+		cur_state = TAP_TLR;
+		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)
+    {
+        high_output |= nSRST;
+    }
+    else if (srst == 0)
+    {
+        high_output &= ~nSRST;
+    }
+
+    /* command "set data bits high byte" */
+    BUFFER_ADD = 0x82;
+    BUFFER_ADD = high_output;
+    BUFFER_ADD = high_direction;
+    DEBUG("trst: %i, srst: %i, high_output: 0x%2.2x, high_direction: 0x%2.2x", trst, srst, high_output, high_direction);
+}
+
+void flyswatter_reset(int trst, int srst)
+{
+	if (trst == 1)
+	{
+		cur_state = TAP_TLR;
+		low_output &= ~nTRST;
+	}
+	else if (trst == 0)
+	{
+		low_output |= nTRST;
+	}
+
+    if (srst == 1)
+    {
+        low_output |= nSRST;
+    }
+    else if (srst == 0)
+    {
+        low_output &= ~nSRST;
+    }
+
+    /* command "set data bits low byte" */
+    BUFFER_ADD = 0x80;
+    BUFFER_ADD = low_output;
+    BUFFER_ADD = low_direction;
+    DEBUG("trst: %i, srst: %i, low_output: 0x%2.2x, low_direction: 0x%2.2x", trst, srst, low_output, low_direction);
+}
+
+void turtle_reset(int trst, int srst)
+{
+	trst = trst;
+	
+	if (srst == 1)
+	{
+		low_output |= nSRST;
+	}
+	else if (srst == 0)
+	{
+		low_output &= ~nSRST;
+	}
+	
+	/* command "set data bits low byte" */
+	BUFFER_ADD = 0x80;
+	BUFFER_ADD = low_output;
+	BUFFER_ADD = low_direction;
+	DEBUG("srst: %i, low_output: 0x%2.2x, low_direction: 0x%2.2x", srst, low_output, low_direction);
+}
+
+void comstick_reset(int trst, int srst)
+{
+	if (trst == 1)
+	{
+		cur_state = TAP_TLR;
+		high_output &= ~nTRST;
+	}
+	else if (trst == 0)
+	{
+		high_output |= nTRST;
+	}
+
+    if (srst == 1)
+    {
+        high_output &= ~nSRST;
+    }
+    else if (srst == 0)
+    {
+        high_output |= nSRST;
+    }
+	
+	/* command "set data bits high byte" */
+	BUFFER_ADD = 0x82;
+	BUFFER_ADD = high_output;
+	BUFFER_ADD = high_direction;
+	DEBUG("trst: %i, srst: %i, high_output: 0x%2.2x, high_direction: 0x%2.2x", trst, srst, high_output, high_direction);
+}
+
+void stm32stick_reset(int trst, int srst)
+{
+	if (trst == 1)
+	{
+		cur_state = TAP_TLR;
+		high_output &= ~nTRST;
+	}
+	else if (trst == 0)
+	{
+		high_output |= nTRST;
+	}
+
+    if (srst == 1)
+    {
+        low_output &= ~nSRST;
+    }
+    else if (srst == 0)
+    {
+        low_output |= nSRST;
+    }
+	
+	/* command "set data bits low byte" */
+	BUFFER_ADD = 0x80;
+	BUFFER_ADD = low_output;
+	BUFFER_ADD = low_direction;
+	
+	/* command "set data bits high byte" */
+	BUFFER_ADD = 0x82;
+	BUFFER_ADD = high_output;
+	BUFFER_ADD = high_direction;
+	DEBUG("trst: %i, srst: %i, high_output: 0x%2.2x, high_direction: 0x%2.2x", trst, srst, high_output, high_direction);
+}
+
+int ft2232_execute_queue()
+{
+	jtag_command_t *cmd = jtag_command_queue; /* currently processed command */
+	jtag_command_t *first_unsent = cmd;	/* next command that has to be sent */
+	u8 *buffer;
+	int scan_size;	/* size of IR or DR scan */
+	enum scan_type type;
+	int i;
+	int predicted_size = 0;
+	int require_send = 0;
+	int retval;
+	
+	/* return ERROR_OK, unless ft2232_send_and_recv reports a failed check
+	 * that wasn't handled by a caller-provided error handler
+	 */ 
+	retval = ERROR_OK;
+
+	ft2232_buffer_size = 0;
+	ft2232_expect_read = 0;
+	
+	/* blink, if the current layout has that feature */
+	if (layout->blink)
+		layout->blink();
+
+	while (cmd)
+	{
+		switch(cmd->type)
+		{
+			case JTAG_END_STATE:
+				if (cmd->cmd.end_state->end_state != -1)
+					ft2232_end_state(cmd->cmd.end_state->end_state);
+				break;
+			case JTAG_RESET:
+				/* only send the maximum buffer size that FT2232C can handle */
+				predicted_size = 3;
+				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;
+					require_send = 0;
+					first_unsent = cmd;
+				}
+
+				layout->reset(cmd->cmd.reset->trst, cmd->cmd.reset->srst);
+				require_send = 1;
+				
+#ifdef _DEBUG_JTAG_IO_				
+				DEBUG("trst: %i, srst: %i", cmd->cmd.reset->trst, cmd->cmd.reset->srst);
+#endif
+				break;
+			case JTAG_RUNTEST:
+				/* only send the maximum buffer size that FT2232C can handle */
+				predicted_size = 0;
+				if (cur_state != TAP_RTI)
+					predicted_size += 3;
+				predicted_size += 3 * CEIL(cmd->cmd.runtest->num_cycles, 7);
+				if ((cmd->cmd.runtest->end_state != -1) && (cmd->cmd.runtest->end_state != TAP_RTI))
+					predicted_size += 3;
+				if ((cmd->cmd.runtest->end_state == -1) && (end_state != TAP_RTI))
+					predicted_size += 3;
+				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;
+					require_send = 0;
+					first_unsent = cmd;
+				}
+				if (cur_state != TAP_RTI)
+				{
+					/* command "Clock Data to TMS/CS Pin (no Read)" */
+					BUFFER_ADD = 0x4b;
+					/* scan 7 bit */
+					BUFFER_ADD = 0x6;
+					/* TMS data bits */
+					BUFFER_ADD = TAP_MOVE(cur_state, TAP_RTI);
+					cur_state = TAP_RTI;
+					require_send = 1;
+				}
+				i = cmd->cmd.runtest->num_cycles;
+				while (i > 0)
+				{
+					/* command "Clock Data to TMS/CS Pin (no Read)" */
+					BUFFER_ADD = 0x4b;
+					/* scan 7 bit */
+					BUFFER_ADD = (i > 7) ? 6 : (i - 1);
+					/* TMS data bits */
+					BUFFER_ADD = 0x0;
+					cur_state = TAP_RTI;
+					i -= (i > 7) ? 7 : i;
+					//DEBUG("added TMS scan (no read)");
+				}
+				if (cmd->cmd.runtest->end_state != -1)
+					ft2232_end_state(cmd->cmd.runtest->end_state);
+				if (cur_state != 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_MOVE(cur_state, end_state);
+					cur_state = end_state;
+					//DEBUG("added TMS scan (no read)");
+				}
+				require_send = 1;
+#ifdef _DEBUG_JTAG_IO_				
+				DEBUG("runtest: %i, end in %i", cmd->cmd.runtest->num_cycles, end_state);
+#endif
+				break;
+			case JTAG_STATEMOVE:
+				/* only send the maximum buffer size that FT2232C can handle */
+				predicted_size = 3;
+				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;
+					require_send = 0;
+					first_unsent = cmd;
+				}
+				if (cmd->cmd.statemove->end_state != -1)
+					ft2232_end_state(cmd->cmd.statemove->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_MOVE(cur_state, end_state);
+				//DEBUG("added TMS scan (no read)");
+				cur_state = end_state;
+				require_send = 1;
+#ifdef _DEBUG_JTAG_IO_				
+				DEBUG("statemove: %i", end_state);
+#endif
+				break;
+			case JTAG_PATHMOVE:
+				/* only send the maximum buffer size that FT2232C can handle */
+				predicted_size = 3 * CEIL(cmd->cmd.pathmove->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;
+					require_send = 0;
+					first_unsent = cmd;
+				}
+				ft2232_add_pathmove(cmd->cmd.pathmove);
+				require_send = 1;
+#ifdef _DEBUG_JTAG_IO_				
+				DEBUG("pathmove: %i states, end in %i", cmd->cmd.pathmove->num_states, cmd->cmd.pathmove->path[cmd->cmd.pathmove->num_states - 1]);
+#endif
+				break;
+			case JTAG_SCAN:
+				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)
+				{
+					DEBUG("oversized ft2232 scan (predicted_size > FT2232_BUFFER_SIZE)");
+					/* unsent commands before this */
+					if (first_unsent != cmd)
+						if (ft2232_send_and_recv(first_unsent, cmd) != ERROR_OK)
+							retval = ERROR_JTAG_QUEUE_FAILED;
+					
+					/* current command */
+					if (cmd->cmd.scan->end_state != -1)
+						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;
+					if (buffer)
+						free(buffer);
+					break;
+				}
+				else if (ft2232_buffer_size + predicted_size + 1 > FT2232_BUFFER_SIZE)
+				{
+					DEBUG("ft2232 buffer size reached, sending queued commands (first_unsent: %p, cmd: %p)", first_unsent, cmd);
+					if (ft2232_send_and_recv(first_unsent, cmd) != ERROR_OK)
+						retval = ERROR_JTAG_QUEUE_FAILED;
+					require_send = 0;
+					first_unsent = cmd;
+				}
+				ft2232_expect_read += ft2232_predict_scan_in(scan_size, type);
+				//DEBUG("new read size: %i", ft2232_expect_read);
+				if (cmd->cmd.scan->end_state != -1)
+					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_				
+				DEBUG("%s scan, %i bit, end in %i", (cmd->cmd.scan->ir_scan) ? "IR" : "DR", scan_size, end_state);
+#endif
+				break;
+			case JTAG_SLEEP:
+				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_				
+				DEBUG("sleep %i usec", cmd->cmd.sleep->us);
+#endif
+				break;
+			default:
+				ERROR("BUG: unknown JTAG command type encountered");
+				exit(-1);
+		}
+		cmd = cmd->next;
+	}
+
+	if (require_send > 0)
+		if (ft2232_send_and_recv(first_unsent, cmd) != ERROR_OK)
+			retval = ERROR_JTAG_QUEUE_FAILED;
+
+	return retval;
+}
+
+#if BUILD_FT2232_FTD2XX == 1
+static int ft2232_init_ftd2xx(u16 vid, u16 pid, int more, int *try_more)
+{
+	FT_STATUS status;
+	DWORD openex_flags = 0;
+	char *openex_string = NULL;
+	u8 latency_timer;
+
+	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)
+	{
+		WARNING("couldn't add %4.4x:%4.4x",
+		    vid, pid);
+	}
+#endif
+
+	if (ft2232_device_desc && ft2232_serial)
+	{
+		WARNING("can't open by device description and serial number, giving precedence to serial");
+		ft2232_device_desc = NULL;
+	}
+	
+	if (ft2232_device_desc)
+	{
+		openex_string = ft2232_device_desc;
+		openex_flags = FT_OPEN_BY_DESCRIPTION;
+	}
+	else if (ft2232_serial)
+	{
+		openex_string = ft2232_serial;
+		openex_flags = FT_OPEN_BY_SERIAL_NUMBER;
+	}
+	else
+	{
+		ERROR("neither device description nor serial number specified");
+		ERROR("please add \"ft2232_device_desc <string>\" or \"ft2232_serial <string>\" to your .cfg file");
+		
+		return ERROR_JTAG_INIT_FAILED;	
+	}
+
+	if ((status = FT_OpenEx(openex_string, openex_flags, &ftdih)) != FT_OK)
+	{
+		DWORD num_devices;
+		
+		if (more) {
+			WARNING("unable to open ftdi device (trying more): %lu",
+			    status);
+			*try_more = 1;
+			return ERROR_JTAG_INIT_FAILED;
+		}
+		ERROR("unable to open ftdi device: %lu", status);
+		status = FT_ListDevices(&num_devices, NULL, FT_LIST_NUMBER_ONLY);
+		if (status == FT_OK)
+		{
+			char **desc_array = malloc(sizeof(char*) * (num_devices + 1));
+			int i;
+
+			for (i = 0; i < num_devices; i++)
+				desc_array[i] = malloc(64);
+			desc_array[num_devices] = NULL;
+
+			status = FT_ListDevices(desc_array, &num_devices, FT_LIST_ALL | openex_flags);
+
+			if (status == FT_OK)
+			{
+				ERROR("ListDevices: %lu\n", num_devices);
+				for (i = 0; i < num_devices; i++)
+					ERROR("%i: %s", i, desc_array[i]);
+			}
+			
+			for (i = 0; i < num_devices; i++)
+				free(desc_array[i]);
+			free(desc_array);
+		}
+		else
+		{
+			ERROR("ListDevices: NONE\n");
+		}
+		return ERROR_JTAG_INIT_FAILED;
+	}
+
+	if ((status = FT_SetLatencyTimer(ftdih, ft2232_latency)) != FT_OK)
+	{
+		ERROR("unable to set latency timer: %lu", status);
+		return ERROR_JTAG_INIT_FAILED;
+	}
+	
+	if ((status = FT_GetLatencyTimer(ftdih, &latency_timer)) != FT_OK)
+	{
+		ERROR("unable to get latency timer: %lu", status);
+		return ERROR_JTAG_INIT_FAILED;
+	}
+	else
+	{
+		DEBUG("current latency timer: %i", latency_timer);
+	}
+	
+	if ((status = FT_SetTimeouts(ftdih, 5000, 5000)) != FT_OK)
+	{
+		ERROR("unable to set timeouts: %lu", status);
+		return ERROR_JTAG_INIT_FAILED;
+	}
+
+	if ((status = FT_SetBitMode(ftdih, 0x0b, 2)) != FT_OK)
+	{
+		ERROR("unable to enable bit i/o mode: %lu", status);
+		return ERROR_JTAG_INIT_FAILED;
+	}
+
+	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)
+	{
+		ERROR("error purging ftd2xx device: %lu", status);
+		return ERROR_JTAG_INIT_FAILED;
+	}
+
+	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)
+{
+	u8 latency_timer;
+
+	DEBUG("'ft2232' interface using libftdi with '%s' layout (%4.4x:%4.4x)",
+	    ft2232_layout, vid, pid);
+
+	if (ftdi_init(&ftdic) < 0)
+		return ERROR_JTAG_INIT_FAILED;
+
+	/* context, vendor id, product id */
+	if (ftdi_usb_open_desc(&ftdic, vid, pid, ft2232_device_desc,
+	    ft2232_serial) < 0) {
+		if (more)
+			WARNING("unable to open ftdi device (trying more): %s",
+			     ftdic.error_str);
+		else
+			ERROR("unable to open ftdi device: %s", ftdic.error_str);
+		*try_more = 1;
+		return ERROR_JTAG_INIT_FAILED;
+	}
+
+	if (ftdi_set_interface(&ftdic, INTERFACE_A) < 0)
+	{
+		ERROR("unable to select FT2232 channel A: %s", ftdic.error_str);
+		return ERROR_JTAG_INIT_FAILED;
+	}
+
+	if (ftdi_usb_reset(&ftdic) < 0)
+	{
+		ERROR("unable to reset ftdi device");
+		return ERROR_JTAG_INIT_FAILED;
+	}
+
+	if (ftdi_set_latency_timer(&ftdic, ft2232_latency) < 0)
+	{
+		ERROR("unable to set latency timer");
+		return ERROR_JTAG_INIT_FAILED;
+	}
+	
+	if (ftdi_get_latency_timer(&ftdic, &latency_timer) < 0)
+	{
+		ERROR("unable to get latency timer");
+		return ERROR_JTAG_INIT_FAILED;
+	}
+	else
+	{
+		DEBUG("current latency timer: %i", latency_timer);
+	}
+
+	ftdi_set_bitmode(&ftdic, 0x0b, 2); /* ctx, JTAG I/O mask */
+
+	return ERROR_OK;
+}
+
+static int ft2232_purge_libftdi(void)
+{
+	if (ftdi_usb_purge_buffers(&ftdic) < 0)
+	{
+		ERROR("ftdi_purge_buffers: %s", ftdic.error_str);
+		return ERROR_JTAG_INIT_FAILED;
+	}
+
+	return ERROR_OK;
+}
+#endif /* BUILD_FT2232_LIBFTDI == 1 */
+
+int ft2232_init(void)
+{
+	u8 buf[1];
+	int retval;
+	u32 bytes_written;
+	ft2232_layout_t *cur_layout = ft2232_layouts;
+	int i;
+	
+	if ((ft2232_layout == NULL) || (ft2232_layout[0] == 0))
+	{
+		ft2232_layout = "usbjtag";
+		WARNING("No ft2232 layout specified, using default 'usbjtag'");
+	}
+	
+	while (cur_layout->name)
+	{
+		if (strcmp(cur_layout->name, ft2232_layout) == 0)
+		{
+			layout = cur_layout;
+			break;
+		}
+		cur_layout++;
+	}
+
+	if (!layout)
+	{
+		ERROR("No matching layout found for %s", ft2232_layout);
+		return ERROR_JTAG_INIT_FAILED;
+	}
+	
+	for (i = 0; 1; i++) {
+		/*
+		 * "more indicates that there are more IDs to try, so we should
+		 * not print an error for an ID mismatch (but for anything
+		 * else, we should).
+		 *
+		 * try_more indicates that the error code returned indicates an
+		 * ID mismatch (and nothing else) and that we should proceeed
+		 * with the next ID pair.
+		 */
+		int more = ft2232_vid[i+1] || ft2232_pid[i+1];
+		int try_more = 0;
+
+#if BUILD_FT2232_FTD2XX == 1
+		retval = ft2232_init_ftd2xx(ft2232_vid[i], ft2232_pid[i],
+		    more, &try_more);
+#elif BUILD_FT2232_LIBFTDI == 1
+		retval = ft2232_init_libftdi(ft2232_vid[i], ft2232_pid[i],
+		    more, &try_more);
+#endif	
+		if (retval >= 0)
+			break;
+		if (!more || !try_more)
+			return retval;
+	}
+
+	ft2232_buffer_size = 0;
+	ft2232_buffer = malloc(FT2232_BUFFER_SIZE);
+
+	if (layout->init() != ERROR_OK)
+		return ERROR_JTAG_INIT_FAILED;
+
+	ft2232_speed(jtag_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))
+	{
+		ERROR("couldn't write to FT2232 to disable loopback");
+		return ERROR_JTAG_INIT_FAILED;
+	}
+
+#if BUILD_FT2232_FTD2XX == 1
+	return ft2232_purge_ftd2xx();
+#elif BUILD_FT2232_LIBFTDI == 1
+	return ft2232_purge_libftdi();
+#endif	
+
+	return ERROR_OK;
+}
+
+int usbjtag_init(void)
+{
+	u8 buf[3];
+	u32 bytes_written;
+	
+	low_output = 0x08;
+	low_direction = 0x0b;
+	
+	if (strcmp(ft2232_layout, "usbjtag") == 0)
+	{
+		nTRST = 0x10;
+		nTRSTnOE = 0x10;
+		nSRST = 0x40;
+		nSRSTnOE = 0x40;
+	}
+	else if (strcmp(ft2232_layout, "signalyzer") == 0)
+	{
+		nTRST = 0x10;
+		nTRSTnOE = 0x10;
+		nSRST = 0x20;
+		nSRSTnOE = 0x20;
+	}
+	else if (strcmp(ft2232_layout, "evb_lm3s811") == 0)
+	{
+		nTRST = 0x0;
+		nTRSTnOE = 0x00;
+		nSRST = 0x20;
+		nSRSTnOE = 0x20;
+		low_output = 0x88;
+		low_direction = 0x8b;
+	}
+	else
+	{
+		ERROR("BUG: usbjtag_init called for unknown layout '%s'", ft2232_layout);
+		return ERROR_JTAG_INIT_FAILED;	
+	}
+	
+	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 */
+	}
+	
+	/* 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 */
+	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))
+	{
+		ERROR("couldn't initialize FT2232 with 'USBJTAG' layout"); 
+		return ERROR_JTAG_INIT_FAILED;
+	}
+
+	return ERROR_OK;
+}
+
+int jtagkey_init(void)
+{
+	u8 buf[3];
+	u32 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 */
+	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))
+	{
+		ERROR("couldn't initialize FT2232 with 'JTAGkey' layout"); 
+		return ERROR_JTAG_INIT_FAILED;
+	}
+	
+	if (strcmp(layout->name, "jtagkey") == 0)
+	{
+		nTRST = 0x01;
+		nTRSTnOE = 0x4;
+		nSRST = 0x02;
+		nSRSTnOE = 0x08;
+	}
+	else if ((strcmp(layout->name, "jtagkey_prototype_v1") == 0) ||
+		(strcmp(layout->name, "oocdlink") == 0))
+	{
+		nTRST = 0x02;
+		nTRSTnOE = 0x1;
+		nSRST = 0x08;
+		nSRSTnOE = 0x04;
+	}
+	else
+	{
+		ERROR("BUG: jtagkey_init called for non jtagkey layout");
+		exit(-1);
+	}
+	
+	high_output = 0x0;
+	high_direction = 0x0f;
+
+	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;   /* all outputs (xRST and xRSTnOE) */
+	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))
+	{
+		ERROR("couldn't initialize FT2232 with 'JTAGkey' layout"); 
+		return ERROR_JTAG_INIT_FAILED;
+	}
+	
+	return ERROR_OK;
+}
+
+int olimex_jtag_init(void)
+{
+	u8 buf[3];
+	u32 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 */
+	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))
+	{
+		ERROR("couldn't initialize FT2232 with 'JTAGkey' layout"); 
+		return ERROR_JTAG_INIT_FAILED;
+	}
+	
+	nTRST = 0x01;
+	nTRSTnOE = 0x4;
+	nSRST = 0x02;
+	nSRSTnOE = 0x00; /* no output enable for nSRST */
+
+	high_output = 0x0;
+	high_direction = 0x0f;
+
+	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)
+	{
+		ERROR("can't set nSRST to push-pull on the Olimex ARM-USB-OCD");
+	}
+	else
+	{
+		high_output &= ~nSRST;
+	}
+	
+	/* turn red LED on */
+	high_output |= 0x08;
+	
+	/* initialize high port */
+	buf[0] = 0x82; /* command "set data bits high byte" */
+	buf[1] = high_output; /* value */
+	buf[2] = high_direction;   /* all outputs (xRST and xRSTnOE) */
+	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))
+	{
+		ERROR("couldn't initialize FT2232 with 'JTAGkey' layout"); 
+		return ERROR_JTAG_INIT_FAILED;
+	}
+	
+	return ERROR_OK;
+}
+
+int flyswatter_init(void)
+{
+	u8 buf[3];
+	u32 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 */
+	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))
+	{
+		ERROR("couldn't initialize FT2232 with 'flyswatter' layout"); 
+		return ERROR_JTAG_INIT_FAILED;
+	}
+	
+	nTRST = 0x10;
+	nTRSTnOE = 0x0; /* not output enable for nTRST */
+	nSRST = 0x20;
+	nSRSTnOE = 0x00; /* no output enable for nSRST */
+
+	high_output = 0x00;
+	high_direction = 0x0c;
+
+	/* turn red LED1 on, LED2 off */
+	high_output |= 0x08;
+	
+	/* initialize high port */
+	buf[0] = 0x82; /* command "set data bits high byte" */
+	buf[1] = high_output; /* value */
+	buf[2] = high_direction;   /* all outputs (xRST and xRSTnOE) */
+	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))
+	{
+		ERROR("couldn't initialize FT2232 with 'flyswatter' layout"); 
+		return ERROR_JTAG_INIT_FAILED;
+	}
+	
+	return ERROR_OK;
+}
+
+int turtle_init(void)
+{
+	u8 buf[3];
+	u32 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 */
+	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))
+	{
+		ERROR("couldn't initialize FT2232 with 'turtelizer2' layout"); 
+		return ERROR_JTAG_INIT_FAILED;
+	}
+	
+	nSRST = 0x40;
+  	
+	high_output = 0x00;
+	high_direction = 0x0C;
+	
+	/* initialize high port */
+	buf[0] = 0x82; /* command "set data bits high byte" */
+	buf[1] = high_output;
+	buf[2] = high_direction;
+	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))
+	{
+		ERROR("couldn't initialize FT2232 with 'turtelizer2' layout"); 
+		return ERROR_JTAG_INIT_FAILED;
+	}
+	
+	return ERROR_OK;
+}
+
+int comstick_init(void)
+{
+	u8 buf[3];
+	u32 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 */
+	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))
+	{
+		ERROR("couldn't initialize FT2232 with 'comstick' 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;
+	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))
+	{
+		ERROR("couldn't initialize FT2232 with 'comstick' layout"); 
+		return ERROR_JTAG_INIT_FAILED;
+	}
+	
+	return ERROR_OK;
+}
+
+int stm32stick_init(void)
+{
+	u8 buf[3];
+	u32 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 */
+	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))
+	{
+		ERROR("couldn't initialize FT2232 with 'stm32stick' layout"); 
+		return ERROR_JTAG_INIT_FAILED;
+	}
+		
+	nTRST = 0x01;
+	nTRSTnOE = 0x00; /* no output enable for nTRST */
+	nSRST = 0x80;
+	nSRSTnOE = 0x00; /* no output enable for nSRST */
+  	
+	high_output = 0x01;
+	high_direction = 0x03;
+	
+	/* initialize high port */
+	buf[0] = 0x82; /* command "set data bits high byte" */
+	buf[1] = high_output;
+	buf[2] = high_direction;
+	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))
+	{
+		ERROR("couldn't initialize FT2232 with 'stm32stick' layout"); 
+		return ERROR_JTAG_INIT_FAILED;
+	}
+	
+	return ERROR_OK;
+}
+
+void olimex_jtag_blink(void)
+{
+	/* Olimex ARM-USB-OCD has a LED connected to ACBUS3
+	 * ACBUS3 is bit 3 of the GPIOH port
+	 */
+	if (high_output & 0x08)
+	{
+		/* set port pin high */
+		high_output &= 0x07;
+	}
+	else
+	{
+		/* set port pin low */
+		high_output |= 0x08;
+	}
+	
+	BUFFER_ADD = 0x82;
+	BUFFER_ADD = high_output;
+	BUFFER_ADD = high_direction;
+}
+
+void turtle_jtag_blink(void)
+{
+	/* 
+   * Turtelizer2 has two LEDs connected to ACBUS2 and ACBUS3
+	 */
+	if (high_output & 0x08)
+	{
+		high_output = 0x04;
+	}
+	else
+	{
+		high_output = 0x08;
+	}
+	
+	BUFFER_ADD = 0x82;
+	BUFFER_ADD = high_output;
+	BUFFER_ADD = high_direction;
+}
+
+
+int ft2232_quit(void)
+{
+#if BUILD_FT2232_FTD2XX == 1
+	FT_STATUS status;
+
+	status = FT_Close(ftdih);
+#elif BUILD_FT2232_LIBFTDI == 1
+	ftdi_disable_bitbang(&ftdic);
+	
+	ftdi_usb_close(&ftdic);
+	
+	ftdi_deinit(&ftdic);
+#endif
+
+	free(ft2232_buffer);
+	ft2232_buffer = NULL;
+
+	return ERROR_OK;
+}
+
+int ft2232_handle_device_desc_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	if (argc == 1)
+	{
+		ft2232_device_desc = strdup(args[0]);
+	}
+	else
+	{
+		ERROR("expected exactly one argument to ft2232_device_desc <description>");
+	}
+	
+	return ERROR_OK;
+}
+
+int ft2232_handle_serial_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	if (argc == 1)
+	{
+		ft2232_serial = strdup(args[0]);
+	}
+	else
+	{
+		ERROR("expected exactly one argument to ft2232_serial <serial-number>");
+	}
+	
+	return ERROR_OK;
+}
+
+int ft2232_handle_layout_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	if (argc == 0)
+		return ERROR_OK;
+
+	ft2232_layout = malloc(strlen(args[0]) + 1);
+	strcpy(ft2232_layout, args[0]);
+
+	return ERROR_OK;
+}
+
+int ft2232_handle_vid_pid_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	int i;
+
+	if (argc > MAX_USB_IDS*2) {
+		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))
+	{
+		WARNING("incomplete ft2232_vid_pid configuration directive");
+		if (argc < 2)
+			return ERROR_OK;
+	}
+
+	for (i = 0; i+1 < argc; i += 2) {
+		ft2232_vid[i >> 1] = strtol(args[i], NULL, 0);
+		ft2232_pid[i >> 1] = strtol(args[i+1], NULL, 0);
+	}
+	/*
+	 * Explicitly terminate, in case there are multiples instances of
+	 * ft2232_vid_pid.
+	 */
+	ft2232_vid[i >> 1] = ft2232_pid[i >> 1] = 0;
+
+	return ERROR_OK;
+}
+
+int ft2232_handle_latency_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	if (argc == 1)
+	{
+		ft2232_latency = atoi(args[0]);
+	}
+	else
+	{
+		ERROR("expected exactly one argument to ft2232_latency <ms>");
+	}
+	
+	return ERROR_OK;
+}
+
+
diff --git a/src/jtag/jtag.c b/src/jtag/jtag.c
index 77c4b3f2..600fe19a 100644
--- a/src/jtag/jtag.c
+++ b/src/jtag/jtag.c
@@ -1,1841 +1,1841 @@
-/***************************************************************************

- *   Copyright (C) 2005 by Dominic Rath                                    *

- *   Dominic.Rath@gmx.de                                                   *

- *                                                                         *

- *   This program is free software; you can redistribute it and/or modify  *

- *   it under the terms of the GNU General Public License as published by  *

- *   the Free Software Foundation; either version 2 of the License, or     *

- *   (at your option) any later version.                                   *

- *                                                                         *

- *   This program is distributed in the hope that it will be useful,       *

- *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *

- *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *

- *   GNU General Public License for more details.                          *

- *                                                                         *

- *   You should have received a copy of the GNU General Public License     *

- *   along with this program; if not, write to the                         *

- *   Free Software Foundation, Inc.,                                       *

- *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *

- ***************************************************************************/

-#ifdef HAVE_CONFIG_H

-#include "config.h"

-#endif

-

-#include "replacements.h"

-

-#include "jtag.h"

-

-#include "command.h"

-#include "log.h"

-#include "interpreter.h"

-

-#include "stdlib.h"

-#include "string.h"

-#include <unistd.h>

-

-char* tap_state_strings[16] =

-{

-	"tlr", 

-	"sds", "cd", "sd", "e1d", "pd", "e2d", "ud",

-	"rti",

-	"sis", "ci", "si", "e1i", "pi", "e2i", "ui"

-};

-

-typedef struct cmd_queue_page_s

-{

-	void *address;

-	size_t used;

-	struct cmd_queue_page_s *next;

-} cmd_queue_page_t;

-

-#define CMD_QUEUE_PAGE_SIZE (1024 * 1024)

-static cmd_queue_page_t *cmd_queue_pages = NULL;

-

-/* tap_move[i][j]: tap movement command to go from state i to state j

- * 0: Test-Logic-Reset

- * 1: Run-Test/Idle

- * 2: Shift-DR

- * 3: Pause-DR

- * 4: Shift-IR

- * 5: Pause-IR

- * 

- * SD->SD and SI->SI have to be caught in interface specific code

- */

-u8 tap_move[6][6] =

-{

-/*	  TLR   RTI   SD    PD    SI    PI             */

-	{0x7f, 0x00, 0x17, 0x0a, 0x1b, 0x16},	/* TLR */

-	{0x7f, 0x00, 0x25, 0x05, 0x2b, 0x0b},	/* RTI */

-	{0x7f, 0x31, 0x00, 0x01, 0x0f, 0x2f},	/* SD  */

-	{0x7f, 0x30, 0x20, 0x17, 0x1e, 0x2f},	/* PD  */

-	{0x7f, 0x31, 0x07, 0x17, 0x00, 0x01},	/* SI  */

-	{0x7f, 0x30, 0x1c, 0x17, 0x20, 0x2f}	/* PI  */

-};

-

-int tap_move_map[16] = {

-	0, -1, -1,  2, -1,  3, -1, -1,

-	1, -1, -1,  4, -1,  5, -1, -1

-};

-

-tap_transition_t tap_transitions[16] =

-{

-	{TAP_TLR, TAP_RTI},		/* TLR */

-	{TAP_SIS, TAP_CD},		/* SDS */

-	{TAP_E1D, TAP_SD},		/* CD  */

-	{TAP_E1D, TAP_SD},		/* SD  */

-	{TAP_UD,  TAP_PD}, 		/* E1D */

-	{TAP_E2D, TAP_PD},		/* PD  */

-	{TAP_UD,  TAP_SD},		/* E2D */

-	{TAP_SDS, TAP_RTI},		/* UD  */

-	{TAP_SDS, TAP_RTI},		/* RTI */

-	{TAP_TLR, TAP_CI},		/* SIS */

-	{TAP_E1I, TAP_SI},		/* CI  */

-	{TAP_E1I, TAP_SI},		/* SI  */

-	{TAP_UI,  TAP_PI}, 		/* E1I */

-	{TAP_E2I, TAP_PI},		/* PI  */

-	{TAP_UI,  TAP_SI},		/* E2I */

-	{TAP_SDS, TAP_RTI}		/* UI  */

-};

-

-char* jtag_event_strings[] =

-{

-	"SRST asserted",

-	"TRST asserted",

-	"SRST released",

-	"TRST released"

-};

-

-enum tap_state end_state = TAP_TLR;

-enum tap_state cur_state = TAP_TLR;

-int jtag_trst = 0;

-int jtag_srst = 0;

-

-jtag_command_t *jtag_command_queue = NULL;

-jtag_command_t **last_comand_pointer = &jtag_command_queue;

-jtag_device_t *jtag_devices = NULL;

-int jtag_num_devices = 0;

-int jtag_ir_scan_size = 0;

-enum reset_types jtag_reset_config = RESET_NONE;

-enum tap_state cmd_queue_end_state = TAP_TLR;

-enum tap_state cmd_queue_cur_state = TAP_TLR;

-

-int jtag_verify_capture_ir = 1;

-

-/* how long the OpenOCD should wait before attempting JTAG communication after reset lines deasserted (in ms) */

-int jtag_nsrst_delay = 0; /* default to no nSRST delay */

-int jtag_ntrst_delay = 0; /* default to no nTRST delay */ 

-

-/* maximum number of JTAG devices expected in the chain

- */

-#define JTAG_MAX_CHAIN_SIZE 20 

-

-/* callbacks to inform high-level handlers about JTAG state changes */

-jtag_event_callback_t *jtag_event_callbacks;

-

-/* jtag interfaces (parport, FTDI-USB, TI-USB, ...)

- */

-#if BUILD_PARPORT == 1

-	extern jtag_interface_t parport_interface;

-#endif

-

-#if BUILD_FT2232_FTD2XX == 1

-	extern jtag_interface_t ft2232_interface;

-#endif

-

-#if BUILD_FT2232_LIBFTDI == 1

-	extern jtag_interface_t ft2232_interface;

-#endif

-

-#if BUILD_AMTJTAGACCEL == 1

-	extern jtag_interface_t amt_jtagaccel_interface;

-#endif

-

-#if BUILD_EP93XX == 1

-	extern jtag_interface_t ep93xx_interface;

-#endif

-

-#if BUILD_AT91RM9200 == 1

-	extern jtag_interface_t at91rm9200_interface;

-#endif

-

-#if BUILD_GW16012 == 1

-	extern jtag_interface_t gw16012_interface;

-#endif

-

-#if BUILD_PRESTO_LIBFTDI == 1 || BUILD_PRESTO_FTD2XX == 1

-	extern jtag_interface_t presto_interface;

-#endif

-

-#if BUILD_USBPROG == 1

-	extern jtag_interface_t usbprog_interface;

-#endif

-

-jtag_interface_t *jtag_interfaces[] = {

-#if BUILD_PARPORT == 1

-	&parport_interface,

-#endif

-#if BUILD_FT2232_FTD2XX == 1

-	&ft2232_interface,

-#endif

-#if BUILD_FT2232_LIBFTDI == 1

-	&ft2232_interface,

-#endif

-#if BUILD_AMTJTAGACCEL == 1

-	&amt_jtagaccel_interface,

-#endif

-#if BUILD_EP93XX == 1

-	&ep93xx_interface,

-#endif

-#if BUILD_AT91RM9200 == 1

-	&at91rm9200_interface,

-#endif

-#if BUILD_GW16012 == 1

-	&gw16012_interface,

-#endif

-#if BUILD_PRESTO_LIBFTDI == 1 || BUILD_PRESTO_FTD2XX == 1

-	&presto_interface,

-#endif

-#if BUILD_USBPROG == 1

-	&usbprog_interface,

-#endif

-	NULL,

-};

-

-jtag_interface_t *jtag = NULL;

-

-/* configuration */

-jtag_interface_t *jtag_interface = NULL;

-int jtag_speed = 0;

-

-

-/* forward declarations */

-int jtag_add_statemove(enum tap_state endstate);

-int jtag_add_pathmove(int num_states, enum tap_state *path);

-int jtag_add_runtest(int num_cycles, enum tap_state endstate);

-int jtag_add_reset(int trst, int srst);

-int jtag_add_end_state(enum tap_state endstate);

-int jtag_add_sleep(u32 us);

-int jtag_execute_queue(void);

-int jtag_cancel_queue(void);

-

-/* jtag commands */

-int handle_interface_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

-int handle_jtag_speed_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

-int handle_jtag_device_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

-int handle_reset_config_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

-int handle_jtag_nsrst_delay_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

-int handle_jtag_ntrst_delay_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

-

-int handle_scan_chain_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

-

-int handle_endstate_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

-int handle_jtag_reset_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

-int handle_runtest_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

-int handle_statemove_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

-int handle_irscan_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

-int handle_drscan_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

-

-int handle_verify_ircapture_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

-

-int jtag_register_event_callback(int (*callback)(enum jtag_event event, void *priv), void *priv)

-{

-	jtag_event_callback_t **callbacks_p = &jtag_event_callbacks;

-	

-	if (callback == NULL)

-	{

-		return ERROR_INVALID_ARGUMENTS;

-	}

-	

-	if (*callbacks_p)

-	{

-		while ((*callbacks_p)->next)

-			callbacks_p = &((*callbacks_p)->next);

-		callbacks_p = &((*callbacks_p)->next);

-	}

-	

-	(*callbacks_p) = malloc(sizeof(jtag_event_callback_t));

-	(*callbacks_p)->callback = callback;

-	(*callbacks_p)->priv = priv;

-	(*callbacks_p)->next = NULL;

-	

-	return ERROR_OK;

-}

-

-int jtag_unregister_event_callback(int (*callback)(enum jtag_event event, void *priv))

-{

-	jtag_event_callback_t **callbacks_p = &jtag_event_callbacks;

-	

-	if (callback == NULL)

-	{

-		return ERROR_INVALID_ARGUMENTS;

-	}

-		

-	while (*callbacks_p)

-	{

-		jtag_event_callback_t **next = &((*callbacks_p)->next);

-		if ((*callbacks_p)->callback == callback)

-		{

-			free(*callbacks_p);

-			*callbacks_p = *next;

-		}

-		callbacks_p = next;

-	}

-	

-	return ERROR_OK;

-}

-

-int jtag_call_event_callbacks(enum jtag_event event)

-{

-	jtag_event_callback_t *callback = jtag_event_callbacks;

-	

-	DEBUG("jtag event: %s", jtag_event_strings[event]);

-	

-	while (callback)

-	{

-		callback->callback(event, callback->priv);

-		callback = callback->next;

-	}

-	

-	return ERROR_OK;

-}

-

-/* returns a pointer to the pointer of the last command in queue

- * this may be a pointer to the root pointer (jtag_command_queue)

- * or to the next member of the last but one command

- */

-jtag_command_t** jtag_get_last_command_p(void)

-{

-/*	jtag_command_t *cmd = jtag_command_queue;

-	

-	if (cmd)

-		while (cmd->next)

-			cmd = cmd->next;

-	else

-		return &jtag_command_queue;

-	

-	return &cmd->next;*/

-	

-	return last_comand_pointer;

-}

-

-/* returns a pointer to the n-th device in the scan chain */

-jtag_device_t* jtag_get_device(int num)

-{

-	jtag_device_t *device = jtag_devices;

-	int i = 0;

-

-	while (device)

-	{

-		if (num == i)

-			return device;

-		device = device->next;

-		i++;

-	}

-	

-	ERROR("jtag device number %d not defined", num);

-	exit(-1);

-}

-

-void* cmd_queue_alloc(size_t size)

-{

-	cmd_queue_page_t **p_page = &cmd_queue_pages;

-	int offset;

-

-	if (*p_page)

-	{

-		while ((*p_page)->next)

-			p_page = &((*p_page)->next);

-		if (CMD_QUEUE_PAGE_SIZE - (*p_page)->used < size)

-			p_page = &((*p_page)->next);

-	}

-

-	if (!*p_page)

-	{

-		*p_page = malloc(sizeof(cmd_queue_page_t));

-		(*p_page)->used = 0;

-		(*p_page)->address = malloc(CMD_QUEUE_PAGE_SIZE);

-		(*p_page)->next = NULL;

-	}

-

-	offset = (*p_page)->used;

-	(*p_page)->used += size;

-	

-	u8 *t=(u8 *)((*p_page)->address);

-	return t + offset;

-}

-

-void cmd_queue_free()

-{

-	cmd_queue_page_t *page = cmd_queue_pages;

-

-	while (page)

-	{

-		cmd_queue_page_t *last = page;

-		free(page->address);

-		page = page->next;

-		free(last);

-	}

-

-	cmd_queue_pages = NULL;

-}

-

-int jtag_add_ir_scan(int num_fields, scan_field_t *fields, enum tap_state state)

-{

-	jtag_command_t **last_cmd;

-	jtag_device_t *device;

-	int i, j;

-	int scan_size = 0;

-

-	if (jtag_trst == 1)

-	{

-		WARNING("JTAG command queued, while TRST is low (TAP in reset)");

-		return ERROR_JTAG_TRST_ASSERTED;

-	}

-

-	last_cmd = jtag_get_last_command_p();

-	

-	/* allocate memory for a new list member */

-	*last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));

-	(*last_cmd)->next = NULL;

-	last_comand_pointer = &((*last_cmd)->next);

-	(*last_cmd)->type = JTAG_SCAN;

-

-	/* allocate memory for ir scan command */

-	(*last_cmd)->cmd.scan = cmd_queue_alloc(sizeof(scan_command_t));

-	(*last_cmd)->cmd.scan->ir_scan = 1;

-	(*last_cmd)->cmd.scan->num_fields = jtag_num_devices;	/* one field per device */

-	(*last_cmd)->cmd.scan->fields = cmd_queue_alloc(jtag_num_devices * sizeof(scan_field_t));

-	(*last_cmd)->cmd.scan->end_state = state;

-		

-	if (state != -1)

-		cmd_queue_end_state = state;

-

-	if (cmd_queue_cur_state == TAP_TLR && cmd_queue_end_state != TAP_TLR)

-		jtag_call_event_callbacks(JTAG_TRST_RELEASED);

-	

-	if (cmd_queue_end_state == TAP_TLR)

-		jtag_call_event_callbacks(JTAG_TRST_ASSERTED);

-	

-	cmd_queue_cur_state = cmd_queue_end_state;

-		

-	for (i = 0; i < jtag_num_devices; i++)

-	{

-		int found = 0;

-		device = jtag_get_device(i);

-		scan_size = device->ir_length;

-		(*last_cmd)->cmd.scan->fields[i].device = i;

-		(*last_cmd)->cmd.scan->fields[i].num_bits = scan_size;

-		(*last_cmd)->cmd.scan->fields[i].in_value = NULL;

-		(*last_cmd)->cmd.scan->fields[i].in_handler = NULL;	/* disable verification by default */

-

-		/* search the list */

-		for (j = 0; j < num_fields; j++)

-		{

-			if (i == fields[j].device)

-			{

-				found = 1;

-				(*last_cmd)->cmd.scan->fields[i].out_value = buf_cpy(fields[j].out_value, cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);

-				(*last_cmd)->cmd.scan->fields[i].out_mask = buf_cpy(fields[j].out_mask, cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);

-			

-				if (jtag_verify_capture_ir)

-				{

-					if (fields[j].in_handler==NULL)

-					{

-						jtag_set_check_value((*last_cmd)->cmd.scan->fields+i, device->expected, device->expected_mask, NULL);

-					} else

-					{

-						(*last_cmd)->cmd.scan->fields[i].in_handler = fields[j].in_handler;

-						(*last_cmd)->cmd.scan->fields[i].in_handler_priv = fields[j].in_handler_priv;

-						(*last_cmd)->cmd.scan->fields[i].in_check_value = device->expected; 

-						(*last_cmd)->cmd.scan->fields[i].in_check_mask = device->expected_mask;

-					}

-				}

-				

-				device->bypass = 0;

-				break;

-			}

-		}

-	

-		if (!found)

-		{

-			/* if a device isn't listed, set it to BYPASS */

-			(*last_cmd)->cmd.scan->fields[i].out_value = buf_set_ones(cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);

-			(*last_cmd)->cmd.scan->fields[i].out_mask = NULL;

-			device->bypass = 1;

-		

-		}

-		

-		/* update device information */

-		buf_cpy((*last_cmd)->cmd.scan->fields[i].out_value, jtag_get_device(i)->cur_instr, scan_size);

-	}

-	

-	return ERROR_OK;

-}

-

-int jtag_add_plain_ir_scan(int num_fields, scan_field_t *fields, enum tap_state state)

-{

-	jtag_command_t **last_cmd;

-	int i;

-

-	if (jtag_trst == 1)

-	{

-		WARNING("JTAG command queued, while TRST is low (TAP in reset)");

-		return ERROR_JTAG_TRST_ASSERTED;

-	}

-

-	last_cmd = jtag_get_last_command_p();

-	

-	/* allocate memory for a new list member */

-	*last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));

-	(*last_cmd)->next = NULL;

-	last_comand_pointer = &((*last_cmd)->next);

-	(*last_cmd)->type = JTAG_SCAN;

-

-	/* allocate memory for ir scan command */

-	(*last_cmd)->cmd.scan = cmd_queue_alloc(sizeof(scan_command_t));

-	(*last_cmd)->cmd.scan->ir_scan = 1;

-	(*last_cmd)->cmd.scan->num_fields = num_fields;

-	(*last_cmd)->cmd.scan->fields = cmd_queue_alloc(num_fields * sizeof(scan_field_t));

-	(*last_cmd)->cmd.scan->end_state = state;

-

-	if (state != -1)

-		cmd_queue_end_state = state;

-

-	if (cmd_queue_cur_state == TAP_TLR && cmd_queue_end_state != TAP_TLR)

-		jtag_call_event_callbacks(JTAG_TRST_RELEASED);

-	

-	if (cmd_queue_end_state == TAP_TLR)

-		jtag_call_event_callbacks(JTAG_TRST_ASSERTED);

-		

-	cmd_queue_cur_state = cmd_queue_end_state;

-	

-	for (i = 0; i < num_fields; i++)

-	{

-		int num_bits = fields[i].num_bits;

-		int num_bytes = CEIL(fields[i].num_bits, 8);

-		(*last_cmd)->cmd.scan->fields[i].device = fields[i].device;

-		(*last_cmd)->cmd.scan->fields[i].num_bits = num_bits;

-		(*last_cmd)->cmd.scan->fields[i].out_value = buf_cpy(fields[i].out_value, cmd_queue_alloc(num_bytes), num_bits);

-		(*last_cmd)->cmd.scan->fields[i].out_mask = buf_cpy(fields[i].out_mask, cmd_queue_alloc(num_bytes), num_bits);

-		(*last_cmd)->cmd.scan->fields[i].in_value = fields[i].in_value;

-		(*last_cmd)->cmd.scan->fields[i].in_check_value = fields[i].in_check_value;

-		(*last_cmd)->cmd.scan->fields[i].in_check_mask = fields[i].in_check_mask;

-		(*last_cmd)->cmd.scan->fields[i].in_handler = NULL;

-		(*last_cmd)->cmd.scan->fields[i].in_handler_priv = NULL;

-	}

-	return ERROR_OK;

-}

-

-int jtag_add_dr_scan(int num_fields, scan_field_t *fields, enum tap_state state)

-{

-	int i, j;

-	int bypass_devices = 0;

-	int field_count = 0;

-	jtag_command_t **last_cmd = jtag_get_last_command_p();

-	jtag_device_t *device = jtag_devices;

-	int scan_size;

-

-	if (jtag_trst == 1)

-	{

-		WARNING("JTAG command queued, while TRST is low (TAP in reset)");

-		return ERROR_JTAG_TRST_ASSERTED;

-	}

-

-	/* count devices in bypass */

-	while (device)

-	{

-		if (device->bypass)

-			bypass_devices++;

-		device = device->next;

-	}

-	

-	/* allocate memory for a new list member */

-	*last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));

-	last_comand_pointer = &((*last_cmd)->next);

-	(*last_cmd)->next = NULL;

-	(*last_cmd)->type = JTAG_SCAN;

-

-	/* allocate memory for dr scan command */

-	(*last_cmd)->cmd.scan = cmd_queue_alloc(sizeof(scan_command_t));

-	(*last_cmd)->cmd.scan->ir_scan = 0;

-	(*last_cmd)->cmd.scan->num_fields = num_fields + bypass_devices;

-	(*last_cmd)->cmd.scan->fields = cmd_queue_alloc((num_fields + bypass_devices) * sizeof(scan_field_t));

-	(*last_cmd)->cmd.scan->end_state = state;

-	

-	if (state != -1)

-		cmd_queue_end_state = state;

-

-	if (cmd_queue_cur_state == TAP_TLR && cmd_queue_end_state != TAP_TLR)

-		jtag_call_event_callbacks(JTAG_TRST_RELEASED);

-	

-	if (cmd_queue_end_state == TAP_TLR)

-		jtag_call_event_callbacks(JTAG_TRST_ASSERTED);

-			

-	cmd_queue_cur_state = cmd_queue_end_state;

-	

-	for (i = 0; i < jtag_num_devices; i++)

-	{

-		int found = 0;

-		(*last_cmd)->cmd.scan->fields[field_count].device = i;

-	

-		for (j = 0; j < num_fields; j++)

-		{

-			if (i == fields[j].device)

-			{

-				found = 1;

-				scan_size = fields[j].num_bits;

-				(*last_cmd)->cmd.scan->fields[field_count].num_bits = scan_size;

-				(*last_cmd)->cmd.scan->fields[field_count].out_value = buf_cpy(fields[j].out_value, cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);

-				(*last_cmd)->cmd.scan->fields[field_count].out_mask = buf_cpy(fields[j].out_mask, cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);

-				(*last_cmd)->cmd.scan->fields[field_count].in_value = fields[j].in_value;

-				(*last_cmd)->cmd.scan->fields[field_count].in_check_value = fields[j].in_check_value;

-				(*last_cmd)->cmd.scan->fields[field_count].in_check_mask = fields[j].in_check_mask;

-				(*last_cmd)->cmd.scan->fields[field_count].in_handler = fields[j].in_handler;

-				(*last_cmd)->cmd.scan->fields[field_count++].in_handler_priv = fields[j].in_handler_priv;

-			}

-		}

-		if (!found)

-		{

-			/* if a device isn't listed, the BYPASS register should be selected */

-			if (!jtag_get_device(i)->bypass)

-			{

-				ERROR("BUG: no scan data for a device not in BYPASS");

-				exit(-1);

-			}

-	

-			/* program the scan field to 1 bit length, and ignore it's value */

-			(*last_cmd)->cmd.scan->fields[field_count].num_bits = 1;

-			(*last_cmd)->cmd.scan->fields[field_count].out_value = NULL;

-			(*last_cmd)->cmd.scan->fields[field_count].out_mask = NULL;

-			(*last_cmd)->cmd.scan->fields[field_count].in_value = NULL;

-			(*last_cmd)->cmd.scan->fields[field_count].in_check_value = NULL;

-			(*last_cmd)->cmd.scan->fields[field_count].in_check_mask = NULL;

-			(*last_cmd)->cmd.scan->fields[field_count].in_handler = NULL;

-			(*last_cmd)->cmd.scan->fields[field_count++].in_handler_priv = NULL;

-		}

-		else

-		{

-			/* if a device is listed, the BYPASS register must not be selected */

-			if (jtag_get_device(i)->bypass)

-			{

-				WARNING("scan data for a device in BYPASS");

-			}

-		}

-	}

-	return ERROR_OK;

-}

-

-

-int jtag_add_plain_dr_scan(int num_fields, scan_field_t *fields, enum tap_state state)

-{

-	int i;

-	jtag_command_t **last_cmd = jtag_get_last_command_p();

-	

-	if (jtag_trst == 1)

-	{

-		WARNING("JTAG command queued, while TRST is low (TAP in reset)");

-		return ERROR_JTAG_TRST_ASSERTED;

-	}

-

-	/* allocate memory for a new list member */

-	*last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));

-	last_comand_pointer = &((*last_cmd)->next);

-	(*last_cmd)->next = NULL;

-	(*last_cmd)->type = JTAG_SCAN;

-

-	/* allocate memory for scan command */

-	(*last_cmd)->cmd.scan = cmd_queue_alloc(sizeof(scan_command_t));

-	(*last_cmd)->cmd.scan->ir_scan = 0;

-	(*last_cmd)->cmd.scan->num_fields = num_fields;

-	(*last_cmd)->cmd.scan->fields = cmd_queue_alloc(num_fields * sizeof(scan_field_t));

-	(*last_cmd)->cmd.scan->end_state = state;

-		

-	if (state != -1)

-		cmd_queue_end_state = state;

-

-	if (cmd_queue_cur_state == TAP_TLR && cmd_queue_end_state != TAP_TLR)

-		jtag_call_event_callbacks(JTAG_TRST_RELEASED);

-	

-	if (cmd_queue_end_state == TAP_TLR)

-		jtag_call_event_callbacks(JTAG_TRST_ASSERTED);

-			

-	cmd_queue_cur_state = cmd_queue_end_state;

-	

-	for (i = 0; i < num_fields; i++)

-	{

-		int num_bits = fields[i].num_bits;

-		int num_bytes = CEIL(fields[i].num_bits, 8);

-		(*last_cmd)->cmd.scan->fields[i].device = fields[i].device;

-		(*last_cmd)->cmd.scan->fields[i].num_bits = num_bits;

-		(*last_cmd)->cmd.scan->fields[i].out_value = buf_cpy(fields[i].out_value, cmd_queue_alloc(num_bytes), num_bits);

-		(*last_cmd)->cmd.scan->fields[i].out_mask = buf_cpy(fields[i].out_mask, cmd_queue_alloc(num_bytes), num_bits);

-		(*last_cmd)->cmd.scan->fields[i].in_value = fields[i].in_value;

-		(*last_cmd)->cmd.scan->fields[i].in_check_value = fields[i].in_check_value;

-		(*last_cmd)->cmd.scan->fields[i].in_check_mask = fields[i].in_check_mask;

-		(*last_cmd)->cmd.scan->fields[i].in_handler = fields[i].in_handler;

-		(*last_cmd)->cmd.scan->fields[i].in_handler_priv = fields[i].in_handler_priv;

-	}

-

-	return ERROR_OK;

-}

-int jtag_add_statemove(enum tap_state state)

-{

-	jtag_command_t **last_cmd = jtag_get_last_command_p();

-	

-	if (jtag_trst == 1)

-	{

-		WARNING("JTAG command queued, while TRST is low (TAP in reset)");

-		return ERROR_JTAG_TRST_ASSERTED;

-	}

-

-	/* allocate memory for a new list member */

-	*last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));

-	last_comand_pointer = &((*last_cmd)->next);

-	(*last_cmd)->next = NULL;

-	(*last_cmd)->type = JTAG_STATEMOVE;

-

-	(*last_cmd)->cmd.statemove = cmd_queue_alloc(sizeof(statemove_command_t));

-	(*last_cmd)->cmd.statemove->end_state = state;

-	

-	if (state != -1)

-		cmd_queue_end_state = state;

-

-	if (cmd_queue_cur_state == TAP_TLR && cmd_queue_end_state != TAP_TLR)

-		jtag_call_event_callbacks(JTAG_TRST_RELEASED);

-	

-	if (cmd_queue_end_state == TAP_TLR)

-		jtag_call_event_callbacks(JTAG_TRST_ASSERTED);

-			

-	cmd_queue_cur_state = cmd_queue_end_state;

-	

-	return ERROR_OK;

-}

-

-int jtag_add_pathmove(int num_states, enum tap_state *path)

-{

-	jtag_command_t **last_cmd = jtag_get_last_command_p();

-	int i;

-	

-	if (jtag_trst == 1)

-	{

-		WARNING("JTAG command queued, while TRST is low (TAP in reset)");

-		return ERROR_JTAG_TRST_ASSERTED;

-	}

-	

-	/* the last state has to be a stable state */

-	if (tap_move_map[path[num_states - 1]] == -1)

-	{

-		ERROR("TAP path doesn't finish in a stable state");

-		return ERROR_JTAG_NOT_IMPLEMENTED;

-	}

-	

-	/* allocate memory for a new list member */

-	*last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));

-	last_comand_pointer = &((*last_cmd)->next);

-	(*last_cmd)->next = NULL;

-	(*last_cmd)->type = JTAG_PATHMOVE;

-

-	(*last_cmd)->cmd.pathmove = cmd_queue_alloc(sizeof(pathmove_command_t));

-	(*last_cmd)->cmd.pathmove->num_states = num_states;

-	(*last_cmd)->cmd.pathmove->path = cmd_queue_alloc(sizeof(enum tap_state) * num_states);

-	

-	for (i = 0; i < num_states; i++)

-		(*last_cmd)->cmd.pathmove->path[i] = path[i];

-

-	if (cmd_queue_cur_state == TAP_TLR && cmd_queue_end_state != TAP_TLR)

-		jtag_call_event_callbacks(JTAG_TRST_RELEASED);

-	

-	if (cmd_queue_end_state == TAP_TLR)

-		jtag_call_event_callbacks(JTAG_TRST_ASSERTED);

-	

-	cmd_queue_cur_state = path[num_states - 1];

-	

-	return ERROR_OK;

-}

-

-int jtag_add_runtest(int num_cycles, enum tap_state state)

-{

-	jtag_command_t **last_cmd = jtag_get_last_command_p();

-	

-	if (jtag_trst == 1)

-	{

-		WARNING("JTAG command queued, while TRST is low (TAP in reset)");

-		return ERROR_JTAG_TRST_ASSERTED;

-	}

-

-	/* allocate memory for a new list member */

-	*last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));

-	(*last_cmd)->next = NULL;

-	last_comand_pointer = &((*last_cmd)->next);

-	(*last_cmd)->type = JTAG_RUNTEST;

-

-	(*last_cmd)->cmd.runtest = cmd_queue_alloc(sizeof(runtest_command_t));

-	(*last_cmd)->cmd.runtest->num_cycles = num_cycles;

-	(*last_cmd)->cmd.runtest->end_state = state;

-	

-	if (state != -1)

-		cmd_queue_end_state = state;

-

-	if (cmd_queue_cur_state == TAP_TLR && cmd_queue_end_state != TAP_TLR)

-		jtag_call_event_callbacks(JTAG_TRST_RELEASED);

-	

-	if (cmd_queue_end_state == TAP_TLR)

-		jtag_call_event_callbacks(JTAG_TRST_ASSERTED);

-			

-	cmd_queue_cur_state = cmd_queue_end_state;

-	

-	return ERROR_OK;

-}

-

-int jtag_add_reset(int req_trst, int req_srst)

-{

-	int trst_with_tms = 0;

-	

-	jtag_command_t **last_cmd = jtag_get_last_command_p();

-	

-	if (req_trst == -1)

-		req_trst = jtag_trst;

-	

-	if (req_srst == -1)

-		req_srst = jtag_srst;

-

-	/* Make sure that jtag_reset_config allows the requested reset */

-	/* if SRST pulls TRST, we can't fulfill srst == 1 with trst == 0 */

-	if (((jtag_reset_config & RESET_SRST_PULLS_TRST) && (req_srst == 1)) && (req_trst == 0))

-		return ERROR_JTAG_RESET_WOULD_ASSERT_TRST;

-		

-	/* if TRST pulls SRST, we reset with TAP T-L-R */

-	if (((jtag_reset_config & RESET_TRST_PULLS_SRST) && (req_trst == 1)) && (req_srst == 0))

-	{

-		req_trst = 0;

-		trst_with_tms = 1;

-	}

-	

-	if (req_srst && !(jtag_reset_config & RESET_HAS_SRST))

-	{

-		ERROR("requested nSRST assertion, but the current configuration doesn't support this");

-		return ERROR_JTAG_RESET_CANT_SRST;

-	}

-	

-	if (req_trst && !(jtag_reset_config & RESET_HAS_TRST))

-	{

-		req_trst = 0;

-		trst_with_tms = 1;

-	}

-	

-	/* allocate memory for a new list member */

-	*last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));

-	(*last_cmd)->next = NULL;

-	last_comand_pointer = &((*last_cmd)->next);

-	(*last_cmd)->type = JTAG_RESET;

-

-	(*last_cmd)->cmd.reset = cmd_queue_alloc(sizeof(reset_command_t));

-	(*last_cmd)->cmd.reset->trst = req_trst;

-	(*last_cmd)->cmd.reset->srst = req_srst;

-

-	jtag_trst = req_trst;

-	jtag_srst = req_srst;

-

-	if (jtag_srst)

-	{

-		jtag_call_event_callbacks(JTAG_SRST_ASSERTED);

-	}

-	else

-	{

-		jtag_call_event_callbacks(JTAG_SRST_RELEASED);

-		if (jtag_nsrst_delay)

-			jtag_add_sleep(jtag_nsrst_delay * 1000);

-	}

-	

-	if (trst_with_tms)

-	{

-		last_cmd = &((*last_cmd)->next);

-		

-		/* allocate memory for a new list member */

-		*last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));

-		(*last_cmd)->next = NULL;

-		last_comand_pointer = &((*last_cmd)->next);

-		(*last_cmd)->type = JTAG_STATEMOVE;

-

-		(*last_cmd)->cmd.statemove = cmd_queue_alloc(sizeof(statemove_command_t));

-		(*last_cmd)->cmd.statemove->end_state = TAP_TLR;

-		

-		jtag_call_event_callbacks(JTAG_TRST_ASSERTED);

-		cmd_queue_cur_state = TAP_TLR;

-		cmd_queue_end_state = TAP_TLR;

-		

-		return ERROR_OK;

-	}

-	else

-	{

-		if (jtag_trst)

-		{

-			/* we just asserted nTRST, so we're now in Test-Logic-Reset,

-			 * and inform possible listeners about this

-			 */

-			cmd_queue_cur_state = TAP_TLR;

-			jtag_call_event_callbacks(JTAG_TRST_ASSERTED);

-		}

-		else

-		{

-			/* the nTRST line got deasserted, so we're still in Test-Logic-Reset,

-			 * but we might want to add a delay to give the TAP time to settle

-			 */

-			if (jtag_ntrst_delay)

-				jtag_add_sleep(jtag_ntrst_delay * 1000);

-		}

-	}

-

-	return ERROR_OK;

-}

-

-int jtag_add_end_state(enum tap_state state)

-{

-	jtag_command_t **last_cmd = jtag_get_last_command_p();

-	

-	/* allocate memory for a new list member */

-	*last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));

-	(*last_cmd)->next = NULL;

-	last_comand_pointer = &((*last_cmd)->next);

-	(*last_cmd)->type = JTAG_END_STATE;

-

-	(*last_cmd)->cmd.end_state = cmd_queue_alloc(sizeof(end_state_command_t));

-	(*last_cmd)->cmd.end_state->end_state = state;

-

-	if (state != -1)

-		cmd_queue_end_state = state;

-	

-	return ERROR_OK;

-}

-

-int jtag_add_sleep(u32 us)

-{

-	jtag_command_t **last_cmd = jtag_get_last_command_p();

-	

-	/* allocate memory for a new list member */

-	*last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));

-	(*last_cmd)->next = NULL;

-	last_comand_pointer = &((*last_cmd)->next);

-	(*last_cmd)->type = JTAG_SLEEP;

-

-	(*last_cmd)->cmd.sleep = cmd_queue_alloc(sizeof(sleep_command_t));

-	(*last_cmd)->cmd.sleep->us = us;

-	

-	return ERROR_OK;

-}

-

-int jtag_scan_size(scan_command_t *cmd)

-{

-	int bit_count = 0;

-	int i;

-

-	/* count bits in scan command */

-	for (i = 0; i < cmd->num_fields; i++)

-	{

-		bit_count += cmd->fields[i].num_bits;

-	}

-

-	return bit_count;

-}

-

-int jtag_build_buffer(scan_command_t *cmd, u8 **buffer)

-{

-	int bit_count = 0;

-	int i;

-	

-	bit_count = jtag_scan_size(cmd);

-	*buffer = malloc(CEIL(bit_count, 8));

-	

-	bit_count = 0;

-

-	for (i = 0; i < cmd->num_fields; i++)

-	{

-		if (cmd->fields[i].out_value)

-		{

-#ifdef _DEBUG_JTAG_IO_

-			char* char_buf = buf_to_str(cmd->fields[i].out_value, (cmd->fields[i].num_bits > 64) ? 64 : cmd->fields[i].num_bits, 16);

-#endif

-			buf_set_buf(cmd->fields[i].out_value, 0, *buffer, bit_count, cmd->fields[i].num_bits);

-#ifdef _DEBUG_JTAG_IO_

-			DEBUG("fields[%i].out_value: 0x%s", i, char_buf);

-			free(char_buf);

-#endif

-		}

-		

-		bit_count += cmd->fields[i].num_bits;

-	}

-

-	return bit_count;

-

-}

-

-int jtag_read_buffer(u8 *buffer, scan_command_t *cmd)

-{

-	int i;

-	int bit_count = 0;

-	int retval;

-	

-	/* we return ERROR_OK, unless a check fails, or a handler reports a problem */

-	retval = ERROR_OK;

-	

-	for (i = 0; i < cmd->num_fields; i++)

-	{

-		/* if neither in_value nor in_handler

-		 * are specified we don't have to examine this field

-		 */

-		if (cmd->fields[i].in_value || cmd->fields[i].in_handler)

-		{

-			int num_bits = cmd->fields[i].num_bits;

-			u8 *captured = buf_set_buf(buffer, bit_count, malloc(CEIL(num_bits, 8)), 0, num_bits);

-			

-#ifdef _DEBUG_JTAG_IO_

-			char *char_buf;

-

-			char_buf = buf_to_str(captured, (num_bits > 64) ? 64 : num_bits, 16);

-			DEBUG("fields[%i].in_value: 0x%s", i, char_buf);

-			free(char_buf);

-#endif

-			

-			if (cmd->fields[i].in_value)

-			{

-				buf_cpy(captured, cmd->fields[i].in_value, num_bits);

-				

-				if (cmd->fields[i].in_handler)

-				{

-					if (cmd->fields[i].in_handler(cmd->fields[i].in_value, cmd->fields[i].in_handler_priv, cmd->fields+i) != ERROR_OK)

-					{

-						WARNING("in_handler reported a failed check");

-						retval = ERROR_JTAG_QUEUE_FAILED;

-					}

-				}

-			}

-			

-			/* no in_value specified, but a handler takes care of the scanned data */

-			if (cmd->fields[i].in_handler && (!cmd->fields[i].in_value))

-			{

-				if (cmd->fields[i].in_handler(captured, cmd->fields[i].in_handler_priv, cmd->fields+i) != ERROR_OK)

-				{

-					/* We're going to call the error:handler later, but if the in_handler

-					 * reported an error we report this failure upstream

-					 */

-					WARNING("in_handler reported a failed check");

-					retval = ERROR_JTAG_QUEUE_FAILED;

-				}

-			}

-

-			free(captured);

-		}

-		bit_count += cmd->fields[i].num_bits;

-	}

-

-	return retval;

-}

-

-int jtag_check_value(u8 *captured, void *priv, scan_field_t *field)

-{

-	int retval = ERROR_OK;

-	int num_bits = field->num_bits;

-	

-	int compare_failed = 0;

-	

-	if (field->in_check_mask)

-		compare_failed = buf_cmp_mask(captured, field->in_check_value, field->in_check_mask, num_bits);

-	else

-		compare_failed = buf_cmp(captured, field->in_check_value, num_bits);

-	

-	if (compare_failed)

-		{

-		/* An error handler could have caught the failing check

-		 * only report a problem when there wasn't a handler, or if the handler

-		 * acknowledged the error

-		 */ 

-		if (compare_failed)

-		{

-			char *captured_char = buf_to_str(captured, (num_bits > 64) ? 64 : num_bits, 16);

-			char *in_check_value_char = buf_to_str(field->in_check_value, (num_bits > 64) ? 64 : num_bits, 16);

-

-			if (field->in_check_mask)

-			{

-				char *in_check_mask_char;

-				in_check_mask_char = buf_to_str(field->in_check_mask, (num_bits > 64) ? 64 : num_bits, 16);

-				WARNING("value captured during scan didn't pass the requested check: captured: 0x%s check_value: 0x%s check_mask: 0x%s", captured_char, in_check_value_char, in_check_mask_char);

-				free(in_check_mask_char);

-			}

-			else

-			{

-				WARNING("value captured during scan didn't pass the requested check: captured: 0x%s check_value: 0x%s", captured_char, in_check_value_char);

-			}

-

-			free(captured_char);

-			free(in_check_value_char);

-			

-			retval = ERROR_JTAG_QUEUE_FAILED;

-		}

-		

-	}

-	return retval;

-}

-

-/* 

-  set up checking of this field using the in_handler. The values passed in must be valid until

-  after jtag_execute() has completed.

- */

-void jtag_set_check_value(scan_field_t *field, u8 *value, u8 *mask, error_handler_t *in_error_handler)

-{

-	if (value)

-		field->in_handler = jtag_check_value;

-	else

-		field->in_handler = NULL;	/* No check, e.g. embeddedice uses value==NULL to indicate no check */

-	field->in_handler_priv = NULL;	/* this will be filled in at the invocation site to point to the field duplicate */ 

-	field->in_check_value = value;

-	field->in_check_mask = mask;

-}

-

-enum scan_type jtag_scan_type(scan_command_t *cmd)

-{

-	int i;

-	int type = 0;

-	

-	for (i = 0; i < cmd->num_fields; i++)

-	{

-		if (cmd->fields[i].in_value || cmd->fields[i].in_handler)

-			type |= SCAN_IN;

-		if (cmd->fields[i].out_value)

-			type |= SCAN_OUT;

-	}

-

-	return type;

-}

-

-int jtag_execute_queue(void)

-{

-	int retval;

-

-	retval = jtag->execute_queue();

-	

-	cmd_queue_free();

-

-	jtag_command_queue = NULL;

-	last_comand_pointer = &jtag_command_queue;

-

-	return retval;

-}

-

-int jtag_reset_callback(enum jtag_event event, void *priv)

-{

-	jtag_device_t *device = priv;

-

-	DEBUG("-");

-	

-	if (event == JTAG_TRST_ASSERTED)

-	{

-		buf_set_ones(device->cur_instr, device->ir_length);

-		device->bypass = 1;

-	}

-	

-	return ERROR_OK;

-}

-

-void jtag_sleep(u32 us)

-{

-	usleep(us);

-}

-

-/* Try to examine chain layout according to IEEE 1149.1 §12

- */

-int jtag_examine_chain()

-{

-	jtag_device_t *device = jtag_devices;

-	scan_field_t field;

-	u8 idcode_buffer[JTAG_MAX_CHAIN_SIZE * 4];

-	int i;

-	int bit_count;

-	int device_count = 0;

-	u8 zero_check = 0x0;

-	u8 one_check = 0xff;

-	

-	field.device = 0;

-	field.num_bits = sizeof(idcode_buffer) * 8;

-	field.out_value = idcode_buffer;

-	field.out_mask = NULL;

-	field.in_value = idcode_buffer;

-	field.in_check_value = NULL;

-	field.in_check_mask = NULL;

-	field.in_handler = NULL;

-	field.in_handler_priv = NULL;

-	

-	for (i = 0; i < JTAG_MAX_CHAIN_SIZE; i++)

-	{

-		buf_set_u32(idcode_buffer, i * 32, 32, 0x000000FF);

-	}

-	

-	jtag_add_plain_dr_scan(1, &field, TAP_TLR);

-	jtag_execute_queue();

-	

-	for (i = 0; i < JTAG_MAX_CHAIN_SIZE * 4; i++)

-	{

-		zero_check |= idcode_buffer[i];

-		one_check &= idcode_buffer[i];

-	}

-	

-	/* if there wasn't a single non-zero bit or if all bits were one, the scan isn't valid */

-	if ((zero_check == 0x00) || (one_check == 0xff))

-	{

-		ERROR("JTAG communication failure, check connection, JTAG interface, target power etc.");

-		return ERROR_JTAG_INIT_FAILED;

-	}

-	

-	for (bit_count = 0; bit_count < (JTAG_MAX_CHAIN_SIZE * 32) - 31;)

-	{

-		u32 idcode = buf_get_u32(idcode_buffer, bit_count, 32);

-		if ((idcode & 1) == 0)

-		{

-			/* LSB must not be 0, this indicates a device in bypass */

-			device_count++;

-			

-			bit_count += 1;

-		}

-		else

-		{

-			u32 manufacturer;

-			u32 part;

-			u32 version;

-			

-			if (idcode == 0x000000FF)

-			{

-				/* End of chain (invalid manufacturer ID) */

-				break;

-			}

-			

-			if (device)

-			{

-				device->idcode = idcode;

-				device = device->next;

-			}

-			device_count++;

-			

-			manufacturer = (idcode & 0xffe) >> 1;

-			part = (idcode & 0xffff000) >> 12;

-			version = (idcode & 0xf0000000) >> 28;

-

-			INFO("JTAG device found: 0x%8.8x (Manufacturer: 0x%3.3x, Part: 0x%4.4x, Version: 0x%1.1x)", 

-				idcode, manufacturer, part, version);

-			

-			bit_count += 32;

-		}

-	}

-	

-	/* see if number of discovered devices matches configuration */

-	if (device_count != jtag_num_devices)

-	{

-		ERROR("number of discovered devices in JTAG chain (%i) doesn't match configuration (%i)", 

-			device_count, jtag_num_devices);

-		ERROR("check the config file and ensure proper JTAG communication (connections, speed, ...)");

-		return ERROR_JTAG_INIT_FAILED;

-	}

-	

-	return ERROR_OK;

-}

-

-int jtag_validate_chain()

-{

-	jtag_device_t *device = jtag_devices;

-	int total_ir_length = 0;

-	u8 *ir_test = NULL;

-	scan_field_t field;

-	int chain_pos = 0;

-	

-	while (device)

-	{

-		total_ir_length += device->ir_length;

-		device = device->next;

-	}

-	

-	total_ir_length += 2;

-	ir_test = malloc(CEIL(total_ir_length, 8));

-	buf_set_ones(ir_test, total_ir_length);

-	

-	field.device = 0;

-	field.num_bits = total_ir_length;

-	field.out_value = ir_test;

-	field.out_mask = NULL;

-	field.in_value = ir_test;

-	field.in_check_value = NULL;

-	field.in_check_mask = NULL;

-	field.in_handler = NULL;

-	field.in_handler_priv = NULL;

-	

-	jtag_add_plain_ir_scan(1, &field, TAP_TLR);

-	jtag_execute_queue();

-	

-	device = jtag_devices;

-	while (device)

-	{

-		if (buf_get_u32(ir_test, chain_pos, 2) != 0x1)

-		{

-			char *cbuf = buf_to_str(ir_test, total_ir_length, 16);

-			ERROR("Error validating JTAG scan chain, IR mismatch, scan returned 0x%s", cbuf);

-			free(cbuf);

-			free(ir_test);

-			return ERROR_JTAG_INIT_FAILED;

-		}

-		chain_pos += device->ir_length;

-		device = device->next;

-	}

-	

-	if (buf_get_u32(ir_test, chain_pos, 2) != 0x3)

-	{

-		char *cbuf = buf_to_str(ir_test, total_ir_length, 16);

-		ERROR("Error validating JTAG scan chain, IR mismatch, scan returned 0x%s", cbuf);

-		free(cbuf);

-		free(ir_test);

-		return ERROR_JTAG_INIT_FAILED;

-	}

-	

-	free(ir_test);

-	

-	return ERROR_OK;

-}

-

-int jtag_register_commands(struct command_context_s *cmd_ctx)

-{

-	register_command(cmd_ctx, NULL, "interface", handle_interface_command,

-		COMMAND_CONFIG, NULL);

-	register_command(cmd_ctx, NULL, "jtag_speed", handle_jtag_speed_command,

-		COMMAND_ANY, "set jtag speed (if supported) <speed>");

-	register_command(cmd_ctx, NULL, "jtag_device", handle_jtag_device_command,

-		COMMAND_CONFIG, "jtag_device <ir_length> <ir_expected> <ir_mask>");

-	register_command(cmd_ctx, NULL, "reset_config", handle_reset_config_command,

-		COMMAND_CONFIG, NULL);

-	register_command(cmd_ctx, NULL, "jtag_nsrst_delay", handle_jtag_nsrst_delay_command,

-		COMMAND_CONFIG, NULL);

-	register_command(cmd_ctx, NULL, "jtag_ntrst_delay", handle_jtag_ntrst_delay_command,

-		COMMAND_CONFIG, NULL);

-		

-	register_command(cmd_ctx, NULL, "scan_chain", handle_scan_chain_command,

-		COMMAND_EXEC, "print current scan chain configuration");

-

-	register_command(cmd_ctx, NULL, "endstate", handle_endstate_command,

-		COMMAND_EXEC, "finish JTAG operations in <tap_state>");

-	register_command(cmd_ctx, NULL, "jtag_reset", handle_jtag_reset_command,

-		COMMAND_EXEC, "toggle reset lines <trst> <srst>");

-	register_command(cmd_ctx, NULL, "runtest", handle_runtest_command,

-		COMMAND_EXEC, "move to Run-Test/Idle, and execute <num_cycles>");

-	register_command(cmd_ctx, NULL, "statemove", handle_statemove_command,

-		COMMAND_EXEC, "move to current endstate or [tap_state]");

-	register_command(cmd_ctx, NULL, "irscan", handle_irscan_command,

-		COMMAND_EXEC, "execute IR scan <device> <instr> [dev2] [instr2] ...");

-	register_command(cmd_ctx, NULL, "drscan", handle_drscan_command,

-		COMMAND_EXEC, "execute DR scan <device> <var> [dev2] [var2] ...");

-

-	register_command(cmd_ctx, NULL, "verify_ircapture", handle_verify_ircapture_command,

-		COMMAND_ANY, "verify value captured during Capture-IR <enable|disable>");

-	return ERROR_OK;

-}

-

-int jtag_interface_init(struct command_context_s *cmd_ctx)

-{

-	if (!jtag_interface)

-	{

-		/* nothing was previously specified by "interface" command */

-		ERROR("JTAG interface has to be specified, see \"interface\" command");

-		return ERROR_JTAG_INVALID_INTERFACE;

-	}

-

-	if (jtag_interface->init() != ERROR_OK)

-		return ERROR_JTAG_INIT_FAILED;

-

-	jtag = jtag_interface;

-	return ERROR_OK;

-}

-

-int jtag_init(struct command_context_s *cmd_ctx)

-{

-	int i, validate_tries = 0;

-	jtag_device_t *device;

-

-	DEBUG("-");

-	

-	if (!jtag && jtag_interface_init(cmd_ctx) != ERROR_OK)

-		return ERROR_JTAG_INIT_FAILED;

-

-	device = jtag_devices;

-	jtag_ir_scan_size = 0;

-	jtag_num_devices = 0;

-	while (device != NULL)

-	{

-		jtag_ir_scan_size += device->ir_length;

-		jtag_num_devices++;

-		device = device->next;

-	}

-	

-	jtag_add_statemove(TAP_TLR);

-	jtag_execute_queue();

-

-	/* examine chain first, as this could discover the real chain layout */

-	if (jtag_examine_chain() != ERROR_OK)

-	{

-		ERROR("trying to validate configured JTAG chain anyway...");

-	}

-	

-	while (jtag_validate_chain() != ERROR_OK)

-	{

-		validate_tries++;

-		if (validate_tries > 5)

-		{

-			ERROR("Could not validate JTAG chain, exit");

-			jtag = NULL;

-			return ERROR_JTAG_INVALID_INTERFACE;

-		}

-		usleep(10000);

-	}

-

-	return ERROR_OK;

-}

-

-int handle_interface_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

-{

-	int i;

-

-	/* check whether the interface is already configured */

-	if (jtag_interface)

-	{

-		WARNING("Interface already configured, ignoring");

-		return ERROR_OK;

-	}

-

-	/* interface name is a mandatory argument */

-	if (argc < 1 || args[0][0] == '\0')

-	{

-		return ERROR_COMMAND_SYNTAX_ERROR;

-	}

-

-	for (i=0; jtag_interfaces[i]; i++)

-	{

-		if (strcmp(args[0], jtag_interfaces[i]->name) == 0)

-		{

-			if (jtag_interfaces[i]->register_commands(cmd_ctx) != ERROR_OK)

-				exit(-1);

-

-			jtag_interface = jtag_interfaces[i];

-			return ERROR_OK;

-		}

-	}

-

-	/* no valid interface was found (i.e. the configuration option,

-	 * didn't match one of the compiled-in interfaces

-	 */

-	ERROR("No valid jtag interface found (%s)", args[0]);

-	ERROR("compiled-in jtag interfaces:");

-	for (i = 0; jtag_interfaces[i]; i++)

-	{

-		ERROR("%i: %s", i, jtag_interfaces[i]->name);

-	}

-

-	return ERROR_JTAG_INVALID_INTERFACE;

-}

-

-int handle_jtag_device_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

-{

-	jtag_device_t **last_device_p = &jtag_devices;

-

-	if (*last_device_p)

-	{

-		while ((*last_device_p)->next)

-			last_device_p = &((*last_device_p)->next);

-		last_device_p = &((*last_device_p)->next);

-	}

-

-	if (argc < 3)

-		return ERROR_OK;

-

-	*last_device_p = malloc(sizeof(jtag_device_t));

-	(*last_device_p)->ir_length = strtoul(args[0], NULL, 0);

-

-	(*last_device_p)->expected = malloc((*last_device_p)->ir_length);

-	buf_set_u32((*last_device_p)->expected, 0, (*last_device_p)->ir_length, strtoul(args[1], NULL, 0));

-	(*last_device_p)->expected_mask = malloc((*last_device_p)->ir_length);

-	buf_set_u32((*last_device_p)->expected_mask, 0, (*last_device_p)->ir_length, strtoul(args[2], NULL, 0));

-

-	(*last_device_p)->cur_instr = malloc((*last_device_p)->ir_length);

-	(*last_device_p)->bypass = 1;

-	buf_set_ones((*last_device_p)->cur_instr, (*last_device_p)->ir_length);

-	

-	(*last_device_p)->next = NULL;

-	

-	jtag_register_event_callback(jtag_reset_callback, (*last_device_p));

-	

-	jtag_num_devices++;

-

-	return ERROR_OK;

-}

-

-int handle_scan_chain_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

-{

-	jtag_device_t *device = jtag_devices;

-	int device_count = 0;

-	

-	while (device)

-	{

-		u32 expected, expected_mask, cur_instr;

-		expected = buf_get_u32(device->expected, 0, device->ir_length);

-		expected_mask = buf_get_u32(device->expected_mask, 0, device->ir_length);

-		cur_instr = buf_get_u32(device->cur_instr, 0, device->ir_length);

-		command_print(cmd_ctx, "%i: idcode: 0x%8.8x ir length %i, ir capture 0x%x, ir mask 0x%x, current instruction 0x%x", device_count, device->idcode, device->ir_length, expected, expected_mask, cur_instr);

-		device = device->next;

-		device_count++;

-	}

-

-	return ERROR_OK;

-}

-

-int handle_reset_config_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

-{

-	if (argc >= 1)

-	{

-		if (strcmp(args[0], "none") == 0)

-			jtag_reset_config = RESET_NONE;

-		else if (strcmp(args[0], "trst_only") == 0)

-			jtag_reset_config = RESET_HAS_TRST;

-		else if (strcmp(args[0], "srst_only") == 0)

-			jtag_reset_config = RESET_HAS_SRST;

-		else if (strcmp(args[0], "trst_and_srst") == 0)

-			jtag_reset_config = RESET_TRST_AND_SRST;

-		else

-		{

-			ERROR("invalid reset_config argument, defaulting to none");

-			jtag_reset_config = RESET_NONE;

-			return ERROR_INVALID_ARGUMENTS;

-		}

-	}

-	

-	if (argc >= 2)

-	{

-		if (strcmp(args[1], "srst_pulls_trst") == 0)

-			jtag_reset_config |= RESET_SRST_PULLS_TRST;

-		else if (strcmp(args[1], "trst_pulls_srst") == 0)

-			jtag_reset_config |= RESET_TRST_PULLS_SRST;

-		else if (strcmp(args[1], "combined") == 0)

-			jtag_reset_config |= RESET_SRST_PULLS_TRST | RESET_TRST_PULLS_SRST;

-		else if (strcmp(args[1], "separate") == 0)

-			jtag_reset_config &= ~(RESET_SRST_PULLS_TRST | RESET_TRST_PULLS_SRST);

-		else

-		{

-			ERROR("invalid reset_config argument, defaulting to none");

-			jtag_reset_config = RESET_NONE;

-			return ERROR_INVALID_ARGUMENTS;

-		}

-	}

-	

-	if (argc >= 3)

-	{

-		if (strcmp(args[2], "trst_open_drain") == 0)

-			jtag_reset_config |= RESET_TRST_OPEN_DRAIN;

-		else if (strcmp(args[2], "trst_push_pull") == 0)

-			jtag_reset_config &= ~RESET_TRST_OPEN_DRAIN;

-		else

-		{

-			ERROR("invalid reset_config argument, defaulting to none");

-			jtag_reset_config = RESET_NONE;

-			return ERROR_INVALID_ARGUMENTS;

-		}

-	}

-

-	if (argc >= 4)

-	{

-		if (strcmp(args[3], "srst_push_pull") == 0)

-			jtag_reset_config |= RESET_SRST_PUSH_PULL;

-		else if (strcmp(args[3], "srst_open_drain") == 0)

-			jtag_reset_config &= ~RESET_SRST_PUSH_PULL;

-		else

-		{

-			ERROR("invalid reset_config argument, defaulting to none");

-			jtag_reset_config = RESET_NONE;

-			return ERROR_INVALID_ARGUMENTS;

-		}

-	}

-	

-	return ERROR_OK;

-}

-

-int handle_jtag_nsrst_delay_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

-{

-	if (argc < 1)

-	{

-		ERROR("jtag_nsrst_delay <ms> command takes one required argument");

-		exit(-1);

-	}

-	else

-	{

-		jtag_nsrst_delay = strtoul(args[0], NULL, 0);

-	}

-	

-	return ERROR_OK;

-}

-

-int handle_jtag_ntrst_delay_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

-{

-	if (argc < 1)

-	{

-		ERROR("jtag_ntrst_delay <ms> command takes one required argument");

-		exit(-1);

-	}

-	else

-	{

-		jtag_ntrst_delay = strtoul(args[0], NULL, 0);

-	}

-	

-	return ERROR_OK;

-}

-

-int handle_jtag_speed_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

-{

-	if (argc == 0)

-		command_print(cmd_ctx, "jtag_speed: %i", jtag_speed);

-

-	if (argc > 0)

-	{

-		/* this command can be called during CONFIG, 

-		 * in which case jtag isn't initialized */

-		if (jtag)

-			jtag->speed(strtoul(args[0], NULL, 0));

-		else

-			jtag_speed = strtoul(args[0], NULL, 0);

-	}

-

-	return ERROR_OK;

-}

-

-int handle_endstate_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

-{

-	enum tap_state state;

-

-	if (argc < 1)

-	{

-		return ERROR_COMMAND_SYNTAX_ERROR;

-	}

-	else

-	{

-		for (state = 0; state < 16; state++)

-		{

-			if (strcmp(args[0], tap_state_strings[state]) == 0)

-			{

-				jtag_add_end_state(state);

-				jtag_execute_queue();

-			}

-		}

-	}

-	command_print(cmd_ctx, "current endstate: %s", tap_state_strings[end_state]);

-	

-	return ERROR_OK;

-}

-

-int handle_jtag_reset_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

-{

-	int trst = -1;

-	int srst = -1;

-	int retval;

-	

-	if (argc < 2)

-	{

-		return ERROR_COMMAND_SYNTAX_ERROR;

-

-	}

-

-	if (args[0][0] == '1')

-		trst = 1;

-	else if (args[0][0] == '0')

-		trst = 0;

-	else

-	{

-		return ERROR_COMMAND_SYNTAX_ERROR;

-	}

-

-	if (args[1][0] == '1')

-		srst = 1;

-	else if (args[1][0] == '0')

-		srst = 0;

-	else

-	{

-		return ERROR_COMMAND_SYNTAX_ERROR;

-	}

-

-	if (!jtag && jtag_interface_init(cmd_ctx) != ERROR_OK)

-		return ERROR_JTAG_INIT_FAILED;

-

-	if ((retval = jtag_add_reset(trst, srst)) != ERROR_OK)

-	{

-		switch (retval)

-		{

-			case ERROR_JTAG_RESET_WOULD_ASSERT_TRST:

-				command_print(cmd_ctx, "requested reset would assert trst\nif this is acceptable, use jtag_reset 1 %c", args[1][0]);

-				break;

-			case ERROR_JTAG_RESET_CANT_SRST:

-				command_print(cmd_ctx, "can't assert srst because the current reset_config doesn't support it");

-				break;

-			default:

-				command_print(cmd_ctx, "unknown error");

-		}

-	}

-	jtag_execute_queue();

-

-	return ERROR_OK;

-}

-

-int handle_runtest_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

-{

-	if (argc < 1)

-	{

-		return ERROR_COMMAND_SYNTAX_ERROR;

-	}

-

-	jtag_add_runtest(strtol(args[0], NULL, 0), -1);

-	jtag_execute_queue();

-

-	return ERROR_OK;

-

-}

-

-int handle_statemove_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

-{

-	enum tap_state state;

-

-	state = -1;

-	if (argc == 1)

-	{

-		for (state = 0; state < 16; state++)

-		{

-			if (strcmp(args[0], tap_state_strings[state]) == 0)

-			{

-				break;

-			}

-		}

-	}

-

-	jtag_add_statemove(state);

-	jtag_execute_queue();

-

-	return ERROR_OK;

-

-}

-

-int handle_irscan_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

-{

-	int i;

-	scan_field_t *fields;

-	

-	if ((argc < 2) || (argc % 2))

-	{

-		return ERROR_COMMAND_SYNTAX_ERROR;

-	}

-

-	fields = malloc(sizeof(scan_field_t) * argc / 2);

-	

-	for (i = 0; i < argc / 2; i++)

-	{

-		int device = strtoul(args[i*2], NULL, 0);

-		int field_size = jtag_get_device(device)->ir_length;

-		fields[i].device = device;

-		fields[i].out_value = malloc(CEIL(field_size, 8));

-		buf_set_u32(fields[i].out_value, 0, field_size, strtoul(args[i*2+1], NULL, 0));

-		fields[i].out_mask = NULL;

-		fields[i].in_value = NULL;

-		fields[i].in_check_mask = NULL;

-		fields[i].in_handler = NULL;

-		fields[i].in_handler_priv = NULL;

-	}

-

-	jtag_add_ir_scan(argc / 2, fields, -1);

-	jtag_execute_queue();

-

-	for (i = 0; i < argc / 2; i++)

-		free(fields[i].out_value);

-

-	free (fields);

-

-	return ERROR_OK;

-}

-

-int handle_drscan_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

-{

-	scan_field_t *fields;

-	int num_fields = 0;

-	int field_count = 0;

-	var_t *var;

-	int i, j;

-	

-	if ((argc < 2) || (argc % 2))

-	{

-		return ERROR_COMMAND_SYNTAX_ERROR;

-	}

-

-	for (i = 0; i < argc; i+=2)

-	{

-		var = get_var_by_namenum(args[i+1]);

-		if (var)

-		{

-			num_fields += var->num_fields;

-		}

-		else

-		{

-			command_print(cmd_ctx, "variable %s doesn't exist", args[i+1]);

-			return ERROR_OK;

-		}

-	}

-

-	fields = malloc(sizeof(scan_field_t) * num_fields);

-

-	for (i = 0; i < argc; i+=2)

-	{

-		var = get_var_by_namenum(args[i+1]);

-	

-		for (j = 0; j < var->num_fields; j++)

-		{

-			fields[field_count].device = strtol(args[i], NULL, 0);

-			fields[field_count].num_bits = var->fields[j].num_bits;

-			fields[field_count].out_value = malloc(CEIL(var->fields[j].num_bits, 8));

-			buf_set_u32(fields[field_count].out_value, 0, var->fields[j].num_bits, var->fields[j].value);

-			fields[field_count].out_mask = NULL;

-			fields[field_count].in_value = fields[field_count].out_value;

-			fields[field_count].in_check_mask = NULL;

-			fields[field_count].in_check_value = NULL;

-			fields[field_count].in_handler = field_le_to_host;

-			fields[field_count++].in_handler_priv = &(var->fields[j]);

-		}

-	}

-

-	jtag_add_dr_scan(num_fields, fields, -1);

-	jtag_execute_queue();

-	

-	for (i = 0; i < argc / 2; i++)

-		free(fields[i].out_value);

-

-	free(fields);

-

-	return ERROR_OK;

-}

-

-int handle_verify_ircapture_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

-{

-	if (argc == 1)

-	{

-		if (strcmp(args[0], "enable") == 0)

-		{

-			jtag_verify_capture_ir = 1;

-		}

-		else if (strcmp(args[0], "disable") == 0)

-		{

-			jtag_verify_capture_ir = 0;

-		} else

-		{

-			return ERROR_COMMAND_SYNTAX_ERROR;

-		}

-	} else if (argc != 0)

-	{

-		return ERROR_COMMAND_SYNTAX_ERROR;

-	}

-	

-	command_print(cmd_ctx, "verify Capture-IR is %s", (jtag_verify_capture_ir) ? "enabled": "disabled");

-	

-	return ERROR_OK;

-}

+/***************************************************************************
+ *   Copyright (C) 2005 by Dominic Rath                                    *
+ *   Dominic.Rath@gmx.de                                                   *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "replacements.h"
+
+#include "jtag.h"
+
+#include "command.h"
+#include "log.h"
+#include "interpreter.h"
+
+#include "stdlib.h"
+#include "string.h"
+#include <unistd.h>
+
+char* tap_state_strings[16] =
+{
+	"tlr", 
+	"sds", "cd", "sd", "e1d", "pd", "e2d", "ud",
+	"rti",
+	"sis", "ci", "si", "e1i", "pi", "e2i", "ui"
+};
+
+typedef struct cmd_queue_page_s
+{
+	void *address;
+	size_t used;
+	struct cmd_queue_page_s *next;
+} cmd_queue_page_t;
+
+#define CMD_QUEUE_PAGE_SIZE (1024 * 1024)
+static cmd_queue_page_t *cmd_queue_pages = NULL;
+
+/* tap_move[i][j]: tap movement command to go from state i to state j
+ * 0: Test-Logic-Reset
+ * 1: Run-Test/Idle
+ * 2: Shift-DR
+ * 3: Pause-DR
+ * 4: Shift-IR
+ * 5: Pause-IR
+ * 
+ * SD->SD and SI->SI have to be caught in interface specific code
+ */
+u8 tap_move[6][6] =
+{
+/*	  TLR   RTI   SD    PD    SI    PI             */
+	{0x7f, 0x00, 0x17, 0x0a, 0x1b, 0x16},	/* TLR */
+	{0x7f, 0x00, 0x25, 0x05, 0x2b, 0x0b},	/* RTI */
+	{0x7f, 0x31, 0x00, 0x01, 0x0f, 0x2f},	/* SD  */
+	{0x7f, 0x30, 0x20, 0x17, 0x1e, 0x2f},	/* PD  */
+	{0x7f, 0x31, 0x07, 0x17, 0x00, 0x01},	/* SI  */
+	{0x7f, 0x30, 0x1c, 0x17, 0x20, 0x2f}	/* PI  */
+};
+
+int tap_move_map[16] = {
+	0, -1, -1,  2, -1,  3, -1, -1,
+	1, -1, -1,  4, -1,  5, -1, -1
+};
+
+tap_transition_t tap_transitions[16] =
+{
+	{TAP_TLR, TAP_RTI},		/* TLR */
+	{TAP_SIS, TAP_CD},		/* SDS */
+	{TAP_E1D, TAP_SD},		/* CD  */
+	{TAP_E1D, TAP_SD},		/* SD  */
+	{TAP_UD,  TAP_PD}, 		/* E1D */
+	{TAP_E2D, TAP_PD},		/* PD  */
+	{TAP_UD,  TAP_SD},		/* E2D */
+	{TAP_SDS, TAP_RTI},		/* UD  */
+	{TAP_SDS, TAP_RTI},		/* RTI */
+	{TAP_TLR, TAP_CI},		/* SIS */
+	{TAP_E1I, TAP_SI},		/* CI  */
+	{TAP_E1I, TAP_SI},		/* SI  */
+	{TAP_UI,  TAP_PI}, 		/* E1I */
+	{TAP_E2I, TAP_PI},		/* PI  */
+	{TAP_UI,  TAP_SI},		/* E2I */
+	{TAP_SDS, TAP_RTI}		/* UI  */
+};
+
+char* jtag_event_strings[] =
+{
+	"SRST asserted",
+	"TRST asserted",
+	"SRST released",
+	"TRST released"
+};
+
+enum tap_state end_state = TAP_TLR;
+enum tap_state cur_state = TAP_TLR;
+int jtag_trst = 0;
+int jtag_srst = 0;
+
+jtag_command_t *jtag_command_queue = NULL;
+jtag_command_t **last_comand_pointer = &jtag_command_queue;
+jtag_device_t *jtag_devices = NULL;
+int jtag_num_devices = 0;
+int jtag_ir_scan_size = 0;
+enum reset_types jtag_reset_config = RESET_NONE;
+enum tap_state cmd_queue_end_state = TAP_TLR;
+enum tap_state cmd_queue_cur_state = TAP_TLR;
+
+int jtag_verify_capture_ir = 1;
+
+/* how long the OpenOCD should wait before attempting JTAG communication after reset lines deasserted (in ms) */
+int jtag_nsrst_delay = 0; /* default to no nSRST delay */
+int jtag_ntrst_delay = 0; /* default to no nTRST delay */ 
+
+/* maximum number of JTAG devices expected in the chain
+ */
+#define JTAG_MAX_CHAIN_SIZE 20 
+
+/* callbacks to inform high-level handlers about JTAG state changes */
+jtag_event_callback_t *jtag_event_callbacks;
+
+/* jtag interfaces (parport, FTDI-USB, TI-USB, ...)
+ */
+#if BUILD_PARPORT == 1
+	extern jtag_interface_t parport_interface;
+#endif
+
+#if BUILD_FT2232_FTD2XX == 1
+	extern jtag_interface_t ft2232_interface;
+#endif
+
+#if BUILD_FT2232_LIBFTDI == 1
+	extern jtag_interface_t ft2232_interface;
+#endif
+
+#if BUILD_AMTJTAGACCEL == 1
+	extern jtag_interface_t amt_jtagaccel_interface;
+#endif
+
+#if BUILD_EP93XX == 1
+	extern jtag_interface_t ep93xx_interface;
+#endif
+
+#if BUILD_AT91RM9200 == 1
+	extern jtag_interface_t at91rm9200_interface;
+#endif
+
+#if BUILD_GW16012 == 1
+	extern jtag_interface_t gw16012_interface;
+#endif
+
+#if BUILD_PRESTO_LIBFTDI == 1 || BUILD_PRESTO_FTD2XX == 1
+	extern jtag_interface_t presto_interface;
+#endif
+
+#if BUILD_USBPROG == 1
+	extern jtag_interface_t usbprog_interface;
+#endif
+
+jtag_interface_t *jtag_interfaces[] = {
+#if BUILD_PARPORT == 1
+	&parport_interface,
+#endif
+#if BUILD_FT2232_FTD2XX == 1
+	&ft2232_interface,
+#endif
+#if BUILD_FT2232_LIBFTDI == 1
+	&ft2232_interface,
+#endif
+#if BUILD_AMTJTAGACCEL == 1
+	&amt_jtagaccel_interface,
+#endif
+#if BUILD_EP93XX == 1
+	&ep93xx_interface,
+#endif
+#if BUILD_AT91RM9200 == 1
+	&at91rm9200_interface,
+#endif
+#if BUILD_GW16012 == 1
+	&gw16012_interface,
+#endif
+#if BUILD_PRESTO_LIBFTDI == 1 || BUILD_PRESTO_FTD2XX == 1
+	&presto_interface,
+#endif
+#if BUILD_USBPROG == 1
+	&usbprog_interface,
+#endif
+	NULL,
+};
+
+jtag_interface_t *jtag = NULL;
+
+/* configuration */
+jtag_interface_t *jtag_interface = NULL;
+int jtag_speed = 0;
+
+
+/* forward declarations */
+int jtag_add_statemove(enum tap_state endstate);
+int jtag_add_pathmove(int num_states, enum tap_state *path);
+int jtag_add_runtest(int num_cycles, enum tap_state endstate);
+int jtag_add_reset(int trst, int srst);
+int jtag_add_end_state(enum tap_state endstate);
+int jtag_add_sleep(u32 us);
+int jtag_execute_queue(void);
+int jtag_cancel_queue(void);
+
+/* jtag commands */
+int handle_interface_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int handle_jtag_speed_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int handle_jtag_device_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int handle_reset_config_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int handle_jtag_nsrst_delay_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int handle_jtag_ntrst_delay_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+
+int handle_scan_chain_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+
+int handle_endstate_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int handle_jtag_reset_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int handle_runtest_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int handle_statemove_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int handle_irscan_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int handle_drscan_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+
+int handle_verify_ircapture_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+
+int jtag_register_event_callback(int (*callback)(enum jtag_event event, void *priv), void *priv)
+{
+	jtag_event_callback_t **callbacks_p = &jtag_event_callbacks;
+	
+	if (callback == NULL)
+	{
+		return ERROR_INVALID_ARGUMENTS;
+	}
+	
+	if (*callbacks_p)
+	{
+		while ((*callbacks_p)->next)
+			callbacks_p = &((*callbacks_p)->next);
+		callbacks_p = &((*callbacks_p)->next);
+	}
+	
+	(*callbacks_p) = malloc(sizeof(jtag_event_callback_t));
+	(*callbacks_p)->callback = callback;
+	(*callbacks_p)->priv = priv;
+	(*callbacks_p)->next = NULL;
+	
+	return ERROR_OK;
+}
+
+int jtag_unregister_event_callback(int (*callback)(enum jtag_event event, void *priv))
+{
+	jtag_event_callback_t **callbacks_p = &jtag_event_callbacks;
+	
+	if (callback == NULL)
+	{
+		return ERROR_INVALID_ARGUMENTS;
+	}
+		
+	while (*callbacks_p)
+	{
+		jtag_event_callback_t **next = &((*callbacks_p)->next);
+		if ((*callbacks_p)->callback == callback)
+		{
+			free(*callbacks_p);
+			*callbacks_p = *next;
+		}
+		callbacks_p = next;
+	}
+	
+	return ERROR_OK;
+}
+
+int jtag_call_event_callbacks(enum jtag_event event)
+{
+	jtag_event_callback_t *callback = jtag_event_callbacks;
+	
+	DEBUG("jtag event: %s", jtag_event_strings[event]);
+	
+	while (callback)
+	{
+		callback->callback(event, callback->priv);
+		callback = callback->next;
+	}
+	
+	return ERROR_OK;
+}
+
+/* returns a pointer to the pointer of the last command in queue
+ * this may be a pointer to the root pointer (jtag_command_queue)
+ * or to the next member of the last but one command
+ */
+jtag_command_t** jtag_get_last_command_p(void)
+{
+/*	jtag_command_t *cmd = jtag_command_queue;
+	
+	if (cmd)
+		while (cmd->next)
+			cmd = cmd->next;
+	else
+		return &jtag_command_queue;
+	
+	return &cmd->next;*/
+	
+	return last_comand_pointer;
+}
+
+/* returns a pointer to the n-th device in the scan chain */
+jtag_device_t* jtag_get_device(int num)
+{
+	jtag_device_t *device = jtag_devices;
+	int i = 0;
+
+	while (device)
+	{
+		if (num == i)
+			return device;
+		device = device->next;
+		i++;
+	}
+	
+	ERROR("jtag device number %d not defined", num);
+	exit(-1);
+}
+
+void* cmd_queue_alloc(size_t size)
+{
+	cmd_queue_page_t **p_page = &cmd_queue_pages;
+	int offset;
+
+	if (*p_page)
+	{
+		while ((*p_page)->next)
+			p_page = &((*p_page)->next);
+		if (CMD_QUEUE_PAGE_SIZE - (*p_page)->used < size)
+			p_page = &((*p_page)->next);
+	}
+
+	if (!*p_page)
+	{
+		*p_page = malloc(sizeof(cmd_queue_page_t));
+		(*p_page)->used = 0;
+		(*p_page)->address = malloc(CMD_QUEUE_PAGE_SIZE);
+		(*p_page)->next = NULL;
+	}
+
+	offset = (*p_page)->used;
+	(*p_page)->used += size;
+	
+	u8 *t=(u8 *)((*p_page)->address);
+	return t + offset;
+}
+
+void cmd_queue_free()
+{
+	cmd_queue_page_t *page = cmd_queue_pages;
+
+	while (page)
+	{
+		cmd_queue_page_t *last = page;
+		free(page->address);
+		page = page->next;
+		free(last);
+	}
+
+	cmd_queue_pages = NULL;
+}
+
+int jtag_add_ir_scan(int num_fields, scan_field_t *fields, enum tap_state state)
+{
+	jtag_command_t **last_cmd;
+	jtag_device_t *device;
+	int i, j;
+	int scan_size = 0;
+
+	if (jtag_trst == 1)
+	{
+		WARNING("JTAG command queued, while TRST is low (TAP in reset)");
+		return ERROR_JTAG_TRST_ASSERTED;
+	}
+
+	last_cmd = jtag_get_last_command_p();
+	
+	/* allocate memory for a new list member */
+	*last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
+	(*last_cmd)->next = NULL;
+	last_comand_pointer = &((*last_cmd)->next);
+	(*last_cmd)->type = JTAG_SCAN;
+
+	/* allocate memory for ir scan command */
+	(*last_cmd)->cmd.scan = cmd_queue_alloc(sizeof(scan_command_t));
+	(*last_cmd)->cmd.scan->ir_scan = 1;
+	(*last_cmd)->cmd.scan->num_fields = jtag_num_devices;	/* one field per device */
+	(*last_cmd)->cmd.scan->fields = cmd_queue_alloc(jtag_num_devices * sizeof(scan_field_t));
+	(*last_cmd)->cmd.scan->end_state = state;
+		
+	if (state != -1)
+		cmd_queue_end_state = state;
+
+	if (cmd_queue_cur_state == TAP_TLR && cmd_queue_end_state != TAP_TLR)
+		jtag_call_event_callbacks(JTAG_TRST_RELEASED);
+	
+	if (cmd_queue_end_state == TAP_TLR)
+		jtag_call_event_callbacks(JTAG_TRST_ASSERTED);
+	
+	cmd_queue_cur_state = cmd_queue_end_state;
+		
+	for (i = 0; i < jtag_num_devices; i++)
+	{
+		int found = 0;
+		device = jtag_get_device(i);
+		scan_size = device->ir_length;
+		(*last_cmd)->cmd.scan->fields[i].device = i;
+		(*last_cmd)->cmd.scan->fields[i].num_bits = scan_size;
+		(*last_cmd)->cmd.scan->fields[i].in_value = NULL;
+		(*last_cmd)->cmd.scan->fields[i].in_handler = NULL;	/* disable verification by default */
+
+		/* search the list */
+		for (j = 0; j < num_fields; j++)
+		{
+			if (i == fields[j].device)
+			{
+				found = 1;
+				(*last_cmd)->cmd.scan->fields[i].out_value = buf_cpy(fields[j].out_value, cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);
+				(*last_cmd)->cmd.scan->fields[i].out_mask = buf_cpy(fields[j].out_mask, cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);
+			
+				if (jtag_verify_capture_ir)
+				{
+					if (fields[j].in_handler==NULL)
+					{
+						jtag_set_check_value((*last_cmd)->cmd.scan->fields+i, device->expected, device->expected_mask, NULL);
+					} else
+					{
+						(*last_cmd)->cmd.scan->fields[i].in_handler = fields[j].in_handler;
+						(*last_cmd)->cmd.scan->fields[i].in_handler_priv = fields[j].in_handler_priv;
+						(*last_cmd)->cmd.scan->fields[i].in_check_value = device->expected; 
+						(*last_cmd)->cmd.scan->fields[i].in_check_mask = device->expected_mask;
+					}
+				}
+				
+				device->bypass = 0;
+				break;
+			}
+		}
+	
+		if (!found)
+		{
+			/* if a device isn't listed, set it to BYPASS */
+			(*last_cmd)->cmd.scan->fields[i].out_value = buf_set_ones(cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);
+			(*last_cmd)->cmd.scan->fields[i].out_mask = NULL;
+			device->bypass = 1;
+		
+		}
+		
+		/* update device information */
+		buf_cpy((*last_cmd)->cmd.scan->fields[i].out_value, jtag_get_device(i)->cur_instr, scan_size);
+	}
+	
+	return ERROR_OK;
+}
+
+int jtag_add_plain_ir_scan(int num_fields, scan_field_t *fields, enum tap_state state)
+{
+	jtag_command_t **last_cmd;
+	int i;
+
+	if (jtag_trst == 1)
+	{
+		WARNING("JTAG command queued, while TRST is low (TAP in reset)");
+		return ERROR_JTAG_TRST_ASSERTED;
+	}
+
+	last_cmd = jtag_get_last_command_p();
+	
+	/* allocate memory for a new list member */
+	*last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
+	(*last_cmd)->next = NULL;
+	last_comand_pointer = &((*last_cmd)->next);
+	(*last_cmd)->type = JTAG_SCAN;
+
+	/* allocate memory for ir scan command */
+	(*last_cmd)->cmd.scan = cmd_queue_alloc(sizeof(scan_command_t));
+	(*last_cmd)->cmd.scan->ir_scan = 1;
+	(*last_cmd)->cmd.scan->num_fields = num_fields;
+	(*last_cmd)->cmd.scan->fields = cmd_queue_alloc(num_fields * sizeof(scan_field_t));
+	(*last_cmd)->cmd.scan->end_state = state;
+
+	if (state != -1)
+		cmd_queue_end_state = state;
+
+	if (cmd_queue_cur_state == TAP_TLR && cmd_queue_end_state != TAP_TLR)
+		jtag_call_event_callbacks(JTAG_TRST_RELEASED);
+	
+	if (cmd_queue_end_state == TAP_TLR)
+		jtag_call_event_callbacks(JTAG_TRST_ASSERTED);
+		
+	cmd_queue_cur_state = cmd_queue_end_state;
+	
+	for (i = 0; i < num_fields; i++)
+	{
+		int num_bits = fields[i].num_bits;
+		int num_bytes = CEIL(fields[i].num_bits, 8);
+		(*last_cmd)->cmd.scan->fields[i].device = fields[i].device;
+		(*last_cmd)->cmd.scan->fields[i].num_bits = num_bits;
+		(*last_cmd)->cmd.scan->fields[i].out_value = buf_cpy(fields[i].out_value, cmd_queue_alloc(num_bytes), num_bits);
+		(*last_cmd)->cmd.scan->fields[i].out_mask = buf_cpy(fields[i].out_mask, cmd_queue_alloc(num_bytes), num_bits);
+		(*last_cmd)->cmd.scan->fields[i].in_value = fields[i].in_value;
+		(*last_cmd)->cmd.scan->fields[i].in_check_value = fields[i].in_check_value;
+		(*last_cmd)->cmd.scan->fields[i].in_check_mask = fields[i].in_check_mask;
+		(*last_cmd)->cmd.scan->fields[i].in_handler = NULL;
+		(*last_cmd)->cmd.scan->fields[i].in_handler_priv = NULL;
+	}
+	return ERROR_OK;
+}
+
+int jtag_add_dr_scan(int num_fields, scan_field_t *fields, enum tap_state state)
+{
+	int i, j;
+	int bypass_devices = 0;
+	int field_count = 0;
+	jtag_command_t **last_cmd = jtag_get_last_command_p();
+	jtag_device_t *device = jtag_devices;
+	int scan_size;
+
+	if (jtag_trst == 1)
+	{
+		WARNING("JTAG command queued, while TRST is low (TAP in reset)");
+		return ERROR_JTAG_TRST_ASSERTED;
+	}
+
+	/* count devices in bypass */
+	while (device)
+	{
+		if (device->bypass)
+			bypass_devices++;
+		device = device->next;
+	}
+	
+	/* allocate memory for a new list member */
+	*last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
+	last_comand_pointer = &((*last_cmd)->next);
+	(*last_cmd)->next = NULL;
+	(*last_cmd)->type = JTAG_SCAN;
+
+	/* allocate memory for dr scan command */
+	(*last_cmd)->cmd.scan = cmd_queue_alloc(sizeof(scan_command_t));
+	(*last_cmd)->cmd.scan->ir_scan = 0;
+	(*last_cmd)->cmd.scan->num_fields = num_fields + bypass_devices;
+	(*last_cmd)->cmd.scan->fields = cmd_queue_alloc((num_fields + bypass_devices) * sizeof(scan_field_t));
+	(*last_cmd)->cmd.scan->end_state = state;
+	
+	if (state != -1)
+		cmd_queue_end_state = state;
+
+	if (cmd_queue_cur_state == TAP_TLR && cmd_queue_end_state != TAP_TLR)
+		jtag_call_event_callbacks(JTAG_TRST_RELEASED);
+	
+	if (cmd_queue_end_state == TAP_TLR)
+		jtag_call_event_callbacks(JTAG_TRST_ASSERTED);
+			
+	cmd_queue_cur_state = cmd_queue_end_state;
+	
+	for (i = 0; i < jtag_num_devices; i++)
+	{
+		int found = 0;
+		(*last_cmd)->cmd.scan->fields[field_count].device = i;
+	
+		for (j = 0; j < num_fields; j++)
+		{
+			if (i == fields[j].device)
+			{
+				found = 1;
+				scan_size = fields[j].num_bits;
+				(*last_cmd)->cmd.scan->fields[field_count].num_bits = scan_size;
+				(*last_cmd)->cmd.scan->fields[field_count].out_value = buf_cpy(fields[j].out_value, cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);
+				(*last_cmd)->cmd.scan->fields[field_count].out_mask = buf_cpy(fields[j].out_mask, cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);
+				(*last_cmd)->cmd.scan->fields[field_count].in_value = fields[j].in_value;
+				(*last_cmd)->cmd.scan->fields[field_count].in_check_value = fields[j].in_check_value;
+				(*last_cmd)->cmd.scan->fields[field_count].in_check_mask = fields[j].in_check_mask;
+				(*last_cmd)->cmd.scan->fields[field_count].in_handler = fields[j].in_handler;
+				(*last_cmd)->cmd.scan->fields[field_count++].in_handler_priv = fields[j].in_handler_priv;
+			}
+		}
+		if (!found)
+		{
+			/* if a device isn't listed, the BYPASS register should be selected */
+			if (!jtag_get_device(i)->bypass)
+			{
+				ERROR("BUG: no scan data for a device not in BYPASS");
+				exit(-1);
+			}
+	
+			/* program the scan field to 1 bit length, and ignore it's value */
+			(*last_cmd)->cmd.scan->fields[field_count].num_bits = 1;
+			(*last_cmd)->cmd.scan->fields[field_count].out_value = NULL;
+			(*last_cmd)->cmd.scan->fields[field_count].out_mask = NULL;
+			(*last_cmd)->cmd.scan->fields[field_count].in_value = NULL;
+			(*last_cmd)->cmd.scan->fields[field_count].in_check_value = NULL;
+			(*last_cmd)->cmd.scan->fields[field_count].in_check_mask = NULL;
+			(*last_cmd)->cmd.scan->fields[field_count].in_handler = NULL;
+			(*last_cmd)->cmd.scan->fields[field_count++].in_handler_priv = NULL;
+		}
+		else
+		{
+			/* if a device is listed, the BYPASS register must not be selected */
+			if (jtag_get_device(i)->bypass)
+			{
+				WARNING("scan data for a device in BYPASS");
+			}
+		}
+	}
+	return ERROR_OK;
+}
+
+
+int jtag_add_plain_dr_scan(int num_fields, scan_field_t *fields, enum tap_state state)
+{
+	int i;
+	jtag_command_t **last_cmd = jtag_get_last_command_p();
+	
+	if (jtag_trst == 1)
+	{
+		WARNING("JTAG command queued, while TRST is low (TAP in reset)");
+		return ERROR_JTAG_TRST_ASSERTED;
+	}
+
+	/* allocate memory for a new list member */
+	*last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
+	last_comand_pointer = &((*last_cmd)->next);
+	(*last_cmd)->next = NULL;
+	(*last_cmd)->type = JTAG_SCAN;
+
+	/* allocate memory for scan command */
+	(*last_cmd)->cmd.scan = cmd_queue_alloc(sizeof(scan_command_t));
+	(*last_cmd)->cmd.scan->ir_scan = 0;
+	(*last_cmd)->cmd.scan->num_fields = num_fields;
+	(*last_cmd)->cmd.scan->fields = cmd_queue_alloc(num_fields * sizeof(scan_field_t));
+	(*last_cmd)->cmd.scan->end_state = state;
+		
+	if (state != -1)
+		cmd_queue_end_state = state;
+
+	if (cmd_queue_cur_state == TAP_TLR && cmd_queue_end_state != TAP_TLR)
+		jtag_call_event_callbacks(JTAG_TRST_RELEASED);
+	
+	if (cmd_queue_end_state == TAP_TLR)
+		jtag_call_event_callbacks(JTAG_TRST_ASSERTED);
+			
+	cmd_queue_cur_state = cmd_queue_end_state;
+	
+	for (i = 0; i < num_fields; i++)
+	{
+		int num_bits = fields[i].num_bits;
+		int num_bytes = CEIL(fields[i].num_bits, 8);
+		(*last_cmd)->cmd.scan->fields[i].device = fields[i].device;
+		(*last_cmd)->cmd.scan->fields[i].num_bits = num_bits;
+		(*last_cmd)->cmd.scan->fields[i].out_value = buf_cpy(fields[i].out_value, cmd_queue_alloc(num_bytes), num_bits);
+		(*last_cmd)->cmd.scan->fields[i].out_mask = buf_cpy(fields[i].out_mask, cmd_queue_alloc(num_bytes), num_bits);
+		(*last_cmd)->cmd.scan->fields[i].in_value = fields[i].in_value;
+		(*last_cmd)->cmd.scan->fields[i].in_check_value = fields[i].in_check_value;
+		(*last_cmd)->cmd.scan->fields[i].in_check_mask = fields[i].in_check_mask;
+		(*last_cmd)->cmd.scan->fields[i].in_handler = fields[i].in_handler;
+		(*last_cmd)->cmd.scan->fields[i].in_handler_priv = fields[i].in_handler_priv;
+	}
+
+	return ERROR_OK;
+}
+int jtag_add_statemove(enum tap_state state)
+{
+	jtag_command_t **last_cmd = jtag_get_last_command_p();
+	
+	if (jtag_trst == 1)
+	{
+		WARNING("JTAG command queued, while TRST is low (TAP in reset)");
+		return ERROR_JTAG_TRST_ASSERTED;
+	}
+
+	/* allocate memory for a new list member */
+	*last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
+	last_comand_pointer = &((*last_cmd)->next);
+	(*last_cmd)->next = NULL;
+	(*last_cmd)->type = JTAG_STATEMOVE;
+
+	(*last_cmd)->cmd.statemove = cmd_queue_alloc(sizeof(statemove_command_t));
+	(*last_cmd)->cmd.statemove->end_state = state;
+	
+	if (state != -1)
+		cmd_queue_end_state = state;
+
+	if (cmd_queue_cur_state == TAP_TLR && cmd_queue_end_state != TAP_TLR)
+		jtag_call_event_callbacks(JTAG_TRST_RELEASED);
+	
+	if (cmd_queue_end_state == TAP_TLR)
+		jtag_call_event_callbacks(JTAG_TRST_ASSERTED);
+			
+	cmd_queue_cur_state = cmd_queue_end_state;
+	
+	return ERROR_OK;
+}
+
+int jtag_add_pathmove(int num_states, enum tap_state *path)
+{
+	jtag_command_t **last_cmd = jtag_get_last_command_p();
+	int i;
+	
+	if (jtag_trst == 1)
+	{
+		WARNING("JTAG command queued, while TRST is low (TAP in reset)");
+		return ERROR_JTAG_TRST_ASSERTED;
+	}
+	
+	/* the last state has to be a stable state */
+	if (tap_move_map[path[num_states - 1]] == -1)
+	{
+		ERROR("TAP path doesn't finish in a stable state");
+		return ERROR_JTAG_NOT_IMPLEMENTED;
+	}
+	
+	/* allocate memory for a new list member */
+	*last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
+	last_comand_pointer = &((*last_cmd)->next);
+	(*last_cmd)->next = NULL;
+	(*last_cmd)->type = JTAG_PATHMOVE;
+
+	(*last_cmd)->cmd.pathmove = cmd_queue_alloc(sizeof(pathmove_command_t));
+	(*last_cmd)->cmd.pathmove->num_states = num_states;
+	(*last_cmd)->cmd.pathmove->path = cmd_queue_alloc(sizeof(enum tap_state) * num_states);
+	
+	for (i = 0; i < num_states; i++)
+		(*last_cmd)->cmd.pathmove->path[i] = path[i];
+
+	if (cmd_queue_cur_state == TAP_TLR && cmd_queue_end_state != TAP_TLR)
+		jtag_call_event_callbacks(JTAG_TRST_RELEASED);
+	
+	if (cmd_queue_end_state == TAP_TLR)
+		jtag_call_event_callbacks(JTAG_TRST_ASSERTED);
+	
+	cmd_queue_cur_state = path[num_states - 1];
+	
+	return ERROR_OK;
+}
+
+int jtag_add_runtest(int num_cycles, enum tap_state state)
+{
+	jtag_command_t **last_cmd = jtag_get_last_command_p();
+	
+	if (jtag_trst == 1)
+	{
+		WARNING("JTAG command queued, while TRST is low (TAP in reset)");
+		return ERROR_JTAG_TRST_ASSERTED;
+	}
+
+	/* allocate memory for a new list member */
+	*last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
+	(*last_cmd)->next = NULL;
+	last_comand_pointer = &((*last_cmd)->next);
+	(*last_cmd)->type = JTAG_RUNTEST;
+
+	(*last_cmd)->cmd.runtest = cmd_queue_alloc(sizeof(runtest_command_t));
+	(*last_cmd)->cmd.runtest->num_cycles = num_cycles;
+	(*last_cmd)->cmd.runtest->end_state = state;
+	
+	if (state != -1)
+		cmd_queue_end_state = state;
+
+	if (cmd_queue_cur_state == TAP_TLR && cmd_queue_end_state != TAP_TLR)
+		jtag_call_event_callbacks(JTAG_TRST_RELEASED);
+	
+	if (cmd_queue_end_state == TAP_TLR)
+		jtag_call_event_callbacks(JTAG_TRST_ASSERTED);
+			
+	cmd_queue_cur_state = cmd_queue_end_state;
+	
+	return ERROR_OK;
+}
+
+int jtag_add_reset(int req_trst, int req_srst)
+{
+	int trst_with_tms = 0;
+	
+	jtag_command_t **last_cmd = jtag_get_last_command_p();
+	
+	if (req_trst == -1)
+		req_trst = jtag_trst;
+	
+	if (req_srst == -1)
+		req_srst = jtag_srst;
+
+	/* Make sure that jtag_reset_config allows the requested reset */
+	/* if SRST pulls TRST, we can't fulfill srst == 1 with trst == 0 */
+	if (((jtag_reset_config & RESET_SRST_PULLS_TRST) && (req_srst == 1)) && (req_trst == 0))
+		return ERROR_JTAG_RESET_WOULD_ASSERT_TRST;
+		
+	/* if TRST pulls SRST, we reset with TAP T-L-R */
+	if (((jtag_reset_config & RESET_TRST_PULLS_SRST) && (req_trst == 1)) && (req_srst == 0))
+	{
+		req_trst = 0;
+		trst_with_tms = 1;
+	}
+	
+	if (req_srst && !(jtag_reset_config & RESET_HAS_SRST))
+	{
+		ERROR("requested nSRST assertion, but the current configuration doesn't support this");
+		return ERROR_JTAG_RESET_CANT_SRST;
+	}
+	
+	if (req_trst && !(jtag_reset_config & RESET_HAS_TRST))
+	{
+		req_trst = 0;
+		trst_with_tms = 1;
+	}
+	
+	/* allocate memory for a new list member */
+	*last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
+	(*last_cmd)->next = NULL;
+	last_comand_pointer = &((*last_cmd)->next);
+	(*last_cmd)->type = JTAG_RESET;
+
+	(*last_cmd)->cmd.reset = cmd_queue_alloc(sizeof(reset_command_t));
+	(*last_cmd)->cmd.reset->trst = req_trst;
+	(*last_cmd)->cmd.reset->srst = req_srst;
+
+	jtag_trst = req_trst;
+	jtag_srst = req_srst;
+
+	if (jtag_srst)
+	{
+		jtag_call_event_callbacks(JTAG_SRST_ASSERTED);
+	}
+	else
+	{
+		jtag_call_event_callbacks(JTAG_SRST_RELEASED);
+		if (jtag_nsrst_delay)
+			jtag_add_sleep(jtag_nsrst_delay * 1000);
+	}
+	
+	if (trst_with_tms)
+	{
+		last_cmd = &((*last_cmd)->next);
+		
+		/* allocate memory for a new list member */
+		*last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
+		(*last_cmd)->next = NULL;
+		last_comand_pointer = &((*last_cmd)->next);
+		(*last_cmd)->type = JTAG_STATEMOVE;
+
+		(*last_cmd)->cmd.statemove = cmd_queue_alloc(sizeof(statemove_command_t));
+		(*last_cmd)->cmd.statemove->end_state = TAP_TLR;
+		
+		jtag_call_event_callbacks(JTAG_TRST_ASSERTED);
+		cmd_queue_cur_state = TAP_TLR;
+		cmd_queue_end_state = TAP_TLR;
+		
+		return ERROR_OK;
+	}
+	else
+	{
+		if (jtag_trst)
+		{
+			/* we just asserted nTRST, so we're now in Test-Logic-Reset,
+			 * and inform possible listeners about this
+			 */
+			cmd_queue_cur_state = TAP_TLR;
+			jtag_call_event_callbacks(JTAG_TRST_ASSERTED);
+		}
+		else
+		{
+			/* the nTRST line got deasserted, so we're still in Test-Logic-Reset,
+			 * but we might want to add a delay to give the TAP time to settle
+			 */
+			if (jtag_ntrst_delay)
+				jtag_add_sleep(jtag_ntrst_delay * 1000);
+		}
+	}
+
+	return ERROR_OK;
+}
+
+int jtag_add_end_state(enum tap_state state)
+{
+	jtag_command_t **last_cmd = jtag_get_last_command_p();
+	
+	/* allocate memory for a new list member */
+	*last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
+	(*last_cmd)->next = NULL;
+	last_comand_pointer = &((*last_cmd)->next);
+	(*last_cmd)->type = JTAG_END_STATE;
+
+	(*last_cmd)->cmd.end_state = cmd_queue_alloc(sizeof(end_state_command_t));
+	(*last_cmd)->cmd.end_state->end_state = state;
+
+	if (state != -1)
+		cmd_queue_end_state = state;
+	
+	return ERROR_OK;
+}
+
+int jtag_add_sleep(u32 us)
+{
+	jtag_command_t **last_cmd = jtag_get_last_command_p();
+	
+	/* allocate memory for a new list member */
+	*last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
+	(*last_cmd)->next = NULL;
+	last_comand_pointer = &((*last_cmd)->next);
+	(*last_cmd)->type = JTAG_SLEEP;
+
+	(*last_cmd)->cmd.sleep = cmd_queue_alloc(sizeof(sleep_command_t));
+	(*last_cmd)->cmd.sleep->us = us;
+	
+	return ERROR_OK;
+}
+
+int jtag_scan_size(scan_command_t *cmd)
+{
+	int bit_count = 0;
+	int i;
+
+	/* count bits in scan command */
+	for (i = 0; i < cmd->num_fields; i++)
+	{
+		bit_count += cmd->fields[i].num_bits;
+	}
+
+	return bit_count;
+}
+
+int jtag_build_buffer(scan_command_t *cmd, u8 **buffer)
+{
+	int bit_count = 0;
+	int i;
+	
+	bit_count = jtag_scan_size(cmd);
+	*buffer = malloc(CEIL(bit_count, 8));
+	
+	bit_count = 0;
+
+	for (i = 0; i < cmd->num_fields; i++)
+	{
+		if (cmd->fields[i].out_value)
+		{
+#ifdef _DEBUG_JTAG_IO_
+			char* char_buf = buf_to_str(cmd->fields[i].out_value, (cmd->fields[i].num_bits > 64) ? 64 : cmd->fields[i].num_bits, 16);
+#endif
+			buf_set_buf(cmd->fields[i].out_value, 0, *buffer, bit_count, cmd->fields[i].num_bits);
+#ifdef _DEBUG_JTAG_IO_
+			DEBUG("fields[%i].out_value: 0x%s", i, char_buf);
+			free(char_buf);
+#endif
+		}
+		
+		bit_count += cmd->fields[i].num_bits;
+	}
+
+	return bit_count;
+
+}
+
+int jtag_read_buffer(u8 *buffer, scan_command_t *cmd)
+{
+	int i;
+	int bit_count = 0;
+	int retval;
+	
+	/* we return ERROR_OK, unless a check fails, or a handler reports a problem */
+	retval = ERROR_OK;
+	
+	for (i = 0; i < cmd->num_fields; i++)
+	{
+		/* if neither in_value nor in_handler
+		 * are specified we don't have to examine this field
+		 */
+		if (cmd->fields[i].in_value || cmd->fields[i].in_handler)
+		{
+			int num_bits = cmd->fields[i].num_bits;
+			u8 *captured = buf_set_buf(buffer, bit_count, malloc(CEIL(num_bits, 8)), 0, num_bits);
+			
+#ifdef _DEBUG_JTAG_IO_
+			char *char_buf;
+
+			char_buf = buf_to_str(captured, (num_bits > 64) ? 64 : num_bits, 16);
+			DEBUG("fields[%i].in_value: 0x%s", i, char_buf);
+			free(char_buf);
+#endif
+			
+			if (cmd->fields[i].in_value)
+			{
+				buf_cpy(captured, cmd->fields[i].in_value, num_bits);
+				
+				if (cmd->fields[i].in_handler)
+				{
+					if (cmd->fields[i].in_handler(cmd->fields[i].in_value, cmd->fields[i].in_handler_priv, cmd->fields+i) != ERROR_OK)
+					{
+						WARNING("in_handler reported a failed check");
+						retval = ERROR_JTAG_QUEUE_FAILED;
+					}
+				}
+			}
+			
+			/* no in_value specified, but a handler takes care of the scanned data */
+			if (cmd->fields[i].in_handler && (!cmd->fields[i].in_value))
+			{
+				if (cmd->fields[i].in_handler(captured, cmd->fields[i].in_handler_priv, cmd->fields+i) != ERROR_OK)
+				{
+					/* We're going to call the error:handler later, but if the in_handler
+					 * reported an error we report this failure upstream
+					 */
+					WARNING("in_handler reported a failed check");
+					retval = ERROR_JTAG_QUEUE_FAILED;
+				}
+			}
+
+			free(captured);
+		}
+		bit_count += cmd->fields[i].num_bits;
+	}
+
+	return retval;
+}
+
+int jtag_check_value(u8 *captured, void *priv, scan_field_t *field)
+{
+	int retval = ERROR_OK;
+	int num_bits = field->num_bits;
+	
+	int compare_failed = 0;
+	
+	if (field->in_check_mask)
+		compare_failed = buf_cmp_mask(captured, field->in_check_value, field->in_check_mask, num_bits);
+	else
+		compare_failed = buf_cmp(captured, field->in_check_value, num_bits);
+	
+	if (compare_failed)
+		{
+		/* An error handler could have caught the failing check
+		 * only report a problem when there wasn't a handler, or if the handler
+		 * acknowledged the error
+		 */ 
+		if (compare_failed)
+		{
+			char *captured_char = buf_to_str(captured, (num_bits > 64) ? 64 : num_bits, 16);
+			char *in_check_value_char = buf_to_str(field->in_check_value, (num_bits > 64) ? 64 : num_bits, 16);
+
+			if (field->in_check_mask)
+			{
+				char *in_check_mask_char;
+				in_check_mask_char = buf_to_str(field->in_check_mask, (num_bits > 64) ? 64 : num_bits, 16);
+				WARNING("value captured during scan didn't pass the requested check: captured: 0x%s check_value: 0x%s check_mask: 0x%s", captured_char, in_check_value_char, in_check_mask_char);
+				free(in_check_mask_char);
+			}
+			else
+			{
+				WARNING("value captured during scan didn't pass the requested check: captured: 0x%s check_value: 0x%s", captured_char, in_check_value_char);
+			}
+
+			free(captured_char);
+			free(in_check_value_char);
+			
+			retval = ERROR_JTAG_QUEUE_FAILED;
+		}
+		
+	}
+	return retval;
+}
+
+/* 
+  set up checking of this field using the in_handler. The values passed in must be valid until
+  after jtag_execute() has completed.
+ */
+void jtag_set_check_value(scan_field_t *field, u8 *value, u8 *mask, error_handler_t *in_error_handler)
+{
+	if (value)
+		field->in_handler = jtag_check_value;
+	else
+		field->in_handler = NULL;	/* No check, e.g. embeddedice uses value==NULL to indicate no check */
+	field->in_handler_priv = NULL;	/* this will be filled in at the invocation site to point to the field duplicate */ 
+	field->in_check_value = value;
+	field->in_check_mask = mask;
+}
+
+enum scan_type jtag_scan_type(scan_command_t *cmd)
+{
+	int i;
+	int type = 0;
+	
+	for (i = 0; i < cmd->num_fields; i++)
+	{
+		if (cmd->fields[i].in_value || cmd->fields[i].in_handler)
+			type |= SCAN_IN;
+		if (cmd->fields[i].out_value)
+			type |= SCAN_OUT;
+	}
+
+	return type;
+}
+
+int jtag_execute_queue(void)
+{
+	int retval;
+
+	retval = jtag->execute_queue();
+	
+	cmd_queue_free();
+
+	jtag_command_queue = NULL;
+	last_comand_pointer = &jtag_command_queue;
+
+	return retval;
+}
+
+int jtag_reset_callback(enum jtag_event event, void *priv)
+{
+	jtag_device_t *device = priv;
+
+	DEBUG("-");
+	
+	if (event == JTAG_TRST_ASSERTED)
+	{
+		buf_set_ones(device->cur_instr, device->ir_length);
+		device->bypass = 1;
+	}
+	
+	return ERROR_OK;
+}
+
+void jtag_sleep(u32 us)
+{
+	usleep(us);
+}
+
+/* Try to examine chain layout according to IEEE 1149.1 §12
+ */
+int jtag_examine_chain()
+{
+	jtag_device_t *device = jtag_devices;
+	scan_field_t field;
+	u8 idcode_buffer[JTAG_MAX_CHAIN_SIZE * 4];
+	int i;
+	int bit_count;
+	int device_count = 0;
+	u8 zero_check = 0x0;
+	u8 one_check = 0xff;
+	
+	field.device = 0;
+	field.num_bits = sizeof(idcode_buffer) * 8;
+	field.out_value = idcode_buffer;
+	field.out_mask = NULL;
+	field.in_value = idcode_buffer;
+	field.in_check_value = NULL;
+	field.in_check_mask = NULL;
+	field.in_handler = NULL;
+	field.in_handler_priv = NULL;
+	
+	for (i = 0; i < JTAG_MAX_CHAIN_SIZE; i++)
+	{
+		buf_set_u32(idcode_buffer, i * 32, 32, 0x000000FF);
+	}
+	
+	jtag_add_plain_dr_scan(1, &field, TAP_TLR);
+	jtag_execute_queue();
+	
+	for (i = 0; i < JTAG_MAX_CHAIN_SIZE * 4; i++)
+	{
+		zero_check |= idcode_buffer[i];
+		one_check &= idcode_buffer[i];
+	}
+	
+	/* if there wasn't a single non-zero bit or if all bits were one, the scan isn't valid */
+	if ((zero_check == 0x00) || (one_check == 0xff))
+	{
+		ERROR("JTAG communication failure, check connection, JTAG interface, target power etc.");
+		return ERROR_JTAG_INIT_FAILED;
+	}
+	
+	for (bit_count = 0; bit_count < (JTAG_MAX_CHAIN_SIZE * 32) - 31;)
+	{
+		u32 idcode = buf_get_u32(idcode_buffer, bit_count, 32);
+		if ((idcode & 1) == 0)
+		{
+			/* LSB must not be 0, this indicates a device in bypass */
+			device_count++;
+			
+			bit_count += 1;
+		}
+		else
+		{
+			u32 manufacturer;
+			u32 part;
+			u32 version;
+			
+			if (idcode == 0x000000FF)
+			{
+				/* End of chain (invalid manufacturer ID) */
+				break;
+			}
+			
+			if (device)
+			{
+				device->idcode = idcode;
+				device = device->next;
+			}
+			device_count++;
+			
+			manufacturer = (idcode & 0xffe) >> 1;
+			part = (idcode & 0xffff000) >> 12;
+			version = (idcode & 0xf0000000) >> 28;
+
+			INFO("JTAG device found: 0x%8.8x (Manufacturer: 0x%3.3x, Part: 0x%4.4x, Version: 0x%1.1x)", 
+				idcode, manufacturer, part, version);
+			
+			bit_count += 32;
+		}
+	}
+	
+	/* see if number of discovered devices matches configuration */
+	if (device_count != jtag_num_devices)
+	{
+		ERROR("number of discovered devices in JTAG chain (%i) doesn't match configuration (%i)", 
+			device_count, jtag_num_devices);
+		ERROR("check the config file and ensure proper JTAG communication (connections, speed, ...)");
+		return ERROR_JTAG_INIT_FAILED;
+	}
+	
+	return ERROR_OK;
+}
+
+int jtag_validate_chain()
+{
+	jtag_device_t *device = jtag_devices;
+	int total_ir_length = 0;
+	u8 *ir_test = NULL;
+	scan_field_t field;
+	int chain_pos = 0;
+	
+	while (device)
+	{
+		total_ir_length += device->ir_length;
+		device = device->next;
+	}
+	
+	total_ir_length += 2;
+	ir_test = malloc(CEIL(total_ir_length, 8));
+	buf_set_ones(ir_test, total_ir_length);
+	
+	field.device = 0;
+	field.num_bits = total_ir_length;
+	field.out_value = ir_test;
+	field.out_mask = NULL;
+	field.in_value = ir_test;
+	field.in_check_value = NULL;
+	field.in_check_mask = NULL;
+	field.in_handler = NULL;
+	field.in_handler_priv = NULL;
+	
+	jtag_add_plain_ir_scan(1, &field, TAP_TLR);
+	jtag_execute_queue();
+	
+	device = jtag_devices;
+	while (device)
+	{
+		if (buf_get_u32(ir_test, chain_pos, 2) != 0x1)
+		{
+			char *cbuf = buf_to_str(ir_test, total_ir_length, 16);
+			ERROR("Error validating JTAG scan chain, IR mismatch, scan returned 0x%s", cbuf);
+			free(cbuf);
+			free(ir_test);
+			return ERROR_JTAG_INIT_FAILED;
+		}
+		chain_pos += device->ir_length;
+		device = device->next;
+	}
+	
+	if (buf_get_u32(ir_test, chain_pos, 2) != 0x3)
+	{
+		char *cbuf = buf_to_str(ir_test, total_ir_length, 16);
+		ERROR("Error validating JTAG scan chain, IR mismatch, scan returned 0x%s", cbuf);
+		free(cbuf);
+		free(ir_test);
+		return ERROR_JTAG_INIT_FAILED;
+	}
+	
+	free(ir_test);
+	
+	return ERROR_OK;
+}
+
+int jtag_register_commands(struct command_context_s *cmd_ctx)
+{
+	register_command(cmd_ctx, NULL, "interface", handle_interface_command,
+		COMMAND_CONFIG, NULL);
+	register_command(cmd_ctx, NULL, "jtag_speed", handle_jtag_speed_command,
+		COMMAND_ANY, "set jtag speed (if supported) <speed>");
+	register_command(cmd_ctx, NULL, "jtag_device", handle_jtag_device_command,
+		COMMAND_CONFIG, "jtag_device <ir_length> <ir_expected> <ir_mask>");
+	register_command(cmd_ctx, NULL, "reset_config", handle_reset_config_command,
+		COMMAND_CONFIG, NULL);
+	register_command(cmd_ctx, NULL, "jtag_nsrst_delay", handle_jtag_nsrst_delay_command,
+		COMMAND_CONFIG, NULL);
+	register_command(cmd_ctx, NULL, "jtag_ntrst_delay", handle_jtag_ntrst_delay_command,
+		COMMAND_CONFIG, NULL);
+		
+	register_command(cmd_ctx, NULL, "scan_chain", handle_scan_chain_command,
+		COMMAND_EXEC, "print current scan chain configuration");
+
+	register_command(cmd_ctx, NULL, "endstate", handle_endstate_command,
+		COMMAND_EXEC, "finish JTAG operations in <tap_state>");
+	register_command(cmd_ctx, NULL, "jtag_reset", handle_jtag_reset_command,
+		COMMAND_EXEC, "toggle reset lines <trst> <srst>");
+	register_command(cmd_ctx, NULL, "runtest", handle_runtest_command,
+		COMMAND_EXEC, "move to Run-Test/Idle, and execute <num_cycles>");
+	register_command(cmd_ctx, NULL, "statemove", handle_statemove_command,
+		COMMAND_EXEC, "move to current endstate or [tap_state]");
+	register_command(cmd_ctx, NULL, "irscan", handle_irscan_command,
+		COMMAND_EXEC, "execute IR scan <device> <instr> [dev2] [instr2] ...");
+	register_command(cmd_ctx, NULL, "drscan", handle_drscan_command,
+		COMMAND_EXEC, "execute DR scan <device> <var> [dev2] [var2] ...");
+
+	register_command(cmd_ctx, NULL, "verify_ircapture", handle_verify_ircapture_command,
+		COMMAND_ANY, "verify value captured during Capture-IR <enable|disable>");
+	return ERROR_OK;
+}
+
+int jtag_interface_init(struct command_context_s *cmd_ctx)
+{
+	if (!jtag_interface)
+	{
+		/* nothing was previously specified by "interface" command */
+		ERROR("JTAG interface has to be specified, see \"interface\" command");
+		return ERROR_JTAG_INVALID_INTERFACE;
+	}
+
+	if (jtag_interface->init() != ERROR_OK)
+		return ERROR_JTAG_INIT_FAILED;
+
+	jtag = jtag_interface;
+	return ERROR_OK;
+}
+
+int jtag_init(struct command_context_s *cmd_ctx)
+{
+	int i, validate_tries = 0;
+	jtag_device_t *device;
+
+	DEBUG("-");
+	
+	if (!jtag && jtag_interface_init(cmd_ctx) != ERROR_OK)
+		return ERROR_JTAG_INIT_FAILED;
+
+	device = jtag_devices;
+	jtag_ir_scan_size = 0;
+	jtag_num_devices = 0;
+	while (device != NULL)
+	{
+		jtag_ir_scan_size += device->ir_length;
+		jtag_num_devices++;
+		device = device->next;
+	}
+	
+	jtag_add_statemove(TAP_TLR);
+	jtag_execute_queue();
+
+	/* examine chain first, as this could discover the real chain layout */
+	if (jtag_examine_chain() != ERROR_OK)
+	{
+		ERROR("trying to validate configured JTAG chain anyway...");
+	}
+	
+	while (jtag_validate_chain() != ERROR_OK)
+	{
+		validate_tries++;
+		if (validate_tries > 5)
+		{
+			ERROR("Could not validate JTAG chain, exit");
+			jtag = NULL;
+			return ERROR_JTAG_INVALID_INTERFACE;
+		}
+		usleep(10000);
+	}
+
+	return ERROR_OK;
+}
+
+int handle_interface_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	int i;
+
+	/* check whether the interface is already configured */
+	if (jtag_interface)
+	{
+		WARNING("Interface already configured, ignoring");
+		return ERROR_OK;
+	}
+
+	/* interface name is a mandatory argument */
+	if (argc < 1 || args[0][0] == '\0')
+	{
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	for (i=0; jtag_interfaces[i]; i++)
+	{
+		if (strcmp(args[0], jtag_interfaces[i]->name) == 0)
+		{
+			if (jtag_interfaces[i]->register_commands(cmd_ctx) != ERROR_OK)
+				exit(-1);
+
+			jtag_interface = jtag_interfaces[i];
+			return ERROR_OK;
+		}
+	}
+
+	/* no valid interface was found (i.e. the configuration option,
+	 * didn't match one of the compiled-in interfaces
+	 */
+	ERROR("No valid jtag interface found (%s)", args[0]);
+	ERROR("compiled-in jtag interfaces:");
+	for (i = 0; jtag_interfaces[i]; i++)
+	{
+		ERROR("%i: %s", i, jtag_interfaces[i]->name);
+	}
+
+	return ERROR_JTAG_INVALID_INTERFACE;
+}
+
+int handle_jtag_device_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	jtag_device_t **last_device_p = &jtag_devices;
+
+	if (*last_device_p)
+	{
+		while ((*last_device_p)->next)
+			last_device_p = &((*last_device_p)->next);
+		last_device_p = &((*last_device_p)->next);
+	}
+
+	if (argc < 3)
+		return ERROR_OK;
+
+	*last_device_p = malloc(sizeof(jtag_device_t));
+	(*last_device_p)->ir_length = strtoul(args[0], NULL, 0);
+
+	(*last_device_p)->expected = malloc((*last_device_p)->ir_length);
+	buf_set_u32((*last_device_p)->expected, 0, (*last_device_p)->ir_length, strtoul(args[1], NULL, 0));
+	(*last_device_p)->expected_mask = malloc((*last_device_p)->ir_length);
+	buf_set_u32((*last_device_p)->expected_mask, 0, (*last_device_p)->ir_length, strtoul(args[2], NULL, 0));
+
+	(*last_device_p)->cur_instr = malloc((*last_device_p)->ir_length);
+	(*last_device_p)->bypass = 1;
+	buf_set_ones((*last_device_p)->cur_instr, (*last_device_p)->ir_length);
+	
+	(*last_device_p)->next = NULL;
+	
+	jtag_register_event_callback(jtag_reset_callback, (*last_device_p));
+	
+	jtag_num_devices++;
+
+	return ERROR_OK;
+}
+
+int handle_scan_chain_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	jtag_device_t *device = jtag_devices;
+	int device_count = 0;
+	
+	while (device)
+	{
+		u32 expected, expected_mask, cur_instr;
+		expected = buf_get_u32(device->expected, 0, device->ir_length);
+		expected_mask = buf_get_u32(device->expected_mask, 0, device->ir_length);
+		cur_instr = buf_get_u32(device->cur_instr, 0, device->ir_length);
+		command_print(cmd_ctx, "%i: idcode: 0x%8.8x ir length %i, ir capture 0x%x, ir mask 0x%x, current instruction 0x%x", device_count, device->idcode, device->ir_length, expected, expected_mask, cur_instr);
+		device = device->next;
+		device_count++;
+	}
+
+	return ERROR_OK;
+}
+
+int handle_reset_config_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	if (argc >= 1)
+	{
+		if (strcmp(args[0], "none") == 0)
+			jtag_reset_config = RESET_NONE;
+		else if (strcmp(args[0], "trst_only") == 0)
+			jtag_reset_config = RESET_HAS_TRST;
+		else if (strcmp(args[0], "srst_only") == 0)
+			jtag_reset_config = RESET_HAS_SRST;
+		else if (strcmp(args[0], "trst_and_srst") == 0)
+			jtag_reset_config = RESET_TRST_AND_SRST;
+		else
+		{
+			ERROR("invalid reset_config argument, defaulting to none");
+			jtag_reset_config = RESET_NONE;
+			return ERROR_INVALID_ARGUMENTS;
+		}
+	}
+	
+	if (argc >= 2)
+	{
+		if (strcmp(args[1], "srst_pulls_trst") == 0)
+			jtag_reset_config |= RESET_SRST_PULLS_TRST;
+		else if (strcmp(args[1], "trst_pulls_srst") == 0)
+			jtag_reset_config |= RESET_TRST_PULLS_SRST;
+		else if (strcmp(args[1], "combined") == 0)
+			jtag_reset_config |= RESET_SRST_PULLS_TRST | RESET_TRST_PULLS_SRST;
+		else if (strcmp(args[1], "separate") == 0)
+			jtag_reset_config &= ~(RESET_SRST_PULLS_TRST | RESET_TRST_PULLS_SRST);
+		else
+		{
+			ERROR("invalid reset_config argument, defaulting to none");
+			jtag_reset_config = RESET_NONE;
+			return ERROR_INVALID_ARGUMENTS;
+		}
+	}
+	
+	if (argc >= 3)
+	{
+		if (strcmp(args[2], "trst_open_drain") == 0)
+			jtag_reset_config |= RESET_TRST_OPEN_DRAIN;
+		else if (strcmp(args[2], "trst_push_pull") == 0)
+			jtag_reset_config &= ~RESET_TRST_OPEN_DRAIN;
+		else
+		{
+			ERROR("invalid reset_config argument, defaulting to none");
+			jtag_reset_config = RESET_NONE;
+			return ERROR_INVALID_ARGUMENTS;
+		}
+	}
+
+	if (argc >= 4)
+	{
+		if (strcmp(args[3], "srst_push_pull") == 0)
+			jtag_reset_config |= RESET_SRST_PUSH_PULL;
+		else if (strcmp(args[3], "srst_open_drain") == 0)
+			jtag_reset_config &= ~RESET_SRST_PUSH_PULL;
+		else
+		{
+			ERROR("invalid reset_config argument, defaulting to none");
+			jtag_reset_config = RESET_NONE;
+			return ERROR_INVALID_ARGUMENTS;
+		}
+	}
+	
+	return ERROR_OK;
+}
+
+int handle_jtag_nsrst_delay_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	if (argc < 1)
+	{
+		ERROR("jtag_nsrst_delay <ms> command takes one required argument");
+		exit(-1);
+	}
+	else
+	{
+		jtag_nsrst_delay = strtoul(args[0], NULL, 0);
+	}
+	
+	return ERROR_OK;
+}
+
+int handle_jtag_ntrst_delay_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	if (argc < 1)
+	{
+		ERROR("jtag_ntrst_delay <ms> command takes one required argument");
+		exit(-1);
+	}
+	else
+	{
+		jtag_ntrst_delay = strtoul(args[0], NULL, 0);
+	}
+	
+	return ERROR_OK;
+}
+
+int handle_jtag_speed_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	if (argc == 0)
+		command_print(cmd_ctx, "jtag_speed: %i", jtag_speed);
+
+	if (argc > 0)
+	{
+		/* this command can be called during CONFIG, 
+		 * in which case jtag isn't initialized */
+		if (jtag)
+			jtag->speed(strtoul(args[0], NULL, 0));
+		else
+			jtag_speed = strtoul(args[0], NULL, 0);
+	}
+
+	return ERROR_OK;
+}
+
+int handle_endstate_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	enum tap_state state;
+
+	if (argc < 1)
+	{
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+	else
+	{
+		for (state = 0; state < 16; state++)
+		{
+			if (strcmp(args[0], tap_state_strings[state]) == 0)
+			{
+				jtag_add_end_state(state);
+				jtag_execute_queue();
+			}
+		}
+	}
+	command_print(cmd_ctx, "current endstate: %s", tap_state_strings[end_state]);
+	
+	return ERROR_OK;
+}
+
+int handle_jtag_reset_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	int trst = -1;
+	int srst = -1;
+	int retval;
+	
+	if (argc < 2)
+	{
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	}
+
+	if (args[0][0] == '1')
+		trst = 1;
+	else if (args[0][0] == '0')
+		trst = 0;
+	else
+	{
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	if (args[1][0] == '1')
+		srst = 1;
+	else if (args[1][0] == '0')
+		srst = 0;
+	else
+	{
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	if (!jtag && jtag_interface_init(cmd_ctx) != ERROR_OK)
+		return ERROR_JTAG_INIT_FAILED;
+
+	if ((retval = jtag_add_reset(trst, srst)) != ERROR_OK)
+	{
+		switch (retval)
+		{
+			case ERROR_JTAG_RESET_WOULD_ASSERT_TRST:
+				command_print(cmd_ctx, "requested reset would assert trst\nif this is acceptable, use jtag_reset 1 %c", args[1][0]);
+				break;
+			case ERROR_JTAG_RESET_CANT_SRST:
+				command_print(cmd_ctx, "can't assert srst because the current reset_config doesn't support it");
+				break;
+			default:
+				command_print(cmd_ctx, "unknown error");
+		}
+	}
+	jtag_execute_queue();
+
+	return ERROR_OK;
+}
+
+int handle_runtest_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	if (argc < 1)
+	{
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	jtag_add_runtest(strtol(args[0], NULL, 0), -1);
+	jtag_execute_queue();
+
+	return ERROR_OK;
+
+}
+
+int handle_statemove_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	enum tap_state state;
+
+	state = -1;
+	if (argc == 1)
+	{
+		for (state = 0; state < 16; state++)
+		{
+			if (strcmp(args[0], tap_state_strings[state]) == 0)
+			{
+				break;
+			}
+		}
+	}
+
+	jtag_add_statemove(state);
+	jtag_execute_queue();
+
+	return ERROR_OK;
+
+}
+
+int handle_irscan_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	int i;
+	scan_field_t *fields;
+	
+	if ((argc < 2) || (argc % 2))
+	{
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	fields = malloc(sizeof(scan_field_t) * argc / 2);
+	
+	for (i = 0; i < argc / 2; i++)
+	{
+		int device = strtoul(args[i*2], NULL, 0);
+		int field_size = jtag_get_device(device)->ir_length;
+		fields[i].device = device;
+		fields[i].out_value = malloc(CEIL(field_size, 8));
+		buf_set_u32(fields[i].out_value, 0, field_size, strtoul(args[i*2+1], NULL, 0));
+		fields[i].out_mask = NULL;
+		fields[i].in_value = NULL;
+		fields[i].in_check_mask = NULL;
+		fields[i].in_handler = NULL;
+		fields[i].in_handler_priv = NULL;
+	}
+
+	jtag_add_ir_scan(argc / 2, fields, -1);
+	jtag_execute_queue();
+
+	for (i = 0; i < argc / 2; i++)
+		free(fields[i].out_value);
+
+	free (fields);
+
+	return ERROR_OK;
+}
+
+int handle_drscan_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	scan_field_t *fields;
+	int num_fields = 0;
+	int field_count = 0;
+	var_t *var;
+	int i, j;
+	
+	if ((argc < 2) || (argc % 2))
+	{
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	for (i = 0; i < argc; i+=2)
+	{
+		var = get_var_by_namenum(args[i+1]);
+		if (var)
+		{
+			num_fields += var->num_fields;
+		}
+		else
+		{
+			command_print(cmd_ctx, "variable %s doesn't exist", args[i+1]);
+			return ERROR_OK;
+		}
+	}
+
+	fields = malloc(sizeof(scan_field_t) * num_fields);
+
+	for (i = 0; i < argc; i+=2)
+	{
+		var = get_var_by_namenum(args[i+1]);
+	
+		for (j = 0; j < var->num_fields; j++)
+		{
+			fields[field_count].device = strtol(args[i], NULL, 0);
+			fields[field_count].num_bits = var->fields[j].num_bits;
+			fields[field_count].out_value = malloc(CEIL(var->fields[j].num_bits, 8));
+			buf_set_u32(fields[field_count].out_value, 0, var->fields[j].num_bits, var->fields[j].value);
+			fields[field_count].out_mask = NULL;
+			fields[field_count].in_value = fields[field_count].out_value;
+			fields[field_count].in_check_mask = NULL;
+			fields[field_count].in_check_value = NULL;
+			fields[field_count].in_handler = field_le_to_host;
+			fields[field_count++].in_handler_priv = &(var->fields[j]);
+		}
+	}
+
+	jtag_add_dr_scan(num_fields, fields, -1);
+	jtag_execute_queue();
+	
+	for (i = 0; i < argc / 2; i++)
+		free(fields[i].out_value);
+
+	free(fields);
+
+	return ERROR_OK;
+}
+
+int handle_verify_ircapture_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	if (argc == 1)
+	{
+		if (strcmp(args[0], "enable") == 0)
+		{
+			jtag_verify_capture_ir = 1;
+		}
+		else if (strcmp(args[0], "disable") == 0)
+		{
+			jtag_verify_capture_ir = 0;
+		} else
+		{
+			return ERROR_COMMAND_SYNTAX_ERROR;
+		}
+	} else if (argc != 0)
+	{
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+	
+	command_print(cmd_ctx, "verify Capture-IR is %s", (jtag_verify_capture_ir) ? "enabled": "disabled");
+	
+	return ERROR_OK;
+}
diff --git a/src/jtag/usbprog.c b/src/jtag/usbprog.c
index 2ff785ab..d0011f0d 100644
--- a/src/jtag/usbprog.c
+++ b/src/jtag/usbprog.c
@@ -1,681 +1,681 @@
-/***************************************************************************

- *   Copyright (C) 2007 by Benedikt Sauter sauter@ixbat.de                 *

- *   based on Dominic Rath's amt_jtagaccel.c                               *

- *                                                                         *

- *   usbprog is a free programming adapter. You can easily install         *

- *   different firmware versions from an "online pool" over USB.           *

- *   The adapter can be used for programming and debugging AVR and ARM     *

- *   processors, as USB to RS232 converter, as JTAG interface or as        *

- *   simple I/O interface (5 lines).                                       *

- *                                                                         *

- *   http://www.embedded-projects.net/usbprog                              *

- *                                                                         *

- *   This program is free software; you can redistribute it and/or modify  *

- *   it under the terms of the GNU General Public License as published by  *

- *   the Free Software Foundation; either version 2 of the License, or     *

- *   (at your option) any later version.                                   *

- *                                                                         *

- *   This program is distributed in the hope that it will be useful,       *

- *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *

- *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *

- *   GNU General Public License for more details.                          *

- *                                                                         *

- *   You should have received a copy of the GNU General Public License     *

- *   along with this program; if not, write to the                         *

- *   Free Software Foundation, Inc.,                                       *

- *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *

- ***************************************************************************/

-#ifdef HAVE_CONFIG_H

-#include "config.h"

-#endif

-

-#include "replacements.h"

-

-#include "jtag.h"

-#include <usb.h>

-

-/* system includes */

-

-#include "log.h"

-

-#define VID 0x1781

-#define PID 0x0c63

-

-/* Pins at usbprog */

-#define TDO_BIT         0

-#define TDI_BIT         3

-#define TCK_BIT         2

-#define TMS_BIT         1

-

-int usbprog_execute_queue(void);

-int usbprog_speed(int speed);

-int usbprog_register_commands(struct command_context_s *cmd_ctx);

-int usbprog_init(void);

-int usbprog_quit(void);

-

-void usbprog_end_state(enum tap_state state);

-void usbprog_state_move(void);

-void usbprog_path_move(pathmove_command_t *cmd);

-void usbprog_runtest(int num_cycles);

-void usbprog_scan(int ir_scan, enum scan_type type, u8 *buffer, int scan_size);

-

-jtag_interface_t usbprog_interface =

-{

-	.name = "usbprog",

-	.execute_queue = usbprog_execute_queue,

-	.speed = usbprog_speed,

-	.register_commands = usbprog_register_commands,

-	.init = usbprog_init,

-	.quit = usbprog_quit

-};

-

-#define UNKOWN_COMMAND  0x00

-#define PORT_DIRECTION  0x01

-#define PORT_SET        0x02

-#define PORT_GET        0x03

-#define PORT_SETBIT     0x04

-#define PORT_GETBIT     0x05

-#define WRITE_TDI     	0x06

-#define READ_TDO     	0x07

-#define WRITE_AND_READ 	0x08

-#define WRITE_TMS     	0x09

-#define WRITE_TMS_CHAIN 0x0A

-

-struct usbprog_jtag

-{

-	struct usb_dev_handle* usb_handle;

-};

-

-struct usbprog_jtag * usbprog_jtag_handle;

-

-struct usbprog_jtag* usbprog_jtag_open();

-void usbprog_jtag_close(struct usbprog_jtag *usbprog_jtag);

-void usbprog_jtag_init(struct usbprog_jtag *usbprog_jtag);

-unsigned char usbprog_jtag_message(struct usbprog_jtag *usbprog_jtag, char *msg, int msglen);

-

-void usbprog_jtag_read_tdo(struct usbprog_jtag *usbprog_jtag, char * buffer, int size);

-void usbprog_jtag_write_tdi(struct usbprog_jtag *usbprog_jtag, char * buffer, int size);

-void usbprog_jtag_write_and_read(struct usbprog_jtag *usbprog_jtag, char * buffer, int size);

-void usbprog_jtag_write_tms(struct usbprog_jtag *usbprog_jtag, char tms_scan);

-

-char tms_chain[64];

-int tms_chain_index;

-void usbprog_jtag_tms_collect(char tms_scan);

-void usbprog_jtag_tms_send(struct usbprog_jtag *usbprog_jtag);

-

-void usbprog_write(int tck, int tms, int tdi);

-void usbprog_reset(int trst, int srst);

-

-void usbprog_jtag_set_direction(struct usbprog_jtag *usbprog_jtag, unsigned char direction);

-void usbprog_jtag_write_slice(struct usbprog_jtag *usbprog_jtag,unsigned char value);

-unsigned char usbprog_jtag_get_port(struct usbprog_jtag *usbprog_jtag);

-void usbprog_jtag_set_bit(struct usbprog_jtag *usbprog_jtag,int bit, int value);

-int usbprog_jtag_get_bit(struct usbprog_jtag *usbprog_jtag, int bit);

-

-int usbprog_speed(int speed)

-{

-	return ERROR_OK;

-}

-

-int usbprog_register_commands(struct command_context_s *cmd_ctx)

-{

-	return ERROR_OK;

-}

-

-int usbprog_execute_queue(void)

-{

-	jtag_command_t *cmd = jtag_command_queue; /* currently processed command */

-	int scan_size;

-	enum scan_type type;

-	u8 *buffer;

-

-	while (cmd)

-	{

-		switch (cmd->type)

-		{

-			case JTAG_END_STATE:

-#ifdef _DEBUG_JTAG_IO_

-				DEBUG("end_state: %i", cmd->cmd.end_state->end_state);

-#endif

-				if (cmd->cmd.end_state->end_state != -1)

-					usbprog_end_state(cmd->cmd.end_state->end_state);

-				break;

-			case JTAG_RESET:

-#ifdef _DEBUG_JTAG_IO_

-				DEBUG("reset trst: %i srst %i", cmd->cmd.reset->trst, cmd->cmd.reset->srst);

-#endif

-				if (cmd->cmd.reset->trst == 1)

-				{

-					cur_state = TAP_TLR;

-				}

-				usbprog_reset(cmd->cmd.reset->trst, cmd->cmd.reset->srst);

-				break;

-			case JTAG_RUNTEST:

-#ifdef _DEBUG_JTAG_IO_

-				DEBUG("runtest %i cycles, end in %i", cmd->cmd.runtest->num_cycles, cmd->cmd.runtest->end_state);

-#endif

-				if (cmd->cmd.runtest->end_state != -1)

-					usbprog_end_state(cmd->cmd.runtest->end_state);

-				usbprog_runtest(cmd->cmd.runtest->num_cycles);

-				break;

-			case JTAG_STATEMOVE:

-#ifdef _DEBUG_JTAG_IO_

-				DEBUG("statemove end in %i", cmd->cmd.statemove->end_state);

-#endif

-				if (cmd->cmd.statemove->end_state != -1)

-					usbprog_end_state(cmd->cmd.statemove->end_state);

-				usbprog_state_move();

-				break;

-			case JTAG_PATHMOVE:

-#ifdef _DEBUG_JTAG_IO_

-				DEBUG("pathmove: %i states, end in %i", cmd->cmd.pathmove->num_states,

-					cmd->cmd.pathmove->path[cmd->cmd.pathmove->num_states - 1]);

-#endif

-				usbprog_path_move(cmd->cmd.pathmove);

-				break;

-			case JTAG_SCAN:

-#ifdef _DEBUG_JTAG_IO_

-				DEBUG("scan end in %i", cmd->cmd.scan->end_state);

-#endif

-				if (cmd->cmd.scan->end_state != -1)

-					usbprog_end_state(cmd->cmd.scan->end_state);

-				scan_size = jtag_build_buffer(cmd->cmd.scan, &buffer);

-				type = jtag_scan_type(cmd->cmd.scan);

-				usbprog_scan(cmd->cmd.scan->ir_scan, type, buffer, scan_size);

-				if (jtag_read_buffer(buffer, cmd->cmd.scan) != ERROR_OK)

-					return ERROR_JTAG_QUEUE_FAILED;

-				if (buffer)

-					free(buffer);

-				break;

-			case JTAG_SLEEP:

-#ifdef _DEBUG_JTAG_IO_

-				DEBUG("sleep %i", cmd->cmd.sleep->us);

-#endif

-				jtag_sleep(cmd->cmd.sleep->us);

-					break;

-			default:

-				ERROR("BUG: unknown JTAG command type encountered");

-				exit(-1);

-		}

-

-		cmd = cmd->next;

-	}

-

-	return ERROR_OK;

-}

-

-int usbprog_init(void)

-{

-	usbprog_jtag_handle = usbprog_jtag_open();

-

-	tms_chain_index = 0;

-	if (usbprog_jtag_handle == 0)

-	{

-		ERROR("Can't find USB JTAG Interface! Please check connection and permissions.");

-		return ERROR_JTAG_INIT_FAILED;

-	}

-

-	INFO("USB JTAG Interface ready!");

-

-	usbprog_jtag_init(usbprog_jtag_handle);

-	usbprog_reset(0, 0);

-	usbprog_write(0, 0, 0);

-

-	return ERROR_OK;

-}

-

-int usbprog_quit(void)

-{

-	return ERROR_OK;

-}

-

-/*************** jtag execute commands **********************/

-void usbprog_end_state(enum tap_state state)

-{

-	if (tap_move_map[state] != -1)

-		end_state = state;

-	else

-	{

-		ERROR("BUG: %i is not a valid end state", state);

-		exit(-1);

-	}

-}

-

-void usbprog_state_move(void)

-{

-	int i = 0, tms = 0;

-	u8 tms_scan = TAP_MOVE(cur_state, end_state);

-

-	usbprog_jtag_write_tms(usbprog_jtag_handle, (char)tms_scan);

-	for (i = 0; i < 7; i++)

-	{

-		tms = (tms_scan >> i) & 1;

-	}

-

-	cur_state = end_state;

-}

-

-void usbprog_path_move(pathmove_command_t *cmd)

-{

-	int num_states = cmd->num_states;

-	int state_count;

-

-	state_count = 0;

-	while (num_states)

-	{

-		if (tap_transitions[cur_state].low == cmd->path[state_count])

-		{

-			//INFO("1");

-			usbprog_write(0, 0, 0);

-			usbprog_write(1, 0, 0);

-		}

-		else if (tap_transitions[cur_state].high == cmd->path[state_count])

-		{

-			//INFO("2");

-			usbprog_write(0, 1, 0);

-			usbprog_write(1, 1, 0);

-		}

-		else

-		{

-			ERROR("BUG: %s -> %s isn't a valid TAP transition", tap_state_strings[cur_state], tap_state_strings[cmd->path[state_count]]);

-			exit(-1);

-		}

-

-		cur_state = cmd->path[state_count];

-		state_count++;

-		num_states--;

-	}

-

-	end_state = cur_state;

-}

-

-void usbprog_runtest(int num_cycles)

-{

-	int i;

-

-	/* only do a state_move when we're not already in RTI */

-	if (cur_state != TAP_RTI)

-	{

-		usbprog_end_state(TAP_RTI);

-		usbprog_state_move();

-	}

-

-	/* execute num_cycles */

-	if (num_cycles > 0)

-	{

-		usbprog_jtag_tms_send(usbprog_jtag_handle);

-		usbprog_write(0, 0, 0);

-	}

-	else

-	{

-		usbprog_jtag_tms_send(usbprog_jtag_handle);

-		//INFO("NUM CYCLES %i",num_cycles);

-	}

-

-	for (i = 0; i < num_cycles; i++)

-	{

-		usbprog_write(1, 0, 0);

-		usbprog_write(0, 0, 0);

-	}

-

-	/* finish in end_state */

-	/*

-	usbprog_end_state(saved_end_state);

-	if (cur_state != end_state)

-		usbprog_state_move();

-	*/

-}

-

-void usbprog_scan(int ir_scan, enum scan_type type, u8 *buffer, int scan_size)

-{

-	enum tap_state saved_end_state = end_state;

-

-	if (ir_scan)

-		usbprog_end_state(TAP_SI);

-	else

-		usbprog_end_state(TAP_SD);

-

-	//usbprog_jtag_tms_send(usbprog_jtag_handle);

-

-	usbprog_state_move();

-	usbprog_end_state(saved_end_state);

-

-	usbprog_jtag_tms_send(usbprog_jtag_handle);

-

-	if (type == SCAN_OUT)

-	{

-		usbprog_jtag_write_tdi(usbprog_jtag_handle,buffer, scan_size);

-	}

-	if (type == SCAN_IN)

-	{

-		usbprog_jtag_read_tdo(usbprog_jtag_handle,buffer, scan_size);

-	}

-	if (type == SCAN_IO)

-	{

-		usbprog_jtag_write_and_read(usbprog_jtag_handle,buffer, scan_size);

-	}

-

-	if (ir_scan)

-		cur_state = TAP_PI;

-	else

-		cur_state = TAP_PD;

-

-	if (cur_state != end_state)

-		usbprog_state_move();

-}

-

-/*************** jtag wrapper functions *********************/

-

-void usbprog_write(int tck, int tms, int tdi)

-{

-	unsigned char output_value=0x00;

-

-	if (tms)

-		output_value |= (1<<TMS_BIT);

-	if (tdi)

-		output_value |= (1<<TDI_BIT);

-	if (tck)

-		output_value |= (1<<TCK_BIT);

-

-	usbprog_jtag_write_slice(usbprog_jtag_handle,output_value);

-}

-

-/* (1) assert or (0) deassert reset lines */

-void usbprog_reset(int trst, int srst)

-{

-	DEBUG("trst: %i, srst: %i", trst, srst);

-

-	if (trst)

-		usbprog_jtag_set_bit(usbprog_jtag_handle, 5, 0);

-	else

-		usbprog_jtag_set_bit(usbprog_jtag_handle, 5, 1);

-

-	if (srst)

-		usbprog_jtag_set_bit(usbprog_jtag_handle, 4, 0);

-	else

-		usbprog_jtag_set_bit(usbprog_jtag_handle, 4, 1);

-}

-

-/*************** jtag lowlevel functions ********************/

-

-struct usb_bus *busses;

-

-struct usbprog_jtag* usbprog_jtag_open()

-{

-	struct usb_bus *bus;

-	struct usb_device *dev;

-

-	struct usbprog_jtag *tmp;

-

-	tmp = (struct usbprog_jtag*)malloc(sizeof(struct usbprog_jtag));

-

-	usb_set_debug(10);

-	usb_init();

-	usb_find_busses();

-	usb_find_devices();

-

-	busses = usb_get_busses();

-

-	/* find usbprog_jtag device in usb bus */

-

-	for (bus = busses; bus; bus = bus->next)

-	{

-		for (dev = bus->devices; dev; dev = dev->next)

-		{

-			/* condition for sucessfully hit (too bad, I only check the vendor id)*/

-			if (dev->descriptor.idVendor == VID && dev->descriptor.idProduct == PID)

-			{

-				tmp->usb_handle = usb_open(dev);

-				usb_set_configuration(tmp->usb_handle, 1);

-				usb_claim_interface(tmp->usb_handle, 0);

-				usb_set_altinterface(tmp->usb_handle, 0);

-				return tmp;

-			}

-		}

-	}

-	return 0;

-}

-

-void usbprog_jtag_close(struct usbprog_jtag *usbprog_jtag)

-{

-	usb_close(usbprog_jtag->usb_handle);

-	free(usbprog_jtag);

-}

-

-unsigned char usbprog_jtag_message(struct usbprog_jtag *usbprog_jtag, char *msg, int msglen)

-{

-	int res = usb_bulk_write(usbprog_jtag->usb_handle, 3, msg,msglen, 100);

-	if ((msg[0] == 2) || (msg[0] == 1) || (msg[0] == 4) || (msg[0] == 0) || \

-			(msg[0] == 6) || (msg[0] == 0x0A) || (msg[0] == 9))

-		return 1;

-	if (res == msglen)

-	{

-		//INFO("HALLLLOOO %i",(int)msg[0]);

-		res =  usb_bulk_read(usbprog_jtag->usb_handle, 0x82, msg, 2, 100);

-		if (res > 0)

-			return (unsigned char)msg[1];

-		else

-			return -1;

-	}

-	else

-		return -1;

-	return 0;

-}

-

-void usbprog_jtag_init(struct usbprog_jtag *usbprog_jtag)

-{

-	usbprog_jtag_set_direction(usbprog_jtag, 0xFE);

-}

-

-void usbprog_jtag_write_and_read(struct usbprog_jtag *usbprog_jtag, char * buffer, int size)

-{

-	char tmp[64];	/* fastes packet size for usb controller */

-	int send_bits, bufindex = 0, fillindex = 0, i, loops;

-

-	char swap;

-	/* 61 byte can be transfered (488 bit) */

-

-	while (size > 0)

-	{

-		if (size > 488)

-		{

-			send_bits = 488;

-			size = size - 488;

-			loops = 61;

-		}

-		else

-		{

-			send_bits = size;

-			loops = size / 8;

-			loops++;

-			size = 0;

-		}

-		tmp[0] = WRITE_AND_READ;

-		tmp[1] = (char)(send_bits >> 8);	/* high */

-		tmp[2] = (char)(send_bits);			/* low */

-		i = 0;

-

-		for (i = 0; i < loops; i++)

-		{

-			tmp[3 + i] = buffer[bufindex];

-			bufindex++;

-		}

-

-		if (usb_bulk_write(usbprog_jtag->usb_handle, 3, tmp, 64, 1000) == 64)

-		{

-			//INFO("HALLLLOOO2 %i",(int)tmp[0]);

-			usleep(1);

-			int timeout = 0;

-			while (usb_bulk_read(usbprog_jtag->usb_handle, 0x82, tmp, 64, 1000) < 1)

-			{

-				timeout++;

-				if (timeout > 10)

-					break;

-			}

-

-			for (i = 0; i < loops; i++)

-			{

-				swap =  tmp[3 + i];

-				buffer[fillindex++] = swap;

-			}

-		}

-	}

-}

-

-void usbprog_jtag_read_tdo(struct usbprog_jtag *usbprog_jtag, char * buffer, int size)

-{

-	char tmp[64];	/* fastes packet size for usb controller */

-	int send_bits, fillindex = 0, i, loops;

-

-	char swap;

-	/* 61 byte can be transfered (488 bit) */

-

-	while (size > 0)

-	{

-		if (size > 488)

-		{

-			send_bits = 488;

-			size = size - 488;

-			loops = 61;

-		}

-		else

-		{

-			send_bits = size;

-			loops = size / 8;

-			loops++;

-			size = 0;

-		}

-		tmp[0] = WRITE_AND_READ;

-		tmp[1] = (char)(send_bits >> 8);	/* high */

-		tmp[2] = (char)(send_bits);			/* low */

-

-		usb_bulk_write(usbprog_jtag->usb_handle, 3, tmp, 3, 1000);

-

-		//INFO("HALLLLOOO3 %i",(int)tmp[0]);

-		int timeout = 0;

-		usleep(1);

-		while (usb_bulk_read(usbprog_jtag->usb_handle, 0x82, tmp, 64, 10) < 1)

-		{

-			timeout++;

-			if (timeout > 10)

-				break;

-		}

-

-		for (i = 0; i < loops; i++)

-		{

-			swap = tmp[3 + i];

-			buffer[fillindex++] = swap;

-		}

-	}

-}

-

-void usbprog_jtag_write_tdi(struct usbprog_jtag *usbprog_jtag, char * buffer, int size)

-{

-	char tmp[64];	/* fastes packet size for usb controller */

-	int send_bits, bufindex = 0, i, loops;

-

-	/* 61 byte can be transfered (488 bit) */

-	while (size > 0)

-	{

-		if (size > 488)

-		{

-			send_bits = 488;

-			size = size - 488;

-			loops = 61;

-		}

-		else

-		{

-			send_bits = size;

-			loops = size/8;

-			//if(loops==0)

-			loops++;

-			size = 0;

-		}

-		tmp[0] = WRITE_TDI;

-		tmp[1] = (char)(send_bits >> 8);	/* high */

-		tmp[2] = (char)(send_bits);			/* low */

-		i = 0;

-

-		for (i = 0; i < loops; i++)

-		{

-			tmp[3 + i] = buffer[bufindex];

-			bufindex++;

-		}

-		usb_bulk_write(usbprog_jtag->usb_handle, 3, tmp, 64, 1000);

-	}

-}

-

-void usbprog_jtag_write_tms(struct usbprog_jtag *usbprog_jtag, char tms_scan)

-{

-	usbprog_jtag_tms_collect(tms_scan);

-}

-

-void usbprog_jtag_set_direction(struct usbprog_jtag *usbprog_jtag, unsigned char direction)

-{

-	char tmp[2];

-	tmp[0] = PORT_DIRECTION;

-	tmp[1] = (char)direction;

-	usbprog_jtag_message(usbprog_jtag, tmp, 2);

-}

-

-void usbprog_jtag_write_slice(struct usbprog_jtag *usbprog_jtag,unsigned char value)

-{

-	char tmp[2];

-	tmp[0] = PORT_SET;

-	tmp[1] = (char)value;

-	usbprog_jtag_message(usbprog_jtag, tmp, 2);

-}

-

-unsigned char usbprog_jtag_get_port(struct usbprog_jtag *usbprog_jtag)

-{

-	char tmp[2];

-	tmp[0] = PORT_GET;

-	tmp[1] = 0x00;

-	return usbprog_jtag_message(usbprog_jtag, tmp, 2);

-}

-

-void usbprog_jtag_set_bit(struct usbprog_jtag *usbprog_jtag,int bit, int value)

-{

-	char tmp[3];

-	tmp[0] = PORT_SETBIT;

-	tmp[1] = (char)bit;

-	if (value == 1)

-		tmp[2] = 0x01;

-	else

-		tmp[2] = 0x00;

-	usbprog_jtag_message(usbprog_jtag, tmp, 3);

-}

-

-int usbprog_jtag_get_bit(struct usbprog_jtag *usbprog_jtag, int bit)

-{

-	char tmp[2];

-	tmp[0] = PORT_GETBIT;

-	tmp[1] = (char)bit;

-

-	if (usbprog_jtag_message(usbprog_jtag, tmp, 2) > 0)

-		return 1;

-	else

-		return 0;

-}

-

-void usbprog_jtag_tms_collect(char tms_scan)

-{

-	tms_chain[tms_chain_index] = tms_scan;

-	tms_chain_index++;

-}

-

-void usbprog_jtag_tms_send(struct usbprog_jtag *usbprog_jtag)

-{

-	int i;

-	//INFO("TMS SEND");

-	if (tms_chain_index > 0)

-	{

-		char tmp[tms_chain_index + 2];

-		tmp[0] = WRITE_TMS_CHAIN;

-		tmp[1] = (char)(tms_chain_index);

-		for (i = 0; i < tms_chain_index + 1; i++)

-			tmp[2 + i] = tms_chain[i];

-		usb_bulk_write(usbprog_jtag->usb_handle, 3, tmp, tms_chain_index + 2, 1000);

-		tms_chain_index = 0;

-	}

-}

+/***************************************************************************
+ *   Copyright (C) 2007 by Benedikt Sauter sauter@ixbat.de                 *
+ *   based on Dominic Rath's amt_jtagaccel.c                               *
+ *                                                                         *
+ *   usbprog is a free programming adapter. You can easily install         *
+ *   different firmware versions from an "online pool" over USB.           *
+ *   The adapter can be used for programming and debugging AVR and ARM     *
+ *   processors, as USB to RS232 converter, as JTAG interface or as        *
+ *   simple I/O interface (5 lines).                                       *
+ *                                                                         *
+ *   http://www.embedded-projects.net/usbprog                              *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "replacements.h"
+
+#include "jtag.h"
+#include <usb.h>
+
+/* system includes */
+
+#include "log.h"
+
+#define VID 0x1781
+#define PID 0x0c63
+
+/* Pins at usbprog */
+#define TDO_BIT         0
+#define TDI_BIT         3
+#define TCK_BIT         2
+#define TMS_BIT         1
+
+int usbprog_execute_queue(void);
+int usbprog_speed(int speed);
+int usbprog_register_commands(struct command_context_s *cmd_ctx);
+int usbprog_init(void);
+int usbprog_quit(void);
+
+void usbprog_end_state(enum tap_state state);
+void usbprog_state_move(void);
+void usbprog_path_move(pathmove_command_t *cmd);
+void usbprog_runtest(int num_cycles);
+void usbprog_scan(int ir_scan, enum scan_type type, u8 *buffer, int scan_size);
+
+jtag_interface_t usbprog_interface =
+{
+	.name = "usbprog",
+	.execute_queue = usbprog_execute_queue,
+	.speed = usbprog_speed,
+	.register_commands = usbprog_register_commands,
+	.init = usbprog_init,
+	.quit = usbprog_quit
+};
+
+#define UNKOWN_COMMAND  0x00
+#define PORT_DIRECTION  0x01
+#define PORT_SET        0x02
+#define PORT_GET        0x03
+#define PORT_SETBIT     0x04
+#define PORT_GETBIT     0x05
+#define WRITE_TDI     	0x06
+#define READ_TDO     	0x07
+#define WRITE_AND_READ 	0x08
+#define WRITE_TMS     	0x09
+#define WRITE_TMS_CHAIN 0x0A
+
+struct usbprog_jtag
+{
+	struct usb_dev_handle* usb_handle;
+};
+
+struct usbprog_jtag * usbprog_jtag_handle;
+
+struct usbprog_jtag* usbprog_jtag_open();
+void usbprog_jtag_close(struct usbprog_jtag *usbprog_jtag);
+void usbprog_jtag_init(struct usbprog_jtag *usbprog_jtag);
+unsigned char usbprog_jtag_message(struct usbprog_jtag *usbprog_jtag, char *msg, int msglen);
+
+void usbprog_jtag_read_tdo(struct usbprog_jtag *usbprog_jtag, char * buffer, int size);
+void usbprog_jtag_write_tdi(struct usbprog_jtag *usbprog_jtag, char * buffer, int size);
+void usbprog_jtag_write_and_read(struct usbprog_jtag *usbprog_jtag, char * buffer, int size);
+void usbprog_jtag_write_tms(struct usbprog_jtag *usbprog_jtag, char tms_scan);
+
+char tms_chain[64];
+int tms_chain_index;
+void usbprog_jtag_tms_collect(char tms_scan);
+void usbprog_jtag_tms_send(struct usbprog_jtag *usbprog_jtag);
+
+void usbprog_write(int tck, int tms, int tdi);
+void usbprog_reset(int trst, int srst);
+
+void usbprog_jtag_set_direction(struct usbprog_jtag *usbprog_jtag, unsigned char direction);
+void usbprog_jtag_write_slice(struct usbprog_jtag *usbprog_jtag,unsigned char value);
+unsigned char usbprog_jtag_get_port(struct usbprog_jtag *usbprog_jtag);
+void usbprog_jtag_set_bit(struct usbprog_jtag *usbprog_jtag,int bit, int value);
+int usbprog_jtag_get_bit(struct usbprog_jtag *usbprog_jtag, int bit);
+
+int usbprog_speed(int speed)
+{
+	return ERROR_OK;
+}
+
+int usbprog_register_commands(struct command_context_s *cmd_ctx)
+{
+	return ERROR_OK;
+}
+
+int usbprog_execute_queue(void)
+{
+	jtag_command_t *cmd = jtag_command_queue; /* currently processed command */
+	int scan_size;
+	enum scan_type type;
+	u8 *buffer;
+
+	while (cmd)
+	{
+		switch (cmd->type)
+		{
+			case JTAG_END_STATE:
+#ifdef _DEBUG_JTAG_IO_
+				DEBUG("end_state: %i", cmd->cmd.end_state->end_state);
+#endif
+				if (cmd->cmd.end_state->end_state != -1)
+					usbprog_end_state(cmd->cmd.end_state->end_state);
+				break;
+			case JTAG_RESET:
+#ifdef _DEBUG_JTAG_IO_
+				DEBUG("reset trst: %i srst %i", cmd->cmd.reset->trst, cmd->cmd.reset->srst);
+#endif
+				if (cmd->cmd.reset->trst == 1)
+				{
+					cur_state = TAP_TLR;
+				}
+				usbprog_reset(cmd->cmd.reset->trst, cmd->cmd.reset->srst);
+				break;
+			case JTAG_RUNTEST:
+#ifdef _DEBUG_JTAG_IO_
+				DEBUG("runtest %i cycles, end in %i", cmd->cmd.runtest->num_cycles, cmd->cmd.runtest->end_state);
+#endif
+				if (cmd->cmd.runtest->end_state != -1)
+					usbprog_end_state(cmd->cmd.runtest->end_state);
+				usbprog_runtest(cmd->cmd.runtest->num_cycles);
+				break;
+			case JTAG_STATEMOVE:
+#ifdef _DEBUG_JTAG_IO_
+				DEBUG("statemove end in %i", cmd->cmd.statemove->end_state);
+#endif
+				if (cmd->cmd.statemove->end_state != -1)
+					usbprog_end_state(cmd->cmd.statemove->end_state);
+				usbprog_state_move();
+				break;
+			case JTAG_PATHMOVE:
+#ifdef _DEBUG_JTAG_IO_
+				DEBUG("pathmove: %i states, end in %i", cmd->cmd.pathmove->num_states,
+					cmd->cmd.pathmove->path[cmd->cmd.pathmove->num_states - 1]);
+#endif
+				usbprog_path_move(cmd->cmd.pathmove);
+				break;
+			case JTAG_SCAN:
+#ifdef _DEBUG_JTAG_IO_
+				DEBUG("scan end in %i", cmd->cmd.scan->end_state);
+#endif
+				if (cmd->cmd.scan->end_state != -1)
+					usbprog_end_state(cmd->cmd.scan->end_state);
+				scan_size = jtag_build_buffer(cmd->cmd.scan, &buffer);
+				type = jtag_scan_type(cmd->cmd.scan);
+				usbprog_scan(cmd->cmd.scan->ir_scan, type, buffer, scan_size);
+				if (jtag_read_buffer(buffer, cmd->cmd.scan) != ERROR_OK)
+					return ERROR_JTAG_QUEUE_FAILED;
+				if (buffer)
+					free(buffer);
+				break;
+			case JTAG_SLEEP:
+#ifdef _DEBUG_JTAG_IO_
+				DEBUG("sleep %i", cmd->cmd.sleep->us);
+#endif
+				jtag_sleep(cmd->cmd.sleep->us);
+					break;
+			default:
+				ERROR("BUG: unknown JTAG command type encountered");
+				exit(-1);
+		}
+
+		cmd = cmd->next;
+	}
+
+	return ERROR_OK;
+}
+
+int usbprog_init(void)
+{
+	usbprog_jtag_handle = usbprog_jtag_open();
+
+	tms_chain_index = 0;
+	if (usbprog_jtag_handle == 0)
+	{
+		ERROR("Can't find USB JTAG Interface! Please check connection and permissions.");
+		return ERROR_JTAG_INIT_FAILED;
+	}
+
+	INFO("USB JTAG Interface ready!");
+
+	usbprog_jtag_init(usbprog_jtag_handle);
+	usbprog_reset(0, 0);
+	usbprog_write(0, 0, 0);
+
+	return ERROR_OK;
+}
+
+int usbprog_quit(void)
+{
+	return ERROR_OK;
+}
+
+/*************** jtag execute commands **********************/
+void usbprog_end_state(enum tap_state state)
+{
+	if (tap_move_map[state] != -1)
+		end_state = state;
+	else
+	{
+		ERROR("BUG: %i is not a valid end state", state);
+		exit(-1);
+	}
+}
+
+void usbprog_state_move(void)
+{
+	int i = 0, tms = 0;
+	u8 tms_scan = TAP_MOVE(cur_state, end_state);
+
+	usbprog_jtag_write_tms(usbprog_jtag_handle, (char)tms_scan);
+	for (i = 0; i < 7; i++)
+	{
+		tms = (tms_scan >> i) & 1;
+	}
+
+	cur_state = end_state;
+}
+
+void usbprog_path_move(pathmove_command_t *cmd)
+{
+	int num_states = cmd->num_states;
+	int state_count;
+
+	state_count = 0;
+	while (num_states)
+	{
+		if (tap_transitions[cur_state].low == cmd->path[state_count])
+		{
+			//INFO("1");
+			usbprog_write(0, 0, 0);
+			usbprog_write(1, 0, 0);
+		}
+		else if (tap_transitions[cur_state].high == cmd->path[state_count])
+		{
+			//INFO("2");
+			usbprog_write(0, 1, 0);
+			usbprog_write(1, 1, 0);
+		}
+		else
+		{
+			ERROR("BUG: %s -> %s isn't a valid TAP transition", tap_state_strings[cur_state], tap_state_strings[cmd->path[state_count]]);
+			exit(-1);
+		}
+
+		cur_state = cmd->path[state_count];
+		state_count++;
+		num_states--;
+	}
+
+	end_state = cur_state;
+}
+
+void usbprog_runtest(int num_cycles)
+{
+	int i;
+
+	/* only do a state_move when we're not already in RTI */
+	if (cur_state != TAP_RTI)
+	{
+		usbprog_end_state(TAP_RTI);
+		usbprog_state_move();
+	}
+
+	/* execute num_cycles */
+	if (num_cycles > 0)
+	{
+		usbprog_jtag_tms_send(usbprog_jtag_handle);
+		usbprog_write(0, 0, 0);
+	}
+	else
+	{
+		usbprog_jtag_tms_send(usbprog_jtag_handle);
+		//INFO("NUM CYCLES %i",num_cycles);
+	}
+
+	for (i = 0; i < num_cycles; i++)
+	{
+		usbprog_write(1, 0, 0);
+		usbprog_write(0, 0, 0);
+	}
+
+	/* finish in end_state */
+	/*
+	usbprog_end_state(saved_end_state);
+	if (cur_state != end_state)
+		usbprog_state_move();
+	*/
+}
+
+void usbprog_scan(int ir_scan, enum scan_type type, u8 *buffer, int scan_size)
+{
+	enum tap_state saved_end_state = end_state;
+
+	if (ir_scan)
+		usbprog_end_state(TAP_SI);
+	else
+		usbprog_end_state(TAP_SD);
+
+	//usbprog_jtag_tms_send(usbprog_jtag_handle);
+
+	usbprog_state_move();
+	usbprog_end_state(saved_end_state);
+
+	usbprog_jtag_tms_send(usbprog_jtag_handle);
+
+	if (type == SCAN_OUT)
+	{
+		usbprog_jtag_write_tdi(usbprog_jtag_handle,buffer, scan_size);
+	}
+	if (type == SCAN_IN)
+	{
+		usbprog_jtag_read_tdo(usbprog_jtag_handle,buffer, scan_size);
+	}
+	if (type == SCAN_IO)
+	{
+		usbprog_jtag_write_and_read(usbprog_jtag_handle,buffer, scan_size);
+	}
+
+	if (ir_scan)
+		cur_state = TAP_PI;
+	else
+		cur_state = TAP_PD;
+
+	if (cur_state != end_state)
+		usbprog_state_move();
+}
+
+/*************** jtag wrapper functions *********************/
+
+void usbprog_write(int tck, int tms, int tdi)
+{
+	unsigned char output_value=0x00;
+
+	if (tms)
+		output_value |= (1<<TMS_BIT);
+	if (tdi)
+		output_value |= (1<<TDI_BIT);
+	if (tck)
+		output_value |= (1<<TCK_BIT);
+
+	usbprog_jtag_write_slice(usbprog_jtag_handle,output_value);
+}
+
+/* (1) assert or (0) deassert reset lines */
+void usbprog_reset(int trst, int srst)
+{
+	DEBUG("trst: %i, srst: %i", trst, srst);
+
+	if (trst)
+		usbprog_jtag_set_bit(usbprog_jtag_handle, 5, 0);
+	else
+		usbprog_jtag_set_bit(usbprog_jtag_handle, 5, 1);
+
+	if (srst)
+		usbprog_jtag_set_bit(usbprog_jtag_handle, 4, 0);
+	else
+		usbprog_jtag_set_bit(usbprog_jtag_handle, 4, 1);
+}
+
+/*************** jtag lowlevel functions ********************/
+
+struct usb_bus *busses;
+
+struct usbprog_jtag* usbprog_jtag_open()
+{
+	struct usb_bus *bus;
+	struct usb_device *dev;
+
+	struct usbprog_jtag *tmp;
+
+	tmp = (struct usbprog_jtag*)malloc(sizeof(struct usbprog_jtag));
+
+	usb_set_debug(10);
+	usb_init();
+	usb_find_busses();
+	usb_find_devices();
+
+	busses = usb_get_busses();
+
+	/* find usbprog_jtag device in usb bus */
+
+	for (bus = busses; bus; bus = bus->next)
+	{
+		for (dev = bus->devices; dev; dev = dev->next)
+		{
+			/* condition for sucessfully hit (too bad, I only check the vendor id)*/
+			if (dev->descriptor.idVendor == VID && dev->descriptor.idProduct == PID)
+			{
+				tmp->usb_handle = usb_open(dev);
+				usb_set_configuration(tmp->usb_handle, 1);
+				usb_claim_interface(tmp->usb_handle, 0);
+				usb_set_altinterface(tmp->usb_handle, 0);
+				return tmp;
+			}
+		}
+	}
+	return 0;
+}
+
+void usbprog_jtag_close(struct usbprog_jtag *usbprog_jtag)
+{
+	usb_close(usbprog_jtag->usb_handle);
+	free(usbprog_jtag);
+}
+
+unsigned char usbprog_jtag_message(struct usbprog_jtag *usbprog_jtag, char *msg, int msglen)
+{
+	int res = usb_bulk_write(usbprog_jtag->usb_handle, 3, msg,msglen, 100);
+	if ((msg[0] == 2) || (msg[0] == 1) || (msg[0] == 4) || (msg[0] == 0) || \
+			(msg[0] == 6) || (msg[0] == 0x0A) || (msg[0] == 9))
+		return 1;
+	if (res == msglen)
+	{
+		//INFO("HALLLLOOO %i",(int)msg[0]);
+		res =  usb_bulk_read(usbprog_jtag->usb_handle, 0x82, msg, 2, 100);
+		if (res > 0)
+			return (unsigned char)msg[1];
+		else
+			return -1;
+	}
+	else
+		return -1;
+	return 0;
+}
+
+void usbprog_jtag_init(struct usbprog_jtag *usbprog_jtag)
+{
+	usbprog_jtag_set_direction(usbprog_jtag, 0xFE);
+}
+
+void usbprog_jtag_write_and_read(struct usbprog_jtag *usbprog_jtag, char * buffer, int size)
+{
+	char tmp[64];	/* fastes packet size for usb controller */
+	int send_bits, bufindex = 0, fillindex = 0, i, loops;
+
+	char swap;
+	/* 61 byte can be transfered (488 bit) */
+
+	while (size > 0)
+	{
+		if (size > 488)
+		{
+			send_bits = 488;
+			size = size - 488;
+			loops = 61;
+		}
+		else
+		{
+			send_bits = size;
+			loops = size / 8;
+			loops++;
+			size = 0;
+		}
+		tmp[0] = WRITE_AND_READ;
+		tmp[1] = (char)(send_bits >> 8);	/* high */
+		tmp[2] = (char)(send_bits);			/* low */
+		i = 0;
+
+		for (i = 0; i < loops; i++)
+		{
+			tmp[3 + i] = buffer[bufindex];
+			bufindex++;
+		}
+
+		if (usb_bulk_write(usbprog_jtag->usb_handle, 3, tmp, 64, 1000) == 64)
+		{
+			//INFO("HALLLLOOO2 %i",(int)tmp[0]);
+			usleep(1);
+			int timeout = 0;
+			while (usb_bulk_read(usbprog_jtag->usb_handle, 0x82, tmp, 64, 1000) < 1)
+			{
+				timeout++;
+				if (timeout > 10)
+					break;
+			}
+
+			for (i = 0; i < loops; i++)
+			{
+				swap =  tmp[3 + i];
+				buffer[fillindex++] = swap;
+			}
+		}
+	}
+}
+
+void usbprog_jtag_read_tdo(struct usbprog_jtag *usbprog_jtag, char * buffer, int size)
+{
+	char tmp[64];	/* fastes packet size for usb controller */
+	int send_bits, fillindex = 0, i, loops;
+
+	char swap;
+	/* 61 byte can be transfered (488 bit) */
+
+	while (size > 0)
+	{
+		if (size > 488)
+		{
+			send_bits = 488;
+			size = size - 488;
+			loops = 61;
+		}
+		else
+		{
+			send_bits = size;
+			loops = size / 8;
+			loops++;
+			size = 0;
+		}
+		tmp[0] = WRITE_AND_READ;
+		tmp[1] = (char)(send_bits >> 8);	/* high */
+		tmp[2] = (char)(send_bits);			/* low */
+
+		usb_bulk_write(usbprog_jtag->usb_handle, 3, tmp, 3, 1000);
+
+		//INFO("HALLLLOOO3 %i",(int)tmp[0]);
+		int timeout = 0;
+		usleep(1);
+		while (usb_bulk_read(usbprog_jtag->usb_handle, 0x82, tmp, 64, 10) < 1)
+		{
+			timeout++;
+			if (timeout > 10)
+				break;
+		}
+
+		for (i = 0; i < loops; i++)
+		{
+			swap = tmp[3 + i];
+			buffer[fillindex++] = swap;
+		}
+	}
+}
+
+void usbprog_jtag_write_tdi(struct usbprog_jtag *usbprog_jtag, char * buffer, int size)
+{
+	char tmp[64];	/* fastes packet size for usb controller */
+	int send_bits, bufindex = 0, i, loops;
+
+	/* 61 byte can be transfered (488 bit) */
+	while (size > 0)
+	{
+		if (size > 488)
+		{
+			send_bits = 488;
+			size = size - 488;
+			loops = 61;
+		}
+		else
+		{
+			send_bits = size;
+			loops = size/8;
+			//if(loops==0)
+			loops++;
+			size = 0;
+		}
+		tmp[0] = WRITE_TDI;
+		tmp[1] = (char)(send_bits >> 8);	/* high */
+		tmp[2] = (char)(send_bits);			/* low */
+		i = 0;
+
+		for (i = 0; i < loops; i++)
+		{
+			tmp[3 + i] = buffer[bufindex];
+			bufindex++;
+		}
+		usb_bulk_write(usbprog_jtag->usb_handle, 3, tmp, 64, 1000);
+	}
+}
+
+void usbprog_jtag_write_tms(struct usbprog_jtag *usbprog_jtag, char tms_scan)
+{
+	usbprog_jtag_tms_collect(tms_scan);
+}
+
+void usbprog_jtag_set_direction(struct usbprog_jtag *usbprog_jtag, unsigned char direction)
+{
+	char tmp[2];
+	tmp[0] = PORT_DIRECTION;
+	tmp[1] = (char)direction;
+	usbprog_jtag_message(usbprog_jtag, tmp, 2);
+}
+
+void usbprog_jtag_write_slice(struct usbprog_jtag *usbprog_jtag,unsigned char value)
+{
+	char tmp[2];
+	tmp[0] = PORT_SET;
+	tmp[1] = (char)value;
+	usbprog_jtag_message(usbprog_jtag, tmp, 2);
+}
+
+unsigned char usbprog_jtag_get_port(struct usbprog_jtag *usbprog_jtag)
+{
+	char tmp[2];
+	tmp[0] = PORT_GET;
+	tmp[1] = 0x00;
+	return usbprog_jtag_message(usbprog_jtag, tmp, 2);
+}
+
+void usbprog_jtag_set_bit(struct usbprog_jtag *usbprog_jtag,int bit, int value)
+{
+	char tmp[3];
+	tmp[0] = PORT_SETBIT;
+	tmp[1] = (char)bit;
+	if (value == 1)
+		tmp[2] = 0x01;
+	else
+		tmp[2] = 0x00;
+	usbprog_jtag_message(usbprog_jtag, tmp, 3);
+}
+
+int usbprog_jtag_get_bit(struct usbprog_jtag *usbprog_jtag, int bit)
+{
+	char tmp[2];
+	tmp[0] = PORT_GETBIT;
+	tmp[1] = (char)bit;
+
+	if (usbprog_jtag_message(usbprog_jtag, tmp, 2) > 0)
+		return 1;
+	else
+		return 0;
+}
+
+void usbprog_jtag_tms_collect(char tms_scan)
+{
+	tms_chain[tms_chain_index] = tms_scan;
+	tms_chain_index++;
+}
+
+void usbprog_jtag_tms_send(struct usbprog_jtag *usbprog_jtag)
+{
+	int i;
+	//INFO("TMS SEND");
+	if (tms_chain_index > 0)
+	{
+		char tmp[tms_chain_index + 2];
+		tmp[0] = WRITE_TMS_CHAIN;
+		tmp[1] = (char)(tms_chain_index);
+		for (i = 0; i < tms_chain_index + 1; i++)
+			tmp[2 + i] = tms_chain[i];
+		usb_bulk_write(usbprog_jtag->usb_handle, 3, tmp, tms_chain_index + 2, 1000);
+		tms_chain_index = 0;
+	}
+}