1 files changed, 54 insertions, 18 deletions

diff --git a/src/jtag/bitbang.c b/src/jtag/bitbang.c
index f12355c6..819d8e58 100644
--- a/src/jtag/bitbang.c
+++ b/src/jtag/bitbang.c
@@ -75,7 +75,7 @@ void bitbang_path_move(pathmove_command_t *cmd)
 {
 	int num_states = cmd->num_states;
 	int state_count;
-	int tms = 0;
+	int tms;
 
 	state_count = 0;
 	while (num_states)
@@ -138,6 +138,7 @@ void bitbang_scan(int ir_scan, enum scan_type type, u8 *buffer, int scan_size)
 {
 	enum tap_state saved_end_state = end_state;
 	int bit_cnt;
+	int last_bit, last_bit_in;
 	
 	if (!((!ir_scan && (cur_state == TAP_SD)) || (ir_scan && (cur_state == TAP_SI))))
 	{
@@ -150,7 +151,7 @@ void bitbang_scan(int ir_scan, enum scan_type type, u8 *buffer, int scan_size)
 		bitbang_end_state(saved_end_state);
 	}
 
-	for (bit_cnt = 0; bit_cnt < scan_size; bit_cnt++)
+	for (bit_cnt = 0; bit_cnt < scan_size - 1; bit_cnt++)
 	{
 		/* if we're just reading the scan, but don't care about the output
 		 * default to outputting 'low', this also makes valgrind traces more readable,
@@ -158,11 +159,11 @@ void bitbang_scan(int ir_scan, enum scan_type type, u8 *buffer, int scan_size)
 		 */ 
 		if ((type != SCAN_IN) && ((buffer[bit_cnt/8] >> (bit_cnt % 8)) & 0x1))
 		{
-			bitbang_interface->write(0, (bit_cnt==scan_size-1) ? 1 : 0, 1);
-			bitbang_interface->write(1, (bit_cnt==scan_size-1) ? 1 : 0, 1);
+			bitbang_interface->write(0, 0, 1);
+			bitbang_interface->write(1, 0, 1);
 		} else {
-			bitbang_interface->write(0, (bit_cnt==scan_size-1) ? 1 : 0, 0);
-			bitbang_interface->write(1, (bit_cnt==scan_size-1) ? 1 : 0, 0);
+			bitbang_interface->write(0, 0, 0);
+			bitbang_interface->write(1, 0, 0);
 		}
 		
 		if (type != SCAN_OUT)
@@ -173,19 +174,54 @@ void bitbang_scan(int ir_scan, enum scan_type type, u8 *buffer, int scan_size)
 				buffer[(bit_cnt)/8] &= ~(1 << ((bit_cnt) % 8));
 		}
 	}
-	
-	/* Exit1 -> Pause */
-	bitbang_interface->write(0, 0, 0);
-	bitbang_interface->write(1, 0, 0);
-	bitbang_interface->write(0, 0, 0);
-	
-	if (ir_scan)
-		cur_state = TAP_PI;
+
+	if ((type != SCAN_IN) && ((buffer[bit_cnt/8] >> (bit_cnt % 8)) & 0x1))
+		last_bit = 1;
 	else
-		cur_state = TAP_PD;
-	
-	if (cur_state != end_state)
-		bitbang_state_move();
+		last_bit = 0;
+
+	if ((ir_scan && (end_state == TAP_SI)) ||
+		(!ir_scan && (end_state == TAP_SD)))
+	{
+		bitbang_interface->write(0, 0, last_bit);
+		bitbang_interface->write(1, 0, last_bit);
+
+		if (type != SCAN_OUT)
+			last_bit_in = bitbang_interface->read();
+
+		bitbang_interface->write(0, 0, last_bit);
+	}
+	else
+	{
+		/* Shift-[ID]R -> Exit1-[ID]R */
+		bitbang_interface->write(0, 1, last_bit);
+		bitbang_interface->write(1, 1, last_bit);
+		
+		if (type != SCAN_OUT)
+			last_bit_in = bitbang_interface->read();
+
+		/* Exit1-[ID]R -> Pause-[ID]R */
+		bitbang_interface->write(0, 0, 0);
+		bitbang_interface->write(1, 0, 0);
+		
+		if (cur_state == TAP_SI)
+			cur_state = TAP_PI;
+		else
+			cur_state = TAP_PD;
+
+		if (cur_state != end_state)
+			bitbang_state_move();
+		else
+			bitbang_interface->write(0, 0, 0);
+	}
+		
+	if (type != SCAN_OUT)
+	{
+		if (last_bit_in)
+			buffer[(bit_cnt)/8] |= 1 << ((bit_cnt) % 8);
+		else
+			buffer[(bit_cnt)/8] &= ~(1 << ((bit_cnt) % 8));
+	}
 }
 
 int bitbang_execute_queue(void)