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