diff --git a/src/usbBlaster.cpp b/src/usbBlaster.cpp
index cc2a040..07265a4 100644
--- a/src/usbBlaster.cpp
+++ b/src/usbBlaster.cpp
@@ -260,8 +260,8 @@ int UsbBlaster::writeTDI(uint8_t *tx, uint8_t *rx, uint32_t len, bool end)
 		if (writeBit((rx)? &tmp:NULL, 1) < 0)
 			return -EXIT_FAILURE;
 		if (rx)
-			*rx_ptr |= ((tmp & 0x80) << (7 - nb_bit));
-		_in_buf[_nb_bit++] = mask | mode;
+			*rx_ptr = (tmp & 0x80) | ((*rx_ptr) >> 1);
+		_in_buf[_nb_bit++] = mask;
 		if (writeBit(NULL, 0) < 0)
 			return -EXIT_FAILURE;
 	}