From 6a7bd29e0d158d56dbf2efe07b5b9d7f6072eab1 Mon Sep 17 00:00:00 2001
From: Gwenhael Goavec-Merou <gwenhael.goavec-merou@trabucayre.com>
Date: Fri, 2 Jan 2026 09:52:41 +0100
Subject: [PATCH] dirtyJtag: writeTDI: fixed last Byte align

---
 src/dirtyJtag.cpp | 17 ++++++++++++++---
 1 file changed, 14 insertions(+), 3 deletions(-)

diff --git a/src/dirtyJtag.cpp b/src/dirtyJtag.cpp
index cfdc2ee..6eba1f9 100644
--- a/src/dirtyJtag.cpp
+++ b/src/dirtyJtag.cpp
@@ -214,11 +214,10 @@ int DirtyJtag::writeTMS(const uint8_t *tms, uint32_t len,
 	return len;
 }
 
-int DirtyJtag::toggleClk(uint8_t tms, __attribute__((unused)) uint8_t tdi,
-	uint32_t clk_len)
+int DirtyJtag::toggleClk(__attribute__((unused)) uint8_t tms,
+	__attribute__((unused)) uint8_t tdi, uint32_t clk_len)
 {
 	int actual_length;
-	_tms = tms ? SIG_TMS : 0;
 	uint8_t buf[] = {CMD_CLK,
 		static_cast<uint8_t>(_tms | _tdi),
 		0,
@@ -319,6 +318,18 @@ int DirtyJtag::writeTDI(const uint8_t *tx, uint8_t *rx, uint32_t len, bool end)
 				rx_ptr[rx_cnt >> 3] = (rx_ptr[rx_cnt >> 3] >> 1) |
 						(((rx_buf[i >> 3] << (i&0x07)) & 0x80));
 			}
+			/* Last xfer:
+			 * if bit_to_send is not a multiple of 8bits a shift must
+			 * be applied to align rigth the last Byte
+			 */
+			if (bit_to_send < max_bit_transfer_length) {
+				const uint32_t b = (bit_to_send >> 3) << 3; // floor
+				if (b < bit_to_send) { // difference ?
+					const uint32_t diff = bit_to_send - b;
+					const uint8_t t = rx_ptr[(rx_cnt-1) >> 3] >> (8 - diff);
+					rx_ptr[(rx_cnt - 1) >> 3] = t;
+				}
+			}
 		}
 
 		real_bit_len -= bit_to_send;