From d994565f2e95afe55a54ca51035a48b4bd8c8f92 Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Sat, 31 Aug 2019 09:57:32 +0200
Subject: [PATCH] pc-stlinkv2: Fix crash with serial given not matching
 connected devices.

---
 src/platforms/pc-stlinkv2/stlinkv2.c | 70 ++++++++++++++--------------
 1 file changed, 36 insertions(+), 34 deletions(-)

diff --git a/src/platforms/pc-stlinkv2/stlinkv2.c b/src/platforms/pc-stlinkv2/stlinkv2.c
index f3de39e4..be691a62 100644
--- a/src/platforms/pc-stlinkv2/stlinkv2.c
+++ b/src/platforms/pc-stlinkv2/stlinkv2.c
@@ -683,7 +683,7 @@ void stlink_init(int argc, char **argv)
 		goto error;
 	}
 	int i = 0;
-	bool multiple_devices = false;
+	int nr_stlinks = 0;
 	while ((dev = devs[i++]) != NULL) {
 		struct libusb_device_descriptor desc;
 		int r = libusb_get_device_descriptor(dev, &desc);
@@ -699,10 +699,6 @@ void stlink_init(int argc, char **argv)
 				DEBUG("STLINKV1 not supported\n");
 				continue;
 			}
-			if (Stlink.handle) {
-				libusb_close(Stlink.handle);
-				multiple_devices = (serial)? false : true;
-			}
 			r = libusb_open(dev, &Stlink.handle);
 			if (r == LIBUSB_SUCCESS) {
 				uint8_t data[32];
@@ -736,44 +732,50 @@ void stlink_init(int argc, char **argv)
 				}
 				if (serial && (!strncmp(Stlink.serial, serial, strlen(serial))))
 					DEBUG("Found ");
-				if (!serial || (!strncmp(Stlink.serial, serial, strlen(serial)))) {
-					if (desc.idProduct == PRODUCT_ID_STLINKV2) {
-						DEBUG("STLINKV20 serial %s\n", Stlink.serial);
-						Stlink.ver_hw = 20;
-						Stlink.ep_tx = 2;
-					} else if (desc.idProduct == PRODUCT_ID_STLINKV21) {
-						DEBUG("STLINKV21 serial %s\n", Stlink.serial);
-						Stlink.ver_hw = 21;
-						Stlink.ep_tx = 1;
-					} else if (desc.idProduct == PRODUCT_ID_STLINKV21_MSD) {
-						DEBUG("STLINKV21_MSD serial %s\n", Stlink.serial);
-						Stlink.ver_hw = 21;
-						Stlink.ep_tx = 1;
-					} else if (desc.idProduct == PRODUCT_ID_STLINKV3E) {
-						DEBUG("STLINKV3E serial %s\n", Stlink.serial);
-						Stlink.ver_hw = 30;
-						Stlink.ep_tx = 1;
-					} else if (desc.idProduct == PRODUCT_ID_STLINKV3) {
-						DEBUG("STLINKV3  serial %s\n", Stlink.serial);
-						Stlink.ver_hw = 30;
-						Stlink.ep_tx = 1;
+				if (desc.idProduct == PRODUCT_ID_STLINKV2) {
+					DEBUG("STLINKV20 serial %s\n", Stlink.serial);
+					Stlink.ver_hw = 20;
+					Stlink.ep_tx = 2;
+				} else if (desc.idProduct == PRODUCT_ID_STLINKV21) {
+					DEBUG("STLINKV21 serial %s\n", Stlink.serial);
+					Stlink.ver_hw = 21;
+					Stlink.ep_tx = 1;
+				} else if (desc.idProduct == PRODUCT_ID_STLINKV21_MSD) {
+					DEBUG("STLINKV21_MSD serial %s\n", Stlink.serial);
+					Stlink.ver_hw = 21;
+					Stlink.ep_tx = 1;
+				} else if (desc.idProduct == PRODUCT_ID_STLINKV3E) {
+					DEBUG("STLINKV3E serial %s\n", Stlink.serial);
+					Stlink.ver_hw = 30;
+					Stlink.ep_tx = 1;
+				} else if (desc.idProduct == PRODUCT_ID_STLINKV3) {
+					DEBUG("STLINKV3  serial %s\n", Stlink.serial);
+					Stlink.ver_hw = 30;
+					Stlink.ep_tx = 1;
+				} else {
+					DEBUG("Unknown STLINK variant, serial %s\n", Stlink.serial);
+				}
+				nr_stlinks++;
+				if (serial) {
+					if (!strncmp(Stlink.serial, serial, strlen(serial))) {
+						break;
 					} else {
-						DEBUG("Unknown STLINK variant, serial %s\n", Stlink.serial);
+						libusb_close(Stlink.handle);
+						Stlink.handle = 0;
 					}
 				}
-				if (serial && (!strncmp(Stlink.serial, serial, strlen(serial))))
-					break;
 			} else {
 				DEBUG("Open failed %s\n", libusb_strerror(r));
 			}
 		}
 	}
-	if (multiple_devices) {
-		DEBUG("Multiple Stlinks. Please specify serial number\n");
-		goto error_1;
-	}
 	if (!Stlink.handle) {
-		DEBUG("No Stlink device found!\n");
+		if (nr_stlinks && serial)
+			DEBUG("No Stlink with given serial number %s\n", serial);
+		else if (nr_stlinks > 1)
+			DEBUG("Multiple Stlinks. Please specify serial number\n");
+		else
+			DEBUG("No Stlink device found!\n");
 		goto error;
 	}
 	int config;