git.gag.com Git - fw/stlink/blob - src/stlink-sg.c

   1 /*
   2  Copyright (c) 2010 "Capt'ns Missing Link" Authors. All rights reserved.
   3  Use of this source code is governed by a BSD-style
   4  license that can be found in the LICENSE file.
   5
   6  A linux stlink access demo. The purpose of this file is to mitigate the usual
   7  "reinventing the wheel" force by incompatible licenses and give you an idea,
   8  how to access the stlink device. That doesn't mean you should be a free-loader
   9  and not contribute your improvements to this code.
  10
  11  Author: Martin Capitanio <m@capitanio.org>
  12  The stlink related constants kindly provided by Oliver Spencer (OpenOCD)
  13  for use in a GPL compatible license.
  14
  15  Tested compatibility: linux, gcc >= 4.3.3
  16
  17  The communication is based on standard USB mass storage device
  18  BOT (Bulk Only Transfer)
  19  - Endpoint 1: BULK_IN, 64 bytes max
  20  - Endpoint 2: BULK_OUT, 64 bytes max
  21
  22  All CBW transfers are ordered with the LSB (byte 0) first (little endian).
  23  Any command must be answered before sending the next command.
  24  Each USB transfer must complete in less than 1s.
  25
  26  SB Device Class Definition for Mass Storage Devices:
  27  www.usb.org/developers/devclass_docs/usbmassbulk_10.pdf
  28
  29  dt             - Data Transfer (IN/OUT)
  30  CBW            - Command Block Wrapper
  31  CSW            - Command Status Wrapper
  32  RFU            - Reserved for Future Use
  33
  34  Originally, this driver used scsi pass through commands, which required the
  35  usb-storage module to be loaded, providing the /dev/sgX links.  The USB mass
  36  storage implementation on the STLinkv1 is however terribly broken, and it can
  37  take many minutes for the kernel to give up.
  38
  39  However, in Nov 2011, the scsi pass through was replaced by raw libusb, so
  40  instead of having to let usb-storage struggle with the device, and also greatly
  41  limiting the portability of the driver, you can now tell usb-storage to simply
  42  ignore this device completely.
  43
  44  usb-storage.quirks
  45  http://git.kernel.org/?p=linux/kernel/git/torvalds/linux-2.6.git;a=blob_plain;f=Documentation/kernel-parameters.txt
  46  Each entry has the form VID:PID:Flags where VID and PID are Vendor and Product
  47  ID values (4-digit hex numbers) and Flags is a set of characters, each corresponding
  48  to a common usb-storage quirk flag as follows:
  49
  50  a = SANE_SENSE (collect more than 18 bytes of sense data);
  51  b = BAD_SENSE (don't collect more than 18 bytes of sense data);
  52  c = FIX_CAPACITY (decrease the reported device capacity by one sector);
  53  h = CAPACITY_HEURISTICS (decrease the reported device capacity by one sector if the number is odd);
  54  i = IGNORE_DEVICE (don't bind to this device);
  55  l = NOT_LOCKABLE (don't try to lock and unlock ejectable media);
  56  m = MAX_SECTORS_64 (don't transfer more than 64 sectors = 32 KB at a time);
  57  o = CAPACITY_OK (accept the capacity reported by the device);
  58  r = IGNORE_RESIDUE (the device reports bogus residue values);
  59  s = SINGLE_LUN (the device has only one Logical Unit);
  60  w = NO_WP_DETECT (don't test whether the medium is write-protected).
  61
  62  Example: quirks=0419:aaf5:rl,0421:0433:rc
  63  http://permalink.gmane.org/gmane.linux.usb.general/35053
  64
  65  For the stlinkv1, you just want the following
  66
  67  modprobe -r usb-storage && modprobe usb-storage quirks=483:3744:i
  68
  69  Equivalently, you can add a line saying
  70
  71  options usb-storage quirks=483:3744:i
  72
  73  to your /etc/modprobe.conf or /etc/modprobe.d/local.conf (or add the "quirks=..."
  74  part to an existing options line for usb-storage).
  75  */
  76
  77
  78 #define __USE_GNU
  79 #include <assert.h>
  80 #include <stdio.h>
  81 #include <string.h>
  82 #include <stdarg.h>
  83 #include <stdlib.h>
  84 #include <unistd.h>
  85 #include <fcntl.h>
  86 #include <sys/types.h>
  87 #include <sys/stat.h>
  88 #include <sys/mman.h>
  89
  90 #include "stlink-common.h"
  91 #include "stlink-sg.h"
  92 #include "uglylogging.h"
  93
  94 #define LOG_TAG __FILE__
  95 #define DLOG(format, args...)         ugly_log(UDEBUG, LOG_TAG, format, ## args)
  96 #define ILOG(format, args...)         ugly_log(UINFO, LOG_TAG, format, ## args)
  97 #define WLOG(format, args...)         ugly_log(UWARN, LOG_TAG, format, ## args)
  98 #define fatal(format, args...)        ugly_log(UFATAL, LOG_TAG, format, ## args)
  99
 100 // Suspends execution of the calling process for
 101 // (at least) ms milliseconds.
 102
 103 static void delay(int ms) {
 104     //fprintf(stderr, "*** wait %d ms\n", ms);
 105     usleep(1000 * ms);
 106 }
 107
 108 static void clear_cdb(struct stlink_libsg *sl) {
 109     for (size_t i = 0; i < sizeof (sl->cdb_cmd_blk); i++)
 110         sl->cdb_cmd_blk[i] = 0;
 111     // set default
 112     sl->cdb_cmd_blk[0] = STLINK_DEBUG_COMMAND;
 113     sl->q_data_dir = Q_DATA_IN;
 114 }
 115
 116 // close the device, free the allocated memory
 117
 118 void _stlink_sg_close(stlink_t *sl) {
 119     if (sl) {
 120 #if FINISHED_WITH_SG
 121         struct stlink_libsg *slsg = sl->backend_data;
 122         scsi_pt_close_device(slsg->sg_fd);
 123         free(slsg);
 124 #endif
 125     }
 126 }
 127
 128 static int get_usb_mass_storage_status(libusb_device_handle *handle, uint8_t endpoint, uint32_t *tag)
 129 {
 130     unsigned char csw[13];
 131     memset(csw, 0, sizeof(csw));
 132     int transferred;
 133     int ret;
 134     int try = 0;
 135     do {
 136         ret = libusb_bulk_transfer(handle, endpoint, (unsigned char *)&csw, sizeof(csw),
 137                                    &transferred, SG_TIMEOUT_MSEC);
 138         if (ret == LIBUSB_ERROR_PIPE) {
 139             libusb_clear_halt(handle, endpoint);
 140         }
 141         try++;
 142     } while ((ret == LIBUSB_ERROR_PIPE) && (try < 3));
 143     if (ret != LIBUSB_SUCCESS) {
 144         fprintf(stderr, "%s: receiving failed: %d\n", __func__, ret);
 145         return -1;
 146     }
 147     if (transferred != sizeof(csw)) {
 148         fprintf(stderr, "%s: received unexpected amount: %d\n", __func__, transferred);
 149         return -1;
 150     }
 151
 152     uint32_t rsig = read_uint32(csw, 0);
 153     uint32_t rtag = read_uint32(csw, 4);
 154     uint32_t residue = read_uint32(csw, 8);
 155 #define USB_CSW_SIGNATURE 0x53425355  // 'U' 'S' 'B' 'S' (reversed)
 156     if (rsig != USB_CSW_SIGNATURE) {
 157         WLOG("status signature was invalid: %#x\n", rsig);
 158         return -1;
 159     }
 160     DLOG("residue was= %#x\n", residue);
 161     *tag = rtag;
 162     uint8_t rstatus = csw[12];
 163     DLOG("rstatus = %x\n", rstatus);
 164     return rstatus;
 165 }
 166
 167 static int dump_CDB_command(uint8_t *cdb, uint8_t cdb_len) {
 168     char dbugblah[100];
 169     char *dbugp = dbugblah;
 170     dbugp += sprintf(dbugp, "Sending CDB [");
 171     for (uint8_t i = 0; i < cdb_len; i++) {
 172         dbugp += sprintf(dbugp, " %#02x", (unsigned int) cdb[i]);
 173     }
 174     sprintf(dbugp, "]\n");
 175     DLOG(dbugblah);
 176     return 0;
 177 }
 178
 179 /**
 180  * Wraps a CDB mass storage command in the appropriate gunk to get it down
 181  * @param handle
 182  * @param endpoint
 183  * @param cdb
 184  * @param cdb_length
 185  * @param lun
 186  * @param flags
 187  * @param expected_rx_size
 188  * @return
 189  */
 190 int send_usb_mass_storage_command(libusb_device_handle *handle, uint8_t endpoint_out,
 191                               uint8_t *cdb, uint8_t cdb_length,
 192                               uint8_t lun, uint8_t flags, uint32_t expected_rx_size) {
 193     DLOG("Sending usb m-s cmd: cdblen:%d, rxsize=%d\n", cdb_length, expected_rx_size);
 194     dump_CDB_command(cdb, cdb_length);
 195
 196     static uint32_t tag;
 197     if (tag == 0) {
 198         tag = 1;
 199     }
 200
 201     int try = 0;
 202     int ret = 0;
 203     int real_transferred;
 204     int i = 0;
 205
 206     uint8_t c_buf[STLINK_SG_SIZE];
 207     // tag is allegedly ignored... TODO - verify
 208     c_buf[i++] = 'U';
 209     c_buf[i++] = 'S';
 210     c_buf[i++] = 'B';
 211     c_buf[i++] = 'C';
 212     write_uint32(&c_buf[i], tag);
 213     uint32_t this_tag = tag++;
 214     write_uint32(&c_buf[i+4], expected_rx_size);
 215     i+= 8;
 216     c_buf[i++] = flags;
 217     c_buf[i++] = lun;
 218
 219     c_buf[i++] = cdb_length;
 220
 221     // Now the actual CDB request
 222     assert(cdb_length <= CDB_SL);
 223     memcpy(&(c_buf[i]), cdb, cdb_length);
 224
 225     int sending_length = STLINK_SG_SIZE;
 226     DLOG("sending length set to: %d\n", sending_length);
 227
 228     // send....
 229     do {
 230         DLOG("attempting tx...\n");
 231         ret = libusb_bulk_transfer(handle, endpoint_out, c_buf, sending_length,
 232                                    &real_transferred, SG_TIMEOUT_MSEC);
 233         if (ret == LIBUSB_ERROR_PIPE) {
 234             libusb_clear_halt(handle, endpoint_out);
 235         }
 236         try++;
 237     } while ((ret == LIBUSB_ERROR_PIPE) && (try < 3));
 238     if (ret != LIBUSB_SUCCESS) {
 239         WLOG("sending failed: %d\n", ret);
 240         return -1;
 241     }
 242     DLOG("Actually sent: %d, returning tag: %d\n", real_transferred, tag);
 243     return this_tag;
 244 }
 245
 246
 247 /**
 248  * Straight from stm8 stlink code...
 249  * @param handle
 250  * @param endpoint_in
 251  * @param endpoint_out
 252  */
 253 static void
 254 get_sense(libusb_device_handle *handle, uint8_t endpoint_in, uint8_t endpoint_out)
 255 {
 256     DLOG("Fetching sense...\n");
 257     uint8_t cdb[16];
 258     memset(cdb, 0, sizeof(cdb));
 259 #define REQUEST_SENSE 0x03
 260 #define REQUEST_SENSE_LENGTH 18
 261     cdb[0] = REQUEST_SENSE;
 262     cdb[4] = REQUEST_SENSE_LENGTH;
 263     uint32_t tag = send_usb_mass_storage_command(handle, endpoint_out, cdb, sizeof(cdb), 0,
 264                                                  LIBUSB_ENDPOINT_IN, REQUEST_SENSE_LENGTH);
 265     if (tag == 0) {
 266         WLOG("refusing to send request sense with tag 0\n");
 267         return;
 268     }
 269     unsigned char sense[REQUEST_SENSE_LENGTH];
 270     int transferred;
 271     int ret;
 272     int try = 0;
 273     do {
 274         ret = libusb_bulk_transfer(handle, endpoint_in, sense, sizeof(sense),
 275                                    &transferred, SG_TIMEOUT_MSEC);
 276         if (ret == LIBUSB_ERROR_PIPE) {
 277             libusb_clear_halt(handle, endpoint_in);
 278         }
 279         try++;
 280     } while ((ret == LIBUSB_ERROR_PIPE) && (try < 3));
 281     if (ret != LIBUSB_SUCCESS) {
 282         WLOG("receiving sense failed: %d\n", ret);
 283         return;
 284     }
 285     if (transferred != sizeof(sense)) {
 286         WLOG("received unexpected amount of sense: %d != %d\n", transferred, sizeof(sense));
 287     }
 288     uint32_t received_tag;
 289     int status = get_usb_mass_storage_status(handle, endpoint_in, &received_tag);
 290     if (status != 0) {
 291         WLOG("receiving sense failed with status: %02x\n", status);
 292         return;
 293     }
 294     if (sense[0] != 0x70 && sense[0] != 0x71) {
 295         WLOG("No sense data\n");
 296     } else {
 297         WLOG("Sense KCQ: %02X %02X %02X\n", sense[2] & 0x0f, sense[12], sense[13]);
 298     }
 299 }
 300
 301
 302 //TODO rewrite/cleanup, save the error in sl
 303
 304 #if FINISHED_WITH_SG
 305 static void stlink_confirm_inq(stlink_t *stl, struct sg_pt_base *ptvp) {
 306     struct stlink_libsg *sl = stl->backend_data;
 307     const int e = sl->do_scsi_pt_err;
 308     if (e < 0) {
 309         fprintf(stderr, "scsi_pt error: pass through os error: %s\n",
 310                 safe_strerror(-e));
 311         return;
 312     } else if (e == SCSI_PT_DO_BAD_PARAMS) {
 313         fprintf(stderr, "scsi_pt error: bad pass through setup\n");
 314         return;
 315     } else if (e == SCSI_PT_DO_TIMEOUT) {
 316         fprintf(stderr, "  pass through timeout\n");
 317         return;
 318     }
 319     const int duration = get_scsi_pt_duration_ms(ptvp);
 320     if ((stl->verbose > 1) && (duration >= 0))
 321         DLOG("      duration=%d ms\n", duration);
 322
 323     // XXX stlink fw sends broken residue, so ignore it and use the known q_len
 324     // "usb-storage quirks=483:3744:r"
 325     // forces residue to be ignored and calculated, but this causes aboard if
 326     // data_len = 0 and by some other data_len values.
 327
 328     const int resid = get_scsi_pt_resid(ptvp);
 329     const int dsize = stl->q_len - resid;
 330
 331     const int cat = get_scsi_pt_result_category(ptvp);
 332     char buf[512];
 333     unsigned int slen;
 334
 335     switch (cat) {
 336         case SCSI_PT_RESULT_GOOD:
 337             if (stl->verbose && (resid > 0))
 338                 DLOG("      notice: requested %d bytes but "
 339                     "got %d bytes, ignore [broken] residue = %d\n",
 340                     stl->q_len, dsize, resid);
 341             break;
 342         case SCSI_PT_RESULT_STATUS:
 343             if (stl->verbose) {
 344                 sg_get_scsi_status_str(
 345                         get_scsi_pt_status_response(ptvp), sizeof (buf),
 346                         buf);
 347                 DLOG("  scsi status: %s\n", buf);
 348             }
 349             return;
 350         case SCSI_PT_RESULT_SENSE:
 351             slen = get_scsi_pt_sense_len(ptvp);
 352             if (stl->verbose) {
 353                 sg_get_sense_str("", sl->sense_buf, slen, (stl->verbose
 354                         > 1), sizeof (buf), buf);
 355                 DLOG("%s", buf);
 356             }
 357             if (stl->verbose && (resid > 0)) {
 358                 if ((stl->verbose) || (stl->q_len > 0))
 359                     DLOG("    requested %d bytes but "
 360                         "got %d bytes\n", stl->q_len, dsize);
 361             }
 362             return;
 363         case SCSI_PT_RESULT_TRANSPORT_ERR:
 364             if (stl->verbose) {
 365                 get_scsi_pt_transport_err_str(ptvp, sizeof (buf), buf);
 366                 // http://tldp.org/HOWTO/SCSI-Generic-HOWTO/x291.html
 367                 // These codes potentially come from the firmware on a host adapter
 368                 // or from one of several hosts that an adapter driver controls.
 369                 // The 'host_status' field has the following values:
 370                 //      [0x07] Internal error detected in the host adapter.
 371                 // This may not be fatal (and the command may have succeeded).
 372                 DLOG("  transport: %s", buf);
 373             }
 374             return;
 375         case SCSI_PT_RESULT_OS_ERR:
 376             if (stl->verbose) {
 377                 get_scsi_pt_os_err_str(ptvp, sizeof (buf), buf);
 378                 DLOG("  os: %s", buf);
 379             }
 380             return;
 381         default:
 382             fprintf(stderr, "  unknown pass through result "
 383                     "category (%d)\n", cat);
 384     }
 385 }
 386 #endif
 387
 388 /**
 389  * Just send a buffer on an endpoint, no questions asked.
 390  * Handles repeats, and time outs.  Also handles reading status reports and sense
 391  * @param handle libusb device *
 392  * @param endpoint_out sends
 393  * @param endpoint_in used to read status reports back in
 394  * @param cbuf  what to send
 395  * @param length how much to send
 396  * @return number of bytes actually sent, or -1 for failures.
 397  */
 398 int send_usb_data_only(libusb_device_handle *handle, unsigned char endpoint_out,
 399     unsigned char endpoint_in, unsigned char *cbuf, unsigned int length) {
 400     int ret;
 401     int real_transferred;
 402     int try;
 403     do {
 404         DLOG("attempting tx...\n");
 405         ret = libusb_bulk_transfer(handle, endpoint_out, cbuf, length,
 406                                    &real_transferred, SG_TIMEOUT_MSEC);
 407         if (ret == LIBUSB_ERROR_PIPE) {
 408             libusb_clear_halt(handle, endpoint_out);
 409         }
 410         try++;
 411     } while ((ret == LIBUSB_ERROR_PIPE) && (try < 3));
 412     if (ret != LIBUSB_SUCCESS) {
 413         WLOG("sending failed: %d\n", ret);
 414         return -1;
 415     }
 416     DLOG("Actually sent: %d\n", real_transferred);
 417
 418     // now, swallow up the status, so that things behave nicely...
 419     uint32_t received_tag;
 420     // -ve is for my errors, 0 is good, +ve is libusb sense status bytes
 421     int status = get_usb_mass_storage_status(handle, endpoint_in, &received_tag);
 422     if (status < 0) {
 423         WLOG("receiving status failed: %d\n", status);
 424         return -1;
 425     }
 426     if (status != 0) {
 427         WLOG("receiving status not passed :(: %02x\n", status);
 428     }
 429     if (status == 1) {
 430         get_sense(handle, endpoint_in, endpoint_out);
 431         return -1;
 432     }
 433
 434     return real_transferred;
 435 }
 436
 437
 438 int stlink_q(stlink_t *sl) {
 439     struct stlink_libsg* sg = sl->backend_data;
 440     //uint8_t cdb_len = 6;  // FIXME varies!!!
 441     uint8_t cdb_len = 10;  // FIXME varies!!!
 442     uint8_t lun = 0;  // always zero...
 443     uint32_t tag = send_usb_mass_storage_command(sg->usb_handle, sg->ep_req,
 444         sg->cdb_cmd_blk, cdb_len, lun, LIBUSB_ENDPOINT_IN, sl->q_len);
 445
 446
 447     // now wait for our response...
 448     // length copied from stlink-usb...
 449     int rx_length = sl->q_len;
 450     int try = 0;
 451     int real_transferred;
 452     int ret;
 453     if (rx_length > 0) {
 454         do {
 455             DLOG("attempting rx\n");
 456             ret = libusb_bulk_transfer(sg->usb_handle, sg->ep_rep, sl->q_buf, rx_length,
 457                 &real_transferred, SG_TIMEOUT_MSEC);
 458             if (ret == LIBUSB_ERROR_PIPE) {
 459                 libusb_clear_halt(sg->usb_handle, sg->ep_req);
 460             }
 461             try++;
 462         } while ((ret == LIBUSB_ERROR_PIPE) && (try < 3));
 463
 464         if (ret != LIBUSB_SUCCESS) {
 465             WLOG("Receiving failed: %d\n", ret);
 466             return -1;
 467         }
 468
 469         if (real_transferred != rx_length) {
 470             WLOG("received unexpected amount: %d != %d\n", real_transferred, rx_length);
 471         }
 472     }
 473
 474     uint32_t received_tag;
 475     // -ve is for my errors, 0 is good, +ve is libusb sense status bytes
 476     int status = get_usb_mass_storage_status(sg->usb_handle, sg->ep_rep, &received_tag);
 477     if (status < 0) {
 478         WLOG("receiving status failed: %d\n", status);
 479         return -1;
 480     }
 481     if (status != 0) {
 482         WLOG("receiving status not passed :(: %02x\n", status);
 483     }
 484     if (status == 1) {
 485         get_sense(sg->usb_handle, sg->ep_rep, sg->ep_req);
 486         return -1;
 487     }
 488     if (received_tag != tag) {
 489         WLOG("received tag %d but expected %d\n", received_tag, tag);
 490         //return -1;
 491     }
 492     if (rx_length > 0 && real_transferred != rx_length) {
 493         return -1;
 494     }
 495     return 0;
 496
 497
 498     DLOG("Actually received: %d\n", real_transferred);
 499
 500 #if FINISHED_WITH_SG
 501     // Get control command descriptor of scsi structure,
 502     // (one object per command!!)
 503     struct sg_pt_base *ptvp = construct_scsi_pt_obj();
 504     if (NULL == ptvp) {
 505         fprintf(stderr, "construct_scsi_pt_obj: out of memory\n");
 506         return;
 507     }
 508
 509     set_scsi_pt_cdb(ptvp, sg->cdb_cmd_blk, sizeof (sg->cdb_cmd_blk));
 510
 511     // set buffer for sense (error information) data
 512     set_scsi_pt_sense(ptvp, sg->sense_buf, sizeof (sg->sense_buf));
 513
 514     // Set a buffer to be used for data transferred from device
 515     if (sg->q_data_dir == Q_DATA_IN) {
 516         //clear_buf(sl);
 517         set_scsi_pt_data_in(ptvp, sl->q_buf, sl->q_len);
 518     } else {
 519         set_scsi_pt_data_out(ptvp, sl->q_buf, sl->q_len);
 520     }
 521     // Executes SCSI command (or at least forwards it to lower layers).
 522     sg->do_scsi_pt_err = do_scsi_pt(ptvp, sg->sg_fd, SG_TIMEOUT_SEC,
 523             sl->verbose);
 524
 525     // check for scsi errors
 526     stlink_confirm_inq(sl, ptvp);
 527     // TODO recycle: clear_scsi_pt_obj(struct sg_pt_base * objp);
 528     destruct_scsi_pt_obj(ptvp);
 529 #endif
 530 }
 531
 532 // TODO thinking, cleanup
 533
 534 void stlink_stat(stlink_t *stl, char *txt) {
 535     if (stl->q_len <= 0)
 536         return;
 537
 538     stlink_print_data(stl);
 539
 540     switch (stl->q_buf[0]) {
 541         case STLINK_OK:
 542             DLOG("  %s: ok\n", txt);
 543             return;
 544         case STLINK_FALSE:
 545             DLOG("  %s: false\n", txt);
 546             return;
 547         default:
 548             DLOG("  %s: unknown\n", txt);
 549     }
 550 }
 551
 552
 553 void _stlink_sg_version(stlink_t *stl) {
 554     struct stlink_libsg *sl = stl->backend_data;
 555     DLOG("\n*** stlink_version ***\n");
 556     clear_cdb(sl);
 557     sl->cdb_cmd_blk[0] = STLINK_GET_VERSION;
 558     stl->q_len = 6;
 559     sl->q_addr = 0;
 560     stlink_q(stl);
 561     // HACK use my own private version right now...
 562
 563 }
 564
 565 // Get stlink mode:
 566 // STLINK_DEV_DFU_MODE || STLINK_DEV_MASS_MODE || STLINK_DEV_DEBUG_MODE
 567 // usb dfu             || usb mass             || jtag or swd
 568
 569 int _stlink_sg_current_mode(stlink_t *stl) {
 570     struct stlink_libsg *sl = stl->backend_data;
 571     clear_cdb(sl);
 572     sl->cdb_cmd_blk[0] = STLINK_GET_CURRENT_MODE;
 573     stl->q_len = 2;
 574     sl->q_addr = 0;
 575     stlink_q(stl);
 576     return stl->q_buf[0];
 577 }
 578
 579 // Exit the mass mode and enter the swd debug mode.
 580
 581 void _stlink_sg_enter_swd_mode(stlink_t *sl) {
 582     struct stlink_libsg *sg = sl->backend_data;
 583     clear_cdb(sg);
 584     sg->cdb_cmd_blk[1] = STLINK_DEBUG_ENTER;
 585     sg->cdb_cmd_blk[2] = STLINK_DEBUG_ENTER_SWD;
 586     sl->q_len = 0; // >0 -> aboard
 587     stlink_q(sl);
 588 }
 589
 590 // Exit the mass mode and enter the jtag debug mode.
 591 // (jtag is disabled in the discovery's stlink firmware)
 592
 593 void _stlink_sg_enter_jtag_mode(stlink_t *sl) {
 594     struct stlink_libsg *sg = sl->backend_data;
 595     DLOG("\n*** stlink_enter_jtag_mode ***\n");
 596     clear_cdb(sg);
 597     sg->cdb_cmd_blk[1] = STLINK_DEBUG_ENTER;
 598     sg->cdb_cmd_blk[2] = STLINK_DEBUG_ENTER_JTAG;
 599     sl->q_len = 0;
 600     stlink_q(sl);
 601 }
 602
 603 // XXX kernel driver performs reset, the device temporally disappears
 604
 605 void _stlink_sg_exit_dfu_mode(stlink_t *sl) {
 606     struct stlink_libsg *sg = sl->backend_data;
 607     DLOG("\n*** stlink_exit_dfu_mode ***\n");
 608     clear_cdb(sg);
 609     sg->cdb_cmd_blk[0] = STLINK_DFU_COMMAND;
 610     sg->cdb_cmd_blk[1] = STLINK_DFU_EXIT;
 611     sl->q_len = 0; // ??
 612     stlink_q(sl);
 613     /*
 614      [135121.844564] sd 19:0:0:0: [sdb] Unhandled error code
 615      [135121.844569] sd 19:0:0:0: [sdb] Result: hostbyte=DID_ERROR driverbyte=DRIVER_OK
 616      [135121.844574] sd 19:0:0:0: [sdb] CDB: Read(10): 28 00 00 00 10 00 00 00 08 00
 617      [135121.844584] end_request: I/O error, dev sdb, sector 4096
 618      [135121.844590] Buffer I/O error on device sdb, logical block 512
 619      [135130.122567] usb 6-1: reset full speed USB device using uhci_hcd and address 7
 620      [135130.274551] usb 6-1: device firmware changed
 621      [135130.274618] usb 6-1: USB disconnect, address 7
 622      [135130.275186] VFS: busy inodes on changed media or resized disk sdb
 623      [135130.275424] VFS: busy inodes on changed media or resized disk sdb
 624      [135130.286758] VFS: busy inodes on changed media or resized disk sdb
 625      [135130.292796] VFS: busy inodes on changed media or resized disk sdb
 626      [135130.301481] VFS: busy inodes on changed media or resized disk sdb
 627      [135130.304316] VFS: busy inodes on changed media or resized disk sdb
 628      [135130.431113] usb 6-1: new full speed USB device using uhci_hcd and address 8
 629      [135130.629444] usb-storage 6-1:1.0: Quirks match for vid 0483 pid 3744: 102a1
 630      [135130.629492] scsi20 : usb-storage 6-1:1.0
 631      [135131.625600] scsi 20:0:0:0: Direct-Access     STM32                          PQ: 0 ANSI: 0
 632      [135131.627010] sd 20:0:0:0: Attached scsi generic sg2 type 0
 633      [135131.633603] sd 20:0:0:0: [sdb] 64000 512-byte logical blocks: (32.7 MB/31.2 MiB)
 634      [135131.633613] sd 20:0:0:0: [sdb] Assuming Write Enabled
 635      [135131.633620] sd 20:0:0:0: [sdb] Assuming drive cache: write through
 636      [135131.640584] sd 20:0:0:0: [sdb] Assuming Write Enabled
 637      [135131.640592] sd 20:0:0:0: [sdb] Assuming drive cache: write through
 638      [135131.640609]  sdb:
 639      [135131.652634] sd 20:0:0:0: [sdb] Assuming Write Enabled
 640      [135131.652639] sd 20:0:0:0: [sdb] Assuming drive cache: write through
 641      [135131.652645] sd 20:0:0:0: [sdb] Attached SCSI removable disk
 642      [135131.671536] sd 20:0:0:0: [sdb] Result: hostbyte=DID_OK driverbyte=DRIVER_SENSE
 643      [135131.671548] sd 20:0:0:0: [sdb] Sense Key : Illegal Request [current]
 644      [135131.671553] sd 20:0:0:0: [sdb] Add. Sense: Logical block address out of range
 645      [135131.671560] sd 20:0:0:0: [sdb] CDB: Read(10): 28 00 00 00 f9 80 00 00 08 00
 646      [135131.671570] end_request: I/O error, dev sdb, sector 63872
 647      [135131.671575] Buffer I/O error on device sdb, logical block 7984
 648      [135131.678527] sd 20:0:0:0: [sdb] Result: hostbyte=DID_OK driverbyte=DRIVER_SENSE
 649      [135131.678532] sd 20:0:0:0: [sdb] Sense Key : Illegal Request [current]
 650      [135131.678537] sd 20:0:0:0: [sdb] Add. Sense: Logical block address out of range
 651      [135131.678542] sd 20:0:0:0: [sdb] CDB: Read(10): 28 00 00 00 f9 80 00 00 08 00
 652      [135131.678551] end_request: I/O error, dev sdb, sector 63872
 653      ...
 654      [135131.853565] end_request: I/O error, dev sdb, sector 4096
 655      */
 656 }
 657
 658 void _stlink_sg_core_id(stlink_t *sl) {
 659     struct stlink_libsg *sg = sl->backend_data;
 660     clear_cdb(sg);
 661     sg->cdb_cmd_blk[1] = STLINK_DEBUG_READCOREID;
 662     sl->q_len = 4;
 663     sg->q_addr = 0;
 664     stlink_q(sl);
 665     sl->core_id = read_uint32(sl->q_buf, 0);
 666 }
 667
 668 // Arm-core reset -> halted state.
 669
 670 void _stlink_sg_reset(stlink_t *sl) {
 671     struct stlink_libsg *sg = sl->backend_data;
 672     clear_cdb(sg);
 673     sg->cdb_cmd_blk[1] = STLINK_DEBUG_RESETSYS;
 674     sl->q_len = 2;
 675     sg->q_addr = 0;
 676     stlink_q(sl);
 677     stlink_stat(sl, "core reset");
 678 }
 679
 680 // Arm-core status: halted or running.
 681
 682 void _stlink_sg_status(stlink_t *sl) {
 683     struct stlink_libsg *sg = sl->backend_data;
 684     DLOG("\n*** stlink_status ***\n");
 685     clear_cdb(sg);
 686     sg->cdb_cmd_blk[1] = STLINK_DEBUG_GETSTATUS;
 687     sl->q_len = 2;
 688     sg->q_addr = 0;
 689     stlink_q(sl);
 690 }
 691
 692 // Force the core into the debug mode -> halted state.
 693
 694 void _stlink_sg_force_debug(stlink_t *sl) {
 695     struct stlink_libsg *sg = sl->backend_data;
 696     DLOG("\n*** stlink_force_debug ***\n");
 697     clear_cdb(sg);
 698     sg->cdb_cmd_blk[1] = STLINK_DEBUG_FORCEDEBUG;
 699     sl->q_len = 2;
 700     sg->q_addr = 0;
 701     stlink_q(sl);
 702     stlink_stat(sl, "force debug");
 703 }
 704
 705 // Read all arm-core registers.
 706
 707 void _stlink_sg_read_all_regs(stlink_t *sl, reg *regp) {
 708     struct stlink_libsg *sg = sl->backend_data;
 709
 710     clear_cdb(sg);
 711     sg->cdb_cmd_blk[1] = STLINK_DEBUG_READALLREGS;
 712     sl->q_len = 84;
 713     sg->q_addr = 0;
 714     stlink_q(sl);
 715     stlink_print_data(sl);
 716
 717     // TODO - most of this should be re-extracted up....
 718
 719     // 0-3 | 4-7 | ... | 60-63 | 64-67 | 68-71   | 72-75      | 76-79 | 80-83
 720     // r0  | r1  | ... | r15   | xpsr  | main_sp | process_sp | rw    | rw2
 721     for (int i = 0; i < 16; i++) {
 722         regp->r[i] = read_uint32(sl->q_buf, 4 * i);
 723         if (sl->verbose > 1)
 724             DLOG("r%2d = 0x%08x\n", i, regp->r[i]);
 725     }
 726     regp->xpsr = read_uint32(sl->q_buf, 64);
 727     regp->main_sp = read_uint32(sl->q_buf, 68);
 728     regp->process_sp = read_uint32(sl->q_buf, 72);
 729     regp->rw = read_uint32(sl->q_buf, 76);
 730     regp->rw2 = read_uint32(sl->q_buf, 80);
 731     if (sl->verbose < 2)
 732         return;
 733
 734     DLOG("xpsr       = 0x%08x\n", regp->xpsr);
 735     DLOG("main_sp    = 0x%08x\n", regp->main_sp);
 736     DLOG("process_sp = 0x%08x\n", regp->process_sp);
 737     DLOG("rw         = 0x%08x\n", regp->rw);
 738     DLOG("rw2        = 0x%08x\n", regp->rw2);
 739 }
 740
 741 // Read an arm-core register, the index must be in the range 0..20.
 742 //  0  |  1  | ... |  15   |  16   |   17    |   18       |  19   |  20
 743 // r0  | r1  | ... | r15   | xpsr  | main_sp | process_sp | rw    | rw2
 744
 745 void _stlink_sg_read_reg(stlink_t *sl, int r_idx, reg *regp) {
 746     struct stlink_libsg *sg = sl->backend_data;
 747     clear_cdb(sg);
 748     sg->cdb_cmd_blk[1] = STLINK_DEBUG_READREG;
 749     sg->cdb_cmd_blk[2] = r_idx;
 750     sl->q_len = 4;
 751     sg->q_addr = 0;
 752     stlink_q(sl);
 753     //  0  |  1  | ... |  15   |  16   |   17    |   18       |  19   |  20
 754     // 0-3 | 4-7 | ... | 60-63 | 64-67 | 68-71   | 72-75      | 76-79 | 80-83
 755     // r0  | r1  | ... | r15   | xpsr  | main_sp | process_sp | rw    | rw2
 756     stlink_print_data(sl);
 757
 758     uint32_t r = read_uint32(sl->q_buf, 0);
 759     DLOG("r_idx (%2d) = 0x%08x\n", r_idx, r);
 760
 761     switch (r_idx) {
 762         case 16:
 763             regp->xpsr = r;
 764             break;
 765         case 17:
 766             regp->main_sp = r;
 767             break;
 768         case 18:
 769             regp->process_sp = r;
 770             break;
 771         case 19:
 772             regp->rw = r; //XXX ?(primask, basemask etc.)
 773             break;
 774         case 20:
 775             regp->rw2 = r; //XXX ?(primask, basemask etc.)
 776             break;
 777         default:
 778             regp->r[r_idx] = r;
 779     }
 780 }
 781
 782 // Write an arm-core register. Index:
 783 //  0  |  1  | ... |  15   |  16   |   17    |   18       |  19   |  20
 784 // r0  | r1  | ... | r15   | xpsr  | main_sp | process_sp | rw    | rw2
 785
 786 void _stlink_sg_write_reg(stlink_t *sl, uint32_t reg, int idx) {
 787     struct stlink_libsg *sg = sl->backend_data;
 788     clear_cdb(sg);
 789     sg->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEREG;
 790     //   2: reg index
 791     // 3-6: reg content
 792     sg->cdb_cmd_blk[2] = idx;
 793     write_uint32(sg->cdb_cmd_blk + 3, reg);
 794     sl->q_len = 2;
 795     sg->q_addr = 0;
 796     stlink_q(sl);
 797     stlink_stat(sl, "write reg");
 798 }
 799
 800 // Write a register of the debug module of the core.
 801 // XXX ?(atomic writes)
 802 // TODO test
 803
 804 void stlink_write_dreg(stlink_t *sl, uint32_t reg, uint32_t addr) {
 805     struct stlink_libsg *sg = sl->backend_data;
 806     DLOG("\n*** stlink_write_dreg ***\n");
 807     clear_cdb(sg);
 808     sg->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEDEBUGREG;
 809     // 2-5: address of reg of the debug module
 810     // 6-9: reg content
 811     write_uint32(sg->cdb_cmd_blk + 2, addr);
 812     write_uint32(sg->cdb_cmd_blk + 6, reg);
 813     sl->q_len = 2;
 814     sg->q_addr = addr;
 815     stlink_q(sl);
 816     stlink_stat(sl, "write debug reg");
 817 }
 818
 819 // Force the core exit the debug mode.
 820
 821 void _stlink_sg_run(stlink_t *sl) {
 822     struct stlink_libsg *sg = sl->backend_data;
 823     DLOG("\n*** stlink_run ***\n");
 824     clear_cdb(sg);
 825     sg->cdb_cmd_blk[1] = STLINK_DEBUG_RUNCORE;
 826     sl->q_len = 2;
 827     sg->q_addr = 0;
 828     stlink_q(sl);
 829     stlink_stat(sl, "run core");
 830 }
 831
 832 // Step the arm-core.
 833
 834 void _stlink_sg_step(stlink_t *sl) {
 835     struct stlink_libsg *sg = sl->backend_data;
 836     clear_cdb(sg);
 837     sg->cdb_cmd_blk[1] = STLINK_DEBUG_STEPCORE;
 838     sl->q_len = 2;
 839     sg->q_addr = 0;
 840     stlink_q(sl);
 841     stlink_stat(sl, "step core");
 842 }
 843
 844 // TODO test
 845 // see Cortex-M3 Technical Reference Manual
 846 // TODO make delegate!
 847 void stlink_set_hw_bp(stlink_t *sl, int fp_nr, uint32_t addr, int fp) {
 848     DLOG("\n*** stlink_set_hw_bp ***\n");
 849     struct stlink_libsg *sg = sl->backend_data;
 850     clear_cdb(sg);
 851     sg->cdb_cmd_blk[1] = STLINK_DEBUG_SETFP;
 852     // 2:The number of the flash patch used to set the breakpoint
 853     // 3-6: Address of the breakpoint (LSB)
 854     // 7: FP_ALL (0x02) / FP_UPPER (0x01) / FP_LOWER (0x00)
 855     sl->q_buf[2] = fp_nr;
 856     write_uint32(sl->q_buf, addr);
 857     sl->q_buf[7] = fp;
 858
 859     sl->q_len = 2;
 860     stlink_q(sl);
 861     stlink_stat(sl, "set flash breakpoint");
 862 }
 863
 864 // TODO test
 865
 866 // TODO make delegate!
 867 void stlink_clr_hw_bp(stlink_t *sl, int fp_nr) {
 868     struct stlink_libsg *sg = sl->backend_data;
 869     DLOG("\n*** stlink_clr_hw_bp ***\n");
 870     clear_cdb(sg);
 871     sg->cdb_cmd_blk[1] = STLINK_DEBUG_CLEARFP;
 872     sg->cdb_cmd_blk[2] = fp_nr;
 873
 874     sl->q_len = 2;
 875     stlink_q(sl);
 876     stlink_stat(sl, "clear flash breakpoint");
 877 }
 878
 879 // Read a "len" bytes to the sl->q_buf from the memory, max 6kB (6144 bytes)
 880
 881 void _stlink_sg_read_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
 882     struct stlink_libsg *sg = sl->backend_data;
 883     clear_cdb(sg);
 884     sg->cdb_cmd_blk[1] = STLINK_DEBUG_READMEM_32BIT;
 885     // 2-5: addr
 886     // 6-7: len
 887     write_uint32(sg->cdb_cmd_blk + 2, addr);
 888     write_uint16(sg->cdb_cmd_blk + 6, len);
 889
 890     // data_in 0-0x40-len
 891     // !!! len _and_ q_len must be max 6k,
 892     //     i.e. >1024 * 6 = 6144 -> aboard)
 893     // !!! if len < q_len: 64*k, 1024*n, n=1..5  -> aboard
 894     //     (broken residue issue)
 895     sl->q_len = len;
 896     sg->q_addr = addr;
 897     stlink_q(sl);
 898     stlink_print_data(sl);
 899 }
 900
 901 // Write a "len" bytes from the sl->q_buf to the memory, max 64 Bytes.
 902
 903 void _stlink_sg_write_mem8(stlink_t *sl, uint32_t addr, uint16_t len) {
 904     struct stlink_libsg *sg = sl->backend_data;
 905     clear_cdb(sg);
 906     sg->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEMEM_8BIT;
 907     // 2-5: addr
 908     // 6-7: len (>0x40 (64) -> aboard)
 909     write_uint32(sg->cdb_cmd_blk + 2, addr);
 910     write_uint16(sg->cdb_cmd_blk + 6, len);
 911
 912     // this sends the command...
 913     send_usb_mass_storage_command(sg->usb_handle, sg->ep_req, sg->cdb_cmd_blk, CDB_SL, 0, 0, 0);
 914     // This sends the data...
 915     send_usb_data_only(sg->usb_handle, sg->ep_req, sg->ep_rep, sl->q_buf, len);
 916     stlink_print_data(sl);
 917 }
 918
 919 // Write a "len" bytes from the sl->q_buf to the memory, max Q_BUF_LEN bytes.
 920
 921 void _stlink_sg_write_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
 922     struct stlink_libsg *sg = sl->backend_data;
 923     clear_cdb(sg);
 924     sg->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEMEM_32BIT;
 925     // 2-5: addr
 926     // 6-7: len "unlimited"
 927     write_uint32(sg->cdb_cmd_blk + 2, addr);
 928     write_uint16(sg->cdb_cmd_blk + 6, len);
 929
 930     // this sends the command...
 931     send_usb_mass_storage_command(sg->usb_handle, sg->ep_req, sg->cdb_cmd_blk, CDB_SL, 0, 0, 0);
 932     // This sends the data...
 933     send_usb_data_only(sg->usb_handle, sg->ep_req, sg->ep_rep, sl->q_buf, len);
 934
 935     stlink_print_data(sl);
 936 }
 937
 938 #if 0 /* not working */
 939
 940 static int write_flash_mem16
 941 (struct stlink* sl, uint32_t addr, uint16_t val) {
 942     /* half word writes */
 943     if (addr % 2) return -1;
 944
 945     /* unlock if locked */
 946     unlock_flash_if(sl);
 947
 948     /* set flash programming chosen bit */
 949     set_flash_cr_pg(sl);
 950
 951     write_uint16(sl->q_buf, val);
 952     stlink_write_mem16(sl, addr, 2);
 953
 954     /* wait for non business */
 955     wait_flash_busy(sl);
 956
 957     lock_flash(sl);
 958
 959     /* check the programmed value back */
 960     stlink_read_mem16(sl, addr, 2);
 961     if (*(const uint16_t*) sl->q_buf != val) {
 962         /* values differ at i * sizeof(uint16_t) */
 963         return -1;
 964     }
 965
 966     /* success */
 967     return 0;
 968 }
 969 #endif /* not working */
 970
 971 // Exit the jtag or swd mode and enter the mass mode.
 972
 973 void _stlink_sg_exit_debug_mode(stlink_t *stl) {
 974
 975     if (stl) {
 976         struct stlink_libsg* sl = stl->backend_data;
 977         clear_cdb(sl);
 978         sl->cdb_cmd_blk[1] = STLINK_DEBUG_EXIT;
 979         stl->q_len = 0; // >0 -> aboard
 980         stlink_q(stl);
 981     }
 982 }
 983
 984
 985 // 1) open a sg device, switch the stlink from dfu to mass mode
 986 // 2) wait 5s until the kernel driver stops reseting the broken device
 987 // 3) reopen the device
 988 // 4) the device driver is now ready for a switch to jtag/swd mode
 989 // TODO thinking, better error handling, wait until the kernel driver stops reseting the plugged-in device
 990
 991 stlink_backend_t _stlink_sg_backend = {
 992     _stlink_sg_close,
 993     _stlink_sg_exit_debug_mode,
 994     _stlink_sg_enter_swd_mode,
 995     _stlink_sg_enter_jtag_mode,
 996     _stlink_sg_exit_dfu_mode,
 997     _stlink_sg_core_id,
 998     _stlink_sg_reset,
 999     _stlink_sg_run,
1000     _stlink_sg_status,
1001     _stlink_sg_version,
1002     _stlink_sg_read_mem32,
1003     _stlink_sg_write_mem32,
1004     _stlink_sg_write_mem8,
1005     _stlink_sg_read_all_regs,
1006     _stlink_sg_read_reg,
1007     _stlink_sg_write_reg,
1008     _stlink_sg_step,
1009     _stlink_sg_current_mode,
1010     _stlink_sg_force_debug
1011 };
1012
1013 static stlink_t* stlink_open(const int verbose) {
1014
1015     stlink_t *sl = malloc(sizeof (stlink_t));
1016     memset(sl, 0, sizeof(stlink_t));
1017     struct stlink_libsg *slsg = malloc(sizeof (struct stlink_libsg));
1018     if (sl == NULL || slsg == NULL) {
1019         WLOG("Couldn't malloc stlink and stlink_sg structures out of memory!\n");
1020         return NULL;
1021     }
1022
1023     if (libusb_init(&(slsg->libusb_ctx))) {
1024         WLOG("failed to init libusb context, wrong version of libraries?\n");
1025         free(sl);
1026         free(slsg);
1027         return NULL;
1028     }
1029
1030     libusb_set_debug(slsg->libusb_ctx, 3);
1031
1032     slsg->usb_handle = libusb_open_device_with_vid_pid(slsg->libusb_ctx, USB_ST_VID, USB_STLINK_PID);
1033     if (slsg->usb_handle == NULL) {
1034         WLOG("Failed to find an stlink v1 by VID:PID\n");
1035         libusb_close(slsg->usb_handle);
1036         free(sl);
1037         free(slsg);
1038         return NULL;
1039     }
1040
1041     // TODO
1042     // Could read the interface config descriptor, and assert lots of the assumptions
1043
1044     // assumption: numInterfaces is always 1...
1045     if (libusb_kernel_driver_active(slsg->usb_handle, 0) == 1) {
1046         int r = libusb_detach_kernel_driver(slsg->usb_handle, 0);
1047         if (r < 0) {
1048             WLOG("libusb_detach_kernel_driver(() error %s\n", strerror(-r));
1049             libusb_close(slsg->usb_handle);
1050             free(sl);
1051             free(slsg);
1052             return NULL;
1053         }
1054         DLOG("Kernel driver was successfully detached\n");
1055     }
1056
1057     int config;
1058     if (libusb_get_configuration(slsg->usb_handle, &config)) {
1059         /* this may fail for a previous configured device */
1060         WLOG("libusb_get_configuration()\n");
1061         libusb_close(slsg->usb_handle);
1062         free(sl);
1063         free(slsg);
1064         return NULL;
1065
1066     }
1067
1068     // assumption: bConfigurationValue is always 1
1069     if (config != 1) {
1070         WLOG("Your stlink got into a real weird configuration, trying to fix it!\n");
1071         DLOG("setting new configuration (%d -> 1)\n", config);
1072         if (libusb_set_configuration(slsg->usb_handle, 1)) {
1073             /* this may fail for a previous configured device */
1074             WLOG("libusb_set_configuration() failed\n");
1075             libusb_close(slsg->usb_handle);
1076             free(sl);
1077             free(slsg);
1078             return NULL;
1079         }
1080     }
1081
1082     if (libusb_claim_interface(slsg->usb_handle, 0)) {
1083         WLOG("libusb_claim_interface() failed\n");
1084         libusb_close(slsg->usb_handle);
1085         free(sl);
1086         free(slsg);
1087         return NULL;
1088     }
1089
1090     // assumption: endpoint config is fixed mang. really.
1091     slsg->ep_rep = 1 /* ep rep */ | LIBUSB_ENDPOINT_IN;
1092     slsg->ep_req = 2 /* ep req */ | LIBUSB_ENDPOINT_OUT;
1093
1094     DLOG("Successfully opened stlinkv1 by libusb :)\n");
1095
1096     sl->verbose = verbose;
1097     sl->backend_data = slsg;
1098     sl->backend = &_stlink_sg_backend;
1099
1100     sl->core_stat = STLINK_CORE_STAT_UNKNOWN;
1101     slsg->q_addr = 0;
1102
1103     return sl;
1104 }
1105
1106
1107
1108 stlink_t* stlink_v1_open(const int verbose) {
1109     ugly_init(verbose);
1110     stlink_t *sl = stlink_open(verbose);
1111     if (sl == NULL) {
1112         fputs("Error: could not open stlink device\n", stderr);
1113         return NULL;
1114     }
1115
1116     stlink_version(sl);
1117     stlink_load_device_params(sl);
1118     if ((sl->version.st_vid != USB_ST_VID) || (sl->version.stlink_pid != USB_STLINK_PID)) {
1119         ugly_log(UERROR, LOG_TAG,
1120             "WTF? successfully opened, but unable to read version details. BROKEN!\n");
1121         return NULL;
1122     }
1123
1124     DLOG("Reading current mode...\n");
1125     switch (stlink_current_mode(sl)) {
1126         case STLINK_DEV_MASS_MODE:
1127             return sl;
1128         case STLINK_DEV_DEBUG_MODE:
1129             // TODO go to mass?
1130             return sl;
1131     }
1132
1133     DLOG("Attempting to exit DFU mode\n");
1134     _stlink_sg_exit_dfu_mode(sl);
1135
1136     // exit the dfu mode -> the device is gone
1137     DLOG("\n*** reopen the stlink device ***\n");
1138     delay(1000);
1139     stlink_close(sl);
1140     delay(5000);
1141
1142     DLOG("Attempting to reopen the stlink...\n");
1143     sl = stlink_open(verbose);
1144     if (sl == NULL) {
1145         fputs("Error: could not open stlink device\n", stderr);
1146         return NULL;
1147     }
1148     // re-query device info
1149     stlink_version(sl);
1150     stlink_load_device_params(sl);
1151     return sl;
1152 }