diff --git a/jtag/include/register.h b/jtag/include/register.h
index 1a392cfa..eb39adb7 100644
--- a/jtag/include/register.h
+++ b/jtag/include/register.h
@@ -37,6 +37,7 @@ tap_register *register_duplicate( const tap_register_t *tr );
 void register_free( tap_register_t *tr );
 tap_register *register_fill( tap_register_t *tr, int val );
 const char *register_get_string( const tap_register_t *tr );
+int register_all_bits_same_value( const tap_register_t *tr );
 tap_register *register_init( tap_register_t *tr, const char *value );
 int register_compare( const tap_register_t *tr, const tap_register_t *tr2 );
 int register_match( const tap_register_t *tr, const char *expr );
diff --git a/jtag/src/discovery.c b/jtag/src/discovery.c
index 58f7ff56..6a5f7a5c 100644
--- a/jtag/src/discovery.c
+++ b/jtag/src/discovery.c
@@ -46,6 +46,9 @@ detect_register_size( chain_t *chain )
 	tap_register *rout;
 	tap_register *rpat;
 
+	/* This seems to be a good place to check if TDO changes at all */
+	int tdo, tdo_stuck = -2;
+
 	for (len = 1; len <= MAX_REGISTER_LENGTH; len++) {
 		int p;
 		int ok = 0;
@@ -67,6 +70,10 @@ detect_register_size( chain_t *chain )
 				tap_shift_register( chain, rz, NULL, 0 );
 				tap_shift_register( chain, rpat, rout, 0 );
 
+				tdo = register_all_bits_same_value(rout);
+				if(tdo_stuck == -2) tdo_stuck = tdo;
+				if(tdo_stuck != tdo) tdo_stuck = -1;
+
 				register_shift_right( rout, len );
 
 				if (register_compare( rpat, rout ) == 0)
@@ -88,6 +95,11 @@ detect_register_size( chain_t *chain )
 			return len;
 	}
 
+	if(tdo_stuck >= 0)
+	{
+		printf(_("Warning: TDO seems to be stuck at %d\n"), tdo_stuck);
+	}
+
 	return -1;
 }
 
diff --git a/jtag/src/tap/register.c b/jtag/src/tap/register.c
index f8d3e97a..dbdd5d4f 100644
--- a/jtag/src/tap/register.c
+++ b/jtag/src/tap/register.c
@@ -102,6 +102,25 @@ register_get_string( const tap_register *tr )
 	return tr->string;
 }
 
+int
+register_all_bits_same_value ( const tap_register *tr )
+{
+	int i, value;
+	if (!tr) return -1;
+	if (tr->len < 0) return -1;
+
+	/* Return -1 if any of the bits in the register
+	 * differs from the others; the value otherwise. */
+
+	value = tr->data[0] & 1;
+
+	for(i=1; i<tr->len; i++)
+	{
+		if((tr->data[i] & 1) != value) return -1;
+	}
+	return value;
+}
+
 tap_register *
 register_init( tap_register *tr, const char *value )
 {