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

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