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

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