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