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