diff --git a/src/altera.cpp b/src/altera.cpp
index 8def602..1f677a7 100644
--- a/src/altera.cpp
+++ b/src/altera.cpp
@@ -65,9 +65,6 @@ void Altera::programMem()
 	/* write */
 	ProgressBar progress("Flash SRAM", byte_length, 50, _quiet);
 
-	/* 2.2.6.5 */
-	_jtag->set_state(Jtag::SHIFT_DR);
-
 	int xfer_len = 512;
 	int tx_len;
 	int tx_end;
@@ -75,12 +72,12 @@ void Altera::programMem()
 	for (int i=0; i < byte_length; i+=xfer_len) {
 		if (i + xfer_len > byte_length) {  // last packet with some size
 			tx_len = (byte_length - i) * 8;
-			tx_end = 1;  // to move in EXIT1_DR
+			tx_end = Jtag::EXIT1_DR;
 		} else {
 			tx_len = xfer_len * 8;
-			tx_end = 0;
+			tx_end = Jtag::SHIFT_DR;
 		}
-		_jtag->read_write(data+i, NULL, tx_len, tx_end);
+		_jtag->shiftDR(data+i, NULL, tx_len, tx_end);
 		progress.display(i);
 	}
 	progress.done();
diff --git a/src/jtag.cpp b/src/jtag.cpp
index 05209be..2907447 100644
--- a/src/jtag.cpp
+++ b/src/jtag.cpp
@@ -203,13 +203,25 @@ void Jtag::toggleClk(int nb)
 
 int Jtag::shiftDR(unsigned char *tdi, unsigned char *tdo, int drlen, int end_state)
 {
-	set_state(SHIFT_DR);
-	// force transmit tms state
-	flushTMS(false);
-	// currently don't care about multiple device in the chain
-	read_write(tdi, tdo, drlen, 1);// 1 since only one device
+	/* if current state not shift DR
+	 * move to this state
+	 */
+	if (_state != SHIFT_DR) {
+		set_state(SHIFT_DR);
+		flushTMS(false);  // force transmit tms state
+	}
+	/* write tdi (and read tdo) to the selected device
+	 * end (ie TMS high) is used when
+	 * a state change must
+	 * be done
+	 */
+	read_write(tdi, tdo, drlen, end_state != SHIFT_DR);
 
-	set_state(end_state);
+	/* if it's asked to move in FSM */
+	if (end_state != SHIFT_DR) {
+		/* move to end_state */
+		set_state(end_state);
+	}
 	return 0;
 }
 
@@ -225,14 +237,30 @@ int Jtag::shiftIR(unsigned char tdi, int irlen, int end_state)
 int Jtag::shiftIR(unsigned char *tdi, unsigned char *tdo, int irlen, int end_state)
 {
 	display("%s: avant shiftIR\n", __func__);
-	set_state(SHIFT_IR);
-	flushTMS(false);
-	// currently don't care about multiple device in the chain
+
+	/* if not in SHIFT IR move to this state */
+	if (_state != SHIFT_IR) {
+		set_state(SHIFT_IR);
+		/* force flush */
+		flushTMS(false);
+	}
+
 
 	display("%s: envoi ircode\n", __func__);
-	read_write(tdi, tdo, irlen, 1);  // 1 since only one device
 
-	set_state(end_state);
+	/* write tdi (and read tdo) to the selected device
+	 * end (ie TMS high) is used only when
+	 * a state change must
+	 * be done
+	 */
+	read_write(tdi, tdo, irlen, end_state != SHIFT_IR);
+
+	/* it's asked to move out of SHIFT IR state */
+	if (end_state != SHIFT_IR) {
+		/* move to the requested state */
+		set_state(end_state);
+	}
+
 	return 0;
 }