From aac9bd0866e5ed15a34bc1dbbd370a1d392eaa08 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?King=20K=C3=A9vin?= <kingkevin@cuvoodoo.info>
Date: Mon, 15 Mar 2021 14:00:22 +0100
Subject: [PATCH] application: scan all channels for SWD

---
 application.c | 85 ++++++++++++++++++++++++++++++++++++++-------------
 1 file changed, 63 insertions(+), 22 deletions(-)

diff --git a/application.c b/application.c
index 8e5d693..05f1c5b 100644
--- a/application.c
+++ b/application.c
@@ -223,31 +223,72 @@ static void mux_select(int8_t channel)
 static void command_swd_scan(void* argument)
 {
 	(void)argument; // we won't use the argument
-	printf("SWD target DPIDR: ");
-	uint32_t data; // data to read/write over SWD
-	enum swd_ack_e ack; // SWD acknowledge response
-	swd_line_reset(); // put target in reset state
-	swd_jtag_to_swd(); // put target SWJ in SWD mode
-	swd_line_reset(); // put target in reset state
-	swd_idle_cycles(2); // idle before packer request
-	swd_packet_request(false, SWD_A_DP_DPIDR, true); // request DPIDR
-	swd_turnaround(1); // switch from writing to reading
-	ack = swd_acknowledge_response(); // get ack
-	if (SWD_ACK_OK != ack) {
-		printf("ack error\n");
+
+	float* voltages = measure_voltages(); // measure voltages
+	if (voltages[1] < 0.5) { // check target voltage connection
+		puts("connect target voltage to test channel type\n");
 		return;
 	}
-	if (!swd_read(&data)) {
-		printf("parity error\n");
-		return;
-	}
-	swd_turnaround(1); // switch from reading to writing
-	printf("0x%08x ", data);
-	if (data & 0x1) {
-		printf("(designer: %03x/%s, version: %u, part number: 0x%02x/%s, revision %u)\n", (data >> 1) & 0x3ff, swd_jep106_manufacturer((data >> 8) & 0x0f, (data >> 1) & 0x7f), (data >> 12) & 0x0f, (data >> 20) & 0xff, swd_dpidr_partno((data >> 1) & 0x3ff, (data >> 20) & 0xff), (data >> 28) & 0x0f);
-	} else {
-		printf("(invalid: RAO != 1)\n");
+
+	mux_select(-1); // disable multiplexer
+	gpio_set(GPIO_PORT(SIGNAL_PU_PIN), GPIO_PIN(SIGNAL_PU_PIN)); // ensure we are not pulling up the signal
+	gpio_set(GPIO_PORT(SIGNAL_PD_PIN), GPIO_PIN(SIGNAL_PD_PIN)); // ensure we are not pulling down the signal
+	gpio_clear(GPIO_PORT(TARGET_EN), GPIO_PIN(TARGET_EN)); // power level shifter
+	sleep_us(100); // wait a tiny bit for the pull-up to be active
+
+	printf("searching SWD on channels (%u combinations): ", (channel_stop - channel_start + 1) * (channel_stop - channel_start));
+
+	mux_select(-1); // disable multiplexer
+	gpio_set(GPIO_PORT(SIGNAL_PU_PIN), GPIO_PIN(SIGNAL_PU_PIN)); // ensure we are not pulling up the signal
+	gpio_set(GPIO_PORT(SIGNAL_PD_PIN), GPIO_PIN(SIGNAL_PD_PIN)); // ensure we are not pulling down the signal
+	gpio_clear(GPIO_PORT(TARGET_EN), GPIO_PIN(TARGET_EN)); // power level shifter
+
+	uint8_t found = 0;
+	for (uint8_t swclk = channel_start; swclk <= channel_stop; swclk++) {
+		for (uint8_t swdio = channel_start; swdio <= channel_stop; swdio++) {
+			// skip when SWCLK and SWDIO share a same pin
+			if (swdio == swclk) {
+				continue;
+			}
+			// set SWCLK/SWDIO combination
+			if (!swd_set_pins(channel_ports[swclk], channel_pins[swclk], channel_ports[swdio], channel_pins[swdio])) {
+				putc('!');
+				continue;
+			}
+			// try enabling SWD
+			uint32_t data; // data to read/write over SWD
+			enum swd_ack_e ack; // SWD acknowledge response
+			swd_line_reset(); // put target in reset state
+			swd_jtag_to_swd(); // put target SWJ in SWD mode
+			swd_line_reset(); // put target in reset state
+			swd_idle_cycles(2); // idle before packet request
+			swd_packet_request(false, SWD_A_DP_DPIDR, true); // request DPIDR
+			swd_turnaround(1); // switch from writing to reading
+			ack = swd_acknowledge_response(); // get ack
+			if (SWD_ACK_OK != ack) { // no SWD on this combination
+				putc('.');
+				continue;
+			}
+			printf("\nSWD found: SWCLK=CH%02u SWDIO=CH%02u, ", swclk, swdio);
+			printf("SWD target DPIDR: ");
+			if (!swd_read(&data)) {
+				printf("parity error\n");
+				return;
+			}
+			swd_turnaround(1); // switch from reading to writing
+			printf("0x%08x ", data);
+			if (data & 0x1) {
+				printf("(designer: %03x/%s, version: %u, part number: 0x%02x/%s, revision %u)\n", (data >> 1) & 0x3ff, swd_jep106_manufacturer((data >> 8) & 0x0f, (data >> 1) & 0x7f), (data >> 12) & 0x0f, (data >> 20) & 0xff, swd_dpidr_partno((data >> 1) & 0x3ff, (data >> 20) & 0xff), (data >> 28) & 0x0f);
+			} else {
+				printf("(invalid: RAO != 1)\n");
+			}
+			swd_release_pins(); // release pins
+			found++; // remember we found an SWD combination
+		}
 	}
+
+	gpio_set(GPIO_PORT(TARGET_EN), GPIO_PIN(TARGET_EN)); // disable level shifters
+	printf("\n%u SWD interface(s) found\n", found);
 }
 
 static void command_voltages(void* argument)