X-Git-Url: https://review.openocd.org/gitweb?p=openocd.git;a=blobdiff_plain;f=src%2Fjtag%2Fjtag.c;h=68706141b502063e2312b5e1021920733c254022;hp=16dbbd22f0248fe90689d8a73b865809ee8a57b2;hb=4f19ef3cce5fe96114a6e444598d5b6571694e76;hpb=3805e53fd89b34ec932b2993a14a727cd31f9e48

diff --git a/src/jtag/jtag.c b/src/jtag/jtag.c
index 16dbbd22f0..68706141b5 100644
--- a/src/jtag/jtag.c
+++ b/src/jtag/jtag.c
@@ -5,6 +5,10 @@
  *   Copyright (C) 2007,2008 Øyvind Harboe                                 *
  *   oyvind.harboe@zylin.com                                               *
  *                                                                         *
+ *   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     *
@@ -31,16 +35,20 @@
 #include "command.h"
 #include "log.h"
 
-#include "stdlib.h"
-#include "string.h"
+#include <stdlib.h>
+#include <string.h>
 #include <unistd.h>
+#ifdef HAVE_STRINGS_H
+#include <strings.h>
+#endif
+
+
 
 /* note that this is not marked as static as it must be available from outside jtag.c for those
    that implement the jtag_xxx() minidriver layer
 */
 int jtag_error=ERROR_OK;
 
-
 typedef struct cmd_queue_page_s
 {
 	void *address;
@@ -51,52 +59,6 @@ typedef struct cmd_queue_page_s
 #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
- *
- * DRSHIFT->DRSHIFT and IRSHIFT->IRSHIFT have to be caught in interface specific code
- */
-u8 tap_move[6][6] =
-{
-/*	  RESET  IDLE  DRSHIFT  DRPAUSE  IRSHIFT  IRPAUSE             */
-	{  0x7f, 0x00,    0x17,    0x0a,    0x1b,    0x16},	/* RESET */
-	{  0x7f, 0x00,    0x25,    0x05,    0x2b,    0x0b},	/* IDLE */
-	{  0x7f, 0x31,    0x00,    0x01,    0x0f,    0x2f},	/* DRSHIFT  */
-	{  0x7f, 0x30,    0x20,    0x17,    0x1e,    0x2f},	/* DRPAUSE  */
-	{  0x7f, 0x31,    0x07,    0x17,    0x00,    0x01},	/* IRSHIFT  */
-	{  0x7f, 0x30,    0x1c,    0x17,    0x20,    0x2f}	/* IRPAUSE  */
-};
-
-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_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_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_IRUPDATE,  TAP_IRPAUSE}, 		/* IREXIT1 */
-	{TAP_IREXIT2, TAP_IRPAUSE},		/* IRPAUSE  */
-	{TAP_IRUPDATE,  TAP_IRSHIFT},		/* IREXIT2 */
-	{TAP_DRSELECT, TAP_IDLE}		/* IRUPDATE  */
-};
-
 char* jtag_event_strings[] =
 {
 	"JTAG controller reset (RESET or TRST)"
@@ -109,13 +71,6 @@ const Jim_Nvp nvp_jtag_tap_event[] = {
 	{ .name = NULL, .value = -1 }
 };
 
-/* kludge!!!! these are just global variables that the
- * interface use internally. They really belong
- * inside the drivers, but we don't want to break
- * linking the drivers!!!!
- */
-enum tap_state end_state = TAP_RESET;
-enum tap_state cur_state = TAP_RESET;
 int jtag_trst = 0;
 int jtag_srst = 0;
 
@@ -124,14 +79,14 @@ jtag_command_t **last_comand_pointer = &jtag_command_queue;
 static jtag_tap_t *jtag_all_taps = NULL;
 
 enum reset_types jtag_reset_config = RESET_NONE;
-enum tap_state cmd_queue_end_state = TAP_RESET;
-enum tap_state cmd_queue_cur_state = TAP_RESET;
+tap_state_t cmd_queue_end_state = TAP_RESET;
+tap_state_t cmd_queue_cur_state = TAP_RESET;
 
 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 */
+static int jtag_nsrst_delay = 0; /* default to no nSRST delay */
+static int jtag_ntrst_delay = 0; /* default to no nTRST delay */
 
 /* maximum number of JTAG devices expected in the chain
  */
@@ -196,10 +151,18 @@ static int hasKHz = 0;
 	extern jtag_interface_t jlink_interface;
 #endif
 
+#if BUILD_VSLLINK == 1
+	extern jtag_interface_t vsllink_interface;
+#endif
+
 #if BUILD_RLINK == 1
 	extern jtag_interface_t rlink_interface;
 #endif
 
+#if BUILD_ARMJTAGEW == 1
+	extern jtag_interface_t armjtagew_interface;
+#endif
+
 jtag_interface_t *jtag_interfaces[] = {
 #if BUILD_ECOSBOARD == 1
 	&zy1000_interface,
@@ -237,8 +200,14 @@ jtag_interface_t *jtag_interfaces[] = {
 #if BUILD_JLINK == 1
 	&jlink_interface,
 #endif
+#if BUILD_VSLLINK == 1
+	&vsllink_interface,
+#endif
 #if BUILD_RLINK == 1
 	&rlink_interface,
+#endif
+#if BUILD_ARMJTAGEW == 1
+	&armjtagew_interface,
 #endif
 	NULL,
 };
@@ -246,35 +215,35 @@ jtag_interface_t *jtag_interfaces[] = {
 jtag_interface_t *jtag = NULL;
 
 /* configuration */
-jtag_interface_t *jtag_interface = NULL;
+static jtag_interface_t *jtag_interface = NULL;
 int jtag_speed = 0;
 
 /* forward declarations */
-void jtag_add_pathmove(int num_states, enum tap_state *path);
-void jtag_add_runtest(int num_cycles, enum tap_state endstate);
-void jtag_add_end_state(enum tap_state endstate);
-void jtag_add_sleep(u32 us);
-int jtag_execute_queue(void);
-
+//void jtag_add_pathmove(int num_states, tap_state_t *path);
+//void jtag_add_runtest(int num_cycles, tap_state_t endstate);
+//void jtag_add_end_state(tap_state_t endstate);
+//void jtag_add_sleep(u32 us);
+//int jtag_execute_queue(void);
+static tap_state_t tap_state_by_name(const char *name);
 
 /* 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_khz_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);
+static int handle_interface_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+static int handle_jtag_speed_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+static int handle_jtag_khz_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+static int handle_jtag_device_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+static int handle_reset_config_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+static int handle_jtag_nsrst_delay_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+static 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);
+static 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_irscan_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int Jim_Command_drscan(Jim_Interp *interp, int argc, Jim_Obj *const *argv);
+static int handle_endstate_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+static int handle_jtag_reset_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+static int handle_runtest_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+static int handle_irscan_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+static int Jim_Command_drscan(Jim_Interp *interp, int argc, Jim_Obj *const *argv);
 
-int handle_verify_ircapture_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+static int handle_verify_ircapture_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
 
 jtag_tap_t *jtag_AllTaps(void)
 {
@@ -311,14 +280,13 @@ int jtag_NumEnabledTaps(void)
 	return n;
 }
 
-
 jtag_tap_t *jtag_TapByString( const char *s )
 {
 	jtag_tap_t *t;
 	char *cp;
 
 	t = jtag_AllTaps();
-	// try name first
+	/* try name first */
 	while(t){
 		if( 0 == strcmp( t->dotted_name, s ) ){
 			break;
@@ -326,7 +294,7 @@ jtag_tap_t *jtag_TapByString( const char *s )
 			t = t->next_tap;
 		}
 	}
-	// backup plan is by number
+	/* backup plan is by number */
 	if( t == NULL ){
 		/* ok - is "s" a number? */
 		int n;
@@ -543,28 +511,78 @@ static void jtag_prelude1(void)
 		jtag_call_event_callbacks(JTAG_TRST_ASSERTED);
 }
 
-static void jtag_prelude(enum tap_state state)
+static void jtag_prelude(tap_state_t state)
 {
 	jtag_prelude1();
 
-	if (state != -1)
+	if (state != TAP_INVALID)
 		jtag_add_end_state(state);
 
 	cmd_queue_cur_state = cmd_queue_end_state;
 }
 
-void jtag_add_ir_scan(int num_fields, scan_field_t *fields, enum tap_state state)
+void jtag_add_ir_scan_noverify(int num_fields, scan_field_t *fields, tap_state_t state)
 {
 	int retval;
-
 	jtag_prelude(state);
 
 	retval=interface_jtag_add_ir_scan(num_fields, fields, cmd_queue_end_state);
 	if (retval!=ERROR_OK)
 		jtag_error=retval;
+
 }
 
-int MINIDRIVER(interface_jtag_add_ir_scan)(int num_fields, scan_field_t *fields, enum tap_state state)
+
+void jtag_add_ir_scan(int num_fields, scan_field_t *fields, tap_state_t state)
+{
+	/* 8 x 32 bit id's is enough for all invoations */
+	u32 id[8];
+	int modified[8];
+
+	/* if we are to run a verification of the ir scan, we need to get the input back.
+	 * We may have to allocate space if the caller didn't ask for the input back.
+	 *
+	 */
+	if (jtag_verify_capture_ir)
+	{
+		int j;
+		for (j = 0; j < num_fields; j++)
+		{
+			modified[j]=0;
+			if ((fields[j].in_value==NULL)&&(fields[j].num_bits<=32))
+			{
+				if (j<8)
+				{
+					modified[j]=1;
+					fields[j].in_value=(u8 *)(id+j);
+				} else
+				{
+					LOG_DEBUG("caller must provide in_value space for verify_capture_ir to work");
+				}
+			}
+		}
+	}
+
+	jtag_add_ir_scan_noverify(num_fields, fields, state);
+
+	if (jtag_verify_capture_ir)
+	{
+		int j;
+		for (j = 0; j < num_fields; j++)
+		{
+			jtag_tap_t *tap=fields[j].tap;
+			jtag_check_value_mask(fields+j, tap->expected, tap->expected_mask);
+
+			if (modified[j])
+			{
+				fields[j].in_value=NULL;
+			}
+		}
+	}
+
+}
+
+int MINIDRIVER(interface_jtag_add_ir_scan)(int num_fields, scan_field_t *fields, tap_state_t state)
 {
 	jtag_command_t **last_cmd;
 	jtag_tap_t *tap;
@@ -573,7 +591,6 @@ int MINIDRIVER(interface_jtag_add_ir_scan)(int num_fields, scan_field_t *fields,
 	int nth_tap;
 	int scan_size = 0;
 
-
 	last_cmd = jtag_get_last_command_p();
 
 	/* allocate memory for a new list member */
@@ -595,7 +612,7 @@ int MINIDRIVER(interface_jtag_add_ir_scan)(int num_fields, scan_field_t *fields,
 	for(;;){
 		int found = 0;
 
-		// do this here so it is not forgotten
+		/* do this here so it is not forgotten */
 		tap = jtag_NextEnabledTap(tap);
 		if( tap == NULL ){
 			break;
@@ -614,21 +631,6 @@ int MINIDRIVER(interface_jtag_add_ir_scan)(int num_fields, scan_field_t *fields,
 			{
 				found = 1;
 				(*last_cmd)->cmd.scan->fields[nth_tap].out_value = buf_cpy(fields[j].out_value, cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);
-				(*last_cmd)->cmd.scan->fields[nth_tap].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+nth_tap, tap->expected, tap->expected_mask, NULL);
-					} else
-					{
-						(*last_cmd)->cmd.scan->fields[nth_tap].in_handler = fields[j].in_handler;
-						(*last_cmd)->cmd.scan->fields[nth_tap].in_handler_priv = fields[j].in_handler_priv;
-						(*last_cmd)->cmd.scan->fields[nth_tap].in_check_value = tap->expected;
-						(*last_cmd)->cmd.scan->fields[nth_tap].in_check_mask = tap->expected_mask;
-					}
-				}
 
 				tap->bypass = 0;
 				break;
@@ -639,7 +641,6 @@ int MINIDRIVER(interface_jtag_add_ir_scan)(int num_fields, scan_field_t *fields,
 		{
 			/* if a tap isn't listed, set it to BYPASS */
 			(*last_cmd)->cmd.scan->fields[nth_tap].out_value = buf_set_ones(cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);
-			(*last_cmd)->cmd.scan->fields[nth_tap].out_mask = NULL;
 			tap->bypass = 1;
 		}
 
@@ -650,7 +651,7 @@ int MINIDRIVER(interface_jtag_add_ir_scan)(int num_fields, scan_field_t *fields,
 	return ERROR_OK;
 }
 
-void jtag_add_plain_ir_scan(int num_fields, scan_field_t *fields, enum tap_state state)
+void jtag_add_plain_ir_scan(int num_fields, scan_field_t *fields, tap_state_t state)
 {
 	int retval;
 
@@ -661,7 +662,7 @@ void jtag_add_plain_ir_scan(int num_fields, scan_field_t *fields, enum tap_state
 		jtag_error=retval;
 }
 
-int MINIDRIVER(interface_jtag_add_plain_ir_scan)(int num_fields, scan_field_t *fields, enum tap_state state)
+int MINIDRIVER(interface_jtag_add_plain_ir_scan)(int num_fields, scan_field_t *fields, tap_state_t state)
 {
 	int i;
 	jtag_command_t **last_cmd;
@@ -687,7 +688,6 @@ int MINIDRIVER(interface_jtag_add_plain_ir_scan)(int num_fields, scan_field_t *f
 		(*last_cmd)->cmd.scan->fields[i].tap = fields[i].tap;
 		(*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;
@@ -697,7 +697,7 @@ int MINIDRIVER(interface_jtag_add_plain_ir_scan)(int num_fields, scan_field_t *f
 	return ERROR_OK;
 }
 
-void jtag_add_dr_scan(int num_fields, scan_field_t *fields, enum tap_state state)
+void jtag_add_dr_scan(int num_fields, scan_field_t *fields, tap_state_t state)
 {
 	int retval;
 
@@ -708,7 +708,13 @@ void jtag_add_dr_scan(int num_fields, scan_field_t *fields, enum tap_state state
 		jtag_error=retval;
 }
 
-int MINIDRIVER(interface_jtag_add_dr_scan)(int num_fields, scan_field_t *fields, enum tap_state state)
+void jtag_add_dr_scan_now(int num_fields, scan_field_t *fields, tap_state_t state)
+{
+	jtag_add_dr_scan(num_fields, fields, state);
+	jtag_execute_queue_noclear();
+}
+
+int MINIDRIVER(interface_jtag_add_dr_scan)(int num_fields, scan_field_t *fields, tap_state_t state)
 {
 	int j;
 	int nth_tap;
@@ -764,7 +770,6 @@ int MINIDRIVER(interface_jtag_add_dr_scan)(int num_fields, scan_field_t *fields,
 				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;
@@ -785,7 +790,6 @@ int MINIDRIVER(interface_jtag_add_dr_scan)(int num_fields, scan_field_t *fields,
 			/* 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;
@@ -811,7 +815,7 @@ void MINIDRIVER(interface_jtag_add_dr_out)(jtag_tap_t *target_tap,
 		int num_fields,
 		const int *num_bits,
 		const u32 *value,
-		enum tap_state end_state)
+		tap_state_t end_state)
 {
 	int nth_tap;
 	int field_count = 0;
@@ -875,7 +879,6 @@ void MINIDRIVER(interface_jtag_add_dr_out)(jtag_tap_t *target_tap,
 				buf_set_u32(out_value, 0, scan_size, value[j]);
 				(*last_cmd)->cmd.scan->fields[field_count].num_bits = scan_size;
 				(*last_cmd)->cmd.scan->fields[field_count].out_value = buf_cpy(out_value, cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);
-				(*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;
@@ -895,7 +898,6 @@ void MINIDRIVER(interface_jtag_add_dr_out)(jtag_tap_t *target_tap,
 			/* 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;
@@ -905,7 +907,7 @@ void MINIDRIVER(interface_jtag_add_dr_out)(jtag_tap_t *target_tap,
 	}
 }
 
-void jtag_add_plain_dr_scan(int num_fields, scan_field_t *fields, enum tap_state state)
+void jtag_add_plain_dr_scan(int num_fields, scan_field_t *fields, tap_state_t state)
 {
 	int retval;
 
@@ -916,7 +918,7 @@ void jtag_add_plain_dr_scan(int num_fields, scan_field_t *fields, enum tap_state
 		jtag_error=retval;
 }
 
-int MINIDRIVER(interface_jtag_add_plain_dr_scan)(int num_fields, scan_field_t *fields, enum tap_state state)
+int MINIDRIVER(interface_jtag_add_plain_dr_scan)(int num_fields, scan_field_t *fields, tap_state_t state)
 {
 	int i;
 	jtag_command_t **last_cmd = jtag_get_last_command_p();
@@ -941,7 +943,6 @@ int MINIDRIVER(interface_jtag_add_plain_dr_scan)(int num_fields, scan_field_t *f
 		(*last_cmd)->cmd.scan->fields[i].tap = fields[i].tap;
 		(*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;
@@ -962,9 +963,9 @@ void jtag_add_tlr(void)
 		jtag_error=retval;
 }
 
-int MINIDRIVER(interface_jtag_add_tlr)()
+int MINIDRIVER(interface_jtag_add_tlr)(void)
 {
-	enum tap_state state = TAP_RESET;
+	tap_state_t state = TAP_RESET;
 	jtag_command_t **last_cmd = jtag_get_last_command_p();
 
 	/* allocate memory for a new list member */
@@ -976,18 +977,17 @@ 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;
 }
 
-void jtag_add_pathmove(int num_states, enum tap_state *path)
+void jtag_add_pathmove(int num_states, tap_state_t *path)
 {
-	enum tap_state cur_state=cmd_queue_cur_state;
+	tap_state_t cur_state = cmd_queue_cur_state;
 	int i;
 	int retval;
 
 	/* the last state has to be a stable state */
-	if (tap_move_map[path[num_states - 1]] == -1)
+	if (!tap_is_state_stable(path[num_states - 1]))
 	{
 		LOG_ERROR("BUG: TAP path doesn't finish in a stable state");
 		exit(-1);
@@ -1000,10 +1000,11 @@ void jtag_add_pathmove(int num_states, enum tap_state *path)
 			LOG_ERROR("BUG: TAP_RESET is not a valid state for pathmove sequences");
 			exit(-1);
 		}
-		if ((tap_transitions[cur_state].low != path[i])&&
-				(tap_transitions[cur_state].high != path[i]))
+
+		if ( tap_state_transition(cur_state, true)  != path[i]
+		  && tap_state_transition(cur_state, false) != path[i])
 		{
-			LOG_ERROR("BUG: %s -> %s isn't a valid TAP transition", jtag_state_name(cur_state), jtag_state_name(path[i]));
+			LOG_ERROR("BUG: %s -> %s isn't a valid TAP transition", tap_state_name(cur_state), tap_state_name(path[i]));
 			exit(-1);
 		}
 		cur_state = path[i];
@@ -1011,13 +1012,13 @@ void jtag_add_pathmove(int num_states, enum tap_state *path)
 
 	jtag_prelude1();
 
-	retval=interface_jtag_add_pathmove(num_states, path);
+	retval = interface_jtag_add_pathmove(num_states, path);
 	cmd_queue_cur_state = path[num_states - 1];
 	if (retval!=ERROR_OK)
 		jtag_error=retval;
 }
 
-int MINIDRIVER(interface_jtag_add_pathmove)(int num_states, enum tap_state *path)
+int MINIDRIVER(interface_jtag_add_pathmove)(int num_states, tap_state_t *path)
 {
 	jtag_command_t **last_cmd = jtag_get_last_command_p();
 	int i;
@@ -1030,7 +1031,7 @@ int MINIDRIVER(interface_jtag_add_pathmove)(int num_states, enum tap_state *path
 
 	(*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);
+	(*last_cmd)->cmd.pathmove->path = cmd_queue_alloc(sizeof(tap_state_t) * num_states);
 
 	for (i = 0; i < num_states; i++)
 		(*last_cmd)->cmd.pathmove->path[i] = path[i];
@@ -1038,7 +1039,7 @@ int MINIDRIVER(interface_jtag_add_pathmove)(int num_states, enum tap_state *path
 	return ERROR_OK;
 }
 
-int MINIDRIVER(interface_jtag_add_runtest)(int num_cycles, enum tap_state state)
+int MINIDRIVER(interface_jtag_add_runtest)(int num_cycles, tap_state_t state)
 {
 	jtag_command_t **last_cmd = jtag_get_last_command_p();
 
@@ -1055,7 +1056,7 @@ int MINIDRIVER(interface_jtag_add_runtest)(int num_cycles, enum tap_state state)
 	return ERROR_OK;
 }
 
-void jtag_add_runtest(int num_cycles, enum tap_state state)
+void jtag_add_runtest(int num_cycles, tap_state_t state)
 {
 	int retval;
 
@@ -1067,6 +1068,44 @@ 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;
+
+	if( !tap_is_state_stable(cmd_queue_cur_state) )
+	{
+		 LOG_ERROR( "jtag_add_clocks() was called with TAP in non-stable state \"%s\"",
+				 tap_state_name(cmd_queue_cur_state) );
+		 jtag_error = ERROR_JTAG_NOT_STABLE_STATE;
+		 return;
+	}
+
+	if( num_cycles > 0 )
+	{
+		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;
@@ -1187,7 +1226,7 @@ int MINIDRIVER(interface_jtag_add_reset)(int req_trst, int req_srst)
 	return ERROR_OK;
 }
 
-void jtag_add_end_state(enum tap_state state)
+void jtag_add_end_state(tap_state_t state)
 {
 	cmd_queue_end_state = state;
 	if ((cmd_queue_end_state == TAP_DRSHIFT)||(cmd_queue_end_state == TAP_IRSHIFT))
@@ -1245,23 +1284,37 @@ int jtag_build_buffer(scan_command_t *cmd, u8 **buffer)
 
 	bit_count = 0;
 
+#ifdef _DEBUG_JTAG_IO_
+	LOG_DEBUG("%s num_fields: %i", cmd->ir_scan ? "IRSCAN" : "DRSCAN", cmd->num_fields);
+#endif
+
 	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);
+			char* char_buf = buf_to_str(cmd->fields[i].out_value, (cmd->fields[i].num_bits > DEBUG_JTAG_IOZ) ? DEBUG_JTAG_IOZ : 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_
-			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
 		}
+		else
+		{
+#ifdef _DEBUG_JTAG_IO_
+			LOG_DEBUG("fields[%i].out_value[%i]: NULL", i, cmd->fields[i].num_bits);
+#endif
+		}
 
 		bit_count += cmd->fields[i].num_bits;
 	}
 
+#ifdef _DEBUG_JTAG_IO_
+	//LOG_DEBUG("bit_count totalling: %i",  bit_count );
+#endif
+
 	return bit_count;
 }
 
@@ -1285,10 +1338,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 > DEBUG_JTAG_IOZ) ? DEBUG_JTAG_IOZ : num_bits, 16);
+			LOG_DEBUG("fields[%i].in_value[%i]: 0x%s", i, num_bits, char_buf);
 			free(char_buf);
 #endif
 
@@ -1327,7 +1378,12 @@ int jtag_read_buffer(u8 *buffer, scan_command_t *cmd)
 	return retval;
 }
 
-int jtag_check_value(u8 *captured, void *priv, scan_field_t *field)
+static const char *jtag_tap_name(jtag_tap_t *tap)
+{
+	return (tap == NULL) ? "(unknown)" : tap->dotted_name;
+}
+
+int jtag_check_value_inner(u8 *captured, scan_field_t *field, u8 *in_check_value, u8 *in_check_mask)
 {
 	int retval = ERROR_OK;
 	int num_bits = field->num_bits;
@@ -1335,9 +1391,9 @@ int jtag_check_value(u8 *captured, void *priv, scan_field_t *field)
 	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);
+		compare_failed = buf_cmp_mask(captured, in_check_value, in_check_mask, num_bits);
 	else
-		compare_failed = buf_cmp(captured, field->in_check_value, num_bits);
+		compare_failed = buf_cmp(captured, in_check_value, num_bits);
 
 	if (compare_failed){
 		/* An error handler could have caught the failing check
@@ -1345,16 +1401,16 @@ int jtag_check_value(u8 *captured, void *priv, scan_field_t *field)
 		 * acknowledged the error
 		 */
 		LOG_WARNING("TAP %s:",
-					(field->tap == NULL) ? "(unknown)" : field->tap->dotted_name );
+					jtag_tap_name(field->tap));
 		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);
+			char *captured_char = buf_to_str(captured, (num_bits > DEBUG_JTAG_IOZ) ? DEBUG_JTAG_IOZ : num_bits, 16);
+			char *in_check_value_char = buf_to_str(in_check_value, (num_bits > DEBUG_JTAG_IOZ) ? DEBUG_JTAG_IOZ : num_bits, 16);
 
-			if (field->in_check_mask)
+			if (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);
+				in_check_mask_char = buf_to_str(in_check_mask, (num_bits > DEBUG_JTAG_IOZ) ? DEBUG_JTAG_IOZ : num_bits, 16);
 				LOG_WARNING("value captured during scan didn't pass the requested check:");
 				LOG_WARNING("captured: 0x%s check_value: 0x%s check_mask: 0x%s",
 							captured_char, in_check_value_char, in_check_mask_char);
@@ -1375,11 +1431,16 @@ int jtag_check_value(u8 *captured, void *priv, scan_field_t *field)
 	return retval;
 }
 
+int jtag_check_value(u8 *captured, void *priv, scan_field_t *field)
+{
+	return jtag_check_value_inner(captured, field, field->in_check_value, field->in_check_mask);
+}
+
 /*
   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)
+void jtag_set_check_value(scan_field_t *field, u8 *value, u8 *mask, struct invalidstruct *obsolete)
 {
 	if (value)
 		field->in_handler = jtag_check_value;
@@ -1390,6 +1451,29 @@ void jtag_set_check_value(scan_field_t *field, u8 *value, u8 *mask, error_handle
 	field->in_check_mask = mask;
 }
 
+void jtag_check_value_mask(scan_field_t *field, u8 *value, u8 *mask)
+{
+	if (field->in_value==NULL)
+	{
+		LOG_ERROR("remember to fill in in_value for jtag_check_value_mask() to work!");
+		return;
+	}
+
+	if (value==NULL)
+	{
+		/* no checking to do */
+		return;
+	}
+
+	jtag_execute_queue_noclear();
+
+	int retval=jtag_check_value_inner(field->in_value, field, value, mask);
+	jtag_set_error(retval);
+
+}
+
+
+
 enum scan_type jtag_scan_type(scan_command_t *cmd)
 {
 	int i;
@@ -1426,13 +1510,21 @@ int MINIDRIVER(interface_jtag_execute_queue)(void)
 	return retval;
 }
 
-int jtag_execute_queue(void)
+void jtag_execute_queue_noclear(void)
 {
 	int retval=interface_jtag_execute_queue();
-	if (retval==ERROR_OK)
+	/* we keep the first error */
+	if ((jtag_error==ERROR_OK)&&(retval!=ERROR_OK))
 	{
-		retval=jtag_error;
+		jtag_error=retval;
 	}
+}
+
+int jtag_execute_queue(void)
+{
+	int retval;
+	jtag_execute_queue_noclear();
+	retval=jtag_error;
 	jtag_error=ERROR_OK;
 	return retval;
 }
@@ -1473,12 +1565,11 @@ int jtag_examine_chain(void)
 	field.tap = NULL;
 	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++)
 	{
@@ -1501,7 +1592,7 @@ int jtag_examine_chain(void)
 		return ERROR_JTAG_INIT_FAILED;
 	}
 
-	// point at the 1st tap
+	/* point at the 1st tap */
 	tap = jtag_NextEnabledTap(NULL);
 	if( tap == NULL ){
 		LOG_ERROR("JTAG: No taps enabled?");
@@ -1525,7 +1616,9 @@ int jtag_examine_chain(void)
 			u32 part;
 			u32 version;
 
-			if (idcode == 0x000000FF)
+			/* some devices, such as AVR will output all 1's instead of TDI
+			input value at end of chain. */
+			if ((idcode == 0x000000FF)||(idcode == 0xFFFFFFFF))
 			{
 				int unexpected=0;
 				/* End of chain (invalid manufacturer ID)
@@ -1542,7 +1635,7 @@ int jtag_examine_chain(void)
 				for (bit_count += 32; bit_count < (JTAG_MAX_CHAIN_SIZE * 32) - 31;bit_count += 32)
 				{
 					idcode = buf_get_u32(idcode_buffer, bit_count, 32);
-					if (unexpected||(idcode != 0x000000FF))
+					if (unexpected||((idcode != 0x000000FF)&&(idcode != 0xFFFFFFFF)))
 					{
 						LOG_WARNING("Unexpected idcode after end of chain! %d 0x%08x", bit_count, idcode);
 						unexpected = 1;
@@ -1649,29 +1742,26 @@ int jtag_validate_chain(void)
 	field.tap = NULL;
 	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_RESET);
 	jtag_execute_queue();
 
 	tap = NULL;
 	chain_pos = 0;
+	int val;
 	for(;;){
 		tap = jtag_NextEnabledTap(tap);
 		if( tap == NULL ){
 			break;
 		}
 
-
-		if (buf_get_u32(ir_test, chain_pos, 2) != 0x1)
+		val = buf_get_u32(ir_test, chain_pos, 2);
+		if (val != 0x1)
 		{
 			char *cbuf = buf_to_str(ir_test, total_ir_length, 16);
-			LOG_ERROR("Error validating JTAG scan chain, IR mismatch, scan returned 0x%s", cbuf);
+			LOG_ERROR("Could not validate JTAG scan chain, IR mismatch, scan returned 0x%s. tap=%s pos=%d expected 0x1 got %0x", cbuf, jtag_tap_name(tap), chain_pos, val);
 			free(cbuf);
 			free(ir_test);
 			return ERROR_JTAG_INIT_FAILED;
@@ -1679,10 +1769,11 @@ int jtag_validate_chain(void)
 		chain_pos += tap->ir_length;
 	}
 
-	if (buf_get_u32(ir_test, chain_pos, 2) != 0x3)
+	val = buf_get_u32(ir_test, chain_pos, 2);
+	if (val != 0x3)
 	{
 		char *cbuf = buf_to_str(ir_test, total_ir_length, 16);
-		LOG_ERROR("Error validating JTAG scan chain, IR mismatch, scan returned 0x%s", cbuf);
+		LOG_ERROR("Could not validate end of JTAG scan chain, IR mismatch, scan returned 0x%s. pos=%d expected 0x3 got %0x", cbuf, chain_pos, val);
 		free(cbuf);
 		free(ir_test);
 		return ERROR_JTAG_INIT_FAILED;
@@ -1703,9 +1794,7 @@ static Jim_Nvp nvp_config_opts[] = {
 	{ .name = NULL,          .value = -1 }
 };
 
-static int
-jtag_tap_configure_cmd( Jim_GetOptInfo *goi,
-		jtag_tap_t * tap)
+static int jtag_tap_configure_cmd( Jim_GetOptInfo *goi, jtag_tap_t * tap)
 {
 	Jim_Nvp *n;
 	Jim_Obj *o;
@@ -1752,7 +1841,7 @@ jtag_tap_configure_cmd( Jim_GetOptInfo *goi,
 					jteap = tap->event_action;
 					/* replace existing? */
 					while (jteap) {
-						if (jteap->event == n->value) {
+						if (jteap->event == (enum jtag_tap_event)n->value) {
 							break;
 						}
 						jteap = jteap->next;
@@ -2064,7 +2153,7 @@ static int jim_jtag_command( Jim_Interp *interp, int argc, Jim_Obj *const *argv
 			}
 			switch( n->value ){
 			case JTAG_CMD_TAPISENABLED:
-				// below
+				e = t->enabled;
 				break;
 			case JTAG_CMD_TAPENABLE:
 				jtag_tap_handle_event( t, JTAG_TAP_EVENT_ENABLE);
@@ -2138,7 +2227,8 @@ int jtag_register_commands(struct command_context_s *cmd_ctx)
 	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);
+		COMMAND_ANY,
+		"[none/trst_only/srst_only/trst_and_srst] [srst_pulls_trst/trst_pulls_srst] [combined/separate] [trst_push_pull/trst_open_drain] [srst_push_pull/srst_open_drain]");
 	register_command(cmd_ctx, NULL, "jtag_nsrst_delay", handle_jtag_nsrst_delay_command,
 		COMMAND_ANY, "jtag_nsrst_delay <ms> - delay after deasserting srst in ms");
 	register_command(cmd_ctx, NULL, "jtag_ntrst_delay", handle_jtag_ntrst_delay_command,
@@ -2211,7 +2301,7 @@ static int jtag_init_inner(struct command_context_s *cmd_ctx)
 
 	if (jtag_validate_chain() != ERROR_OK)
 	{
-		LOG_ERROR("Could not validate JTAG chain, continuing anyway...");
+		LOG_WARNING("Could not validate JTAG chain, continuing anyway...");
 	}
 
 	return ERROR_OK;
@@ -2298,7 +2388,7 @@ static int default_srst_asserted(int *srst_asserted)
 	return ERROR_OK;
 }
 
-int handle_interface_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+static int handle_interface_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
 {
 	int i;
 	int retval;
@@ -2361,7 +2451,7 @@ int handle_interface_command(struct command_context_s *cmd_ctx, char *cmd, char
 	return ERROR_JTAG_INVALID_INTERFACE;
 }
 
-int handle_jtag_device_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+static int handle_jtag_device_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
 {
 	int e;
 	char buf[1024];
@@ -2384,9 +2474,9 @@ int handle_jtag_device_command(struct command_context_s *cmd_ctx, char *cmd, cha
 				   args[0],
 				   args[1],
 				   args[2] );
-	command_print( cmd_ctx, "Example: STM32 has 2 taps, the cortexM3(len4) + boundryscan(len5)");
-	command_print( cmd_ctx, "jtag newtap stm32 cortexm3  ....., thus creating the tap: \"stm32.cortexm3\"");
-	command_print( cmd_ctx, "jtag newtap stm32 boundry  ....., and the tap: \"stm32.boundery\"");
+	command_print( cmd_ctx, "Example: STM32 has 2 taps, the cortexM3(len4) + boundaryscan(len5)");
+	command_print( cmd_ctx, "jtag newtap stm32 cortexm3 ....., thus creating the tap: \"stm32.cortexm3\"");
+	command_print( cmd_ctx, "jtag newtap stm32 boundary ....., and the tap: \"stm32.boundary\"");
 	command_print( cmd_ctx, "And then refer to the taps by the dotted name.");
 
 	newargs[0] = Jim_NewStringObj( interp, "jtag", -1   );
@@ -2422,7 +2512,7 @@ int handle_jtag_device_command(struct command_context_s *cmd_ctx, char *cmd, cha
 	return e;
 }
 
-int handle_scan_chain_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+static int handle_scan_chain_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
 {
 	jtag_tap_t *tap;
 
@@ -2459,7 +2549,7 @@ int handle_scan_chain_command(struct command_context_s *cmd_ctx, char *cmd, char
 	return ERROR_OK;
 }
 
-int handle_reset_config_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+static int handle_reset_config_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
 {
 	if (argc < 1)
 		return ERROR_COMMAND_SYNTAX_ERROR;
@@ -2535,7 +2625,7 @@ int handle_reset_config_command(struct command_context_s *cmd_ctx, char *cmd, ch
 	return ERROR_OK;
 }
 
-int handle_jtag_nsrst_delay_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+static int handle_jtag_nsrst_delay_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
 {
 	if (argc < 1)
 	{
@@ -2550,7 +2640,7 @@ int handle_jtag_nsrst_delay_command(struct command_context_s *cmd_ctx, char *cmd
 	return ERROR_OK;
 }
 
-int handle_jtag_ntrst_delay_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+static int handle_jtag_ntrst_delay_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
 {
 	if (argc < 1)
 	{
@@ -2565,7 +2655,7 @@ int handle_jtag_ntrst_delay_command(struct command_context_s *cmd_ctx, char *cmd
 	return ERROR_OK;
 }
 
-int handle_jtag_speed_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+static int handle_jtag_speed_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
 {
 	int retval=ERROR_OK;
 
@@ -2593,7 +2683,7 @@ int handle_jtag_speed_command(struct command_context_s *cmd_ctx, char *cmd, char
 	return retval;
 }
 
-int handle_jtag_khz_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+static int handle_jtag_khz_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
 {
 	int retval=ERROR_OK;
 	LOG_DEBUG("handle jtag khz");
@@ -2643,9 +2733,9 @@ int handle_jtag_khz_command(struct command_context_s *cmd_ctx, char *cmd, char *
 
 }
 
-int handle_endstate_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+static int handle_endstate_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
 {
-	enum tap_state state;
+	tap_state_t state;
 
 	if (argc < 1)
 	{
@@ -2653,21 +2743,20 @@ int handle_endstate_command(struct command_context_s *cmd_ctx, char *cmd, char *
 	}
 	else
 	{
-		for (state = 0; state < 16; state++)
-		{
-			if (strcmp(args[0], jtag_state_name(state)) == 0)
-			{
-				jtag_add_end_state(state);
-				jtag_execute_queue();
-			}
+		state = tap_state_by_name( args[0] );
+		if( state < 0 ){
+			command_print( cmd_ctx, "Invalid state name: %s\n", args[0] );
+			return ERROR_COMMAND_SYNTAX_ERROR;
 		}
+		jtag_add_end_state(state);
+		jtag_execute_queue();
 	}
-	command_print(cmd_ctx, "current endstate: %s", jtag_state_name(cmd_queue_end_state));
+	command_print(cmd_ctx, "current endstate: %s", tap_state_name(cmd_queue_end_state));
 
 	return ERROR_OK;
 }
 
-int handle_jtag_reset_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+static int handle_jtag_reset_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
 {
 	int trst = -1;
 	int srst = -1;
@@ -2704,31 +2793,59 @@ int handle_jtag_reset_command(struct command_context_s *cmd_ctx, char *cmd, char
 	return ERROR_OK;
 }
 
-int handle_runtest_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+static 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_add_runtest(strtol(args[0], NULL, 0), TAP_INVALID);
 	jtag_execute_queue();
 
 	return ERROR_OK;
 
 }
 
-int handle_irscan_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+static int handle_irscan_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
 {
 	int i;
 	scan_field_t *fields;
 	jtag_tap_t *tap;
+	tap_state_t endstate;
 
 	if ((argc < 2) || (argc % 2))
 	{
 		return ERROR_COMMAND_SYNTAX_ERROR;
 	}
 
+	/* optional "-endstate" */
+	/*          "statename" */
+	/* at the end of the arguments. */
+	/* assume none. */
+	endstate = TAP_INVALID;
+	if( argc >= 4 ){
+		/* have at least one pair of numbers. */
+		/* is last pair the magic text? */
+		if( 0 == strcmp( "-endstate", args[ argc - 2 ] ) ){
+			const char *cpA;
+			const char *cpS;
+			cpA = args[ argc-1 ];
+			for( endstate = 0 ; endstate < TAP_NUM_STATES ; endstate++ ){
+				cpS = tap_state_name( endstate );
+				if( 0 == strcmp( cpA, cpS ) ){
+					break;
+				}
+			}
+			if( endstate >= TAP_NUM_STATES ){
+				return ERROR_COMMAND_SYNTAX_ERROR;
+			} else {
+				/* found - remove the last 2 args */
+				argc -= 2;
+			}
+		}
+	}
+
 	fields = malloc(sizeof(scan_field_t) * argc / 2);
 
 	for (i = 0; i < argc / 2; i++)
@@ -2743,14 +2860,17 @@ int handle_irscan_command(struct command_context_s *cmd_ctx, char *cmd, char **a
 		fields[i].tap = tap;
 		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_add_ir_scan(argc / 2, fields, TAP_INVALID);
+	/* did we have an endstate? */
+	if (endstate != TAP_INVALID)
+		jtag_add_end_state(endstate);
+
 	jtag_execute_queue();
 
 	for (i = 0; i < argc / 2; i++)
@@ -2761,7 +2881,7 @@ int handle_irscan_command(struct command_context_s *cmd_ctx, char *cmd, char **a
 	return ERROR_OK;
 }
 
-int Jim_Command_drscan(Jim_Interp *interp, int argc, Jim_Obj *const *args)
+static int Jim_Command_drscan(Jim_Interp *interp, int argc, Jim_Obj *const *args)
 {
 	int retval;
 	scan_field_t *fields;
@@ -2769,11 +2889,16 @@ int Jim_Command_drscan(Jim_Interp *interp, int argc, Jim_Obj *const *args)
 	int field_count = 0;
 	int i, e;
 	jtag_tap_t *tap;
+	tap_state_t endstate;
 
 	/* args[1] = device
 	 * args[2] = num_bits
 	 * args[3] = hex string
 	 * ... repeat num bits and hex string ...
+	 *
+	 * .. optionally:
+	*     args[N-2] = "-endstate"
+	 *     args[N-1] = statename
 	 */
 	if ((argc < 4) || ((argc % 2)!=0))
 	{
@@ -2781,14 +2906,54 @@ int Jim_Command_drscan(Jim_Interp *interp, int argc, Jim_Obj *const *args)
 		return JIM_ERR;
 	}
 
+	/* assume no endstate */
+	endstate = TAP_INVALID;
+	/* validate arguments as numbers */
+	e = JIM_OK;
 	for (i = 2; i < argc; i+=2)
 	{
 		long bits;
+		const char *cp;
 
 		e = Jim_GetLong(interp, args[i], &bits);
-		if (e != JIM_OK)
+		/* If valid - try next arg */
+		if( e == JIM_OK ){
+			continue;
+		}
+
+		/* Not valid.. are we at the end? */
+		if ( ((i+2) != argc) ){
+			/* nope, then error */
 			return e;
-	}
+		}
+
+		/* it could be: "-endstate FOO" */
+
+		/* get arg as a string. */
+		cp = Jim_GetString( args[i], NULL );
+		/* is it the magic? */
+		if( 0 == strcmp( "-endstate", cp ) ){
+			/* is the statename valid? */
+			cp = Jim_GetString( args[i+1], NULL );
+
+			/* see if it is a valid state name */
+			endstate = tap_state_by_name(cp);
+			if( endstate < 0 ){
+				/* update the error message */
+				Jim_SetResult_sprintf(interp,"endstate: %s invalid", cp );
+			} else {
+				/* valid - so clear the error */
+				e = JIM_OK;
+				/* and remove the last 2 args */
+				argc -= 2;
+			}
+		}
+
+		/* Still an error? */
+		if( e != JIM_OK ){
+			return e; /* too bad */
+		}
+	} /* validate args */
 
 	tap = jtag_TapByJimObj( interp, args[1] );
 	if( tap == NULL ){
@@ -2810,7 +2975,6 @@ int Jim_Command_drscan(Jim_Interp *interp, int argc, Jim_Obj *const *args)
 		fields[field_count].num_bits = bits;
 		fields[field_count].out_value = malloc(CEIL(bits, 8));
 		str_to_buf(str, len, fields[field_count].out_value, bits, 0);
-		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;
@@ -2818,7 +2982,11 @@ int Jim_Command_drscan(Jim_Interp *interp, int argc, Jim_Obj *const *args)
 		fields[field_count++].in_handler_priv = NULL;
 	}
 
-	jtag_add_dr_scan(num_fields, fields, -1);
+	jtag_add_dr_scan(num_fields, fields, TAP_INVALID);
+	/* did we get an end state? */
+	if (endstate != TAP_INVALID)
+		jtag_add_end_state(endstate);
+
 	retval = jtag_execute_queue();
 	if (retval != ERROR_OK)
 	{
@@ -2849,7 +3017,7 @@ int Jim_Command_drscan(Jim_Interp *interp, int argc, Jim_Obj *const *args)
 	return JIM_OK;
 }
 
-int handle_verify_ircapture_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+static int handle_verify_ircapture_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
 {
 	if (argc == 1)
 	{
@@ -2915,16 +3083,317 @@ void jtag_tap_handle_event( jtag_tap_t * tap, enum jtag_tap_event e)
 	}
 }
 
+/*-----<Cable Helper API>---------------------------------------*/
+
+/*  these Cable Helper API functions are all documented in the jtag.h header file,
+	using a Doxygen format.  And since Doxygen's configuration file "Doxyfile",
+	is setup to prefer its docs in the header file, no documentation is here, for
+	if it were, it would have to be doubly maintained.
+*/
+
+/**
+ * @see tap_set_state() and tap_get_state() accessors.
+ * Actual name is not important since accessors hide it.
+ */
+static tap_state_t state_follower = TAP_RESET;
 
-/* map state number to SVF state string */
-const char* jtag_state_name(enum tap_state state)
+void tap_set_state_impl( tap_state_t new_state )
+{
+	/* this is the state we think the TAPs are in now, was cur_state */
+	state_follower = new_state;
+}
+
+tap_state_t tap_get_state()
+{
+	return state_follower;
+}
+
+/**
+ * @see tap_set_end_state() and tap_get_end_state() accessors.
+ * Actual name is not important because accessors hide it.
+ */
+static tap_state_t end_state_follower = TAP_RESET;
+
+void tap_set_end_state( tap_state_t new_end_state )
+{
+	/* this is the state we think the TAPs will be in at completion of the
+	   current TAP operation, was end_state
+	*/
+	end_state_follower = new_end_state;
+}
+
+tap_state_t tap_get_end_state()
+{
+	return end_state_follower;
+}
+
+
+int tap_move_ndx( tap_state_t astate )
+{
+	/* given a stable state, return the index into the tms_seqs[] array within tap_get_tms_path() */
+
+	int ndx;
+
+	switch( astate )
+	{
+	case TAP_RESET:		ndx = 0;			break;
+	case TAP_DRSHIFT:	ndx = 2;			break;
+	case TAP_DRPAUSE:	ndx = 3;			break;
+	case TAP_IDLE:		ndx = 1;			break;
+	case TAP_IRSHIFT:	ndx = 4;			break;
+	case TAP_IRPAUSE:	ndx = 5;			break;
+	default:
+		LOG_ERROR( "fatal: unstable state \"%s\" used in tap_move_ndx()", tap_state_name(astate) );
+		exit(1);
+	}
+
+	return ndx;
+}
+
+
+/* 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
+ *
+ * DRSHIFT->DRSHIFT and IRSHIFT->IRSHIFT have to be caught in interface specific code
+ */
+static struct
+{
+	u8	bits;
+	u8	bit_count;
+
+} tms_seqs[6][6] =		/*  [from_state_ndx][to_state_ndx] */
+{
+	/* value clocked to TMS to move from one of six stable states to another.
+	 * N.B. OOCD clocks TMS from LSB first, so read these right-to-left.
+	 * N.B. These values are tightly bound to the table in tap_get_tms_path_len().
+	 * N.B. Reset only needs to be 0b11111, but in JLink an even byte of 1's is more stable.
+	 * 		These extra ones cause no TAP state problem, because we go into reset and stay in reset.
+	 */
+
+/*
+ * These macros allow us to specify TMS state transitions by bits rather than hex bytes.
+ * Read the bits from LSBit first to MSBit last (right-to-left).
+ */
+#define HEX__(n) 0x##n##LU
+
+#define B8__(x) \
+	 (((x) & 0x0000000FLU)?(1<<0):0) \
+	+(((x) & 0x000000F0LU)?(1<<1):0) \
+	+(((x) & 0x00000F00LU)?(1<<2):0) \
+	+(((x) & 0x0000F000LU)?(1<<3):0) \
+	+(((x) & 0x000F0000LU)?(1<<4):0) \
+	+(((x) & 0x00F00000LU)?(1<<5):0) \
+	+(((x) & 0x0F000000LU)?(1<<6):0) \
+	+(((x) & 0xF0000000LU)?(1<<7):0)
+
+#define B8(bits,count)		{ ((u8)B8__(HEX__(bits))), (count) }
+
+#if 0 && ((BUILD_FT2232_FTD2XX==1) || (BUILD_FT2232_LIBFTDI==1) || (BUILD_JLINK==1))
+	/* 	this is the table submitted by Jeff Williams on 3/30/2009 with this comment:
+
+		OK, I added Peter's version of the state table, and it works OK for
+		me on MC1322x. I've recreated the jlink portion of patch with this
+		new state table. His changes to my state table are pretty minor in
+		terms of total transitions, but Peter feels that his version fixes
+		some long-standing problems.
+		Jeff
+
+		I added the bit count into the table, reduced RESET column to 7 bits from 8.
+		Dick
+
+		state specific comments:
+		------------------------
+		*->RESET		   tried the 5 bit reset and it gave me problems, 7 bits seems to
+					   work better on ARM9 with ft2232 driver.  (Dick)
+
+		RESET->DRSHIFT add 1 extra clock cycles in the RESET state before advancing.
+						needed on ARM9 with ft2232 driver.  (Dick)
+
+		RESET->IRSHIFT add 1 extra clock cycles in the RESET state before advancing.
+						needed on ARM9 with ft2232 driver.  (Dick)
+	*/
+
+	/* to state: */
+	/*	RESET			IDLE				DRSHIFT			DRPAUSE			IRSHIFT	  		IRPAUSE */			/* from state: */
+	{	B8(1111111,7),	B8(0,1),			B8(00101,5), 	B8(01010,5),		B8(001101,6), 	B8(010110,6) },		/* RESET */
+	{	B8(1111111,7),	B8(0,1),			B8(001,3),		B8(0101,4),		B8(0011,4),  	B8(01011,5) },		/* IDLE */
+	{	B8(1111111,7),	B8(011,3),		B8(00111,5),		B8(01,2),		B8(001111,6),	B8(0101111,7) },		/* DRSHIFT */
+	{	B8(1111111,7),	B8(011,3),		B8(01,2),   		B8(0,1),			B8(001111,6),	B8(0101111,7) },		/* DRPAUSE */
+	{	B8(1111111,7),	B8(011,3),		B8(00111,5),		B8(010111,6), 	B8(001111,6),	B8(01,2) },			/* IRSHIFT */
+	{	B8(1111111,7),	B8(011,3),		B8(00111,5),		B8(010111,6),	B8(01,2),		B8(0,1) } 			/* IRPAUSE */
+
+#else	/* this is the old table, converted from hex and with the bit_count set to 7 for each combo, like before */
+
+	/* to state: */
+	/*	RESET			IDLE				DRSHIFT			DRPAUSE			IRSHIFT	  		IRPAUSE */			/* from state: */
+	{	B8(1111111,7),	B8(0000000,7),	B8(0010111,7), 	B8(0001010,7),	B8(0011011,7), 	B8(0010110,7) },		/* RESET */
+	{	B8(1111111,7),	B8(0000000,7),	B8(0100101,7),	B8(0000101,7),	B8(0101011,7),	B8(0001011,7) },		/* IDLE */
+	{	B8(1111111,7),	B8(0110001,7),	B8(0000000,7),	B8(0000001,7),	B8(0001111,7),	B8(0101111,7) },		/* DRSHIFT */
+	{	B8(1111111,7),	B8(0110000,7),	B8(0100000,7),	B8(0010111,7),	B8(0011110,7),	B8(0101111,7) },		/* DRPAUSE */
+	{	B8(1111111,7),	B8(0110001,7),	B8(0000111,7),	B8(0010111,7), 	B8(0000000,7),	B8(0000001,7) },		/* IRSHIFT */
+	{	B8(1111111,7),	B8(0110000,7),	B8(0011100,7),	B8(0010111,7),	B8(0100000,7),	B8(0101111,7) } 		/* IRPAUSE */
+
+#endif
+
+#if 0 /* keeping old hex stuff for awhile, for reference */
+	/* RESET  			IDLE  			DRSHIFT  		DRPAUSE  		IRSHIFT  		IRPAUSE */
+	{  0x7f, 			0x00,    		0x17,    		0x0a,    		0x1b,    		0x16 },	/* RESET */
+	{  0x7f, 			0x00,    		0x25,    		0x05,    		0x2b,    		0x0b },	/* IDLE */
+	{  0x7f, 			0x31,    		0x00,    		0x01,    		0x0f,    		0x2f },	/* DRSHIFT  */
+	{  0x7f, 			0x30,    		0x20,    		0x17,    		0x1e,    		0x2f },	/* DRPAUSE  */
+	{  0x7f, 			0x31,    		0x07,    		0x17,    		0x00,    		0x01 },	/* IRSHIFT  */
+	{  0x7f, 			0x30,    		0x1c,    		0x17,    		0x20,    		0x2f }	/* IRPAUSE  */
+#endif
+};
+
+
+int tap_get_tms_path( tap_state_t from, tap_state_t to )
+{
+	return tms_seqs[tap_move_ndx(from)][tap_move_ndx(to)].bits;
+}
+
+
+int tap_get_tms_path_len( tap_state_t from, tap_state_t to )
+{
+	return tms_seqs[tap_move_ndx(from)][tap_move_ndx(to)].bit_count;
+}
+
+
+bool tap_is_state_stable(tap_state_t astate)
+{
+	bool is_stable;
+
+	/* 	A switch() is used because it is symbol dependent
+		(not value dependent like an array), and can also check bounds.
+	*/
+	switch( astate )
+	{
+	case TAP_RESET:
+	case TAP_IDLE:
+	case TAP_DRSHIFT:
+	case TAP_DRPAUSE:
+	case TAP_IRSHIFT:
+	case TAP_IRPAUSE:
+		is_stable = true;
+		break;
+	default:
+		is_stable = false;
+	}
+
+	return is_stable;
+}
+
+tap_state_t tap_state_transition(tap_state_t cur_state, bool tms)
+{
+	tap_state_t new_state;
+
+	/* 	A switch is used because it is symbol dependent and not value dependent
+		like an array.  Also it can check for out of range conditions.
+	*/
+
+	if (tms)
+	{
+		switch (cur_state)
+		{
+		case TAP_RESET:
+			new_state = cur_state;
+			break;
+		case TAP_IDLE:
+		case TAP_DRUPDATE:
+		case TAP_IRUPDATE:
+			new_state = TAP_DRSELECT;
+			break;
+		case TAP_DRSELECT:
+			new_state = TAP_IRSELECT;
+			break;
+		case TAP_DRCAPTURE:
+		case TAP_DRSHIFT:
+			new_state = TAP_DREXIT1;
+			break;
+		case TAP_DREXIT1:
+		case TAP_DREXIT2:
+			new_state = TAP_DRUPDATE;
+			break;
+		case TAP_DRPAUSE:
+			new_state = TAP_DREXIT2;
+			break;
+		case TAP_IRSELECT:
+			new_state = TAP_RESET;
+			break;
+		case TAP_IRCAPTURE:
+		case TAP_IRSHIFT:
+			new_state = TAP_IREXIT1;
+			break;
+		case TAP_IREXIT1:
+		case TAP_IREXIT2:
+			new_state = TAP_IRUPDATE;
+			break;
+		case TAP_IRPAUSE:
+			new_state = TAP_IREXIT2;
+			break;
+		default:
+			LOG_ERROR( "fatal: invalid argument cur_state=%d", cur_state );
+			exit(1);
+			break;
+		}
+	}
+	else
+	{
+		switch (cur_state)
+		{
+		case TAP_RESET:
+		case TAP_IDLE:
+		case TAP_DRUPDATE:
+		case TAP_IRUPDATE:
+			new_state = TAP_IDLE;
+			break;
+		case TAP_DRSELECT:
+			new_state = TAP_DRCAPTURE;
+			break;
+		case TAP_DRCAPTURE:
+		case TAP_DRSHIFT:
+		case TAP_DREXIT2:
+			new_state = TAP_DRSHIFT;
+			break;
+		case TAP_DREXIT1:
+		case TAP_DRPAUSE:
+			new_state = TAP_DRPAUSE;
+			break;
+		case TAP_IRSELECT:
+			new_state = TAP_IRCAPTURE;
+			break;
+		case TAP_IRCAPTURE:
+		case TAP_IRSHIFT:
+		case TAP_IREXIT2:
+			new_state = TAP_IRSHIFT;
+			break;
+		case TAP_IREXIT1:
+		case TAP_IRPAUSE:
+			new_state = TAP_IRPAUSE;
+			break;
+		default:
+			LOG_ERROR( "fatal: invalid argument cur_state=%d", cur_state );
+			exit(1);
+			break;
+		}
+	}
+
+	return new_state;
+}
+
+const char* tap_state_name(tap_state_t state)
 {
 	const char* ret;
 
 	switch( state )
 	{
 	case TAP_RESET:		ret = "RESET";			break;
-	case TAP_IDLE:		ret = "IDLE";			break;
+	case TAP_IDLE:		ret = "RUN/IDLE";		break;
 	case TAP_DRSELECT:	ret = "DRSELECT";		break;
 	case TAP_DRCAPTURE: ret = "DRCAPTURE";		break;
 	case TAP_DRSHIFT:	ret = "DRSHIFT";			break;
@@ -2945,3 +3414,103 @@ const char* jtag_state_name(enum tap_state state)
 	return ret;
 }
 
+static tap_state_t tap_state_by_name( const char *name )
+{
+	tap_state_t x;
+
+	for( x = 0 ; x < TAP_NUM_STATES ; x++ ){
+		/* be nice to the human */
+		if( 0 == strcasecmp( name, tap_state_name(x) ) ){
+			return x;
+		}
+	}
+	/* not found */
+	return TAP_INVALID;
+}
+
+#ifdef _DEBUG_JTAG_IO_
+
+#define JTAG_DEBUG_STATE_APPEND(buf, len, bit) \
+		do { buf[len] = bit ? '1' : '0'; } while(0)
+#define JTAG_DEBUG_STATE_PRINT(a, b, astr, bstr) \
+		DEBUG_JTAG_IO("TAP/SM: %9s -> %5s\tTMS: %s\tTDI: %s", \
+			tap_state_name(a), tap_state_name(b), astr, bstr)
+
+tap_state_t jtag_debug_state_machine(const void *tms_buf, const void *tdi_buf,
+		unsigned tap_bits, tap_state_t next_state)
+{
+	const u8 *tms_buffer;
+	const u8 *tdi_buffer;
+	unsigned tap_bytes;
+	unsigned cur_byte;
+	unsigned cur_bit;
+
+	unsigned tap_out_bits;
+	char tms_str[33];
+	char tdi_str[33];
+
+	tap_state_t last_state;
+
+	// set startstate (and possibly last, if tap_bits == 0)
+	last_state = next_state;
+	DEBUG_JTAG_IO("TAP/SM: START state: %s", tap_state_name(next_state));
+
+	tms_buffer = (const u8 *)tms_buf;
+	tdi_buffer = (const u8 *)tdi_buf;
+
+	tap_bytes = TAP_SCAN_BYTES(tap_bits);
+	DEBUG_JTAG_IO("TAP/SM: TMS bits: %u (bytes: %u)", tap_bits, tap_bytes);
+
+	tap_out_bits = 0;
+	for(cur_byte = 0; cur_byte < tap_bytes; cur_byte++)
+	{
+		for(cur_bit = 0; cur_bit < 8; cur_bit++)
+		{
+			// make sure we do not run off the end of the buffers
+			unsigned tap_bit = cur_byte * 8 + cur_bit;
+			if (tap_bit == tap_bits)
+				break;
+
+			// check and save TMS bit
+			tap_bit = !!(tms_buffer[cur_byte] & (1 << cur_bit));
+			JTAG_DEBUG_STATE_APPEND(tms_str, tap_out_bits, tap_bit);
+
+			// use TMS bit to find the next TAP state
+			next_state = tap_state_transition(last_state, tap_bit);
+
+			// check and store TDI bit
+			tap_bit = !!(tdi_buffer[cur_byte] & (1 << cur_bit));
+			JTAG_DEBUG_STATE_APPEND(tdi_str, tap_out_bits, tap_bit);
+
+			// increment TAP bits
+			tap_out_bits++;
+
+			// Only show TDO bits on state transitions, or
+			// after some number of bits in the same state.
+			if ((next_state == last_state) && (tap_out_bits < 32))
+				continue;
+
+			// terminate strings and display state transition
+			tms_str[tap_out_bits] = tdi_str[tap_out_bits] = 0;
+			JTAG_DEBUG_STATE_PRINT(last_state, next_state, tms_str, tdi_str);
+
+			// reset state
+			last_state = next_state;
+			tap_out_bits = 0;
+		}
+	}
+
+	if (tap_out_bits)
+	{
+		// terminate strings and display state transition
+		tms_str[tap_out_bits] = tdi_str[tap_out_bits] = 0;
+		JTAG_DEBUG_STATE_PRINT(last_state, next_state, tms_str, tdi_str);
+	}
+
+	DEBUG_JTAG_IO("TAP/SM: FINAL state: %s", tap_state_name(next_state));
+
+	return next_state;
+}
+#endif // _DEBUG_JTAG_IO_
+
+/*-----</Cable Helper API>--------------------------------------*/