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