Dick Hollenbeck <dick@softplc.com> adds jtag_add_clocks() and implements those in the bitbang and ft2232.c. nearly a full rewrite of the xsvf.c. improved some messaging only affected by _DEBUG_JTAG_IO_

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

6 files changed, 1000 insertions, 316 deletions

diff --git a/src/jtag/bitbang.c b/src/jtag/bitbang.c
index 2d04e10..e4191d1 100644
--- a/src/jtag/bitbang.c
+++ b/src/jtag/bitbang.c
@@ -37,6 +37,10 @@
 #include <stdlib.h>
 #include <unistd.h>
 
+
+static void bitbang_stableclocks(int num_cycles);
+
+
 bitbang_interface_t *bitbang_interface;
 
 /* DANGER!!!! clock absolutely *MUST* be 0 in idle or reset won't work!
@@ -61,6 +65,8 @@ bitbang_interface_t *bitbang_interface;
 
 int bitbang_execute_queue(void);
 
+
+
 /* The bitbang driver leaves the TCK 0 when in idle */
 
 void bitbang_end_state(enum tap_state state)
@@ -153,6 +159,21 @@ void bitbang_runtest(int num_cycles)
 		bitbang_state_move();
 }
 
+
+static void bitbang_stableclocks(int num_cycles)
+{
+	int i;
+
+	/* send num_cycles clocks onto the cable */
+	for (i = 0; i < num_cycles; i++)
+	{
+		bitbang_interface->write(1, 0, 0);
+		bitbang_interface->write(0, 0, 0);
+	}
+}
+
+
+
 void bitbang_scan(int ir_scan, enum scan_type type, u8 *buffer, int scan_size)
 {
 	enum tap_state saved_end_state = end_state;
@@ -247,7 +268,7 @@ int bitbang_execute_queue(void)
 		{
 			case JTAG_END_STATE:
 #ifdef _DEBUG_JTAG_IO_
-				LOG_DEBUG("end_state: %i", cmd->cmd.end_state->end_state);
+				LOG_DEBUG("end_state: %s", jtag_state_name(cmd->cmd.end_state->end_state) );
 #endif
 				if (cmd->cmd.end_state->end_state != -1)
 					bitbang_end_state(cmd->cmd.end_state->end_state);
@@ -264,15 +285,20 @@ int bitbang_execute_queue(void)
 				break;
 			case JTAG_RUNTEST:
 #ifdef _DEBUG_JTAG_IO_
-				LOG_DEBUG("runtest %i cycles, end in %i", cmd->cmd.runtest->num_cycles, cmd->cmd.runtest->end_state);
+				LOG_DEBUG("runtest %i cycles, end in %s", cmd->cmd.runtest->num_cycles, jtag_state_name(cmd->cmd.runtest->end_state) );
 #endif
 				if (cmd->cmd.runtest->end_state != -1)
 					bitbang_end_state(cmd->cmd.runtest->end_state);
 				bitbang_runtest(cmd->cmd.runtest->num_cycles);
 				break;
+
+			case JTAG_STABLECLOCKS:
+				bitbang_stableclocks(cmd->cmd.stableclocks->num_cycles);
+				break;
+
 			case JTAG_STATEMOVE:
 #ifdef _DEBUG_JTAG_IO_
-				LOG_DEBUG("statemove end in %i", cmd->cmd.statemove->end_state);
+				LOG_DEBUG("statemove end in %s", jtag_state_name(cmd->cmd.statemove->end_state));
 #endif
 				if (cmd->cmd.statemove->end_state != -1)
 					bitbang_end_state(cmd->cmd.statemove->end_state);
@@ -280,13 +306,14 @@ int bitbang_execute_queue(void)
 				break;
 			case JTAG_PATHMOVE:
 #ifdef _DEBUG_JTAG_IO_
-				LOG_DEBUG("pathmove: %i states, end in %i", cmd->cmd.pathmove->num_states, cmd->cmd.pathmove->path[cmd->cmd.pathmove->num_states - 1]);
+				LOG_DEBUG("pathmove: %i states, end in %s", cmd->cmd.pathmove->num_states,
+					jtag_state_name(cmd->cmd.pathmove->path[cmd->cmd.pathmove->num_states - 1]));
 #endif
 				bitbang_path_move(cmd->cmd.pathmove);
 				break;
 			case JTAG_SCAN:
 #ifdef _DEBUG_JTAG_IO_
-				LOG_DEBUG("%s scan end in %i",  (cmd->cmd.scan->ir_scan) ? "IR" : "DR", cmd->cmd.scan->end_state);
+				LOG_DEBUG("%s scan end in %s",  (cmd->cmd.scan->ir_scan) ? "IR" : "DR", jtag_state_name(cmd->cmd.scan->end_state) );
 #endif
 				if (cmd->cmd.scan->end_state != -1)
 					bitbang_end_state(cmd->cmd.scan->end_state);
diff --git a/src/jtag/dummy.c b/src/jtag/dummy.c
index bfdf0dc..434a28d 100644
--- a/src/jtag/dummy.c
+++ b/src/jtag/dummy.c
@@ -32,8 +32,13 @@ static tap_state_t dummy_state = TAP_RESET;
 

 static int dummy_clock;         /* edge detector */

 

+static int clock_count;         /* count clocks in any stable state, only stable states */

+

+

 static tap_state_t tap_state_transition(tap_state_t cur_state, int tms);

 

+static u32 dummy_data;

+

 

 int dummy_speed(int speed);

 int dummy_register_commands(struct command_context_s *cmd_ctx);

@@ -76,7 +81,9 @@ bitbang_interface_t dummy_bitbang =
 

 int dummy_read(void)

 {

-	return 1;

+	int data = 1 & dummy_data;

+	dummy_data = (dummy_data >> 1) | (1<<31);

+	return data;

 }

 

 

@@ -88,9 +95,30 @@ void dummy_write(int tck, int tms, int tdi)
 		if( tck )

 		{

 			int old_state = dummy_state;

-			dummy_state = tap_state_transition( dummy_state, tms );

+			dummy_state = tap_state_transition( old_state, tms );

+

 			if( old_state != dummy_state )

-				LOG_DEBUG( "dummy_tap=%s", jtag_state_name(dummy_state) );

+			{

+				if( clock_count )

+				{

+					LOG_DEBUG("dummy_tap: %d stable clocks", clock_count);

+					clock_count = 0;

+				}

+

+				LOG_DEBUG("dummy_tap: %s", jtag_state_name(dummy_state) );

+

+#if defined(DEBUG)

+				if(dummy_state == TAP_DRCAPTURE)

+					dummy_data = 0x01255043;

+#endif

+			}

+			else

+			{

+				/* this is a stable state clock edge, no change of state here,

+				 * simply increment clock_count for subsequent logging

+				 */

+				++clock_count;

+			}

 		}

 		dummy_clock = tck;

 	}

@@ -99,8 +127,11 @@ void dummy_write(int tck, int tms, int tdi)
 void dummy_reset(int trst, int srst)

 {

 	dummy_clock = 0;

-	dummy_state = TAP_RESET;

-	LOG_DEBUG( "reset to %s", jtag_state_name(dummy_state) );

+

+	if (trst || (srst && (jtag_reset_config & RESET_SRST_PULLS_TRST)))

+		dummy_state = TAP_RESET;

+

+	LOG_DEBUG("reset to: %s", jtag_state_name(dummy_state) );

 }

 

 static int dummy_khz(int khz, int *jtag_speed)

diff --git a/src/jtag/ft2232.c b/src/jtag/ft2232.c
index ad5e637..c612502 100644
--- a/src/jtag/ft2232.c
+++ b/src/jtag/ft2232.c
@@ -76,6 +76,18 @@ int ft2232_handle_layout_command(struct command_context_s *cmd_ctx, char *cmd, c
 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);
 
+
+/**
+ * Function ft2232_stableclocks
+ * will send out \a num_cycles on the TCK line while the TAP(s)
+ * are in a stable state.  Calling code must ensure that current state is
+ * stable, that verification is not done in here.
+ * @param num_cycles is the count of clocks cycles to send.
+ * @return int - ERROR_OK or ERROR_JTAG_QUEUE_FAILED
+ */
+static int ft2232_stableclocks(int num_cycles, jtag_command_t *cmd);
+
+
 char *ft2232_device_desc = NULL;
 char *ft2232_serial = NULL;
 char *ft2232_layout = NULL;
@@ -149,10 +161,15 @@ static FT_HANDLE ftdih = NULL;
 static struct ftdi_context ftdic;
 #endif
 
+
+static jtag_command_t *first_unsent;		/* next command that has to be sent */
+static int require_send;
+
 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++]
@@ -344,7 +361,7 @@ void ft2232_end_state(enum tap_state state)
 
 void ft2232_read_scan(enum scan_type type, u8* buffer, int scan_size)
 {
-	int num_bytes = ((scan_size + 7) / 8);
+	int num_bytes = (scan_size + 7) / 8;
 	int bits_left = scan_size;
 	int cur_byte = 0;
 
@@ -903,14 +920,14 @@ int ft2232_predict_scan_out(int scan_size, enum scan_type type)
 	if (type == SCAN_IN)	/* only from device to host */
 	{
 		/* complete bytes */
-		predicted_size += (CEIL(num_bytes, 65536)) * 3;
+		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;
+		predicted_size += num_bytes + CEIL(num_bytes, 65536) * 3;
 		/* remaining bits -1 (up to 7) */
 		predicted_size += ((scan_size - 1) % 8) ? 3 : 0;
 	}
@@ -1185,15 +1202,16 @@ void stm32stick_reset(int trst, int srst)
 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;
 
+	first_unsent = cmd;			/* next command that has to be sent */
+	require_send = 0;
+
 	/* return ERROR_OK, unless ft2232_send_and_recv reports a failed check
 	 * that wasn't handled by a caller-provided error handler
 	 */
@@ -1214,6 +1232,7 @@ int ft2232_execute_queue()
 				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;
@@ -1236,6 +1255,7 @@ int ft2232_execute_queue()
 				LOG_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;
@@ -1292,9 +1312,10 @@ int ft2232_execute_queue()
 				}
 				require_send = 1;
 #ifdef _DEBUG_JTAG_IO_
-				LOG_DEBUG("runtest: %i, end in %i", cmd->cmd.runtest->num_cycles, end_state);
+				LOG_DEBUG("runtest: %i, end in %s", cmd->cmd.runtest->num_cycles, jtag_state_name(end_state));
 #endif
 				break;
+
 			case JTAG_STATEMOVE:
 				/* only send the maximum buffer size that FT2232C can handle */
 				predicted_size = 3;
@@ -1317,9 +1338,10 @@ int ft2232_execute_queue()
 				cur_state = end_state;
 				require_send = 1;
 #ifdef _DEBUG_JTAG_IO_
-				LOG_DEBUG("statemove: %i", end_state);
+				LOG_DEBUG("statemove: %s", jtag_state_name(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);
@@ -1333,9 +1355,11 @@ int ft2232_execute_queue()
 				ft2232_add_pathmove(cmd->cmd.pathmove);
 				require_send = 1;
 #ifdef _DEBUG_JTAG_IO_
-				LOG_DEBUG("pathmove: %i states, end in %i", cmd->cmd.pathmove->num_states, cmd->cmd.pathmove->path[cmd->cmd.pathmove->num_states - 1]);
+				LOG_DEBUG("pathmove: %i states, end in %s", cmd->cmd.pathmove->num_states,
+					jtag_state_name(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);
@@ -1375,18 +1399,45 @@ int ft2232_execute_queue()
 				if (buffer)
 					free(buffer);
 #ifdef _DEBUG_JTAG_IO_
-				LOG_DEBUG("%s scan, %i bit, end in %i", (cmd->cmd.scan->ir_scan) ? "IR" : "DR", scan_size, end_state);
+				LOG_DEBUG("%s scan, %i bits, end in %s", (cmd->cmd.scan->ir_scan) ? "IR" : "DR", scan_size,
+					jtag_state_name(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_
-				LOG_DEBUG("sleep %i usec", cmd->cmd.sleep->us);
+				LOG_DEBUG("sleep %i usec while in %s", cmd->cmd.sleep->us, jtag_state_name(cur_state));
+#endif
+				break;
+
+			case JTAG_STABLECLOCKS:
+				/* "if (tap_move_map[cur_state] != -1)" is of no help when cur_state==TAP_IDLE */
+				switch(cur_state)
+				{
+				case TAP_DRSHIFT:
+				case TAP_IDLE:
+				case TAP_RESET:
+				case TAP_DRPAUSE:
+				case TAP_IRSHIFT:
+				case TAP_IRPAUSE:
+					 break;			/* above stable states are OK */
+				default:
+					 LOG_ERROR( "jtag_add_clocks() was called with TAP in non-stable state \"%s\"",
+							 jtag_state_name(cur_state) );
+					 retval = ERROR_JTAG_QUEUE_FAILED;
+				}
+
+				if (ft2232_stableclocks(cmd->cmd.stableclocks->num_cycles, cmd) != ERROR_OK)
+					retval = ERROR_JTAG_QUEUE_FAILED;
+#ifdef _DEBUG_JTAG_IO_
+				LOG_DEBUG("clocks %i while in %s", cmd->cmd.stableclocks->num_cycles, jtag_state_name(cur_state));
 #endif
 				break;
+
 			default:
 				LOG_ERROR("BUG: unknown JTAG command type encountered");
 				exit(-1);
@@ -2279,3 +2330,42 @@ int ft2232_handle_latency_command(struct command_context_s *cmd_ctx, char *cmd,
 
 	return ERROR_OK;
 }
+
+
+static int ft2232_stableclocks(int num_cycles, jtag_command_t *cmd)
+{
+	int retval = 0;
+
+	while (num_cycles > 0)
+	{
+		/* the command 0x4b, "Clock Data to TMS/CS Pin (no Read)" handles
+		 * at most 7 bits per invocation.  Here we invoke it potentially
+		 * several times.
+		 */
+		int bitcount_per_command = (num_cycles > 7) ? 7 : num_cycles;
+
+		if (ft2232_buffer_size + 3 >= FT2232_BUFFER_SIZE)
+		{
+			if (ft2232_send_and_recv(first_unsent, cmd) != ERROR_OK)
+				retval = ERROR_JTAG_QUEUE_FAILED;
+
+			first_unsent = cmd;
+		}
+
+		/* command "Clock Data to TMS/CS Pin (no Read)" */
+		BUFFER_ADD = 0x4b;
+
+		/* scan 7 bit */
+		BUFFER_ADD = bitcount_per_command - 1;
+
+		/* TMS data bits are all zeros to stay in the current stable state */
+		BUFFER_ADD = 0x0;
+
+		require_send = 1;
+
+		num_cycles -= bitcount_per_command;
+	}
+
+	return retval;
+}
+
diff --git a/src/jtag/jtag.c b/src/jtag/jtag.c
index a2af3d8..a0cdf01 100644
--- a/src/jtag/jtag.c
+++ b/src/jtag/jtag.c
@@ -79,22 +79,22 @@ int tap_move_map[16] = {
 
 tap_transition_t tap_transitions[16] =
 {
-	{TAP_RESET, TAP_IDLE},		/* RESET */
-	{TAP_IRSELECT, TAP_DRCAPTURE},		/* DRSELECT */
-	{TAP_DREXIT1, TAP_DRSHIFT},		/* DRCAPTURE  */
-	{TAP_DREXIT1, TAP_DRSHIFT},		/* DRSHIFT  */
+	{TAP_RESET, 		TAP_IDLE},			/* RESET */
+	{TAP_IRSELECT, 	TAP_DRCAPTURE},		/* DRSELECT */
+	{TAP_DREXIT1,	TAP_DRSHIFT},		/* DRCAPTURE  */
+	{TAP_DREXIT1, 	TAP_DRSHIFT},		/* DRSHIFT  */
 	{TAP_DRUPDATE,  TAP_DRPAUSE}, 		/* DREXIT1 */
-	{TAP_DREXIT2, TAP_DRPAUSE},		/* DRPAUSE  */
+	{TAP_DREXIT2, 	TAP_DRPAUSE},		/* DRPAUSE  */
 	{TAP_DRUPDATE,  TAP_DRSHIFT},		/* DREXIT2 */
-	{TAP_DRSELECT, TAP_IDLE},		/* DRUPDATE  */
-	{TAP_DRSELECT, TAP_IDLE},		/* IDLE */
-	{TAP_RESET, TAP_IRCAPTURE},		/* IRSELECT */
-	{TAP_IREXIT1, TAP_IRSHIFT},		/* IRCAPTURE  */
-	{TAP_IREXIT1, TAP_IRSHIFT},		/* IRSHIFT  */
+	{TAP_DRSELECT, 	TAP_IDLE},			/* DRUPDATE  */
+	{TAP_DRSELECT, 	TAP_IDLE},			/* IDLE */
+	{TAP_RESET, 		TAP_IRCAPTURE},		/* IRSELECT */
+	{TAP_IREXIT1, 	TAP_IRSHIFT},		/* IRCAPTURE  */
+	{TAP_IREXIT1, 	TAP_IRSHIFT},		/* IRSHIFT  */
 	{TAP_IRUPDATE,  TAP_IRPAUSE}, 		/* IREXIT1 */
-	{TAP_IREXIT2, TAP_IRPAUSE},		/* IRPAUSE  */
+	{TAP_IREXIT2, 	TAP_IRPAUSE},		/* IRPAUSE  */
 	{TAP_IRUPDATE,  TAP_IRSHIFT},		/* IREXIT2 */
-	{TAP_DRSELECT, TAP_IDLE}		/* IRUPDATE  */
+	{TAP_DRSELECT, 	TAP_IDLE}			/* IRUPDATE  */
 };
 
 char* jtag_event_strings[] =
@@ -983,7 +983,6 @@ int MINIDRIVER(interface_jtag_add_tlr)()
 	(*last_cmd)->cmd.statemove = cmd_queue_alloc(sizeof(statemove_command_t));
 	(*last_cmd)->cmd.statemove->end_state = state;
 
-
 	return ERROR_OK;
 }
 
@@ -1074,6 +1073,33 @@ void jtag_add_runtest(int num_cycles, enum tap_state state)
 		jtag_error=retval;
 }
 
+
+int MINIDRIVER(interface_jtag_add_clocks)( int num_cycles )
+{
+	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_STABLECLOCKS;
+
+	(*last_cmd)->cmd.stableclocks = cmd_queue_alloc(sizeof(stableclocks_command_t));
+	(*last_cmd)->cmd.stableclocks->num_cycles = num_cycles;
+	return ERROR_OK;
+}
+
+void jtag_add_clocks( int num_cycles )
+{
+	int retval;
+
+	jtag_prelude1();
+
+	retval=interface_jtag_add_clocks(num_cycles);
+	if (retval!=ERROR_OK)
+		jtag_error=retval;
+}
+
 void jtag_add_reset(int req_tlr_or_trst, int req_srst)
 {
 	int trst_with_tlr = 0;
@@ -1252,6 +1278,8 @@ int jtag_build_buffer(scan_command_t *cmd, u8 **buffer)
 
 	bit_count = 0;
 
+	LOG_DEBUG("num_fields: %i",cmd->num_fields);
+
 	for (i = 0; i < cmd->num_fields; i++)
 	{
 		if (cmd->fields[i].out_value)
@@ -1261,12 +1289,13 @@ int jtag_build_buffer(scan_command_t *cmd, u8 **buffer)
 #endif
 			buf_set_buf(cmd->fields[i].out_value, 0, *buffer, bit_count, cmd->fields[i].num_bits);
 #ifdef _DEBUG_JTAG_IO_
-			LOG_DEBUG("fields[%i].out_value: 0x%s", i, char_buf);
+			LOG_DEBUG("fields[%i].out_value[%i]: 0x%s", i, cmd->fields[i].num_bits, char_buf);
 			free(char_buf);
 #endif
 		}
 
 		bit_count += cmd->fields[i].num_bits;
+		LOG_DEBUG("bit_count totalling: %i",  bit_count );
 	}
 
 	return bit_count;
@@ -1292,10 +1321,8 @@ int jtag_read_buffer(u8 *buffer, scan_command_t *cmd)
 			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);
-			LOG_DEBUG("fields[%i].in_value: 0x%s", i, char_buf);
+			char *char_buf = buf_to_str(captured, (num_bits > 64) ? 64 : num_bits, 16);
+			LOG_DEBUG("fields[%i].in_value[%i]: 0x%s", i, num_bits, char_buf);
 			free(char_buf);
 #endif
 
diff --git a/src/jtag/jtag.h b/src/jtag/jtag.h
index 82fa061..cc3c9f0 100644
--- a/src/jtag/jtag.h
+++ b/src/jtag/jtag.h
@@ -29,6 +29,7 @@
 
 #include "command.h"
 
+
 #if 0
 #define _DEBUG_JTAG_IO_
 #endif
@@ -112,6 +113,13 @@ typedef struct runtest_command_s
 	enum tap_state end_state;	/* TAP state in which JTAG commands should finish */
 } runtest_command_t;
 
+
+typedef struct stableclocks_command_s
+{
+	int num_cycles;				/* number of clock cycles that should be sent */
+} stableclocks_command_t;
+
+
 typedef struct reset_command_s
 {
 	int trst;			/* trst/srst 0: deassert, 1: assert, -1: don't change */
@@ -134,6 +142,7 @@ typedef union jtag_command_container_u
 	statemove_command_t *statemove;
 	pathmove_command_t *pathmove;
 	runtest_command_t *runtest;
+	stableclocks_command_t *stableclocks;
 	reset_command_t *reset;
 	end_state_command_t *end_state;
 	sleep_command_t *sleep;
@@ -144,7 +153,8 @@ enum jtag_command_type
 	JTAG_SCAN = 1,
 	JTAG_STATEMOVE = 2, JTAG_RUNTEST = 3,
 	JTAG_RESET = 4, JTAG_END_STATE = 5,
-	JTAG_PATHMOVE = 6, JTAG_SLEEP = 7
+	JTAG_PATHMOVE = 6, JTAG_SLEEP = 7,
+	JTAG_STABLECLOCKS = 8
 };
 
 typedef struct jtag_command_s
@@ -434,6 +444,16 @@ extern int interface_jtag_add_end_state(enum tap_state endstate);
 extern void jtag_add_sleep(u32 us);
 extern int interface_jtag_add_sleep(u32 us);
 
+
+/**
+ * Function jtag_add_stable_clocks
+ * first checks that the state in which the clocks are to be issued is
+ * stable, then queues up clock_count clocks for transmission.
+ */
+void jtag_add_clocks( int num_cycles );
+int interface_jtag_add_clocks( int num_cycles );
+
+
 /*
  * For software FIFO implementations, the queued commands can be executed
  * during this call or earlier. A sw queue might decide to push out
diff --git a/src/xsvf/xsvf.c b/src/xsvf/xsvf.c
index 1f594be..1bcc5d0 100644
--- a/src/xsvf/xsvf.c
+++ b/src/xsvf/xsvf.c
@@ -1,28 +1,42 @@
 /***************************************************************************
- *   Copyright (C) 2005 by Dominic Rath                                    *
- *   Dominic.Rath@gmx.de                                                   *
- *                                                                         *
- *   Copyright (C) 2007,2008 Øyvind Harboe                                 *
- *   oyvind.harboe@zylin.com                                               *
- *                                                                         *
- *   Copyright (C) 2008 Peter Hettkamp                                     *
- *   peter.hettkamp@htp-tel.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.             *
+ *	 Copyright (C) 2005 by Dominic Rath									   *
+ *	 Dominic.Rath@gmx.de													   *
+ *																		   *
+ *	 Copyright (C) 2007,2008 Øyvind Harboe								   *
+ *	 oyvind.harboe@zylin.com												   *
+ *																		   *
+ *	 Copyright (C) 2008 Peter Hettkamp									   *
+ *	 peter.hettkamp@htp-tel.de											   *
+ *																		   *
+ *	 Copyright (C) 2009 SoftPLC Corporation. http://softplc.com             *
+ *	 dick@softplc.com											           *
+ *                                                                          *
+ *	 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.			   *
  ***************************************************************************/
+
+
+/* The specification for SVF is available here:
+ * http://www.asset-intertech.com/support/svf.pdf
+ * Below, this document is refered to as the "SVF spec".
+ *
+ * The specification for XSVF is available here:
+ * http://www.xilinx.com/support/documentation/application_notes/xapp503.pdf
+ * Below, this document is refered to as the "XSVF spec".
+ */
+
 #ifdef HAVE_CONFIG_H
 #include "config.h"
 #endif
@@ -43,50 +57,131 @@
 #include <sys/time.h>
 #include <time.h>
 
-#define XSTATE_MAX_PATH (12)
 
-int handle_xsvf_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+/* XSVF commands, from appendix B of xapp503.pdf  */
+#define XCOMPLETE		0x00
+#define XTDOMASK			0x01
+#define XSIR				0x02
+#define XSDR				0x03
+#define XRUNTEST			0x04
+#define XREPEAT			0x07
+#define XSDRSIZE			0x08
+#define XSDRTDO			0x09
+#define XSETSDRMASKS		0x0A
+#define XSDRINC			0x0B
+#define XSDRB			0x0C
+#define XSDRC			0x0D
+#define XSDRE			0x0E
+#define XSDRTDOB			0x0F
+#define XSDRTDOC			0x10
+#define XSDRTDOE			0x11
+#define XSTATE			0x12
+#define XENDIR			0x13
+#define XENDDR			0x14
+#define XSIR2			0x15
+#define XCOMMENT			0x16
+#define XWAIT			0x17
 
-int xsvf_fd = 0;
+/* XWAITSTATE is not in the xilinx XSVF spec, but the svf2xsvf.py translator
+ * generates this.  Arguably it is needed because the XSVF XRUNTEST command
+ * was ill conceived and does not directly flow out of the SVF RUNTEST command.
+ * This XWAITSTATE does map directly from the SVF RUNTEST command.
+ */
+#define XWAITSTATE		0x18
 
-u8 *dr_out_buf;	/* from host to device (TDI) */
-u8 *dr_in_buf;	/* from device to host (TDO) */
-u8 *dr_in_mask;
+/* Lattice has extended the SVF file format, and Dick Hollenbeck's python based
+ * SVF2XSVF converter supports these 3 additional XSVF opcodes, LCOUNT, LDELAY, LSDR.
+ * Here is an example of usage of the 3 lattice opcode extensions:
 
-int xsdrsize = 0;
-int xruntest = 0;	/* number of TCK cycles / microseconds */
-int xrepeat = 0x20; /* number of XC9500 retries */
+! Set the maximum loop count to 25.
+LCOUNT	25;
+! Step to DRPAUSE give 5 clocks and wait for 1.00e+000 SEC.
+LDELAY	DRPAUSE	5 TCK	1.00E-003 SEC;
+! Test for the completed status. Match means pass.
+! Loop back to LDELAY line if not match and loop count less than 25.
 
-int xendir = 0;
-int xenddr = 0;
+LSDR 1  TDI  (0)
+		TDO  (1);
+*/
 
-enum tap_state xsvf_to_tap[] =
-{
-	TAP_RESET, TAP_IDLE,
-	TAP_DRSELECT, TAP_DRCAPTURE, TAP_DRSHIFT, TAP_DREXIT1, TAP_DRPAUSE, TAP_DREXIT2, TAP_DRUPDATE,
-	TAP_IRSELECT, TAP_IRCAPTURE, TAP_IRSHIFT, TAP_IREXIT1, TAP_IRPAUSE, TAP_IREXIT2, TAP_IRUPDATE,
-};
+#define LCOUNT			0x19
+#define LDELAY			0x1A
+#define LSDR				0x1B
+
+
+/* XSVF valid state values for the XSTATE command, from appendix B of xapp503.pdf */
+#define XSV_RESET		0x00
+#define XSV_IDLE			0x01
+#define XSV_DRSELECT		0x02
+#define XSV_DRCAPTURE	0x03
+#define XSV_DRSHIFT		0x04
+#define XSV_DREXIT1		0x05
+#define XSV_DRPAUSE		0x06
+#define XSV_DREXIT2		0x07
+#define XSV_DRUPDATE		0x08
+#define XSV_IRSELECT		0x09
+#define XSV_IRCAPTURE	0x0A
+#define XSV_IRSHIFT		0x0B
+#define XSV_IREXIT1		0x0C
+#define XSV_IRPAUSE		0x0D
+#define XSV_IREXIT2		0x0E
+#define XSV_IRUPDATE		0x0F
+
+
+#define XSTATE_MAX_PATH 12
 
-int tap_to_xsvf[] =
+static int handle_xsvf_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+
+static int xsvf_fd = 0;
+
+
+/* map xsvf tap state to an openocd "enum tap_state" */
+static tap_state_t xsvf_to_tap( int xsvf_state )
 {
-	0x0, 0x2, 0x3, 0x4, 0x5, 0x6, 0x7, 0x8, 0x1, 0x9, 0xa, 0xb, 0xc, 0xe, 0xf
-};
+	tap_state_t	ret;
+
+	switch( xsvf_state )
+	{
+	case XSV_RESET:			ret = TAP_RESET;			break;
+	case XSV_IDLE:			ret = TAP_IDLE;			break;
+	case XSV_DRSELECT:		ret = TAP_DRSELECT;		break;
+	case XSV_DRCAPTURE:		ret = TAP_DRCAPTURE;		break;
+	case XSV_DRSHIFT:		ret = TAP_DRSHIFT;		break;
+	case XSV_DREXIT1:		ret = TAP_DREXIT1;		break;
+	case XSV_DRPAUSE:		ret = TAP_DRPAUSE;		break;
+	case XSV_DREXIT2:		ret = TAP_DREXIT2;		break;
+	case XSV_DRUPDATE:		ret = TAP_DRUPDATE;		break;
+	case XSV_IRSELECT:		ret = TAP_IRSELECT;		break;
+	case XSV_IRCAPTURE:		ret = TAP_IRCAPTURE;		break;
+	case XSV_IRSHIFT:		ret = TAP_IRSHIFT;		break;
+	case XSV_IREXIT1:		ret = TAP_IREXIT1;		break;
+	case XSV_IRPAUSE:		ret = TAP_IRPAUSE;		break;
+	case XSV_IREXIT2:		ret = TAP_IREXIT2;		break;
+	case XSV_IRUPDATE:		ret = TAP_IRUPDATE;		break;
+	default:
+		LOG_ERROR( "UNKNOWN XSVF STATE 0x%02X", xsvf_state );
+		exit(1);
+	}
+
+	return ret;
+}
 
 
 /* xsvf has it's own definition of a statemove. This needs
- * to be handled according to the specs, which has nothing
+ * to be handled according to the xsvf spec, which has nothing
  * to do with the JTAG spec or OpenOCD as such.
  *
  * Implemented via jtag_add_pathmove().
  */
-void xsvf_add_statemove(enum tap_state state)
+static void xsvf_add_statemove(tap_state_t state)
 {
-	enum tap_state moves[7]; /* max # of transitions */
+	tap_state_t moves[7]; 	/* max # of transitions */
+	tap_state_t curstate = cmd_queue_cur_state;
 	int i;
-	enum tap_state curstate = cmd_queue_cur_state;
+
 	u8 move = TAP_MOVE(cmd_queue_cur_state, state);
 
-	if ((state != TAP_RESET) && (state == cmd_queue_cur_state))
+	if (state != TAP_RESET  &&  state==cmd_queue_cur_state)
 		return;
 
 	if(state==TAP_RESET)
@@ -94,13 +189,15 @@ void xsvf_add_statemove(enum tap_state state)
 		jtag_add_tlr();
 		return;
 	}
+
 	for (i=0; i<7; i++)
 	{
 		int j = (move >> i) & 1;
 		if (j)
 		{
 			curstate = tap_transitions[curstate].high;
-		} else
+		}
+		else
 		{
 			curstate = tap_transitions[curstate].low;
 		}
@@ -113,17 +210,18 @@ void xsvf_add_statemove(enum tap_state state)
 int xsvf_register_commands(struct command_context_s *cmd_ctx)
 {
 	register_command(cmd_ctx, NULL, "xsvf", handle_xsvf_command,
-		COMMAND_EXEC, "run xsvf <file>");
+		COMMAND_EXEC, "run xsvf <file> [virt2]");
 
 	return ERROR_OK;
 }
 
-int xsvf_read_buffer(int num_bits, int fd, u8* buf)
+static int xsvf_read_buffer(int num_bits, int fd, u8* buf)
 {
 	int num_bytes;
 
 	for (num_bytes = (num_bits + 7) / 8; num_bytes > 0; num_bytes--)
 	{
+		/* reverse the order of bytes as they are read sequentially from file */
 		if (read(fd, buf + num_bytes - 1, 1) < 0)
 			return ERROR_XSVF_EOF;
 	}
@@ -131,13 +229,16 @@ int xsvf_read_buffer(int num_bits, int fd, u8* buf)
 	return ERROR_OK;
 }
 
-int xsvf_read_xstates(int fd, enum tap_state *path, int max_path, int *path_len)
+
+static int xsvf_read_xstates(int fd, tap_state_t *path, int max_path, int *path_len)
 {
 	char c;
-	unsigned char uc;
+	u8   uc;
 
-	while ((read(fd, &c, 1) > 0) && (c == 0x12))
+	while ((read(fd, &c, 1) > 0) && (c == XSTATE))
 	{
+		tap_state_t	mystate;
+
 		if (*path_len > max_path)
 		{
 			LOG_WARNING("XSTATE path longer than max_path");
@@ -147,7 +248,12 @@ int xsvf_read_xstates(int fd, enum tap_state *path, int max_path, int *path_len)
 		{
 			return ERROR_XSVF_EOF;
 		}
-		path[(*path_len)++] = xsvf_to_tap[uc];
+
+		mystate = xsvf_to_tap(uc);
+
+		LOG_DEBUG("XSTATE %02X %s", uc, jtag_state_name(mystate) );
+
+		path[(*path_len)++] = mystate;
 	}
 
 	lseek(fd, -1, SEEK_CUR);
@@ -155,273 +261,325 @@ int xsvf_read_xstates(int fd, enum tap_state *path, int max_path, int *path_len)
 	return ERROR_OK;
 }
 
-int handle_xsvf_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+
+static int handle_xsvf_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
 {
-	char c;
-	u8 buf4[4], buf2[2];
-	unsigned char uc, uc2;
-	unsigned int ui;
-	unsigned short us;
+	u8 *dr_out_buf = NULL; 				/* from host to device (TDI) */
+	u8 *dr_in_buf = NULL;				/* from device to host (TDO) */
+	u8 *dr_in_mask = NULL;
 
-	int do_abort = 0;
-	int unsupported = 0;
-	int tdo_mismatch = 0;
+	int xsdrsize = 0;
+	int xruntest = 0;					/* number of TCK cycles OR microseconds */
+	int xrepeat	 = 0;					/* number of retries */
+
+	tap_state_t	xendir = TAP_IDLE;		/* see page 8 of the SVF spec, initial xendir to be TAP_IDLE */
+	tap_state_t xenddr = TAP_IDLE;
+
+	u8  		opcode;
+	u8		uc;
+	long		file_offset = 0;
+
+	int		loop_count = 0;
+	tap_state_t	loop_state = TAP_IDLE;
+	int		loop_clocks = 0;
+	int		loop_usecs = 0;
+
+	int 		do_abort = 0;
+	int 		unsupported = 0;
+	int 		tdo_mismatch = 0;
+	int 		result;
+
+	int 		runtest_requires_tck = 0;	/* a flag telling whether to clock TCK during waits, or simply sleep, controled by virt2 */
 
-	int runtest_requires_tck = 0;
 
-	jtag_tap_t *tap = NULL;
 	/* use NULL to indicate a "plain" xsvf file which accounts for
 	   additional devices in the scan chain, otherwise the device
-	   that should be affected */
+	   that should be affected
+	*/
+	jtag_tap_t *tap = NULL;
 
 	if (argc < 2)
 	{
 		command_print(cmd_ctx, "usage: xsvf <device#|plain> <file> <variant>");
-		return ERROR_OK;
+		return ERROR_FAIL;
 	}
 
 	if (strcmp(args[0], "plain") != 0)
 	{
-	  tap = jtag_TapByString( args[0] );
-	  if( !tap ){
-	    command_print( cmd_ctx, "Tap: %s unknown", args[0] );
-	    return ERROR_OK;
-	  }
+		tap = jtag_TapByString( args[0] );
+		if (!tap )
+		{
+			command_print( cmd_ctx, "Tap: %s unknown", args[0] );
+			return ERROR_FAIL;
+		}
 	}
 
 	if ((xsvf_fd = open(args[1], O_RDONLY)) < 0)
 	{
-		command_print(cmd_ctx, "file %s not found", args[0]);
-		return ERROR_OK;
+		command_print(cmd_ctx, "file \"%s\" not found", args[1]);
+		return ERROR_FAIL;
 	}
 
+	/* if this argument is present, then interpret xruntest counts as TCK cycles rather than as usecs */
 	if ((argc > 2) && (strcmp(args[2], "virt2") == 0))
 	{
 		runtest_requires_tck = 1;
 	}
 
-	while (read(xsvf_fd, &c, 1) > 0)
+	LOG_USER("xsvf processing file: \"%s\"", args[1]);
+
+	while( read(xsvf_fd, &opcode, 1) > 0 )
 	{
-		switch (c)
+		/* record the position of the just read opcode within the file */
+		file_offset = lseek(xsvf_fd, 0, SEEK_CUR) - 1;
+
+		switch (opcode)
 		{
-			case 0x00:	/* XCOMPLETE */
+			case XCOMPLETE:
 				LOG_DEBUG("XCOMPLETE");
-				if (jtag_execute_queue() != ERROR_OK)
+
+				result = jtag_execute_queue();
+				if (result != ERROR_OK)
 				{
 					tdo_mismatch = 1;
 					break;
 				}
 				break;
-			case 0x01:	/* XTDOMASK */
+
+			case XTDOMASK:
 				LOG_DEBUG("XTDOMASK");
 				if (dr_in_mask && (xsvf_read_buffer(xsdrsize, xsvf_fd, dr_in_mask) != ERROR_OK))
 					do_abort = 1;
 				break;
-			case 0x02:	/* XSIR */
-				LOG_DEBUG("XSIR");
-				if (read(xsvf_fd, &c, 1) < 0)
-					do_abort = 1;
-				else
+
+			case XRUNTEST:
 				{
-					u8 *ir_buf = malloc((c + 7) / 8);
-					if (xsvf_read_buffer(c, xsvf_fd, ir_buf) != ERROR_OK)
-						do_abort = 1;
-					else
+					u8	xruntest_buf[4];
+
+					if (read(xsvf_fd, xruntest_buf, 4) < 0)
 					{
-						scan_field_t field;
-						field.tap = tap;
-						field.num_bits = c;
-						field.out_value = ir_buf;
-						field.out_mask = NULL;
-						field.in_value = NULL;
-						field.in_check_value = NULL;
-						field.in_check_mask = NULL;
-						field.in_handler = NULL;
-						field.in_handler_priv = NULL;
-						if (tap == NULL)
-							jtag_add_plain_ir_scan(1, &field, TAP_IRPAUSE);
-						else
-							jtag_add_ir_scan(1, &field, TAP_IRPAUSE);
-						if (jtag_execute_queue() != ERROR_OK)
-						{
-							tdo_mismatch = 1;
-							free(ir_buf);
-							break;
-						}
-						if (xruntest)
-						{
-							if (runtest_requires_tck)
-								jtag_add_runtest(xruntest, xsvf_to_tap[xendir]);
-							else
-							{
-								xsvf_add_statemove(TAP_IDLE);
-								jtag_add_sleep(xruntest);
-								xsvf_add_statemove(xsvf_to_tap[xendir]);
-							}
-						}
-						else if (xendir != 0xd)	/* Pause-IR */
-							xsvf_add_statemove(xsvf_to_tap[xendir]);
+						do_abort = 1;
+						break;
 					}
-					free(ir_buf);
+
+					xruntest = be_to_h_u32(xruntest_buf);
+					LOG_DEBUG("XRUNTEST %d 0x%08X", xruntest, xruntest);
 				}
 				break;
-			case 0x03:	/* XSDR */
-				LOG_DEBUG("XSDR");
-				if (xsvf_read_buffer(xsdrsize, xsvf_fd, dr_out_buf) != ERROR_OK)
-					do_abort = 1;
-				else
+
+			case XREPEAT:
 				{
-					scan_field_t field;
-					field.tap = tap;
-					field.num_bits = xsdrsize;
-					field.out_value = dr_out_buf;
-					field.out_mask = NULL;
-					field.in_value = NULL;
-					jtag_set_check_value(&field, dr_in_buf, dr_in_mask, NULL);
-					if (tap == NULL)
-						jtag_add_plain_dr_scan(1, &field, TAP_DRPAUSE);
+					u8 myrepeat;
+
+					if (read(xsvf_fd, &myrepeat, 1) < 0)
+						do_abort = 1;
 					else
-						jtag_add_dr_scan(1, &field, TAP_DRPAUSE);
-					if (jtag_execute_queue() != ERROR_OK)
 					{
-						tdo_mismatch = 1;
-						break;
+						xrepeat = myrepeat;
+						LOG_DEBUG("XREPEAT %d", xrepeat );
 					}
-					if (xruntest)
-					{
-						if (runtest_requires_tck)
-							jtag_add_runtest(xruntest, xsvf_to_tap[xenddr]);
-						else
-						{
-							xsvf_add_statemove(TAP_IDLE);
-							jtag_add_sleep(xruntest);
-							xsvf_add_statemove(xsvf_to_tap[xenddr]);
-						}
-					}
-					else if (xendir != 0x6)	/* Pause-DR */
-						xsvf_add_statemove(xsvf_to_tap[xenddr]);
-				}
-				break;
-			case 0x04:	/* XRUNTEST */
-				LOG_DEBUG("XRUNTEST");
-				if (read(xsvf_fd, buf4, 4) < 0)
-					do_abort = 1;
-				else
-				{
-					xruntest = be_to_h_u32(buf4);
 				}
 				break;
-			case 0x07:	/* XREPEAT */
-				LOG_DEBUG("XREPEAT");
-				if (read(xsvf_fd, &c, 1) < 0)
-					do_abort = 1;
-				else
-				{
-					xrepeat = c;
-				}
-				break;
-			case 0x08:	/* XSDRSIZE */
-				LOG_DEBUG("XSDRSIZE");
-				if (read(xsvf_fd, buf4, 4) < 0)
-					do_abort = 1;
-				else
+
+			case XSDRSIZE:
 				{
-					xsdrsize = be_to_h_u32(buf4);
-					free(dr_out_buf);
-					free(dr_in_buf);
-					free(dr_in_mask);
+					u8	xsdrsize_buf[4];
+
+					if (read(xsvf_fd, xsdrsize_buf, 4) < 0)
+					{
+						do_abort = 1;
+						break;
+					}
+
+					xsdrsize = be_to_h_u32(xsdrsize_buf);
+					LOG_DEBUG("XSDRSIZE %d", xsdrsize);
+
+					if( dr_out_buf ) free(dr_out_buf);
+					if( dr_in_buf)   free(dr_in_buf);
+					if( dr_in_mask)  free(dr_in_mask);
+
 					dr_out_buf = malloc((xsdrsize + 7) / 8);
 					dr_in_buf = malloc((xsdrsize + 7) / 8);
 					dr_in_mask = malloc((xsdrsize + 7) / 8);
 				}
 				break;
-			case 0x09:	/* XSDRTDO */
-				LOG_DEBUG("XSDRTDO");
-				if (xsvf_read_buffer(xsdrsize, xsvf_fd, dr_out_buf) != ERROR_OK)
-					do_abort = 1;
-				else
+
+			case XSDR:		/* these two are identical except for the dr_in_buf */
+			case XSDRTDO:
 				{
-					if (xsvf_read_buffer(xsdrsize, xsvf_fd, dr_in_buf) != ERROR_OK)
+					int limit = xrepeat;
+					int	matched = 0;
+					int attempt;
+
+					const char* op_name = (opcode == XSDR ? "XSDR" : "XSDRTDO");
+
+					if (xsvf_read_buffer(xsdrsize, xsvf_fd, dr_out_buf) != ERROR_OK)
+					{
 						do_abort = 1;
-					else
+						break;
+					}
+
+					if (opcode == XSDRTDO)
+					{
+						if(xsvf_read_buffer(xsdrsize, xsvf_fd, dr_in_buf)  != ERROR_OK )
+						{
+							do_abort = 1;
+							break;
+						}
+					}
+
+					if (limit < 1)
+						limit = 1;
+
+					LOG_DEBUG("%s %d", op_name, xsdrsize);
+
+					for( attempt=0; attempt<limit;  ++attempt )
 					{
 						scan_field_t field;
+
+						if( attempt>0 )
+						{
+							/* perform the XC9500 exception handling sequence shown in xapp067.pdf and
+							   illustrated in psuedo code at end of this file.  We start from state
+							   DRPAUSE:
+							   go to Exit2-DR
+							   go to Shift-DR
+							   go to Exit1-DR
+							   go to Update-DR
+							   go to Run-Test/Idle
+
+							   This sequence should be harmless for other devices, and it
+							   will be skipped entirely if xrepeat is set to zero.
+							*/
+
+							static tap_state_t exception_path[] = {
+								TAP_DREXIT2,
+								TAP_DRSHIFT,
+								TAP_DREXIT1,
+								TAP_DRUPDATE,
+								TAP_IDLE,
+							};
+
+							jtag_add_pathmove( sizeof(exception_path)/sizeof(exception_path[0]), exception_path);
+
+							LOG_USER("%s %d retry %d", op_name, xsdrsize, attempt);
+						}
+
 						field.tap = tap;
 						field.num_bits = xsdrsize;
 						field.out_value = dr_out_buf;
 						field.out_mask = NULL;
 						field.in_value = NULL;
+
 						jtag_set_check_value(&field, dr_in_buf, dr_in_mask, NULL);
+
 						if (tap == NULL)
 							jtag_add_plain_dr_scan(1, &field, TAP_DRPAUSE);
 						else
 							jtag_add_dr_scan(1, &field, TAP_DRPAUSE);
-						if (jtag_execute_queue() != ERROR_OK)
+
+						/* LOG_DEBUG("FLUSHING QUEUE"); */
+						result = jtag_execute_queue();
+						if (result == ERROR_OK)
 						{
-							tdo_mismatch = 1;
+							matched = 1;
 							break;
 						}
-						if (xruntest)
-						{
-							if (runtest_requires_tck)
-								jtag_add_runtest(xruntest, xsvf_to_tap[xenddr]);
-							else
-							{
-								xsvf_add_statemove(TAP_IDLE);
-								jtag_add_sleep(xruntest);
-								xsvf_add_statemove(xsvf_to_tap[xenddr]);
-							}
-						}
-						else if (xendir != 0x6)	/* Pause-DR */
-							xsvf_add_statemove(xsvf_to_tap[xenddr]);
 					}
+
+					if (!matched)
+					{
+						LOG_USER( "%s mismatch", op_name);
+						tdo_mismatch = 1;
+						break;
+					}
+
+					/* See page 19 of XSVF spec regarding opcode "XSDR" */
+					if (xruntest)
+					{
+						xsvf_add_statemove(TAP_IDLE);
+
+						if (runtest_requires_tck)
+							jtag_add_clocks(xruntest);
+						else
+							jtag_add_sleep(xruntest);
+					}
+					else if (xendir != TAP_DRPAUSE)	/* we are already in TAP_DRPAUSE */
+						xsvf_add_statemove(xenddr);
 				}
 				break;
-			case 0x0a:	/* XSETDRMASKS */
+
+			case XSETSDRMASKS:
 				LOG_ERROR("unsupported XSETSDRMASKS\n");
 				unsupported = 1;
 				break;
-			case 0x0b:	/* XSDRINC */
+
+			case XSDRINC:
 				LOG_ERROR("unsupported XSDRINC\n");
 				unsupported = 1;
 				break;
-			case 0x0c:	/* XSDRB */
+
+			case XSDRB:
+				LOG_ERROR("unsupported XSDRB\n");
 				unsupported = 1;
 				break;
-			case 0x0d:	/* XSDRC */
+
+			case XSDRC:
+				LOG_ERROR("unsupported XSDRC\n");
 				unsupported = 1;
 				break;
-			case 0x0e:	/* XSDRE */
+
+			case XSDRE:
+				LOG_ERROR("unsupported XSDRE\n");
 				unsupported = 1;
 				break;
-			case 0x0f:	/* XSDRTDOB */
+
+			case XSDRTDOB:
+				LOG_ERROR("unsupported XSDRTDOB\n");
 				unsupported = 1;
 				break;
-			case 0x10:	/* XSDRTDOB */
+
+			case XSDRTDOC:
+				LOG_ERROR("unsupported XSDRTDOC\n");
 				unsupported = 1;
 				break;
-			case 0x11:	/* XSDRTDOB */
+
+			case XSDRTDOE:
+				LOG_ERROR("unsupported XSDRTDOE\n");
 				unsupported = 1;
 				break;
-			case 0x12:	/* XSTATE */
-				LOG_DEBUG("XSTATE");
-				if (read(xsvf_fd, &uc, 1) < 0)
-					do_abort = 1;
-				else
+
+			case XSTATE:
 				{
-					enum tap_state *path = calloc(XSTATE_MAX_PATH, 4);
-					int path_len = 1;
-					path[0] = xsvf_to_tap[uc];
+					tap_state_t	mystate;
+					tap_state_t *path;
+					int path_len;
+
+					if (read(xsvf_fd, &uc, 1) < 0)
+					{
+						do_abort = 1;
+						break;
+					}
+
+					mystate = xsvf_to_tap(uc);
+
+					LOG_DEBUG("XSTATE 0x%02X %s", uc, jtag_state_name(mystate) );
+
+					path = calloc(XSTATE_MAX_PATH, 4);
+					path_len = 1;
+
+					path[0] = mystate;
 					if (xsvf_read_xstates(xsvf_fd, path, XSTATE_MAX_PATH, &path_len) != ERROR_OK)
 						do_abort = 1;
 					else
 					{
 						int i,lasti;
+
 						/* here the trick is that jtag_add_pathmove() must end in a stable
-						state, so we must only invoke jtag_add_tlr() when we absolutely
-						have to
-						*/
-						for(i=0,lasti=0;i<path_len;i++)
+						 * state, so we must only invoke jtag_add_tlr() when we absolutely
+						 * have to
+						 */
+						for(i=0,lasti=0;  i<path_len;  i++)
 						{
 							if(path[i]==TAP_RESET)
 							{
@@ -441,56 +599,100 @@ int handle_xsvf_command(struct command_context_s *cmd_ctx, char *cmd, char **arg
 					free(path);
 				}
 				break;
-			case 0x13:	/* XENDIR */
-				LOG_DEBUG("XENDIR");
-				if (read(xsvf_fd, &c, 1) < 0)
-					do_abort = 1;
-				else
+
+			case XENDIR:
 				{
-					if (c == 0)
-						xendir = 1;
-					else if (c == 1)
-						xendir = 0xd;
-					else
+					tap_state_t	 mystate;
+
+					if (read(xsvf_fd, &uc, 1) < 0)
+					{
+						do_abort = 1;
+						break;
+					}
+
+					/* see page 22 of XSVF spec */
+					mystate = uc == 1 ? TAP_IRPAUSE : TAP_IDLE;
+
+					LOG_DEBUG("XENDIR 0x%02X %s", uc, jtag_state_name(mystate));
+
+					/* assuming that the XRUNTEST comes from SVF RUNTEST, then only these states
+					 * should come here because the SVF spec only allows these with a RUNTEST
+					 */
+					if (mystate != TAP_IRPAUSE && mystate != TAP_DRPAUSE && mystate != TAP_RESET && mystate != TAP_IDLE )
 					{
-						LOG_ERROR("unknown XENDIR endstate");
+						LOG_ERROR("illegal XENDIR endstate: \"%s\"", jtag_state_name(mystate));
 						unsupported = 1;
+						break;
 					}
+					xendir = mystate;
 				}
 				break;
-			case 0x14:	/* XENDDR */
-				LOG_DEBUG("XENDDR");
-				if (read(xsvf_fd, &c, 1) < 0)
-					do_abort = 1;
-				else
+
+			case XENDDR:
 				{
-					if (c == 0)
-						xenddr = 1;
-					else if (c == 1)
-						xenddr = 0x6;
-					else
+					tap_state_t	 mystate;
+
+					if (read(xsvf_fd, &uc, 1) < 0)
 					{
-						LOG_ERROR("unknown XENDDR endstate");
+						do_abort = 1;
+						break;
+					}
+
+					/* see page 22 of XSVF spec */
+					mystate = uc == 1 ? TAP_DRPAUSE : TAP_IDLE;
+
+					LOG_DEBUG("XENDDR %02X %s", uc, jtag_state_name(mystate));
+
+					if (mystate != TAP_IRPAUSE && mystate != TAP_DRPAUSE && mystate != TAP_RESET && mystate != TAP_IDLE )
+					{
+						LOG_ERROR("illegal XENDDR endstate: \"%s\"", jtag_state_name( mystate ));
 						unsupported = 1;
+						break;
 					}
+					xenddr = mystate;
 				}
 				break;
-			case 0x15:	/* XSIR2 */
-				LOG_DEBUG("XSIR2");
-				if (read(xsvf_fd, buf2, 2) < 0)
-					do_abort = 1;
-				else
+
+			case XSIR:
+			case XSIR2:
 				{
-					u8 *ir_buf;
-					us = be_to_h_u16(buf2);
-					ir_buf = malloc((us + 7) / 8);
-					if (xsvf_read_buffer(us, xsvf_fd, ir_buf) != ERROR_OK)
+					u8	short_buf[2];
+					u8*	ir_buf;
+					int bitcount;
+					tap_state_t my_end_state = xruntest ? TAP_IDLE : xendir;
+
+					if( opcode == XSIR )
+					{
+						/* one byte bitcount */
+						if (read(xsvf_fd, short_buf, 1) < 0)
+						{
+							do_abort = 1;
+							break;
+						}
+						bitcount = short_buf[0];
+						LOG_DEBUG("XSIR %d", bitcount);
+					}
+					else
+					{
+						if (read(xsvf_fd, short_buf, 2) < 0)
+						{
+							do_abort = 1;
+							break;
+						}
+						bitcount = be_to_h_u16(short_buf);
+						LOG_DEBUG("XSIR2 %d", bitcount);
+					}
+
+					ir_buf = malloc((bitcount+7) / 8);
+
+					if (xsvf_read_buffer(bitcount, xsvf_fd, ir_buf) != ERROR_OK)
 						do_abort = 1;
 					else
 					{
 						scan_field_t field;
+
 						field.tap = tap;
-						field.num_bits = us;
+						field.num_bits = bitcount;
 						field.out_value = ir_buf;
 						field.out_mask = NULL;
 						field.in_value = NULL;
@@ -498,61 +700,297 @@ int handle_xsvf_command(struct command_context_s *cmd_ctx, char *cmd, char **arg
 						field.in_check_mask = NULL;
 						field.in_handler = NULL;
 						field.in_handler_priv = NULL;
+
 						if (tap == NULL)
-							jtag_add_plain_ir_scan(1, &field, xsvf_to_tap[xendir]);
+							jtag_add_plain_ir_scan(1, &field, my_end_state);
 						else
-							jtag_add_ir_scan(1, &field, xsvf_to_tap[xendir]);
+							jtag_add_ir_scan(1, &field, my_end_state);
+
+						if (xruntest)
+						{
+							if (runtest_requires_tck)
+								jtag_add_clocks(xruntest);
+							else
+								jtag_add_sleep(xruntest);
+						}
+
+						/* Note that an -irmask of non-zero in your config file
+						 * can cause this to fail.  Setting -irmask to zero cand work
+						 * around the problem.
+						 */
+
+						/* LOG_DEBUG("FLUSHING QUEUE"); */
+						result = jtag_execute_queue();
+						if(result != ERROR_OK)
+						{
+							tdo_mismatch = 1;
+						}
 					}
 					free(ir_buf);
 				}
 				break;
-			case 0x16:	/* XCOMMENT */
-				do
+
+			case XCOMMENT:
 				{
-					if (read(xsvf_fd, &c, 1) < 0)
+					int		ndx = 0;
+					char 	comment[128];
+
+					do
+					{
+						if (read(xsvf_fd, &uc, 1) < 0)
+						{
+							do_abort = 1;
+							break;
+						}
+
+						if ( ndx < sizeof(comment)-1 )
+							comment[ndx++] = uc;
+
+					} while (uc != 0);
+
+					comment[sizeof(comment)-1] = 0;		/* regardless, terminate */
+					LOG_USER(comment);
+				}
+				break;
+
+			case XWAIT:
+				{
+					/* expected in stream:
+					   XWAIT <u8 wait_state> <u8 end_state> <u32 usecs>
+					*/
+
+					u8	wait;
+					u8	end;
+					u8	delay_buf[4];
+
+					tap_state_t wait_state;
+					tap_state_t end_state;
+					int 	delay;
+
+					if ( read(xsvf_fd, &wait, 1) < 0
+					  || read(xsvf_fd, &end, 1) < 0
+					  || read(xsvf_fd, delay_buf, 4) < 0)
 					{
 						do_abort = 1;
 						break;
 					}
-				} while (c != 0);
+
+					wait_state = xsvf_to_tap(wait);
+					end_state  = xsvf_to_tap(end);
+					delay      = be_to_h_u32(delay_buf);
+
+					LOG_DEBUG("XWAIT %s %s usecs:%d", jtag_state_name(wait_state), jtag_state_name(end_state), delay);
+
+					if (runtest_requires_tck && wait_state == TAP_IDLE )
+					{
+						jtag_add_runtest(delay, end_state);
+					}
+					else
+					{
+						xsvf_add_statemove( wait_state );
+						jtag_add_sleep(delay);
+						xsvf_add_statemove( end_state );
+					}
+				}
 				break;
-			case 0x17:	/* XWAIT */
-				LOG_DEBUG("XWAIT");
-				if ((read(xsvf_fd, &uc, 1) < 0) || (read(xsvf_fd, &uc2, 1) < 0) || (read(xsvf_fd, buf4, 4) < 0))
-					do_abort = 1;
-				else
+
+			case XWAITSTATE:
+				{
+					/* expected in stream:
+					   XWAITSTATE <u8 wait_state> <u8 end_state> <u32 clock_count> <u32 usecs>
+					*/
+
+					u8  clock_buf[4];
+					u8  	usecs_buf[4];
+					u8	wait;
+					u8	end;
+					tap_state_t wait_state;
+					tap_state_t end_state;
+					int clock_count;
+					int usecs;
+
+					if ( read(xsvf_fd, &wait, 1) < 0
+					 ||  read(xsvf_fd, &end, 1) < 0
+					 ||  read(xsvf_fd, clock_buf, 4) < 0
+					 ||  read(xsvf_fd, usecs_buf, 4) < 0 )
+					{
+						do_abort = 1;
+						break;
+					}
+
+					wait_state = xsvf_to_tap( wait );
+					end_state  = xsvf_to_tap( end );
+
+					clock_count = be_to_h_u32(clock_buf);
+					usecs       = be_to_h_u32(usecs_buf);
+
+					LOG_DEBUG("XWAITSTATE %s %s clocks:%i usecs:%i",
+						jtag_state_name(wait_state),
+						jtag_state_name(end_state),
+						clock_count, usecs);
+
+					/* the following states are 'stable', meaning that they have a transition
+					 * in the state diagram back to themselves.  This is necessary because we will
+					 * be issuing a number of clocks in this state.  This set of allowed states is also
+					 * determined by the SVF RUNTEST command's allowed states.
+					 */
+					if (wait_state != TAP_IRPAUSE && wait_state != TAP_DRPAUSE && wait_state != TAP_RESET && wait_state != TAP_IDLE)
+					{
+						LOG_ERROR("illegal XWAITSTATE wait_state: \"%s\"", jtag_state_name( wait_state ));
+						unsupported = 1;
+					}
+
+					xsvf_add_statemove( wait_state );
+
+					jtag_add_clocks( clock_count );
+
+					jtag_add_sleep( usecs );
+
+					xsvf_add_statemove( end_state );
+				}
+				break;
+
+			case LCOUNT:
 				{
-					xsvf_add_statemove(xsvf_to_tap[uc]);
-					ui = be_to_h_u32(buf4);
-					jtag_add_sleep(ui);
-					xsvf_add_statemove(xsvf_to_tap[uc2]);
+					/* expected in stream:
+					   LCOUNT <u32 loop_count>
+					*/
+					u8  count_buf[4];
+
+					if ( read(xsvf_fd, count_buf, 4) < 0 )
+					{
+						do_abort = 1;
+						break;
+					}
+
+					loop_count = be_to_h_u32(count_buf);
+					LOG_DEBUG("LCOUNT %d", loop_count);
+				}
+				break;
+
+			case LDELAY:
+				{
+					/* expected in stream:
+					   LDELAY <u8 wait_state> <u32 clock_count> <u32 usecs_to_sleep>
+					*/
+					u8	state;
+					u8  clock_buf[4];
+					u8  usecs_buf[4];
+
+					if ( read(xsvf_fd, &state, 1) < 0
+					  || read(xsvf_fd, clock_buf, 4) < 0
+					  ||	 read(xsvf_fd, usecs_buf, 4) < 0 )
+					{
+						do_abort = 1;
+						break;
+					}
+
+					loop_state  = xsvf_to_tap(state);
+					loop_clocks = be_to_h_u32(clock_buf);
+					loop_usecs  = be_to_h_u32(usecs_buf);
+
+					LOG_DEBUG("LDELAY %s clocks:%d usecs:%d", jtag_state_name(loop_state), loop_clocks, loop_usecs);
+				}
+				break;
+
+			/* LSDR is more like XSDRTDO than it is like XSDR.  It uses LDELAY which
+			 * comes with clocks !AND! sleep requirements.
+			 */
+			case LSDR:
+				{
+					int limit = loop_count;
+					int matched = 0;
+					int attempt;
+
+					LOG_DEBUG("LSDR");
+
+					if ( xsvf_read_buffer(xsdrsize, xsvf_fd, dr_out_buf) != ERROR_OK
+					  || xsvf_read_buffer(xsdrsize, xsvf_fd, dr_in_buf) != ERROR_OK )
+					{
+						do_abort = 1;
+						break;
+					}
+
+					if (limit < 1)
+						limit = 1;
+
+					for( attempt=0; attempt<limit;  ++attempt )
+					{
+						scan_field_t field;
+
+						field.tap = tap;
+						field.num_bits = xsdrsize;
+						field.out_value = dr_out_buf;
+						field.out_mask = NULL;
+						field.in_value = NULL;
+
+						if (attempt > 0)
+							LOG_USER("LSDR retry %d", attempt);
+
+						jtag_set_check_value(&field, dr_in_buf, dr_in_mask, NULL);
+						if (tap == NULL)
+							jtag_add_plain_dr_scan(1, &field, loop_state);
+						else
+							jtag_add_dr_scan(1, &field, loop_state);
+
+						/* LOG_DEBUG("FLUSHING QUEUE"); */
+						result = jtag_execute_queue();
+						if(result == ERROR_OK)
+						{
+							matched = 1;
+							break;
+						}
+
+						jtag_add_clocks(loop_clocks);
+						jtag_add_sleep(loop_usecs);
+					}
+
+					if (!matched )
+					{
+						LOG_USER( "LSDR mismatch" );
+						tdo_mismatch = 1;
+						break;
+					}
 				}
 				break;
+
 			default:
-				LOG_ERROR("unknown xsvf command (0x%2.2x)\n", c);
+				LOG_ERROR("unknown xsvf command (0x%02X)\n", uc);
 				unsupported = 1;
 		}
 
 		if (do_abort || unsupported || tdo_mismatch)
+		{
+			LOG_DEBUG("xsvf failed, setting taps to reasonable state");
+
+			/* upon error, return the TAPs to a reasonable state */
+			xsvf_add_statemove( TAP_IDLE );
+			jtag_execute_queue();
 			break;
+		}
 	}
 
 	if (tdo_mismatch)
 	{
-		command_print(cmd_ctx, "TDO mismatch, aborting");
-		return ERROR_OK;
+		command_print(cmd_ctx, "TDO mismatch, somewhere near offset %lu in xsvf file, aborting",
+					  file_offset );
+
+
+		return ERROR_FAIL;
 	}
 
 	if (unsupported)
 	{
-		command_print(cmd_ctx, "unsupported xsvf command encountered, aborting");
-		return ERROR_OK;
+		command_print(cmd_ctx,
+			 "unsupported xsvf command: 0x%02X in xsvf file at offset %ld, aborting",
+					  uc,  lseek(xsvf_fd, 0, SEEK_CUR)-1 );
+		return ERROR_FAIL;
 	}
 
 	if (do_abort)
 	{
-		command_print(cmd_ctx, "premature end detected, aborting");
-		return ERROR_OK;
+		command_print(cmd_ctx, "premature end of xsvf file detected, aborting");
+		return ERROR_FAIL;
 	}
 
 	if (dr_out_buf)
@@ -570,3 +1008,54 @@ int handle_xsvf_command(struct command_context_s *cmd_ctx, char *cmd, char **arg
 
 	return ERROR_OK;
 }
+
+
+/* PSUEDO-Code from Xilinx Appnote XAPP067.pdf:
+
+the following pseudo code clarifies the intent of the xrepeat support.  The
+flow given is for the entire processing of an SVF file, not an XSVF file.
+No idea if this is just for the XC9500/XL/XV devices or all Xilinx parts.
+
+"Pseudo-Code Algorithm for SVF-Based ISP"
+
+1. Go to Test-Logic-Reset state
+2. Go to Run-Test Idle state
+3. Read SVF record
+
+4. if SIR record then
+	   go to Shift-IR state
+	   Scan in <TDI value>
+
+5. else if SDR record then
+	   set <repeat count> to 0
+	   store <TDI value> as <current TDI value>
+	   store <TDO value> as <current TDO value>
+6. go to Shift-DR state
+	   scan in <current TDI value>
+	   if <current TDO value> is specified then
+		   if <current TDO value> does not equal <actual TDO value> then
+			   if <repeat count> > 32 then
+				   LOG ERROR
+				   go to Run-Test Idle state
+				   go to Step 3
+			   end if
+			   go to Pause-DR
+			   go to Exit2-DR
+			   go to Shift-DR
+			   go to Exit1-DR
+			   go to Update-DR
+			   go to Run-Test/Idle
+			   increment <repeat count> by 1
+			   pause <current pause time> microseconds
+			   go to Step 6)
+		   end if
+	   else
+		   go to Run-Test Idle state
+		   go to Step 3
+	   endif
+else if RUNTEST record then
+   pause tester for <TCK value> microseconds
+   store <TCK value> as <current pause time>
+end if
+
+*/