+static size_t stlink_probe_usb_devs(libusb_device **devs, stlink_t **sldevs[]) {
+ stlink_t **_sldevs;
+ libusb_device *dev;
+ int i = 0;
+ int ret = 0;
+ size_t slcnt = 0;
+ size_t slcur = 0;
+
+ /* Count stlink */
+ while ((dev = devs[i++]) != NULL) {
+ struct libusb_device_descriptor desc;
+ int r = libusb_get_device_descriptor(dev, &desc);
+ if (r < 0) {
+ WLOG("failed to get libusb device descriptor\n");
+ break;
+ }
+
+ if (desc.idProduct != USB_STLINK_32L_PID &&
+ desc.idProduct != USB_STLINK_NUCLEO_PID)
+ continue;
+
+ slcnt++;
+ }
+
+ /* Allocate list of pointers */
+ _sldevs = calloc(slcnt, sizeof(stlink_t *));
+ if (!_sldevs) {
+ *sldevs = NULL;
+ return 0;
+ }
+
+ /* Open stlinks and attach to list */
+ i = 0;
+ while ((dev = devs[i++]) != NULL) {
+ struct libusb_device_descriptor desc;
+ ret = libusb_get_device_descriptor(dev, &desc);
+ if (ret < 0) {
+ WLOG("failed to get libusb device descriptor\n");
+ break;
+ }
+
+ if (desc.idProduct != USB_STLINK_32L_PID &&
+ desc.idProduct != USB_STLINK_NUCLEO_PID)
+ continue;
+
+ struct libusb_device_handle* handle;
+ char serial[13];
+ memset(serial, 0, sizeof(serial));
+
+ ret = libusb_open(dev, &handle);
+ if (ret < 0) {
+ WLOG("failed to get libusb device descriptor\n");
+ break;
+ }
+
+ ret = libusb_get_string_descriptor_ascii(handle, desc.iSerialNumber, (unsigned char *)&serial, sizeof(serial));
+ if (ret < 0)
+ *serial = NULL;
+
+ libusb_close(handle);
+
+ stlink_t *sl = NULL;
+ sl = stlink_open_usb(0, 1, serial);
+ if (!sl)
+ continue;
+
+ _sldevs[slcur] = sl;
+ slcur++;
+ }
+
+ /* Something went wrong */
+ if (ret < 0) {
+ free(_sldevs);
+ *sldevs = NULL;
+ return 0;
+ }
+
+ *sldevs = _sldevs;
+ return slcnt;
+}
+
+size_t stlink_probe_usb(stlink_t **stdevs[]) {
+ libusb_device **devs;
+ stlink_t **sldevs;
+
+ size_t slcnt = 0;
+ int r;
+ ssize_t cnt;
+
+ r = libusb_init(NULL);
+ if (r < 0)
+ return 0;
+
+ cnt = libusb_get_device_list(NULL, &devs);
+ if (cnt < 0)
+ return 0;
+
+ slcnt = stlink_probe_usb_devs(devs, &sldevs);
+ libusb_free_device_list(devs, 1);
+
+ libusb_exit(NULL);
+
+ *stdevs = sldevs;
+ return slcnt;
+}
+
+void stlink_probe_usb_free(stlink_t ***stdevs, size_t size) {
+ if (stdevs == NULL || *stdevs == NULL || size == 0)
+ return;
+
+ for (size_t n = 0; n < size; n++)
+ stlink_close((*stdevs)[n]);
+ free(*stdevs);
+ *stdevs = NULL;
+}