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