diff -ru FTCJTAG_Source.orig/FT2232hMpsseJtag.cpp FTCJTAG_Source/FT2232hMpsseJtag.cpp
--- FTCJTAG_Source.orig/FT2232hMpsseJtag.cpp	2009-02-19 16:38:00.000000000 +0900
+++ FTCJTAG_Source/FT2232hMpsseJtag.cpp	2010-03-18 17:06:49.921875000 +0900
@@ -710,21 +710,25 @@
                                                          PWriteDataByteBuffer pWriteDataBuffer, DWORD dwNumBytesToWrite,
                                                          DWORD dwTapControllerState)
 {
-  DWORD dwModNumBitsToWrite = 0;
+  DWORD dwModNumBitsToWrite(dwNumBitsToWrite);
   DWORD dwDataBufferIndex = 0;
   DWORD dwNumDataBytes = 0;
   DWORD dwNumRemainingDataBits = 0;
   DWORD dwLastDataBit = 0;
   DWORD dwDataBitIndex = 0;
 
-  // adjust for bit count of 1 less than no of bits
-  dwModNumBitsToWrite = (dwNumBitsToWrite - 1);
-
-  if (bInstructionTestData == FALSE)
-    MoveJTAGFromOneStateToAnother(ShiftDataRegister, NO_LAST_DATA_BIT, false);
-  else
-    MoveJTAGFromOneStateToAnother(ShiftInstructionRegister, NO_LAST_DATA_BIT, false);
-
+  enum JtagStates shift_state(
+      (bInstructionTestData == FALSE) ? ShiftDataRegister : ShiftInstructionRegister); 
+  
+  MoveJTAGFromOneStateToAnother(shift_state, NO_LAST_DATA_BIT, false);
+  
+  bool keep_shift(shift_state == JtagStates((dwTapControllerState - 1)));
+  
+  if(!keep_shift){
+    // adjust for bit count of 1 less than no of bits
+    dwModNumBitsToWrite--;
+  }  
+  
   dwNumDataBytes = (dwModNumBitsToWrite / 8);
 
   if (dwNumDataBytes > 0)
@@ -758,6 +762,8 @@
     AddByteToOutputBuffer((*pWriteDataBuffer)[dwDataBufferIndex], false);
   }
 
+  if(keep_shift) return;
+    
   // get last bit
   dwLastDataBit = (*pWriteDataBuffer)[dwDataBufferIndex];
   dwDataBitIndex = (dwNumBitsToWrite % 8);
@@ -866,19 +872,23 @@
 
 DWORD FT2232hMpsseJtag::AddReadCommandToOutputBuffer(BOOL bInstructionTestData, DWORD dwNumBitsToRead, DWORD dwTapControllerState)
 {
-  DWORD dwModNumBitsToRead = 0;
+  DWORD dwModNumBitsToRead(dwNumBitsToRead);
   DWORD dwNumDataBytes = 0;
   DWORD dwNumRemainingDataBits = 0;
   DWORD dwNumTmsClocks = 0;
 
-  // adjust for bit count of 1 less than no of bits
-  dwModNumBitsToRead = (dwNumBitsToRead - 1);
-
-  if (bInstructionTestData == FALSE)
-    MoveJTAGFromOneStateToAnother(ShiftDataRegister, NO_LAST_DATA_BIT, false);
-  else
-    MoveJTAGFromOneStateToAnother(ShiftInstructionRegister, NO_LAST_DATA_BIT, false);
-
+  enum JtagStates shift_state(
+      (bInstructionTestData == FALSE) ? ShiftDataRegister : ShiftInstructionRegister); 
+  
+  MoveJTAGFromOneStateToAnother(shift_state, NO_LAST_DATA_BIT, false);
+  
+  bool keep_shift(shift_state == JtagStates((dwTapControllerState - 1)));
+  
+  if(!keep_shift){
+    // adjust for bit count of 1 less than no of bits
+    dwModNumBitsToRead--;
+  }
+  
   dwNumDataBytes = (dwModNumBitsToRead / 8);
 
   if (dwNumDataBytes > 0)
@@ -903,6 +913,8 @@
     AddByteToOutputBuffer(CLK_DATA_BITS_IN_ON_POS_CLK_LSB_FIRST_CMD, false);
     AddByteToOutputBuffer((dwNumRemainingDataBits & '\xFF'), false);
   }
+  
+  if(keep_shift) return 0;
 
   // end it in state passed in, take 1 off the dwTapControllerState variable to correspond with JtagStates enumerated types
   dwNumTmsClocks = MoveJTAGFromOneStateToAnother(JtagStates((dwTapControllerState - 1)), NO_LAST_DATA_BIT, true);
@@ -935,21 +947,25 @@
                                                               PWriteDataByteBuffer pWriteDataBuffer,
                                                               DWORD dwNumBytesToWrite, DWORD dwTapControllerState)
 {
-  DWORD dwModNumBitsToWriteRead = 0;
+  DWORD dwModNumBitsToWriteRead(dwNumBitsToWriteRead);
   DWORD dwNumWriteDataBytes = 0;
   DWORD dwDataBufferIndex = 0;
   DWORD dwNumRemainingDataBits = 0;
   DWORD dwLastDataBit = 0;
   DWORD dwDataBitIndex = 0;
   DWORD dwNumTmsClocks = 0;
-
-  // adjust for bit count of 1 less than no of bits
-  dwModNumBitsToWriteRead = (dwNumBitsToWriteRead - 1);
-
-  if (bInstructionTestData == FALSE)
-    MoveJTAGFromOneStateToAnother(ShiftDataRegister, NO_LAST_DATA_BIT, false);
-  else
-    MoveJTAGFromOneStateToAnother(ShiftInstructionRegister, NO_LAST_DATA_BIT, false);
+  
+  enum JtagStates shift_state(
+      (bInstructionTestData == FALSE) ? ShiftDataRegister : ShiftInstructionRegister); 
+  
+  MoveJTAGFromOneStateToAnother(shift_state, NO_LAST_DATA_BIT, false);
+  
+  bool keep_shift(shift_state == JtagStates((dwTapControllerState - 1)));
+  
+  if(!keep_shift){
+    // adjust for bit count of 1 less than no of bits
+    dwModNumBitsToWriteRead--;
+  }
 
   dwNumWriteDataBytes = (dwModNumBitsToWriteRead / 8);
 
@@ -983,12 +999,14 @@
     AddByteToOutputBuffer((dwNumRemainingDataBits & '\xFF'), false);
     AddByteToOutputBuffer((*pWriteDataBuffer)[dwDataBufferIndex], false);
   }
+  
+  if(keep_shift) return 0;
 
   // get last bit
   dwLastDataBit = (*pWriteDataBuffer)[dwDataBufferIndex];
   dwDataBitIndex = (dwNumBitsToWriteRead % 8);
 
-  if (dwDataBitIndex == 8)
+  if (dwDataBitIndex == 0)
     dwLastDataBit = (dwLastDataBit >> ((8 - dwDataBitIndex) - 1));
   else
     dwLastDataBit = (dwLastDataBit >> (dwDataBitIndex - 1));