From 1f59dfd671ab0c8a7a83c6bf1fc7bc0c4d0dd1b9 Mon Sep 17 00:00:00 2001
From: Gwenhael Goavec-Merou <gwenhael.goavec-merou@trabucayre.com>
Date: Thu, 13 Jan 2022 07:34:00 +0100
Subject: [PATCH] jtag: improving jtag chain detection: now searching for
 masked and unmasked idcode

---
 src/jtag.cpp | 55 ++++++++++++++++++++++++++++++++--------------------
 src/jtag.hpp |  8 ++++++++
 2 files changed, 42 insertions(+), 21 deletions(-)

diff --git a/src/jtag.cpp b/src/jtag.cpp
index 9970772..0d0efce 100644
--- a/src/jtag.cpp
+++ b/src/jtag.cpp
@@ -135,34 +135,28 @@ int Jtag::detectChain(int max_dev)
 		tmp = 0;
 		for (int ii=0; ii < 4; ii++)
 			tmp |= (rx_buff[ii] << (8*ii));
+
+		/* search IDCODE in fpga_list and misc_dev_list
+		 * since most device have idcode with high nibble masked
+		 * we start to search sub IDCODE
+		 * if IDCODE has no match: try the same with version unmasked
+		 */
 		if (tmp != 0 && tmp != 0xffffffff) {
+			bool found = false;
 			/* ckeck highest nibble to prevent confusion between Cologne Chip
 			 * GateMate and Efinix Trion T4/T8 devices
 			 */
 			if (tmp != 0x20000001)
-				tmp &= 0x0fffffff;
-			else
-				tmp &= 0xffffffff;
-			_devices_list.insert(_devices_list.begin(), tmp);
+				found = search_and_insert_device_with_idcode(tmp & 0x0fffffff);
+			if (!found) /* if masked not found -> search for full */
+				found = search_and_insert_device_with_idcode(tmp);
 
-			/* search for irlength in fpga_list or misc_dev_list */
-			uint16_t irlength = -1;
-			auto dev = fpga_list.find(tmp);
-			if (dev == fpga_list.end()) {
-				auto misc = misc_dev_list.find(tmp);
-				if (misc != misc_dev_list.end()) {
-					irlength = misc->second.irlength;
-				} else {
-					char error[256];
-					snprintf(error, 256, "Unknown device with IDCODE: 0x%08x",
-							tmp);
-					throw std::runtime_error(error);
-				}
-			} else {
-				irlength = dev->second.irlength;
+			if (!found) {
+				char error[256];
+				snprintf(error, 256, "Unknown device with IDCODE: 0x%08x",
+						tmp);
+				throw std::runtime_error(error);
 			}
-
-			_irlength_list.insert(_irlength_list.begin(), irlength);
 		}
 	}
 	go_test_logic_reset();
@@ -170,6 +164,25 @@ int Jtag::detectChain(int max_dev)
 	return _devices_list.size();
 }
 
+bool Jtag::search_and_insert_device_with_idcode(uint32_t idcode)
+{
+	int irlength = -1;
+	auto dev = fpga_list.find(idcode);
+	if (dev != fpga_list.end())
+		irlength = dev->second.irlength;
+	if (irlength == -1) {
+		auto misc = misc_dev_list.find(idcode);
+		if (misc != misc_dev_list.end())
+			irlength = misc->second.irlength;
+	}
+	if (irlength == -1)
+		return false;
+
+	_devices_list.insert(_devices_list.begin(), idcode);
+	_irlength_list.insert(_irlength_list.begin(), irlength);
+	return true;
+}
+
 uint16_t Jtag::device_select(uint16_t index)
 {
 	if (index > (uint16_t) _devices_list.size())
diff --git a/src/jtag.hpp b/src/jtag.hpp
index 90bc7ef..469e929 100644
--- a/src/jtag.hpp
+++ b/src/jtag.hpp
@@ -98,6 +98,14 @@ class Jtag {
 	void init_internal(cable_t &cable, const std::string &dev, const std::string &serial,
 		const jtag_pins_conf_t *pin_conf, uint32_t clkHZ,
 		const std::string &firmware_path);
+	/*!
+	 * \brief search in fpga_list and misc_dev_list for a device with idcode
+	 *        if found insert idcode and irlength in _devices_list and
+	 *        _irlength_list
+	 * \param[in] idcode: device idcode
+	 * \return false if not found, true otherwise
+	 */
+	bool search_and_insert_device_with_idcode(uint32_t idcode);
 	int8_t _verbose;
 	int _state;
 	int _tms_buffer_size;