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