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  Code format ~ TAB = 8, K&R, linux kernel source, golang oriented
  16  Tested compatibility: linux, gcc >= 4.3.3
  17
  18  The communication is based on standard USB mass storage device
  19  BOT (Bulk Only Transfer)
  20  - Endpoint 1: BULK_IN, 64 bytes max
  21  - Endpoint 2: BULK_OUT, 64 bytes max
  22
  23  All CBW transfers are ordered with the LSB (byte 0) first (little endian).
  24  Any command must be answered before sending the next command.
  25  Each USB transfer must complete in less than 1s.
  26
  27  SB Device Class Definition for Mass Storage Devices:
  28  www.usb.org/developers/devclass_docs/usbmassbulk_10.pdf
  29
  30  dt             - Data Transfer (IN/OUT)
  31  CBW            - Command Block Wrapper
  32  CSW            - Command Status Wrapper
  33  RFU            - Reserved for Future Use
  34  scsi_pt        - SCSI pass-through
  35  sg             - SCSI generic
  36
  37  * usb-storage.quirks
  38  http://git.kernel.org/?p=linux/kernel/git/torvalds/linux-2.6.git;a=blob_plain;f=Documentation/kernel-parameters.txt
  39  Each entry has the form VID:PID:Flags where VID and PID are Vendor and Product
  40  ID values (4-digit hex numbers) and Flags is a set of characters, each corresponding
  41  to a common usb-storage quirk flag as follows:
  42
  43  a = SANE_SENSE (collect more than 18 bytes of sense data);
  44  b = BAD_SENSE (don't collect more than 18 bytes of sense data);
  45  c = FIX_CAPACITY (decrease the reported device capacity by one sector);
  46  h = CAPACITY_HEURISTICS (decrease the reported device capacity by one sector if the number is odd);
  47  i = IGNORE_DEVICE (don't bind to this device);
  48  l = NOT_LOCKABLE (don't try to lock and unlock ejectable media);
  49  m = MAX_SECTORS_64 (don't transfer more than 64 sectors = 32 KB at a time);
  50  o = CAPACITY_OK (accept the capacity reported by the device);
  51  r = IGNORE_RESIDUE (the device reports bogus residue values);
  52  s = SINGLE_LUN (the device has only one Logical Unit);
  53  w = NO_WP_DETECT (don't test whether the medium is write-protected).
  54
  55  Example: quirks=0419:aaf5:rl,0421:0433:rc
  56  http://permalink.gmane.org/gmane.linux.usb.general/35053
  57
  58  modprobe -r usb-storage && modprobe usb-storage quirks=483:3744:l
  59
  60  Equivalently, you can add a line saying
  61
  62  options usb-storage quirks=483:3744:l
  63
  64  to your /etc/modprobe.conf or /etc/modprobe.d/local.conf (or add the "quirks=..."
  65  part to an existing options line for usb-storage).
  66
  67  https://wiki.kubuntu.org/Kernel/Debugging/USB explains the protocoll and
  68  would allow to replace the sg access to pure libusb access
  69  */
  70
  71
  72 #define __USE_GNU
  73 #include <stdio.h>
  74 #include <string.h>
  75 #include <stdarg.h>
  76 #include <stdlib.h>
  77 #include <unistd.h>
  78 #include <fcntl.h>
  79 #include <sys/types.h>
  80 #include <sys/stat.h>
  81 #include <sys/mman.h>
  82
  83 #include "stlink-common.h"
  84 #include "stlink-sg.h"
  85 #include "uglylogging.h"
  86
  87 #define LOG_TAG __FILE__
  88 #define DLOG(format, args...)         ugly_log(UDEBUG, LOG_TAG, format, ## args)
  89 #define ILOG(format, args...)         ugly_log(UINFO, LOG_TAG, format, ## args)
  90 #define WLOG(format, args...)         ugly_log(UWARN, LOG_TAG, format, ## args)
  91 #define fatal(format, args...)        ugly_log(UFATAL, LOG_TAG, format, ## args)
  92
  93 // Suspends execution of the calling process for
  94 // (at least) ms milliseconds.
  95
  96 static void delay(int ms) {
  97     //fprintf(stderr, "*** wait %d ms\n", ms);
  98     usleep(1000 * ms);
  99 }
 100
 101 static void clear_cdb(struct stlink_libsg *sl) {
 102     for (size_t i = 0; i < sizeof (sl->cdb_cmd_blk); i++)
 103         sl->cdb_cmd_blk[i] = 0;
 104     // set default
 105     sl->cdb_cmd_blk[0] = STLINK_DEBUG_COMMAND;
 106     sl->q_data_dir = Q_DATA_IN;
 107 }
 108
 109 // E.g. make the valgrind happy.
 110
 111 static void clear_buf(stlink_t *sl) {
 112     DLOG("*** clear_buf ***\n");
 113     for (size_t i = 0; i < sizeof (sl->q_buf); i++)
 114         sl->q_buf[i] = 0;
 115
 116 }
 117
 118 // close the device, free the allocated memory
 119
 120 void _stlink_sg_close(stlink_t *sl) {
 121     if (sl) {
 122 #if FINISHED_WITH_SG
 123         struct stlink_libsg *slsg = sl->backend_data;
 124         scsi_pt_close_device(slsg->sg_fd);
 125         free(slsg);
 126 #endif
 127     }
 128 }
 129
 130
 131 //TODO rewrite/cleanup, save the error in sl
 132
 133 #if FINISHED_WITH_SG
 134 static void stlink_confirm_inq(stlink_t *stl, struct sg_pt_base *ptvp) {
 135     struct stlink_libsg *sl = stl->backend_data;
 136     const int e = sl->do_scsi_pt_err;
 137     if (e < 0) {
 138         fprintf(stderr, "scsi_pt error: pass through os error: %s\n",
 139                 safe_strerror(-e));
 140         return;
 141     } else if (e == SCSI_PT_DO_BAD_PARAMS) {
 142         fprintf(stderr, "scsi_pt error: bad pass through setup\n");
 143         return;
 144     } else if (e == SCSI_PT_DO_TIMEOUT) {
 145         fprintf(stderr, "  pass through timeout\n");
 146         return;
 147     }
 148     const int duration = get_scsi_pt_duration_ms(ptvp);
 149     if ((stl->verbose > 1) && (duration >= 0))
 150         DLOG("      duration=%d ms\n", duration);
 151
 152     // XXX stlink fw sends broken residue, so ignore it and use the known q_len
 153     // "usb-storage quirks=483:3744:r"
 154     // forces residue to be ignored and calculated, but this causes aboard if
 155     // data_len = 0 and by some other data_len values.
 156
 157     const int resid = get_scsi_pt_resid(ptvp);
 158     const int dsize = stl->q_len - resid;
 159
 160     const int cat = get_scsi_pt_result_category(ptvp);
 161     char buf[512];
 162     unsigned int slen;
 163
 164     switch (cat) {
 165         case SCSI_PT_RESULT_GOOD:
 166             if (stl->verbose && (resid > 0))
 167                 DLOG("      notice: requested %d bytes but "
 168                     "got %d bytes, ignore [broken] residue = %d\n",
 169                     stl->q_len, dsize, resid);
 170             break;
 171         case SCSI_PT_RESULT_STATUS:
 172             if (stl->verbose) {
 173                 sg_get_scsi_status_str(
 174                         get_scsi_pt_status_response(ptvp), sizeof (buf),
 175                         buf);
 176                 DLOG("  scsi status: %s\n", buf);
 177             }
 178             return;
 179         case SCSI_PT_RESULT_SENSE:
 180             slen = get_scsi_pt_sense_len(ptvp);
 181             if (stl->verbose) {
 182                 sg_get_sense_str("", sl->sense_buf, slen, (stl->verbose
 183                         > 1), sizeof (buf), buf);
 184                 DLOG("%s", buf);
 185             }
 186             if (stl->verbose && (resid > 0)) {
 187                 if ((stl->verbose) || (stl->q_len > 0))
 188                     DLOG("    requested %d bytes but "
 189                         "got %d bytes\n", stl->q_len, dsize);
 190             }
 191             return;
 192         case SCSI_PT_RESULT_TRANSPORT_ERR:
 193             if (stl->verbose) {
 194                 get_scsi_pt_transport_err_str(ptvp, sizeof (buf), buf);
 195                 // http://tldp.org/HOWTO/SCSI-Generic-HOWTO/x291.html
 196                 // These codes potentially come from the firmware on a host adapter
 197                 // or from one of several hosts that an adapter driver controls.
 198                 // The 'host_status' field has the following values:
 199                 //      [0x07] Internal error detected in the host adapter.
 200                 // This may not be fatal (and the command may have succeeded).
 201                 DLOG("  transport: %s", buf);
 202             }
 203             return;
 204         case SCSI_PT_RESULT_OS_ERR:
 205             if (stl->verbose) {
 206                 get_scsi_pt_os_err_str(ptvp, sizeof (buf), buf);
 207                 DLOG("  os: %s", buf);
 208             }
 209             return;
 210         default:
 211             fprintf(stderr, "  unknown pass through result "
 212                     "category (%d)\n", cat);
 213     }
 214 }
 215 #endif
 216
 217 void stlink_q(stlink_t *sl) {
 218 #if FINISHED_WITH_SG
 219     struct stlink_libsg* sg = sl->backend_data;
 220     DLOG("CDB[");
 221     for (int i = 0; i < CDB_SL; i++)
 222         DLOG(" 0x%02x", (unsigned int) sg->cdb_cmd_blk[i]);
 223     DLOG("]\n");
 224
 225     // Get control command descriptor of scsi structure,
 226     // (one object per command!!)
 227     struct sg_pt_base *ptvp = construct_scsi_pt_obj();
 228     if (NULL == ptvp) {
 229         fprintf(stderr, "construct_scsi_pt_obj: out of memory\n");
 230         return;
 231     }
 232
 233     set_scsi_pt_cdb(ptvp, sg->cdb_cmd_blk, sizeof (sg->cdb_cmd_blk));
 234
 235     // set buffer for sense (error information) data
 236     set_scsi_pt_sense(ptvp, sg->sense_buf, sizeof (sg->sense_buf));
 237
 238     // Set a buffer to be used for data transferred from device
 239     if (sg->q_data_dir == Q_DATA_IN) {
 240         //clear_buf(sl);
 241         set_scsi_pt_data_in(ptvp, sl->q_buf, sl->q_len);
 242     } else {
 243         set_scsi_pt_data_out(ptvp, sl->q_buf, sl->q_len);
 244     }
 245     // Executes SCSI command (or at least forwards it to lower layers).
 246     sg->do_scsi_pt_err = do_scsi_pt(ptvp, sg->sg_fd, SG_TIMEOUT_SEC,
 247             sl->verbose);
 248
 249     // check for scsi errors
 250     stlink_confirm_inq(sl, ptvp);
 251     // TODO recycle: clear_scsi_pt_obj(struct sg_pt_base * objp);
 252     destruct_scsi_pt_obj(ptvp);
 253 #endif
 254 }
 255
 256 // TODO thinking, cleanup
 257
 258 void stlink_stat(stlink_t *stl, char *txt) {
 259     if (stl->q_len <= 0)
 260         return;
 261
 262     stlink_print_data(stl);
 263
 264     switch (stl->q_buf[0]) {
 265         case STLINK_OK:
 266             DLOG("  %s: ok\n", txt);
 267             return;
 268         case STLINK_FALSE:
 269             DLOG("  %s: false\n", txt);
 270             return;
 271         default:
 272             DLOG("  %s: unknown\n", txt);
 273     }
 274 }
 275
 276
 277 void _stlink_sg_version(stlink_t *stl) {
 278     struct stlink_libsg *sl = stl->backend_data;
 279     DLOG("\n*** stlink_version ***\n");
 280     clear_cdb(sl);
 281     sl->cdb_cmd_blk[0] = STLINK_GET_VERSION;
 282     stl->q_len = 6;
 283     sl->q_addr = 0;
 284 //    stlink_q(stl);
 285     // HACK use my own private version right now...
 286
 287     int try = 0;
 288     int ret = 0;
 289     int real_transferred;
 290
 291 /*
 292     uint32_t    dCBWSignature;
 293     uint32_t    dCBWTag;
 294     uint32_t    dCBWDataTransferLength;
 295     uint8_t     bmCBWFlags;
 296     uint8_t     bCBWLUN;
 297     uint8_t     bCBWCBLength;
 298     uint8_t     CBWCB[16];
 299   */
 300
 301 #if using_old_code_examples
 302     /*
 303      * and from old code?
 304     cmd[i++] = 'U';
 305     cmd[i++] = 'S';
 306     cmd[i++] = 'B';
 307     cmd[i++] = 'C';
 308     write_uint32(&cmd[i], sg->sg_transfer_idx);
 309     write_uint32(&cmd[i + 4], len);
 310     i += 8;
 311     cmd[i++] = (dir == SG_DXFER_FROM_DEV)?0x80:0;
 312     cmd[i++] = 0; /* Logical unit */
 313     cmd[i++] = 0xa; /* Command length */
 314      */
 315 #endif
 316
 317     int i = 0;
 318     stl->c_buf[i++] = 'U';
 319     stl->c_buf[i++] = 'S';
 320     stl->c_buf[i++] = 'B';
 321     stl->c_buf[i++] = 'C';
 322     // tag is allegedly ignored... TODO - verify
 323     write_uint32(&stl->c_buf[i], 1);
 324     // TODO - Does this even matter? verify with more commands....
 325     uint32_t command_length = STLINK_CMD_SIZE;
 326     write_uint32(&stl->c_buf[i+4], command_length);
 327     i+= 8;
 328     stl->c_buf[i++] = LIBUSB_ENDPOINT_IN;
 329     // assumption: lun is always 0;
 330     stl->c_buf[i++] = 0;
 331
 332     stl->c_buf[i++] = sizeof(sl->cdb_cmd_blk);
 333
 334     // duh, now the actual data...
 335     memcpy(&(stl->c_buf[i]), sl->cdb_cmd_blk, sizeof(sl->cdb_cmd_blk));
 336
 337     int sending_length = STLINK_SG_SIZE;
 338     DLOG("sending length set to: %d\n", sending_length);
 339
 340     // send....
 341     do {
 342         DLOG("attempting tx...\n");
 343         ret = libusb_bulk_transfer(sl->usb_handle, sl->ep_req, stl->c_buf, sending_length,
 344                                    &real_transferred, 3 * 1000);
 345         if (ret == LIBUSB_ERROR_PIPE) {
 346             libusb_clear_halt(sl->usb_handle, sl->ep_req);
 347         }
 348         try++;
 349     } while ((ret == LIBUSB_ERROR_PIPE) && (try < 3));
 350     if (ret != LIBUSB_SUCCESS) {
 351         WLOG("sending failed: %d\n", ret);
 352         return;
 353     }
 354     DLOG("Actually sent: %d\n", real_transferred);
 355
 356     // now wait for our response...
 357     // length copied from stlink-usb...
 358     int rx_length = 6;
 359     try = 0;
 360     do {
 361         DLOG("attempting rx\n");
 362         ret = libusb_bulk_transfer(sl->usb_handle, sl->ep_rep, stl->q_buf, rx_length,
 363             &real_transferred, 3 * 1000);
 364         if (ret == LIBUSB_ERROR_PIPE) {
 365             libusb_clear_halt(sl->usb_handle, sl->ep_req);
 366         }
 367         try++;
 368     } while ((ret == LIBUSB_ERROR_PIPE) && (try < 3));
 369
 370     if (ret != LIBUSB_SUCCESS) {
 371         WLOG("Receiving failed: %d\n", ret);
 372         return;
 373     }
 374
 375     if (real_transferred != rx_length) {
 376         WLOG("received unexpected amount: %d != %d\n", real_transferred, rx_length);
 377     }
 378
 379     DLOG("Actually received: %d\n", real_transferred);
 380 }
 381
 382 // Get stlink mode:
 383 // STLINK_DEV_DFU_MODE || STLINK_DEV_MASS_MODE || STLINK_DEV_DEBUG_MODE
 384 // usb dfu             || usb mass             || jtag or swd
 385
 386 int _stlink_sg_current_mode(stlink_t *stl) {
 387     struct stlink_libsg *sl = stl->backend_data;
 388     clear_cdb(sl);
 389     sl->cdb_cmd_blk[0] = STLINK_GET_CURRENT_MODE;
 390     stl->q_len = 2;
 391     sl->q_addr = 0;
 392     stlink_q(stl);
 393     return stl->q_buf[0];
 394 }
 395
 396 // Exit the mass mode and enter the swd debug mode.
 397
 398 void _stlink_sg_enter_swd_mode(stlink_t *sl) {
 399     struct stlink_libsg *sg = sl->backend_data;
 400     clear_cdb(sg);
 401     sg->cdb_cmd_blk[1] = STLINK_DEBUG_ENTER;
 402     sg->cdb_cmd_blk[2] = STLINK_DEBUG_ENTER_SWD;
 403     sl->q_len = 0; // >0 -> aboard
 404     stlink_q(sl);
 405 }
 406
 407 // Exit the mass mode and enter the jtag debug mode.
 408 // (jtag is disabled in the discovery's stlink firmware)
 409
 410 void _stlink_sg_enter_jtag_mode(stlink_t *sl) {
 411     struct stlink_libsg *sg = sl->backend_data;
 412     DLOG("\n*** stlink_enter_jtag_mode ***\n");
 413     clear_cdb(sg);
 414     sg->cdb_cmd_blk[1] = STLINK_DEBUG_ENTER;
 415     sg->cdb_cmd_blk[2] = STLINK_DEBUG_ENTER_JTAG;
 416     sl->q_len = 0;
 417     stlink_q(sl);
 418 }
 419
 420 // XXX kernel driver performs reset, the device temporally disappears
 421
 422 void _stlink_sg_exit_dfu_mode(stlink_t *sl) {
 423     struct stlink_libsg *sg = sl->backend_data;
 424     DLOG("\n*** stlink_exit_dfu_mode ***\n");
 425     clear_cdb(sg);
 426     sg->cdb_cmd_blk[0] = STLINK_DFU_COMMAND;
 427     sg->cdb_cmd_blk[1] = STLINK_DFU_EXIT;
 428     sl->q_len = 0; // ??
 429     stlink_q(sl);
 430     /*
 431      [135121.844564] sd 19:0:0:0: [sdb] Unhandled error code
 432      [135121.844569] sd 19:0:0:0: [sdb] Result: hostbyte=DID_ERROR driverbyte=DRIVER_OK
 433      [135121.844574] sd 19:0:0:0: [sdb] CDB: Read(10): 28 00 00 00 10 00 00 00 08 00
 434      [135121.844584] end_request: I/O error, dev sdb, sector 4096
 435      [135121.844590] Buffer I/O error on device sdb, logical block 512
 436      [135130.122567] usb 6-1: reset full speed USB device using uhci_hcd and address 7
 437      [135130.274551] usb 6-1: device firmware changed
 438      [135130.274618] usb 6-1: USB disconnect, address 7
 439      [135130.275186] VFS: busy inodes on changed media or resized disk sdb
 440      [135130.275424] VFS: busy inodes on changed media or resized disk sdb
 441      [135130.286758] VFS: busy inodes on changed media or resized disk sdb
 442      [135130.292796] VFS: busy inodes on changed media or resized disk sdb
 443      [135130.301481] VFS: busy inodes on changed media or resized disk sdb
 444      [135130.304316] VFS: busy inodes on changed media or resized disk sdb
 445      [135130.431113] usb 6-1: new full speed USB device using uhci_hcd and address 8
 446      [135130.629444] usb-storage 6-1:1.0: Quirks match for vid 0483 pid 3744: 102a1
 447      [135130.629492] scsi20 : usb-storage 6-1:1.0
 448      [135131.625600] scsi 20:0:0:0: Direct-Access     STM32                          PQ: 0 ANSI: 0
 449      [135131.627010] sd 20:0:0:0: Attached scsi generic sg2 type 0
 450      [135131.633603] sd 20:0:0:0: [sdb] 64000 512-byte logical blocks: (32.7 MB/31.2 MiB)
 451      [135131.633613] sd 20:0:0:0: [sdb] Assuming Write Enabled
 452      [135131.633620] sd 20:0:0:0: [sdb] Assuming drive cache: write through
 453      [135131.640584] sd 20:0:0:0: [sdb] Assuming Write Enabled
 454      [135131.640592] sd 20:0:0:0: [sdb] Assuming drive cache: write through
 455      [135131.640609]  sdb:
 456      [135131.652634] sd 20:0:0:0: [sdb] Assuming Write Enabled
 457      [135131.652639] sd 20:0:0:0: [sdb] Assuming drive cache: write through
 458      [135131.652645] sd 20:0:0:0: [sdb] Attached SCSI removable disk
 459      [135131.671536] sd 20:0:0:0: [sdb] Result: hostbyte=DID_OK driverbyte=DRIVER_SENSE
 460      [135131.671548] sd 20:0:0:0: [sdb] Sense Key : Illegal Request [current]
 461      [135131.671553] sd 20:0:0:0: [sdb] Add. Sense: Logical block address out of range
 462      [135131.671560] sd 20:0:0:0: [sdb] CDB: Read(10): 28 00 00 00 f9 80 00 00 08 00
 463      [135131.671570] end_request: I/O error, dev sdb, sector 63872
 464      [135131.671575] Buffer I/O error on device sdb, logical block 7984
 465      [135131.678527] sd 20:0:0:0: [sdb] Result: hostbyte=DID_OK driverbyte=DRIVER_SENSE
 466      [135131.678532] sd 20:0:0:0: [sdb] Sense Key : Illegal Request [current]
 467      [135131.678537] sd 20:0:0:0: [sdb] Add. Sense: Logical block address out of range
 468      [135131.678542] sd 20:0:0:0: [sdb] CDB: Read(10): 28 00 00 00 f9 80 00 00 08 00
 469      [135131.678551] end_request: I/O error, dev sdb, sector 63872
 470      ...
 471      [135131.853565] end_request: I/O error, dev sdb, sector 4096
 472      */
 473 }
 474
 475 void _stlink_sg_core_id(stlink_t *sl) {
 476     struct stlink_libsg *sg = sl->backend_data;
 477     clear_cdb(sg);
 478     sg->cdb_cmd_blk[1] = STLINK_DEBUG_READCOREID;
 479     sl->q_len = 4;
 480     sg->q_addr = 0;
 481     stlink_q(sl);
 482     sl->core_id = read_uint32(sl->q_buf, 0);
 483 }
 484
 485 // Arm-core reset -> halted state.
 486
 487 void _stlink_sg_reset(stlink_t *sl) {
 488     struct stlink_libsg *sg = sl->backend_data;
 489     clear_cdb(sg);
 490     sg->cdb_cmd_blk[1] = STLINK_DEBUG_RESETSYS;
 491     sl->q_len = 2;
 492     sg->q_addr = 0;
 493     stlink_q(sl);
 494     stlink_stat(sl, "core reset");
 495 }
 496
 497 // Arm-core status: halted or running.
 498
 499 void _stlink_sg_status(stlink_t *sl) {
 500     struct stlink_libsg *sg = sl->backend_data;
 501     DLOG("\n*** stlink_status ***\n");
 502     clear_cdb(sg);
 503     sg->cdb_cmd_blk[1] = STLINK_DEBUG_GETSTATUS;
 504     sl->q_len = 2;
 505     sg->q_addr = 0;
 506     stlink_q(sl);
 507 }
 508
 509 // Force the core into the debug mode -> halted state.
 510
 511 void _stlink_sg_force_debug(stlink_t *sl) {
 512     struct stlink_libsg *sg = sl->backend_data;
 513     DLOG("\n*** stlink_force_debug ***\n");
 514     clear_cdb(sg);
 515     sg->cdb_cmd_blk[1] = STLINK_DEBUG_FORCEDEBUG;
 516     sl->q_len = 2;
 517     sg->q_addr = 0;
 518     stlink_q(sl);
 519     stlink_stat(sl, "force debug");
 520 }
 521
 522 // Read all arm-core registers.
 523
 524 void _stlink_sg_read_all_regs(stlink_t *sl, reg *regp) {
 525     struct stlink_libsg *sg = sl->backend_data;
 526
 527     /* unused */
 528     regp = regp;
 529
 530     clear_cdb(sg);
 531     sg->cdb_cmd_blk[1] = STLINK_DEBUG_READALLREGS;
 532     sl->q_len = 84;
 533     sg->q_addr = 0;
 534     stlink_q(sl);
 535     stlink_print_data(sl);
 536
 537     // TODO - most of this should be re-extracted up....
 538
 539     // 0-3 | 4-7 | ... | 60-63 | 64-67 | 68-71   | 72-75      | 76-79 | 80-83
 540     // r0  | r1  | ... | r15   | xpsr  | main_sp | process_sp | rw    | rw2
 541     for (int i = 0; i < 16; i++) {
 542         sg->reg.r[i] = read_uint32(sl->q_buf, 4 * i);
 543         if (sl->verbose > 1)
 544             DLOG("r%2d = 0x%08x\n", i, sg->reg.r[i]);
 545     }
 546     sg->reg.xpsr = read_uint32(sl->q_buf, 64);
 547     sg->reg.main_sp = read_uint32(sl->q_buf, 68);
 548     sg->reg.process_sp = read_uint32(sl->q_buf, 72);
 549     sg->reg.rw = read_uint32(sl->q_buf, 76);
 550     sg->reg.rw2 = read_uint32(sl->q_buf, 80);
 551     if (sl->verbose < 2)
 552         return;
 553
 554     DLOG("xpsr       = 0x%08x\n", sg->reg.xpsr);
 555     DLOG("main_sp    = 0x%08x\n", sg->reg.main_sp);
 556     DLOG("process_sp = 0x%08x\n", sg->reg.process_sp);
 557     DLOG("rw         = 0x%08x\n", sg->reg.rw);
 558     DLOG("rw2        = 0x%08x\n", sg->reg.rw2);
 559 }
 560
 561 // Read an arm-core register, the index must be in the range 0..20.
 562 //  0  |  1  | ... |  15   |  16   |   17    |   18       |  19   |  20
 563 // r0  | r1  | ... | r15   | xpsr  | main_sp | process_sp | rw    | rw2
 564
 565 void _stlink_sg_read_reg(stlink_t *sl, int r_idx, reg *regp) {
 566     struct stlink_libsg *sg = sl->backend_data;
 567     clear_cdb(sg);
 568     sg->cdb_cmd_blk[1] = STLINK_DEBUG_READREG;
 569     sg->cdb_cmd_blk[2] = r_idx;
 570     sl->q_len = 4;
 571     sg->q_addr = 0;
 572     stlink_q(sl);
 573     //  0  |  1  | ... |  15   |  16   |   17    |   18       |  19   |  20
 574     // 0-3 | 4-7 | ... | 60-63 | 64-67 | 68-71   | 72-75      | 76-79 | 80-83
 575     // r0  | r1  | ... | r15   | xpsr  | main_sp | process_sp | rw    | rw2
 576     stlink_print_data(sl);
 577
 578     uint32_t r = read_uint32(sl->q_buf, 0);
 579     DLOG("r_idx (%2d) = 0x%08x\n", r_idx, r);
 580
 581     switch (r_idx) {
 582         case 16:
 583             regp->xpsr = r;
 584             break;
 585         case 17:
 586             regp->main_sp = r;
 587             break;
 588         case 18:
 589             regp->process_sp = r;
 590             break;
 591         case 19:
 592             regp->rw = r; //XXX ?(primask, basemask etc.)
 593             break;
 594         case 20:
 595             regp->rw2 = r; //XXX ?(primask, basemask etc.)
 596             break;
 597         default:
 598             regp->r[r_idx] = r;
 599     }
 600 }
 601
 602 // Write an arm-core register. Index:
 603 //  0  |  1  | ... |  15   |  16   |   17    |   18       |  19   |  20
 604 // r0  | r1  | ... | r15   | xpsr  | main_sp | process_sp | rw    | rw2
 605
 606 void _stlink_sg_write_reg(stlink_t *sl, uint32_t reg, int idx) {
 607     struct stlink_libsg *sg = sl->backend_data;
 608     clear_cdb(sg);
 609     sg->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEREG;
 610     //   2: reg index
 611     // 3-6: reg content
 612     sg->cdb_cmd_blk[2] = idx;
 613     write_uint32(sg->cdb_cmd_blk + 3, reg);
 614     sl->q_len = 2;
 615     sg->q_addr = 0;
 616     stlink_q(sl);
 617     stlink_stat(sl, "write reg");
 618 }
 619
 620 // Write a register of the debug module of the core.
 621 // XXX ?(atomic writes)
 622 // TODO test
 623
 624 void stlink_write_dreg(stlink_t *sl, uint32_t reg, uint32_t addr) {
 625     struct stlink_libsg *sg = sl->backend_data;
 626     DLOG("\n*** stlink_write_dreg ***\n");
 627     clear_cdb(sg);
 628     sg->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEDEBUGREG;
 629     // 2-5: address of reg of the debug module
 630     // 6-9: reg content
 631     write_uint32(sg->cdb_cmd_blk + 2, addr);
 632     write_uint32(sg->cdb_cmd_blk + 6, reg);
 633     sl->q_len = 2;
 634     sg->q_addr = addr;
 635     stlink_q(sl);
 636     stlink_stat(sl, "write debug reg");
 637 }
 638
 639 // Force the core exit the debug mode.
 640
 641 void _stlink_sg_run(stlink_t *sl) {
 642     struct stlink_libsg *sg = sl->backend_data;
 643     DLOG("\n*** stlink_run ***\n");
 644     clear_cdb(sg);
 645     sg->cdb_cmd_blk[1] = STLINK_DEBUG_RUNCORE;
 646     sl->q_len = 2;
 647     sg->q_addr = 0;
 648     stlink_q(sl);
 649     stlink_stat(sl, "run core");
 650 }
 651
 652 // Step the arm-core.
 653
 654 void _stlink_sg_step(stlink_t *sl) {
 655     struct stlink_libsg *sg = sl->backend_data;
 656     clear_cdb(sg);
 657     sg->cdb_cmd_blk[1] = STLINK_DEBUG_STEPCORE;
 658     sl->q_len = 2;
 659     sg->q_addr = 0;
 660     stlink_q(sl);
 661     stlink_stat(sl, "step core");
 662 }
 663
 664 // TODO test
 665 // see Cortex-M3 Technical Reference Manual
 666 // TODO make delegate!
 667 void stlink_set_hw_bp(stlink_t *sl, int fp_nr, uint32_t addr, int fp) {
 668     DLOG("\n*** stlink_set_hw_bp ***\n");
 669     struct stlink_libsg *sg = sl->backend_data;
 670     clear_cdb(sg);
 671     sg->cdb_cmd_blk[1] = STLINK_DEBUG_SETFP;
 672     // 2:The number of the flash patch used to set the breakpoint
 673     // 3-6: Address of the breakpoint (LSB)
 674     // 7: FP_ALL (0x02) / FP_UPPER (0x01) / FP_LOWER (0x00)
 675     sl->q_buf[2] = fp_nr;
 676     write_uint32(sl->q_buf, addr);
 677     sl->q_buf[7] = fp;
 678
 679     sl->q_len = 2;
 680     stlink_q(sl);
 681     stlink_stat(sl, "set flash breakpoint");
 682 }
 683
 684 // TODO test
 685
 686 // TODO make delegate!
 687 void stlink_clr_hw_bp(stlink_t *sl, int fp_nr) {
 688     struct stlink_libsg *sg = sl->backend_data;
 689     DLOG("\n*** stlink_clr_hw_bp ***\n");
 690     clear_cdb(sg);
 691     sg->cdb_cmd_blk[1] = STLINK_DEBUG_CLEARFP;
 692     sg->cdb_cmd_blk[2] = fp_nr;
 693
 694     sl->q_len = 2;
 695     stlink_q(sl);
 696     stlink_stat(sl, "clear flash breakpoint");
 697 }
 698
 699 // Read a "len" bytes to the sl->q_buf from the memory, max 6kB (6144 bytes)
 700
 701 void _stlink_sg_read_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
 702     struct stlink_libsg *sg = sl->backend_data;
 703     clear_cdb(sg);
 704     sg->cdb_cmd_blk[1] = STLINK_DEBUG_READMEM_32BIT;
 705     // 2-5: addr
 706     // 6-7: len
 707     write_uint32(sg->cdb_cmd_blk + 2, addr);
 708     write_uint16(sg->cdb_cmd_blk + 6, len);
 709
 710     // data_in 0-0x40-len
 711     // !!! len _and_ q_len must be max 6k,
 712     //     i.e. >1024 * 6 = 6144 -> aboard)
 713     // !!! if len < q_len: 64*k, 1024*n, n=1..5  -> aboard
 714     //     (broken residue issue)
 715     sl->q_len = len;
 716     sg->q_addr = addr;
 717     stlink_q(sl);
 718     stlink_print_data(sl);
 719 }
 720
 721 // Write a "len" bytes from the sl->q_buf to the memory, max 64 Bytes.
 722
 723 void _stlink_sg_write_mem8(stlink_t *sl, uint32_t addr, uint16_t len) {
 724     struct stlink_libsg *sg = sl->backend_data;
 725     clear_cdb(sg);
 726     sg->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEMEM_8BIT;
 727     // 2-5: addr
 728     // 6-7: len (>0x40 (64) -> aboard)
 729     write_uint32(sg->cdb_cmd_blk + 2, addr);
 730     write_uint16(sg->cdb_cmd_blk + 6, len);
 731
 732     // data_out 0-len
 733     sl->q_len = len;
 734     sg->q_addr = addr;
 735     sg->q_data_dir = Q_DATA_OUT;
 736     stlink_q(sl);
 737     stlink_print_data(sl);
 738 }
 739
 740 // Write a "len" bytes from the sl->q_buf to the memory, max Q_BUF_LEN bytes.
 741
 742 void _stlink_sg_write_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
 743     struct stlink_libsg *sg = sl->backend_data;
 744     clear_cdb(sg);
 745     sg->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEMEM_32BIT;
 746     // 2-5: addr
 747     // 6-7: len "unlimited"
 748     write_uint32(sg->cdb_cmd_blk + 2, addr);
 749     write_uint16(sg->cdb_cmd_blk + 6, len);
 750
 751     // data_out 0-0x40-...-len
 752     sl->q_len = len;
 753     sg->q_addr = addr;
 754     sg->q_data_dir = Q_DATA_OUT;
 755     stlink_q(sl);
 756     stlink_print_data(sl);
 757 }
 758
 759 #if 0 /* not working */
 760
 761 static int write_flash_mem16
 762 (struct stlink* sl, uint32_t addr, uint16_t val) {
 763     /* half word writes */
 764     if (addr % 2) return -1;
 765
 766     /* unlock if locked */
 767     unlock_flash_if(sl);
 768
 769     /* set flash programming chosen bit */
 770     set_flash_cr_pg(sl);
 771
 772     write_uint16(sl->q_buf, val);
 773     stlink_write_mem16(sl, addr, 2);
 774
 775     /* wait for non business */
 776     wait_flash_busy(sl);
 777
 778     lock_flash(sl);
 779
 780     /* check the programmed value back */
 781     stlink_read_mem16(sl, addr, 2);
 782     if (*(const uint16_t*) sl->q_buf != val) {
 783         /* values differ at i * sizeof(uint16_t) */
 784         return -1;
 785     }
 786
 787     /* success */
 788     return 0;
 789 }
 790 #endif /* not working */
 791
 792 // Exit the jtag or swd mode and enter the mass mode.
 793
 794 void _stlink_sg_exit_debug_mode(stlink_t *stl) {
 795
 796     if (stl) {
 797         struct stlink_libsg* sl = stl->backend_data;
 798         clear_cdb(sl);
 799         sl->cdb_cmd_blk[1] = STLINK_DEBUG_EXIT;
 800         stl->q_len = 0; // >0 -> aboard
 801         stlink_q(stl);
 802     }
 803 }
 804
 805
 806 // 1) open a sg device, switch the stlink from dfu to mass mode
 807 // 2) wait 5s until the kernel driver stops reseting the broken device
 808 // 3) reopen the device
 809 // 4) the device driver is now ready for a switch to jtag/swd mode
 810 // TODO thinking, better error handling, wait until the kernel driver stops reseting the plugged-in device
 811
 812 stlink_backend_t _stlink_sg_backend = {
 813     _stlink_sg_close,
 814     _stlink_sg_exit_debug_mode,
 815     _stlink_sg_enter_swd_mode,
 816     _stlink_sg_enter_jtag_mode,
 817     _stlink_sg_exit_dfu_mode,
 818     _stlink_sg_core_id,
 819     _stlink_sg_reset,
 820     _stlink_sg_run,
 821     _stlink_sg_status,
 822     _stlink_sg_version,
 823     _stlink_sg_read_mem32,
 824     _stlink_sg_write_mem32,
 825     _stlink_sg_write_mem8,
 826     _stlink_sg_read_all_regs,
 827     _stlink_sg_read_reg,
 828     _stlink_sg_write_reg,
 829     _stlink_sg_step,
 830     _stlink_sg_current_mode,
 831     _stlink_sg_force_debug
 832 };
 833
 834 static stlink_t* stlink_open(const char *dev_name, const int verbose) {
 835     DLOG("*** stlink_open [%s] ***\n", dev_name);
 836
 837     stlink_t *sl = malloc(sizeof (stlink_t));
 838     struct stlink_libsg *slsg = malloc(sizeof (struct stlink_libsg));
 839     if (sl == NULL || slsg == NULL) {
 840         WLOG("Couldn't malloc stlink and stlink_sg structures out of memory!\n");
 841         return NULL;
 842     }
 843
 844     if (libusb_init(&(slsg->libusb_ctx))) {
 845         WLOG("failed to init libusb context, wrong version of libraries?\n");
 846         free(sl);
 847         free(slsg);
 848         return NULL;
 849     }
 850
 851     libusb_set_debug(slsg->libusb_ctx, 3);
 852
 853     slsg->usb_handle = libusb_open_device_with_vid_pid(slsg->libusb_ctx, USB_ST_VID, USB_STLINK_PID);
 854     if (slsg->usb_handle == NULL) {
 855         WLOG("Failed to find an stlink v1 by VID:PID\n");
 856         libusb_close(slsg->usb_handle);
 857         free(sl);
 858         free(slsg);
 859         return NULL;
 860     }
 861
 862     // TODO
 863     // Could read the interface config descriptor, and assert lots of the assumptions
 864
 865     // assumption: numInterfaces is always 1...
 866     if (libusb_kernel_driver_active(slsg->usb_handle, 0) == 1) {
 867         int r = libusb_detach_kernel_driver(slsg->usb_handle, 0);
 868         if (r < 0) {
 869             WLOG("libusb_detach_kernel_driver(() error %s\n", strerror(-r));
 870             libusb_close(slsg->usb_handle);
 871             free(sl);
 872             free(slsg);
 873             return NULL;
 874         }
 875         DLOG("Kernel driver was successfully detached\n");
 876     }
 877
 878     int config;
 879     if (libusb_get_configuration(slsg->usb_handle, &config)) {
 880         /* this may fail for a previous configured device */
 881         WLOG("libusb_get_configuration()\n");
 882         libusb_close(slsg->usb_handle);
 883         free(sl);
 884         free(slsg);
 885         return NULL;
 886
 887     }
 888
 889     // assumption: bConfigurationValue is always 1
 890     if (config != 1) {
 891         WLOG("Your stlink got into a real weird configuration, trying to fix it!\n");
 892         DLOG("setting new configuration (%d -> 1)\n", config);
 893         if (libusb_set_configuration(slsg->usb_handle, 1)) {
 894             /* this may fail for a previous configured device */
 895             WLOG("libusb_set_configuration() failed\n");
 896             libusb_close(slsg->usb_handle);
 897             free(sl);
 898             free(slsg);
 899             return NULL;
 900         }
 901     }
 902
 903     if (libusb_claim_interface(slsg->usb_handle, 0)) {
 904         WLOG("libusb_claim_interface() failed\n");
 905         libusb_close(slsg->usb_handle);
 906         free(sl);
 907         free(slsg);
 908         return NULL;
 909     }
 910
 911     // assumption: endpoint config is fixed mang. really.
 912     slsg->ep_rep = 1 /* ep rep */ | LIBUSB_ENDPOINT_IN;
 913     slsg->ep_req = 2 /* ep req */ | LIBUSB_ENDPOINT_OUT;
 914
 915     DLOG("Successfully opened stlinkv1 by libusb :)\n");
 916
 917     sl->verbose = verbose;
 918     sl->backend_data = slsg;
 919     sl->backend = &_stlink_sg_backend;
 920
 921     sl->core_stat = STLINK_CORE_STAT_UNKNOWN;
 922     slsg->q_addr = 0;
 923     clear_buf(sl);
 924
 925     /* flash memory settings */
 926     sl->flash_base = STM32_FLASH_BASE;
 927     sl->flash_size = STM32_FLASH_SIZE;
 928     sl->flash_pgsz = STM32_FLASH_PGSZ;
 929
 930     /* system memory */
 931     sl->sys_base = STM32_SYSTEM_BASE;
 932     sl->sys_size = STM32_SYSTEM_SIZE;
 933
 934     /* sram memory settings */
 935     sl->sram_base = STM32_SRAM_BASE;
 936     sl->sram_size = STM32_SRAM_SIZE;
 937
 938     return sl;
 939 }
 940
 941
 942
 943 stlink_t* stlink_v1_open(const char *dev_name, const int verbose) {
 944     ugly_init(verbose);
 945     stlink_t *sl = stlink_open(dev_name, verbose);
 946     if (sl == NULL) {
 947         fputs("Error: could not open stlink device\n", stderr);
 948         return NULL;
 949     }
 950
 951     stlink_version(sl);
 952     struct stlink_libsg *sg = sl->backend_data;
 953
 954     if ((sl->version.st_vid != USB_ST_VID) || (sl->version.stlink_pid != USB_STLINK_PID)) {
 955         fprintf(stderr, "Error: the device %s is not a stlink\n",
 956                 dev_name);
 957         fprintf(stderr, "       VID: got %04x expect %04x \n",
 958                 sl->version.st_vid, USB_ST_VID);
 959         fprintf(stderr, "       PID: got %04x expect %04x \n",
 960                 sl->version.stlink_pid, USB_STLINK_PID);
 961         return NULL;
 962     }
 963
 964     DLOG("\n*** stlink_force_open ***\n");
 965     switch (stlink_current_mode(sl)) {
 966         case STLINK_DEV_MASS_MODE:
 967             return sl;
 968         case STLINK_DEV_DEBUG_MODE:
 969             // TODO go to mass?
 970             return sl;
 971     }
 972     DLOG("\n*** switch the stlink to mass mode ***\n");
 973     _stlink_sg_exit_dfu_mode(sl);
 974     // exit the dfu mode -> the device is gone
 975     DLOG("\n*** reopen the stlink device ***\n");
 976     delay(1000);
 977     stlink_close(sl);
 978     delay(5000);
 979
 980     sl = stlink_open(dev_name, verbose);
 981     if (sl == NULL) {
 982         fputs("Error: could not open stlink device\n", stderr);
 983         return NULL;
 984     }
 985     // re-query device info
 986     stlink_version(sl);
 987     return sl;
 988 }
 989
 990 static void __attribute__((unused)) mark_buf(stlink_t *sl) {
 991     clear_buf(sl);
 992     sl->q_buf[0] = 0x12;
 993     sl->q_buf[1] = 0x34;
 994     sl->q_buf[2] = 0x56;
 995     sl->q_buf[3] = 0x78;
 996     sl->q_buf[4] = 0x90;
 997     sl->q_buf[15] = 0x42;
 998     sl->q_buf[16] = 0x43;
 999     sl->q_buf[63] = 0x42;
1000     sl->q_buf[64] = 0x43;
1001     sl->q_buf[1024 * 6 - 1] = 0x42; //6kB
1002     sl->q_buf[1024 * 8 - 1] = 0x42; //8kB
1003 }