From aa752856c3e1acf53433b827fa5c5624e2bc4df2 Mon Sep 17 00:00:00 2001
From: Gwenhael Goavec-Merou <gwenhael.goavec-merou@trabucayre.com>
Date: Tue, 19 May 2020 08:32:43 +0200
Subject: [PATCH] jtag, ftdiJtagBitbang: improve tms flush and fix data
 reconstruct in specific conditions

---
 src/ftdiJtagBitbang.cpp | 14 +++++++-------
 src/ftdiJtagBitbang.hpp |  2 +-
 src/jtag.cpp            |  6 ++++--
 3 files changed, 12 insertions(+), 10 deletions(-)

diff --git a/src/ftdiJtagBitbang.cpp b/src/ftdiJtagBitbang.cpp
index 03c55ef..e99d101 100644
--- a/src/ftdiJtagBitbang.cpp
+++ b/src/ftdiJtagBitbang.cpp
@@ -115,15 +115,14 @@ int FtdiJtagBitBang::storeTMS(uint8_t *tms, int nb_bit, uint8_t tdi, bool read)
 
 		xfer_len -= xfer;
 		if (xfer_len != 0)
-			write(NULL);
+			write(NULL, 0);
 	}
 	return nb_bit;
 }
 
 int FtdiJtagBitBang::writeTMS(uint8_t *tdo, int len)
 {
-	(void) len;
-	return write(tdo);
+	return write(tdo, len);
 }
 
 /**
@@ -159,12 +158,11 @@ int FtdiJtagBitBang::storeTDI(uint8_t *tdi, int nb_byte, bool read)
 
 int FtdiJtagBitBang::writeTDI(uint8_t *tdo, int nb_bit)
 {
-	(void) nb_bit;
-	return write(tdo);
+	return write(tdo, nb_bit);
 }
 
 
-int FtdiJtagBitBang::write(uint8_t *tdo)
+int FtdiJtagBitBang::write(uint8_t *tdo, int nb_bit)
 {
 	int ret = 0;
 	if (_nb_bit == 0)
@@ -184,8 +182,10 @@ int FtdiJtagBitBang::write(uint8_t *tdo)
 		 * even bit are discarded since JTAG read in rising edge
 		 * since jtag is LSB first we need to shift right content by 1
 		 * and add 0x80 (1 << 7) or 0
+		 * the buffer may contains some tms bit, so start with i
+		 * equal to fill exactly nb_bit bits
 		 * */
-		for (int i = 1, offset=0; i < _nb_bit; i+=2, offset++) {
+		for (int i = (_nb_bit-(nb_bit *2) + 1), offset=0; i < _nb_bit; i+=2, offset++) {
 			tdo[offset >> 3] = (((_in_buf[i] & _tdo_pin) ? 0x80 : 0x00) |
 							(tdo[offset >> 3] >> 1));
 		}
diff --git a/src/ftdiJtagBitbang.hpp b/src/ftdiJtagBitbang.hpp
index 41ff9d2..1350e2e 100644
--- a/src/ftdiJtagBitbang.hpp
+++ b/src/ftdiJtagBitbang.hpp
@@ -70,7 +70,7 @@ class FtdiJtagBitBang : public JtagInterface, private FTDIpp_MPSSE {
  private:
 	void init_internal(const FTDIpp_MPSSE::mpsse_bit_config &cable,
 		const jtag_pins_conf_t *pin_conf);
-	int write(uint8_t *tdo);
+	int write(uint8_t *tdo, int nb_bit);
 	int setBitmode(uint8_t mode);
 	uint8_t *_in_buf;
 
diff --git a/src/jtag.cpp b/src/jtag.cpp
index 97e5372..68239b7 100644
--- a/src/jtag.cpp
+++ b/src/jtag.cpp
@@ -147,6 +147,7 @@ void Jtag::setTMS(unsigned char tms)
 
 int Jtag::flushTMS(bool flush_buffer)
 {
+	int xfer = _num_tms;
 	if (_num_tms != 0) {
 		display("%s: %d %x\n", __func__, _num_tms, _tms_buffer[0]);
 
@@ -155,9 +156,10 @@ int Jtag::flushTMS(bool flush_buffer)
 		/* reset buffer and number of bits */
 		bzero(_tms_buffer, _tms_buffer_size);
 		_num_tms = 0;
+		if (flush_buffer) {
+			return _jtag->writeTMS(NULL, xfer);
+		}
 	}
-	if (flush_buffer)
-		return _jtag->writeTMS(NULL);
 	return 0;
 }