diff --git a/application.c b/application.c
index 8a9d16d..941b9e9 100644
--- a/application.c
+++ b/application.c
@@ -302,6 +302,7 @@ static void command_swd_scan(void* argument)
 }
 
 #define JTAG_SPEED 50 /**< time in us between clock edges (i.e. setting the clock speed) */
+#define JTAG_PATTERN 0x0ff06699 /**< pattern to fin TDI pin */
 static int8_t jtag_tms_ch = -1; /**< channel used for JTAG TCK output (-1 = not configured) */
 static int8_t jtag_tck_ch = -1; /**< channel used for JTAG TMS output (-1 = not configured) */
 static int8_t jtag_tdi_ch = -1; /**< channel used for JTAG TMS output (-1 = not configured) */
@@ -374,9 +375,18 @@ static void command_jtag_scan(void* argument)
 	printf("searching JTAG on channels CH%02u-CH%02u\n", channel_start, channel_stop);
 
 	printf("searching for TDO using IDCODE scan on TCK/TMS (%u combinations): ", (channel_stop - channel_start + 1) * (channel_stop - channel_start));
-	//uint32_t tdo[CHANNEL_NUMBERS]; // the output bit stream for all channels
-	bool idcode[CHANNEL_NUMBERS]; // when an IDCODE has been found
-	//uint8_t found = 0; // number of JTAG ports found
+	uint8_t idcodes[CHANNEL_NUMBERS]; // how many IDCODEs have been found on channel
+	for (uint8_t i = 0; i < LENGTH(idcodes); i++) {
+		idcodes[i] = 0;
+	}
+	bool tck_ok[CHANNEL_NUMBERS]; // if channel is a possible TCK
+	for (uint8_t i = 0; i < LENGTH(tck_ok); i++) {
+		tck_ok[i] = false;
+	}
+	bool tms_ok[CHANNEL_NUMBERS]; // if channel is a possible TMS
+	for (uint8_t i = 0; i < LENGTH(tms_ok); i++) {
+		tms_ok[i] = false;
+	}
 	jtag_tdi_ch = -1; // we don't use TDI for now
 	for (uint8_t tck = channel_start; tck <= channel_stop; tck++) { // use channel as TCK output
 		for (uint8_t tms = channel_start; tms <= channel_stop; tms++) { // use channel as TMS output
@@ -387,14 +397,14 @@ static void command_jtag_scan(void* argument)
 			gpio_mode_setup(channel_ports[tck], GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, channel_pins[tck]); // set channel for TCK as output
 			jtag_tck_ch = tck; // remember which channel we use for TCK for the transaction
 			gpio_set(channel_ports[tms], channel_pins[tms]); // start high (to go to reset state)
-			gpio_mode_setup(channel_ports[tms], GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, channel_pins[tms]); // set channel for TMS back to input
+			gpio_mode_setup(channel_ports[tms], GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, channel_pins[tms]); // set channel for TMS as output
 			jtag_tms_ch = tms; // remember which channel we use for TMS for the transaction
 			jtag_transaction(0xffffffff, 0, 26); // ensure we are is reset state, even on SWD devices (needs 50 TMS hig);
 			jtag_transaction(0xffffffff, 0, 26); // continuation
 			jtag_transaction(0xE73C, 0, 16); // send sequence to switch any SWD device back to JTAG (this constant magic value)
 			// all other channel should already be inputs
 			jtag_transaction(0x3f | (0 << 6) | (1 << 7) | (0 << 8) | (0 << 9), 0, 6 + 1 + 1 + 1 + 1); // go back to JTAG TEST-LOGIC_RESET (5 bits should be enough to go from any state to RESET, but we a one just to be sure) -> RUN-TEST/IDLE -> SELECT-DR-SCAN -> CAPTURE-DR -> SHIFT-DR states
-			// initialize array
+			bool idcode[CHANNEL_NUMBERS]; // when a new IDCODE has been found
 			for (uint8_t i = 0; i < LENGTH(idcode); i++) {
 				idcode[i] = true;
 			}
@@ -414,9 +424,12 @@ static void command_jtag_scan(void* argument)
 					} else if (0 == (jtag_tdo[tdo] & 0x1)) { // RAO bit is wrong
 						continue;
 					} else { // IDCODE received
-						printf("\npossible IDCODE found: TCK=CH%02u TMS=CH%02u TDI=CH%02u IDCODE=%+08x (", tck, tms, tdo, jtag_tdo[tdo]); // show finding
+						printf("\nIDCODE found: TCK=CH%02u TMS=CH%02u TDO=CH%02u CHAIN=%u IDCODE=%+08x (", tck, tms, tdo, idcodes[tdo] + 1, jtag_tdo[tdo]); // show finding
 						print_idcode(jtag_tdo[tdo]);
 						puts(")");
+						idcodes[tdo]++; // count the number of IDCODEs found
+						tck_ok[tck] = true; // remember we found TCK on this channel
+						tms_ok[tms] = true; // remember we found TMS on this channel
 						idcode_found = true; // remember we found an IDCODE
 						idcode_scan = true; // continue scanning for the next code
 					}
@@ -434,6 +447,91 @@ static void command_jtag_scan(void* argument)
 		}
 	}
 	putc('\n'); // all combinations completed
+
+	// get max length of scan chain
+	uint8_t chain = 0;
+	for (uint8_t tdo = channel_start; tdo <= channel_stop; tdo++) {
+		if (idcodes[tdo] > chain) {
+			chain = idcodes[tdo];
+		}
+	}
+	if (0 == chain) {
+		puts("no IDCODE found\n");
+		return;
+	}
+
+	printf("searching for TDI using IDCODE feeding on TCK/TMS/TDO: ");
+	jtag_tdi_ch = -1; // we don't use TDI for now
+	for (uint8_t tck = channel_start; tck <= channel_stop; tck++) { // test channel as TCK output
+		if (!tck_ok[tck]) { // this is not one of the possibles TCK
+			continue;
+		}
+		for (uint8_t tms = channel_start; tms <= channel_stop; tms++) { // test channel as TMS output
+			if (tck == tms) { // don't use the same channel for TCK and TMS
+				continue;
+			}
+			if (!tms_ok[tms]) { // this is not one of the possible TMS
+				continue;
+			}
+			for (uint8_t tdi = channel_start; tdi <= channel_stop; tdi++) { // test channel as TDI
+				if (tck == tdi) { // don't use the same channel for TCK and TDI
+					continue;
+				}
+				if (tms == tdi) { // don't use the same channel for TMS and TDI
+					continue;
+				}
+				bool tdi_found = false; // if we found a TDI pin in this combination
+				gpio_set(channel_ports[tck], channel_pins[tck]); // clock is idle high
+				gpio_mode_setup(channel_ports[tck], GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, channel_pins[tck]); // set channel for TCK as output
+				jtag_tck_ch = tck; // remember which channel we use for TCK for the transaction
+				gpio_set(channel_ports[tms], channel_pins[tms]); // start high (to go to reset state)
+				gpio_mode_setup(channel_ports[tms], GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, channel_pins[tms]); // set channel for TMS as output
+				jtag_tms_ch = tms; // remember which channel we use for TMS for the transaction
+				gpio_set(channel_ports[tdi], channel_pins[tdi]); // start high (idle state)
+				gpio_mode_setup(channel_ports[tdi], GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, channel_pins[tdi]); // set channel for TMS back to input
+				jtag_tdi_ch = tdi; // remember which channel we use for TDI for the transaction
+				// all other channels are already inputs (to check TDO)
+				// switching from SWD to JTAG has already been done
+				jtag_transaction(0x3f | (0 << 6) | (1 << 7) | (0 << 8) | (0 << 9), 0, 6 + 1 + 1 + 1 + 1); // go to IDCODE state: back to JTAG TEST-LOGIC_RESET (5 bits should be enough to go from any state to RESET, but we a one just to be sure) -> RUN-TEST/IDLE -> SELECT-DR-SCAN -> CAPTURE-DR -> SHIFT-DR
+				for (uint8_t sequence = 0; sequence <= chain; sequence++) { // go through longest chain
+					jtag_transaction(0, JTAG_PATTERN, 32); // send pattern into chain
+					for (uint8_t tdo = channel_start; tdo <= channel_stop; tdo++) { // test channel as TDO
+						if (tck == tdo) { // don't use the same channel for TCK and TDO
+							continue;
+						}
+						if (tms == tdo) { // don't use the same channel for TMS and TDO
+							continue;
+						}
+						if (0 == idcodes[tdo]) { // we did not seen any IDCODE on this pin
+							continue;
+						}
+						if (sequence < idcodes[tdo]) { // we did not got through the chain yet, thus we don't expect the pattern
+							continue;
+						}
+						if (0 == jtag_tdo[tdo] || 0xffffffff == jtag_tdo[tdo]) { // we received nothing
+							continue;
+						}
+						if (JTAG_PATTERN == jtag_tdo[tdo]) { // we found out pattern
+							printf("\nJTAG found: TCK=CH%02u TMS=CH%02u TDO=CH%02u TDI=CH%02u CHAIN=%u", tck, tms, tdo, tdi, sequence);
+							tdi_found = true; // remember we found one and printed
+						}
+					}
+				}
+				if (tdi_found) {
+					putc('\n'); // continue dot pattern on new line
+				} else {
+					putc('.');
+				}
+				gpio_mode_setup(channel_ports[tck], GPIO_MODE_INPUT, GPIO_PUPD_NONE, channel_pins[tck]); // set channel for TCK back to input
+				jtag_tck_ch = -1; // clear channel configuration
+				gpio_mode_setup(channel_ports[tms], GPIO_MODE_INPUT, GPIO_PUPD_NONE, channel_pins[tms]); // set channel for TMS back to input
+				jtag_tms_ch = -1; // clear channel configuration
+				gpio_mode_setup(channel_ports[tdi], GPIO_MODE_INPUT, GPIO_PUPD_NONE, channel_pins[tdi]); // set channel for TDI back to input
+				jtag_tdi_ch = -1; // clear channel configuration
+			} // end test channel as TDI
+		} // end test channel as TMS
+	} // end test channel as TCK
+	putc('\n'); // all combinations completed
 }
 
 static void command_voltages(void* argument)