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