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