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 reset -> halted state.
 541
 542 void _stlink_sg_jtag_reset(stlink_t *sl, int value) {
 543     struct stlink_libsg *sg = sl->backend_data;
 544     clear_cdb(sg);
 545     sg->cdb_cmd_blk[1] = STLINK_JTAG_DRIVE_NRST;
 546     sg->cdb_cmd_blk[2] = (value)?0:1;
 547     sl->q_len = 3;
 548     sg->q_addr = 2;
 549     stlink_q(sl);
 550     stlink_stat(sl, "core reset");
 551 }
 552
 553 // Arm-core status: halted or running.
 554
 555 void _stlink_sg_status(stlink_t *sl) {
 556     struct stlink_libsg *sg = sl->backend_data;
 557     clear_cdb(sg);
 558     sg->cdb_cmd_blk[1] = STLINK_DEBUG_GETSTATUS;
 559     sl->q_len = 2;
 560     sg->q_addr = 0;
 561     stlink_q(sl);
 562 }
 563
 564 // Force the core into the debug mode -> halted state.
 565
 566 void _stlink_sg_force_debug(stlink_t *sl) {
 567     struct stlink_libsg *sg = sl->backend_data;
 568     clear_cdb(sg);
 569     sg->cdb_cmd_blk[1] = STLINK_DEBUG_FORCEDEBUG;
 570     sl->q_len = 2;
 571     sg->q_addr = 0;
 572     stlink_q(sl);
 573     stlink_stat(sl, "force debug");
 574 }
 575
 576 // Read all arm-core registers.
 577
 578 void _stlink_sg_read_all_regs(stlink_t *sl, reg *regp) {
 579     struct stlink_libsg *sg = sl->backend_data;
 580
 581     clear_cdb(sg);
 582     sg->cdb_cmd_blk[1] = STLINK_DEBUG_READALLREGS;
 583     sl->q_len = 84;
 584     sg->q_addr = 0;
 585     stlink_q(sl);
 586     stlink_print_data(sl);
 587
 588     // TODO - most of this should be re-extracted up....
 589
 590     // 0-3 | 4-7 | ... | 60-63 | 64-67 | 68-71   | 72-75      | 76-79 | 80-83
 591     // r0  | r1  | ... | r15   | xpsr  | main_sp | process_sp | rw    | rw2
 592     for (int i = 0; i < 16; i++) {
 593         regp->r[i] = read_uint32(sl->q_buf, 4 * i);
 594         if (sl->verbose > 1)
 595             DLOG("r%2d = 0x%08x\n", i, regp->r[i]);
 596     }
 597     regp->xpsr = read_uint32(sl->q_buf, 64);
 598     regp->main_sp = read_uint32(sl->q_buf, 68);
 599     regp->process_sp = read_uint32(sl->q_buf, 72);
 600     regp->rw = read_uint32(sl->q_buf, 76);
 601     regp->rw2 = read_uint32(sl->q_buf, 80);
 602     if (sl->verbose < 2)
 603         return;
 604
 605     DLOG("xpsr       = 0x%08x\n", regp->xpsr);
 606     DLOG("main_sp    = 0x%08x\n", regp->main_sp);
 607     DLOG("process_sp = 0x%08x\n", regp->process_sp);
 608     DLOG("rw         = 0x%08x\n", regp->rw);
 609     DLOG("rw2        = 0x%08x\n", regp->rw2);
 610 }
 611
 612 // Read an arm-core register, the index must be in the range 0..20.
 613 //  0  |  1  | ... |  15   |  16   |   17    |   18       |  19   |  20
 614 // r0  | r1  | ... | r15   | xpsr  | main_sp | process_sp | rw    | rw2
 615
 616 void _stlink_sg_read_reg(stlink_t *sl, int r_idx, reg *regp) {
 617     struct stlink_libsg *sg = sl->backend_data;
 618     clear_cdb(sg);
 619     sg->cdb_cmd_blk[1] = STLINK_DEBUG_READREG;
 620     sg->cdb_cmd_blk[2] = r_idx;
 621     sl->q_len = 4;
 622     sg->q_addr = 0;
 623     stlink_q(sl);
 624     //  0  |  1  | ... |  15   |  16   |   17    |   18       |  19   |  20
 625     // 0-3 | 4-7 | ... | 60-63 | 64-67 | 68-71   | 72-75      | 76-79 | 80-83
 626     // r0  | r1  | ... | r15   | xpsr  | main_sp | process_sp | rw    | rw2
 627     stlink_print_data(sl);
 628
 629     uint32_t r = read_uint32(sl->q_buf, 0);
 630     DLOG("r_idx (%2d) = 0x%08x\n", r_idx, r);
 631
 632     switch (r_idx) {
 633         case 16:
 634             regp->xpsr = r;
 635             break;
 636         case 17:
 637             regp->main_sp = r;
 638             break;
 639         case 18:
 640             regp->process_sp = r;
 641             break;
 642         case 19:
 643             regp->rw = r; //XXX ?(primask, basemask etc.)
 644             break;
 645         case 20:
 646             regp->rw2 = r; //XXX ?(primask, basemask etc.)
 647             break;
 648         default:
 649             regp->r[r_idx] = r;
 650     }
 651 }
 652
 653 // Write an arm-core register. Index:
 654 //  0  |  1  | ... |  15   |  16   |   17    |   18       |  19   |  20
 655 // r0  | r1  | ... | r15   | xpsr  | main_sp | process_sp | rw    | rw2
 656
 657 void _stlink_sg_write_reg(stlink_t *sl, uint32_t reg, int idx) {
 658     struct stlink_libsg *sg = sl->backend_data;
 659     clear_cdb(sg);
 660     sg->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEREG;
 661     //   2: reg index
 662     // 3-6: reg content
 663     sg->cdb_cmd_blk[2] = idx;
 664     write_uint32(sg->cdb_cmd_blk + 3, reg);
 665     sl->q_len = 2;
 666     sg->q_addr = 0;
 667     stlink_q(sl);
 668     stlink_stat(sl, "write reg");
 669 }
 670
 671 // Write a register of the debug module of the core.
 672 // XXX ?(atomic writes)
 673 // TODO test
 674
 675 void stlink_write_dreg(stlink_t *sl, uint32_t reg, uint32_t addr) {
 676     struct stlink_libsg *sg = sl->backend_data;
 677     DLOG("\n*** stlink_write_dreg ***\n");
 678     clear_cdb(sg);
 679     sg->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEDEBUGREG;
 680     // 2-5: address of reg of the debug module
 681     // 6-9: reg content
 682     write_uint32(sg->cdb_cmd_blk + 2, addr);
 683     write_uint32(sg->cdb_cmd_blk + 6, reg);
 684     sl->q_len = 2;
 685     sg->q_addr = addr;
 686     stlink_q(sl);
 687     stlink_stat(sl, "write debug reg");
 688 }
 689
 690 // Force the core exit the debug mode.
 691
 692 void _stlink_sg_run(stlink_t *sl) {
 693     struct stlink_libsg *sg = sl->backend_data;
 694     clear_cdb(sg);
 695     sg->cdb_cmd_blk[1] = STLINK_DEBUG_RUNCORE;
 696     sl->q_len = 2;
 697     sg->q_addr = 0;
 698     stlink_q(sl);
 699     stlink_stat(sl, "run core");
 700 }
 701
 702 // Step the arm-core.
 703
 704 void _stlink_sg_step(stlink_t *sl) {
 705     struct stlink_libsg *sg = sl->backend_data;
 706     clear_cdb(sg);
 707     sg->cdb_cmd_blk[1] = STLINK_DEBUG_STEPCORE;
 708     sl->q_len = 2;
 709     sg->q_addr = 0;
 710     stlink_q(sl);
 711     stlink_stat(sl, "step core");
 712 }
 713
 714 // TODO test
 715 // see Cortex-M3 Technical Reference Manual
 716 // TODO make delegate!
 717 void stlink_set_hw_bp(stlink_t *sl, int fp_nr, uint32_t addr, int fp) {
 718     DLOG("\n*** stlink_set_hw_bp ***\n");
 719     struct stlink_libsg *sg = sl->backend_data;
 720     clear_cdb(sg);
 721     sg->cdb_cmd_blk[1] = STLINK_DEBUG_SETFP;
 722     // 2:The number of the flash patch used to set the breakpoint
 723     // 3-6: Address of the breakpoint (LSB)
 724     // 7: FP_ALL (0x02) / FP_UPPER (0x01) / FP_LOWER (0x00)
 725     sl->q_buf[2] = fp_nr;
 726     write_uint32(sl->q_buf, addr);
 727     sl->q_buf[7] = fp;
 728
 729     sl->q_len = 2;
 730     stlink_q(sl);
 731     stlink_stat(sl, "set flash breakpoint");
 732 }
 733
 734 // TODO test
 735
 736 // TODO make delegate!
 737 void stlink_clr_hw_bp(stlink_t *sl, int fp_nr) {
 738     struct stlink_libsg *sg = sl->backend_data;
 739     DLOG("\n*** stlink_clr_hw_bp ***\n");
 740     clear_cdb(sg);
 741     sg->cdb_cmd_blk[1] = STLINK_DEBUG_CLEARFP;
 742     sg->cdb_cmd_blk[2] = fp_nr;
 743
 744     sl->q_len = 2;
 745     stlink_q(sl);
 746     stlink_stat(sl, "clear flash breakpoint");
 747 }
 748
 749 // Read a "len" bytes to the sl->q_buf from the memory, max 6kB (6144 bytes)
 750
 751 void _stlink_sg_read_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
 752     struct stlink_libsg *sg = sl->backend_data;
 753     clear_cdb(sg);
 754     sg->cdb_cmd_blk[1] = STLINK_DEBUG_READMEM_32BIT;
 755     // 2-5: addr
 756     // 6-7: len
 757     write_uint32(sg->cdb_cmd_blk + 2, addr);
 758     write_uint16(sg->cdb_cmd_blk + 6, len);
 759
 760     // data_in 0-0x40-len
 761     // !!! len _and_ q_len must be max 6k,
 762     //     i.e. >1024 * 6 = 6144 -> aboard)
 763     // !!! if len < q_len: 64*k, 1024*n, n=1..5  -> aboard
 764     //     (broken residue issue)
 765     sl->q_len = len;
 766     sg->q_addr = addr;
 767     stlink_q(sl);
 768     stlink_print_data(sl);
 769 }
 770
 771 // Write a "len" bytes from the sl->q_buf to the memory, max 64 Bytes.
 772
 773 void _stlink_sg_write_mem8(stlink_t *sl, uint32_t addr, uint16_t len) {
 774     struct stlink_libsg *sg = sl->backend_data;
 775     clear_cdb(sg);
 776     sg->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEMEM_8BIT;
 777     // 2-5: addr
 778     // 6-7: len (>0x40 (64) -> aboard)
 779     write_uint32(sg->cdb_cmd_blk + 2, addr);
 780     write_uint16(sg->cdb_cmd_blk + 6, len);
 781
 782     // this sends the command...
 783     send_usb_mass_storage_command(sg->usb_handle, sg->ep_req, sg->cdb_cmd_blk, CDB_SL, 0, 0, 0);
 784     // This sends the data...
 785     send_usb_data_only(sg->usb_handle, sg->ep_req, sg->ep_rep, sl->q_buf, len);
 786     stlink_print_data(sl);
 787 }
 788
 789 // Write a "len" bytes from the sl->q_buf to the memory, max Q_BUF_LEN bytes.
 790
 791 void _stlink_sg_write_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
 792     struct stlink_libsg *sg = sl->backend_data;
 793     clear_cdb(sg);
 794     sg->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEMEM_32BIT;
 795     // 2-5: addr
 796     // 6-7: len "unlimited"
 797     write_uint32(sg->cdb_cmd_blk + 2, addr);
 798     write_uint16(sg->cdb_cmd_blk + 6, len);
 799
 800     // this sends the command...
 801     send_usb_mass_storage_command(sg->usb_handle, sg->ep_req, sg->cdb_cmd_blk, CDB_SL, 0, 0, 0);
 802     // This sends the data...
 803     send_usb_data_only(sg->usb_handle, sg->ep_req, sg->ep_rep, sl->q_buf, len);
 804
 805     stlink_print_data(sl);
 806 }
 807
 808 // Write one DWORD data to memory
 809
 810 void _stlink_sg_write_debug32(stlink_t *sl, uint32_t addr, uint32_t data) {
 811     struct stlink_libsg *sg = sl->backend_data;
 812     clear_cdb(sg);
 813     sg->cdb_cmd_blk[1] = STLINK_JTAG_WRITEDEBUG_32BIT;
 814     // 2-5: addr
 815     write_uint32(sg->cdb_cmd_blk + 2, addr);
 816     write_uint32(sg->cdb_cmd_blk + 6, data);
 817     sl->q_len = 2;
 818     stlink_q(sl);
 819
 820 }
 821
 822 // Read one DWORD data from memory
 823
 824 uint32_t _stlink_sg_read_debug32(stlink_t *sl, uint32_t addr) {
 825     struct stlink_libsg *sg = sl->backend_data;
 826     clear_cdb(sg);
 827     sg->cdb_cmd_blk[1] = STLINK_JTAG_READDEBUG_32BIT;
 828     // 2-5: addr
 829     write_uint32(sg->cdb_cmd_blk + 2, addr);
 830     sl->q_len = 8;
 831     stlink_q(sl);
 832
 833     return read_uint32(sl->q_buf, 4);
 834 }
 835
 836 // Exit the jtag or swd mode and enter the mass mode.
 837
 838 void _stlink_sg_exit_debug_mode(stlink_t *stl) {
 839
 840     if (stl) {
 841         struct stlink_libsg* sl = stl->backend_data;
 842         clear_cdb(sl);
 843         sl->cdb_cmd_blk[1] = STLINK_DEBUG_EXIT;
 844         stl->q_len = 0; // >0 -> aboard
 845         stlink_q(stl);
 846     }
 847 }
 848
 849
 850 // 1) open a sg device, switch the stlink from dfu to mass mode
 851 // 2) wait 5s until the kernel driver stops reseting the broken device
 852 // 3) reopen the device
 853 // 4) the device driver is now ready for a switch to jtag/swd mode
 854 // TODO thinking, better error handling, wait until the kernel driver stops reseting the plugged-in device
 855
 856 stlink_backend_t _stlink_sg_backend = {
 857     _stlink_sg_close,
 858     _stlink_sg_exit_debug_mode,
 859     _stlink_sg_enter_swd_mode,
 860     _stlink_sg_enter_jtag_mode,
 861     _stlink_sg_exit_dfu_mode,
 862     _stlink_sg_core_id,
 863     _stlink_sg_reset,
 864     _stlink_sg_jtag_reset,
 865     _stlink_sg_run,
 866     _stlink_sg_status,
 867     _stlink_sg_version,
 868     _stlink_sg_read_debug32,
 869     _stlink_sg_read_mem32,
 870     _stlink_sg_write_debug32,
 871     _stlink_sg_write_mem32,
 872     _stlink_sg_write_mem8,
 873     _stlink_sg_read_all_regs,
 874     _stlink_sg_read_reg,
 875     _stlink_sg_write_reg,
 876     _stlink_sg_step,
 877     _stlink_sg_current_mode,
 878     _stlink_sg_force_debug
 879 };
 880
 881 static stlink_t* stlink_open(const int verbose) {
 882
 883     stlink_t *sl = malloc(sizeof (stlink_t));
 884     memset(sl, 0, sizeof(stlink_t));
 885     struct stlink_libsg *slsg = malloc(sizeof (struct stlink_libsg));
 886     if (sl == NULL || slsg == NULL) {
 887         WLOG("Couldn't malloc stlink and stlink_sg structures out of memory!\n");
 888         return NULL;
 889     }
 890
 891     if (libusb_init(&(slsg->libusb_ctx))) {
 892         WLOG("failed to init libusb context, wrong version of libraries?\n");
 893         free(sl);
 894         free(slsg);
 895         return NULL;
 896     }
 897
 898     libusb_set_debug(slsg->libusb_ctx, 3);
 899
 900     slsg->usb_handle = libusb_open_device_with_vid_pid(slsg->libusb_ctx, USB_ST_VID, USB_STLINK_PID);
 901     if (slsg->usb_handle == NULL) {
 902         WLOG("Failed to find an stlink v1 by VID:PID\n");
 903         libusb_close(slsg->usb_handle);
 904         libusb_exit(slsg->libusb_ctx);
 905         free(sl);
 906         free(slsg);
 907         return NULL;
 908     }
 909
 910     // TODO
 911     // Could read the interface config descriptor, and assert lots of the assumptions
 912
 913     // assumption: numInterfaces is always 1...
 914     if (libusb_kernel_driver_active(slsg->usb_handle, 0) == 1) {
 915         int r = libusb_detach_kernel_driver(slsg->usb_handle, 0);
 916         if (r < 0) {
 917             WLOG("libusb_detach_kernel_driver(() error %s\n", strerror(-r));
 918             libusb_close(slsg->usb_handle);
 919             libusb_exit(slsg->libusb_ctx);
 920             free(sl);
 921             free(slsg);
 922             return NULL;
 923         }
 924         DLOG("Kernel driver was successfully detached\n");
 925     }
 926
 927     int config;
 928     if (libusb_get_configuration(slsg->usb_handle, &config)) {
 929         /* this may fail for a previous configured device */
 930         WLOG("libusb_get_configuration()\n");
 931         libusb_close(slsg->usb_handle);
 932         libusb_exit(slsg->libusb_ctx);
 933         free(sl);
 934         free(slsg);
 935         return NULL;
 936
 937     }
 938
 939     // assumption: bConfigurationValue is always 1
 940     if (config != 1) {
 941         WLOG("Your stlink got into a real weird configuration, trying to fix it!\n");
 942         DLOG("setting new configuration (%d -> 1)\n", config);
 943         if (libusb_set_configuration(slsg->usb_handle, 1)) {
 944             /* this may fail for a previous configured device */
 945             WLOG("libusb_set_configuration() failed\n");
 946             libusb_close(slsg->usb_handle);
 947             libusb_exit(slsg->libusb_ctx);
 948             free(sl);
 949             free(slsg);
 950             return NULL;
 951         }
 952     }
 953
 954     if (libusb_claim_interface(slsg->usb_handle, 0)) {
 955         WLOG("libusb_claim_interface() failed\n");
 956         libusb_close(slsg->usb_handle);
 957         libusb_exit(slsg->libusb_ctx);
 958         free(sl);
 959         free(slsg);
 960         return NULL;
 961     }
 962
 963     // assumption: endpoint config is fixed mang. really.
 964     slsg->ep_rep = 1 /* ep rep */ | LIBUSB_ENDPOINT_IN;
 965     slsg->ep_req = 2 /* ep req */ | LIBUSB_ENDPOINT_OUT;
 966
 967     DLOG("Successfully opened stlinkv1 by libusb :)\n");
 968
 969     sl->verbose = verbose;
 970     sl->backend_data = slsg;
 971     sl->backend = &_stlink_sg_backend;
 972
 973     sl->core_stat = STLINK_CORE_STAT_UNKNOWN;
 974     slsg->q_addr = 0;
 975
 976     return sl;
 977 }
 978
 979
 980 stlink_t* stlink_v1_open_inner(const int verbose) {
 981     ugly_init(verbose);
 982     stlink_t *sl = stlink_open(verbose);
 983     if (sl == NULL) {
 984         fputs("Error: could not open stlink device\n", stderr);
 985         return NULL;
 986     }
 987
 988     stlink_version(sl);
 989     if ((sl->version.st_vid != USB_ST_VID) || (sl->version.stlink_pid != USB_STLINK_PID)) {
 990         ugly_log(UERROR, LOG_TAG,
 991             "WTF? successfully opened, but unable to read version details. BROKEN!\n");
 992         return NULL;
 993     }
 994
 995     DLOG("Reading current mode...\n");
 996     switch (stlink_current_mode(sl)) {
 997     case STLINK_DEV_MASS_MODE:
 998             return sl;
 999     case STLINK_DEV_DEBUG_MODE:
1000         // TODO go to mass?
1001         return sl;
1002     default:
1003         ILOG("Current mode unusable, trying to get back to a useful state...\n");
1004         break;
1005     }
1006
1007     DLOG("Attempting to exit DFU mode\n");
1008     _stlink_sg_exit_dfu_mode(sl);
1009
1010     // re-query device info (and retest)
1011     stlink_version(sl);
1012     if ((sl->version.st_vid != USB_ST_VID) || (sl->version.stlink_pid != USB_STLINK_PID)) {
1013         ugly_log(UERROR, LOG_TAG,
1014             "WTF? successfully opened, but unable to read version details. BROKEN!\n");
1015         return NULL;
1016     }
1017
1018     return sl;
1019 }
1020
1021 stlink_t* stlink_v1_open(const int verbose) {
1022     stlink_t *sl = stlink_v1_open_inner(verbose);
1023     if (sl == NULL) {
1024         fputs("Error: could not open stlink device\n", stderr);
1025         return NULL;
1026     }
1027     // by now, it _must_ be fully open and in a useful mode....
1028         stlink_enter_swd_mode(sl);
1029     /* Now we are ready to read the parameters  */
1030     stlink_reset(sl);
1031     stlink_load_device_params(sl);
1032     ILOG("Successfully opened a stlink v1 debugger\n");
1033     return sl;
1034 }