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 // sgutils2 (apt-get install libsgutils2-dev)
  84 #include <scsi/sg_lib.h>
  85 #include <scsi/sg_pt.h>
  86
  87 #include "stlink-common.h"
  88 #include "stlink-sg.h"
  89
  90
  91 // Suspends execution of the calling process for
  92 // (at least) ms milliseconds.
  93
  94 static void delay(int ms) {
  95     //fprintf(stderr, "*** wait %d ms\n", ms);
  96     usleep(1000 * ms);
  97 }
  98
  99 static void clear_cdb(struct stlink_libsg *sl) {
 100     for (size_t i = 0; i < sizeof (sl->cdb_cmd_blk); i++)
 101         sl->cdb_cmd_blk[i] = 0;
 102     // set default
 103     sl->cdb_cmd_blk[0] = STLINK_DEBUG_COMMAND;
 104     sl->q_data_dir = Q_DATA_IN;
 105 }
 106
 107 // E.g. make the valgrind happy.
 108
 109 static void clear_buf(stlink_t *sl) {
 110     DD(sl, "*** clear_buf ***\n");
 111     for (size_t i = 0; i < sizeof (sl->q_buf); i++)
 112         sl->q_buf[i] = 0;
 113
 114 }
 115
 116 // close the device, free the allocated memory
 117
 118 void _stlink_sg_close(stlink_t *sl) {
 119     if (sl) {
 120         struct stlink_libsg *slsg = sl->backend_data;
 121         scsi_pt_close_device(slsg->sg_fd);
 122         // CAUTION!? s this right?
 123         free(slsg);
 124         free(sl);
 125     }
 126 }
 127
 128
 129 //TODO rewrite/cleanup, save the error in sl
 130
 131 static void stlink_confirm_inq(stlink_t *stl, struct sg_pt_base *ptvp) {
 132     struct stlink_libsg *sl = stl->backend_data;
 133     const int e = sl->do_scsi_pt_err;
 134     if (e < 0) {
 135         fprintf(stderr, "scsi_pt error: pass through os error: %s\n",
 136                 safe_strerror(-e));
 137         return;
 138     } else if (e == SCSI_PT_DO_BAD_PARAMS) {
 139         fprintf(stderr, "scsi_pt error: bad pass through setup\n");
 140         return;
 141     } else if (e == SCSI_PT_DO_TIMEOUT) {
 142         fprintf(stderr, "  pass through timeout\n");
 143         return;
 144     }
 145     const int duration = get_scsi_pt_duration_ms(ptvp);
 146     if ((stl->verbose > 1) && (duration >= 0))
 147         DD(stl, "      duration=%d ms\n", duration);
 148
 149     // XXX stlink fw sends broken residue, so ignore it and use the known q_len
 150     // "usb-storage quirks=483:3744:r"
 151     // forces residue to be ignored and calculated, but this causes aboard if
 152     // data_len = 0 and by some other data_len values.
 153
 154     const int resid = get_scsi_pt_resid(ptvp);
 155     const int dsize = stl->q_len - resid;
 156
 157     const int cat = get_scsi_pt_result_category(ptvp);
 158     char buf[512];
 159     unsigned int slen;
 160
 161     switch (cat) {
 162         case SCSI_PT_RESULT_GOOD:
 163             if (stl->verbose && (resid > 0))
 164                 DD(stl, "      notice: requested %d bytes but "
 165                     "got %d bytes, ignore [broken] residue = %d\n",
 166                     stl->q_len, dsize, resid);
 167             break;
 168         case SCSI_PT_RESULT_STATUS:
 169             if (stl->verbose) {
 170                 sg_get_scsi_status_str(
 171                         get_scsi_pt_status_response(ptvp), sizeof (buf),
 172                         buf);
 173                 DD(stl, "  scsi status: %s\n", buf);
 174             }
 175             return;
 176         case SCSI_PT_RESULT_SENSE:
 177             slen = get_scsi_pt_sense_len(ptvp);
 178             if (stl->verbose) {
 179                 sg_get_sense_str("", sl->sense_buf, slen, (stl->verbose
 180                         > 1), sizeof (buf), buf);
 181                 DD(stl, "%s", buf);
 182             }
 183             if (stl->verbose && (resid > 0)) {
 184                 if ((stl->verbose) || (stl->q_len > 0))
 185                     DD(stl, "    requested %d bytes but "
 186                         "got %d bytes\n", stl->q_len, dsize);
 187             }
 188             return;
 189         case SCSI_PT_RESULT_TRANSPORT_ERR:
 190             if (stl->verbose) {
 191                 get_scsi_pt_transport_err_str(ptvp, sizeof (buf), buf);
 192                 // http://tldp.org/HOWTO/SCSI-Generic-HOWTO/x291.html
 193                 // These codes potentially come from the firmware on a host adapter
 194                 // or from one of several hosts that an adapter driver controls.
 195                 // The 'host_status' field has the following values:
 196                 //      [0x07] Internal error detected in the host adapter.
 197                 // This may not be fatal (and the command may have succeeded).
 198                 DD(stl, "  transport: %s", buf);
 199             }
 200             return;
 201         case SCSI_PT_RESULT_OS_ERR:
 202             if (stl->verbose) {
 203                 get_scsi_pt_os_err_str(ptvp, sizeof (buf), buf);
 204                 DD(stl, "  os: %s", buf);
 205             }
 206             return;
 207         default:
 208             fprintf(stderr, "  unknown pass through result "
 209                     "category (%d)\n", cat);
 210     }
 211 }
 212
 213 void stlink_q(stlink_t *sl) {
 214     struct stlink_libsg* sg = sl->backend_data;
 215     DD(sl, "CDB[");
 216     for (int i = 0; i < CDB_SL; i++)
 217         DD(sl, " 0x%02x", (unsigned int) sg->cdb_cmd_blk[i]);
 218     DD(sl, "]\n");
 219
 220     // Get control command descriptor of scsi structure,
 221     // (one object per command!!)
 222     struct sg_pt_base *ptvp = construct_scsi_pt_obj();
 223     if (NULL == ptvp) {
 224         fprintf(stderr, "construct_scsi_pt_obj: out of memory\n");
 225         return;
 226     }
 227
 228     set_scsi_pt_cdb(ptvp, sg->cdb_cmd_blk, sizeof (sg->cdb_cmd_blk));
 229
 230     // set buffer for sense (error information) data
 231     set_scsi_pt_sense(ptvp, sg->sense_buf, sizeof (sg->sense_buf));
 232
 233     // Set a buffer to be used for data transferred from device
 234     if (sg->q_data_dir == Q_DATA_IN) {
 235         //clear_buf(sl);
 236         set_scsi_pt_data_in(ptvp, sl->q_buf, sl->q_len);
 237     } else {
 238         set_scsi_pt_data_out(ptvp, sl->q_buf, sl->q_len);
 239     }
 240     // Executes SCSI command (or at least forwards it to lower layers).
 241     sg->do_scsi_pt_err = do_scsi_pt(ptvp, sg->sg_fd, SG_TIMEOUT_SEC,
 242             sl->verbose);
 243
 244     // check for scsi errors
 245     stlink_confirm_inq(sl, ptvp);
 246     // TODO recycle: clear_scsi_pt_obj(struct sg_pt_base * objp);
 247     destruct_scsi_pt_obj(ptvp);
 248 }
 249
 250 // TODO thinking, cleanup
 251
 252 void stlink_stat(stlink_t *stl, char *txt) {
 253     if (stl->q_len <= 0)
 254         return;
 255
 256     stlink_print_data(stl);
 257
 258     switch (stl->q_buf[0]) {
 259         case STLINK_OK:
 260             DD(stl, "  %s: ok\n", txt);
 261             return;
 262         case STLINK_FALSE:
 263             DD(stl, "  %s: false\n", txt);
 264             return;
 265         default:
 266             DD(stl, "  %s: unknown\n", txt);
 267     }
 268 }
 269
 270 void _stlink_sg_version(stlink_t *stl) {
 271     struct stlink_libsg *sl = stl->backend_data;
 272     D(stl, "\n*** stlink_version ***\n");
 273     clear_cdb(sl);
 274     sl->cdb_cmd_blk[0] = STLINK_GET_VERSION;
 275     stl->q_len = 6;
 276     sl->q_addr = 0;
 277     stlink_q(stl);
 278 }
 279
 280 // Get stlink mode:
 281 // STLINK_DEV_DFU_MODE || STLINK_DEV_MASS_MODE || STLINK_DEV_DEBUG_MODE
 282 // usb dfu             || usb mass             || jtag or swd
 283
 284 int _stlink_sg_current_mode(stlink_t *stl) {
 285     struct stlink_libsg *sl = stl->backend_data;
 286     clear_cdb(sl);
 287     sl->cdb_cmd_blk[0] = STLINK_GET_CURRENT_MODE;
 288     stl->q_len = 2;
 289     sl->q_addr = 0;
 290     stlink_q(stl);
 291     return stl->q_buf[0];
 292 }
 293
 294 // Exit the mass mode and enter the swd debug mode.
 295
 296 void _stlink_sg_enter_swd_mode(stlink_t *sl) {
 297     struct stlink_libsg *sg = sl->backend_data;
 298     clear_cdb(sg);
 299     sg->cdb_cmd_blk[1] = STLINK_DEBUG_ENTER;
 300     sg->cdb_cmd_blk[2] = STLINK_DEBUG_ENTER_SWD;
 301     sl->q_len = 0; // >0 -> aboard
 302     stlink_q(sl);
 303 }
 304
 305 // Exit the mass mode and enter the jtag debug mode.
 306 // (jtag is disabled in the discovery's stlink firmware)
 307
 308 void _stlink_sg_enter_jtag_mode(stlink_t *sl) {
 309     struct stlink_libsg *sg = sl->backend_data;
 310     D(sl, "\n*** stlink_enter_jtag_mode ***\n");
 311     clear_cdb(sg);
 312     sg->cdb_cmd_blk[1] = STLINK_DEBUG_ENTER;
 313     sg->cdb_cmd_blk[2] = STLINK_DEBUG_ENTER_JTAG;
 314     sl->q_len = 0;
 315     stlink_q(sl);
 316 }
 317
 318 // XXX kernel driver performs reset, the device temporally disappears
 319
 320 void _stlink_sg_exit_dfu_mode(stlink_t *sl) {
 321     struct stlink_libsg *sg = sl->backend_data;
 322     D(sl, "\n*** stlink_exit_dfu_mode ***\n");
 323     clear_cdb(sg);
 324     sg->cdb_cmd_blk[0] = STLINK_DFU_COMMAND;
 325     sg->cdb_cmd_blk[1] = STLINK_DFU_EXIT;
 326     sl->q_len = 0; // ??
 327     stlink_q(sl);
 328     /*
 329      [135121.844564] sd 19:0:0:0: [sdb] Unhandled error code
 330      [135121.844569] sd 19:0:0:0: [sdb] Result: hostbyte=DID_ERROR driverbyte=DRIVER_OK
 331      [135121.844574] sd 19:0:0:0: [sdb] CDB: Read(10): 28 00 00 00 10 00 00 00 08 00
 332      [135121.844584] end_request: I/O error, dev sdb, sector 4096
 333      [135121.844590] Buffer I/O error on device sdb, logical block 512
 334      [135130.122567] usb 6-1: reset full speed USB device using uhci_hcd and address 7
 335      [135130.274551] usb 6-1: device firmware changed
 336      [135130.274618] usb 6-1: USB disconnect, address 7
 337      [135130.275186] VFS: busy inodes on changed media or resized disk sdb
 338      [135130.275424] VFS: busy inodes on changed media or resized disk sdb
 339      [135130.286758] VFS: busy inodes on changed media or resized disk sdb
 340      [135130.292796] VFS: busy inodes on changed media or resized disk sdb
 341      [135130.301481] VFS: busy inodes on changed media or resized disk sdb
 342      [135130.304316] VFS: busy inodes on changed media or resized disk sdb
 343      [135130.431113] usb 6-1: new full speed USB device using uhci_hcd and address 8
 344      [135130.629444] usb-storage 6-1:1.0: Quirks match for vid 0483 pid 3744: 102a1
 345      [135130.629492] scsi20 : usb-storage 6-1:1.0
 346      [135131.625600] scsi 20:0:0:0: Direct-Access     STM32                          PQ: 0 ANSI: 0
 347      [135131.627010] sd 20:0:0:0: Attached scsi generic sg2 type 0
 348      [135131.633603] sd 20:0:0:0: [sdb] 64000 512-byte logical blocks: (32.7 MB/31.2 MiB)
 349      [135131.633613] sd 20:0:0:0: [sdb] Assuming Write Enabled
 350      [135131.633620] sd 20:0:0:0: [sdb] Assuming drive cache: write through
 351      [135131.640584] sd 20:0:0:0: [sdb] Assuming Write Enabled
 352      [135131.640592] sd 20:0:0:0: [sdb] Assuming drive cache: write through
 353      [135131.640609]  sdb:
 354      [135131.652634] sd 20:0:0:0: [sdb] Assuming Write Enabled
 355      [135131.652639] sd 20:0:0:0: [sdb] Assuming drive cache: write through
 356      [135131.652645] sd 20:0:0:0: [sdb] Attached SCSI removable disk
 357      [135131.671536] sd 20:0:0:0: [sdb] Result: hostbyte=DID_OK driverbyte=DRIVER_SENSE
 358      [135131.671548] sd 20:0:0:0: [sdb] Sense Key : Illegal Request [current]
 359      [135131.671553] sd 20:0:0:0: [sdb] Add. Sense: Logical block address out of range
 360      [135131.671560] sd 20:0:0:0: [sdb] CDB: Read(10): 28 00 00 00 f9 80 00 00 08 00
 361      [135131.671570] end_request: I/O error, dev sdb, sector 63872
 362      [135131.671575] Buffer I/O error on device sdb, logical block 7984
 363      [135131.678527] sd 20:0:0:0: [sdb] Result: hostbyte=DID_OK driverbyte=DRIVER_SENSE
 364      [135131.678532] sd 20:0:0:0: [sdb] Sense Key : Illegal Request [current]
 365      [135131.678537] sd 20:0:0:0: [sdb] Add. Sense: Logical block address out of range
 366      [135131.678542] sd 20:0:0:0: [sdb] CDB: Read(10): 28 00 00 00 f9 80 00 00 08 00
 367      [135131.678551] end_request: I/O error, dev sdb, sector 63872
 368      ...
 369      [135131.853565] end_request: I/O error, dev sdb, sector 4096
 370      */
 371 }
 372
 373 void _stlink_sg_core_id(stlink_t *sl) {
 374     struct stlink_libsg *sg = sl->backend_data;
 375     clear_cdb(sg);
 376     sg->cdb_cmd_blk[1] = STLINK_DEBUG_READCOREID;
 377     sl->q_len = 4;
 378     sg->q_addr = 0;
 379     stlink_q(sl);
 380     sl->core_id = read_uint32(sl->q_buf, 0);
 381 }
 382
 383 // Arm-core reset -> halted state.
 384
 385 void _stlink_sg_reset(stlink_t *sl) {
 386     struct stlink_libsg *sg = sl->backend_data;
 387     clear_cdb(sg);
 388     sg->cdb_cmd_blk[1] = STLINK_DEBUG_RESETSYS;
 389     sl->q_len = 2;
 390     sg->q_addr = 0;
 391     stlink_q(sl);
 392     stlink_stat(sl, "core reset");
 393 }
 394
 395 // Arm-core status: halted or running.
 396
 397 void _stlink_sg_status(stlink_t *sl) {
 398     struct stlink_libsg *sg = sl->backend_data;
 399     D(sl, "\n*** stlink_status ***\n");
 400     clear_cdb(sg);
 401     sg->cdb_cmd_blk[1] = STLINK_DEBUG_GETSTATUS;
 402     sl->q_len = 2;
 403     sg->q_addr = 0;
 404     stlink_q(sl);
 405 }
 406
 407 // Force the core into the debug mode -> halted state.
 408
 409 void _stlink_sg_force_debug(stlink_t *sl) {
 410     struct stlink_libsg *sg = sl->backend_data;
 411     D(sl, "\n*** stlink_force_debug ***\n");
 412     clear_cdb(sg);
 413     sg->cdb_cmd_blk[1] = STLINK_DEBUG_FORCEDEBUG;
 414     sl->q_len = 2;
 415     sg->q_addr = 0;
 416     stlink_q(sl);
 417     stlink_stat(sl, "force debug");
 418 }
 419
 420 // Read all arm-core registers.
 421
 422 void _stlink_sg_read_all_regs(stlink_t *sl, reg *regp) {
 423     struct stlink_libsg *sg = sl->backend_data;
 424     clear_cdb(sg);
 425     sg->cdb_cmd_blk[1] = STLINK_DEBUG_READALLREGS;
 426     sl->q_len = 84;
 427     sg->q_addr = 0;
 428     stlink_q(sl);
 429     stlink_print_data(sl);
 430
 431     // TODO - most of this should be re-extracted up....
 432
 433     // 0-3 | 4-7 | ... | 60-63 | 64-67 | 68-71   | 72-75      | 76-79 | 80-83
 434     // r0  | r1  | ... | r15   | xpsr  | main_sp | process_sp | rw    | rw2
 435     for (int i = 0; i < 16; i++) {
 436         sg->reg.r[i] = read_uint32(sl->q_buf, 4 * i);
 437         if (sl->verbose > 1)
 438             DD(sl, "r%2d = 0x%08x\n", i, sg->reg.r[i]);
 439     }
 440     sg->reg.xpsr = read_uint32(sl->q_buf, 64);
 441     sg->reg.main_sp = read_uint32(sl->q_buf, 68);
 442     sg->reg.process_sp = read_uint32(sl->q_buf, 72);
 443     sg->reg.rw = read_uint32(sl->q_buf, 76);
 444     sg->reg.rw2 = read_uint32(sl->q_buf, 80);
 445     if (sl->verbose < 2)
 446         return;
 447
 448     DD(sl, "xpsr       = 0x%08x\n", sg->reg.xpsr);
 449     DD(sl, "main_sp    = 0x%08x\n", sg->reg.main_sp);
 450     DD(sl, "process_sp = 0x%08x\n", sg->reg.process_sp);
 451     DD(sl, "rw         = 0x%08x\n", sg->reg.rw);
 452     DD(sl, "rw2        = 0x%08x\n", sg->reg.rw2);
 453 }
 454
 455 // Read an arm-core register, the index must be in the range 0..20.
 456 //  0  |  1  | ... |  15   |  16   |   17    |   18       |  19   |  20
 457 // r0  | r1  | ... | r15   | xpsr  | main_sp | process_sp | rw    | rw2
 458
 459 void _stlink_sg_read_reg(stlink_t *sl, int r_idx, reg *regp) {
 460     struct stlink_libsg *sg = sl->backend_data;
 461     clear_cdb(sg);
 462     sg->cdb_cmd_blk[1] = STLINK_DEBUG_READREG;
 463     sg->cdb_cmd_blk[2] = r_idx;
 464     sl->q_len = 4;
 465     sg->q_addr = 0;
 466     stlink_q(sl);
 467     //  0  |  1  | ... |  15   |  16   |   17    |   18       |  19   |  20
 468     // 0-3 | 4-7 | ... | 60-63 | 64-67 | 68-71   | 72-75      | 76-79 | 80-83
 469     // r0  | r1  | ... | r15   | xpsr  | main_sp | process_sp | rw    | rw2
 470     stlink_print_data(sl);
 471
 472     uint32_t r = read_uint32(sl->q_buf, 0);
 473     DD(sl, "r_idx (%2d) = 0x%08x\n", r_idx, r);
 474
 475     switch (r_idx) {
 476         case 16:
 477             regp->xpsr = r;
 478             break;
 479         case 17:
 480             regp->main_sp = r;
 481             break;
 482         case 18:
 483             regp->process_sp = r;
 484             break;
 485         case 19:
 486             regp->rw = r; //XXX ?(primask, basemask etc.)
 487             break;
 488         case 20:
 489             regp->rw2 = r; //XXX ?(primask, basemask etc.)
 490             break;
 491         default:
 492             regp->r[r_idx] = r;
 493     }
 494 }
 495
 496 // Write an arm-core register. Index:
 497 //  0  |  1  | ... |  15   |  16   |   17    |   18       |  19   |  20
 498 // r0  | r1  | ... | r15   | xpsr  | main_sp | process_sp | rw    | rw2
 499
 500 void _stlink_sg_write_reg(stlink_t *sl, uint32_t reg, int idx) {
 501     struct stlink_libsg *sg = sl->backend_data;
 502     clear_cdb(sg);
 503     sg->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEREG;
 504     //   2: reg index
 505     // 3-6: reg content
 506     sg->cdb_cmd_blk[2] = idx;
 507     write_uint32(sg->cdb_cmd_blk + 3, reg);
 508     sl->q_len = 2;
 509     sg->q_addr = 0;
 510     stlink_q(sl);
 511     stlink_stat(sl, "write reg");
 512 }
 513
 514 // Write a register of the debug module of the core.
 515 // XXX ?(atomic writes)
 516 // TODO test
 517
 518 void stlink_write_dreg(stlink_t *sl, uint32_t reg, uint32_t addr) {
 519     struct stlink_libsg *sg = sl->backend_data;
 520     D(sl, "\n*** stlink_write_dreg ***\n");
 521     clear_cdb(sg);
 522     sg->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEDEBUGREG;
 523     // 2-5: address of reg of the debug module
 524     // 6-9: reg content
 525     write_uint32(sg->cdb_cmd_blk + 2, addr);
 526     write_uint32(sg->cdb_cmd_blk + 6, reg);
 527     sl->q_len = 2;
 528     sg->q_addr = addr;
 529     stlink_q(sl);
 530     stlink_stat(sl, "write debug reg");
 531 }
 532
 533 // Force the core exit the debug mode.
 534
 535 void _stlink_sg_run(stlink_t *sl) {
 536     struct stlink_libsg *sg = sl->backend_data;
 537     D(sl, "\n*** stlink_run ***\n");
 538     clear_cdb(sg);
 539     sg->cdb_cmd_blk[1] = STLINK_DEBUG_RUNCORE;
 540     sl->q_len = 2;
 541     sg->q_addr = 0;
 542     stlink_q(sl);
 543     stlink_stat(sl, "run core");
 544 }
 545
 546 // Step the arm-core.
 547
 548 void _stlink_sg_step(stlink_t *sl) {
 549     struct stlink_libsg *sg = sl->backend_data;
 550     clear_cdb(sg);
 551     sg->cdb_cmd_blk[1] = STLINK_DEBUG_STEPCORE;
 552     sl->q_len = 2;
 553     sg->q_addr = 0;
 554     stlink_q(sl);
 555     stlink_stat(sl, "step core");
 556 }
 557
 558 // TODO test
 559 // see Cortex-M3 Technical Reference Manual
 560 // TODO make delegate!
 561 void stlink_set_hw_bp(stlink_t *sl, int fp_nr, uint32_t addr, int fp) {
 562     D(sl, "\n*** stlink_set_hw_bp ***\n");
 563     struct stlink_libsg *sg = sl->backend_data;
 564     clear_cdb(sg);
 565     sg->cdb_cmd_blk[1] = STLINK_DEBUG_SETFP;
 566     // 2:The number of the flash patch used to set the breakpoint
 567     // 3-6: Address of the breakpoint (LSB)
 568     // 7: FP_ALL (0x02) / FP_UPPER (0x01) / FP_LOWER (0x00)
 569     sl->q_buf[2] = fp_nr;
 570     write_uint32(sl->q_buf, addr);
 571     sl->q_buf[7] = fp;
 572
 573     sl->q_len = 2;
 574     stlink_q(sl);
 575     stlink_stat(sl, "set flash breakpoint");
 576 }
 577
 578 // TODO test
 579
 580 // TODO make delegate!
 581 void stlink_clr_hw_bp(stlink_t *sl, int fp_nr) {
 582     struct stlink_libsg *sg = sl->backend_data;
 583     D(sl, "\n*** stlink_clr_hw_bp ***\n");
 584     clear_cdb(sg);
 585     sg->cdb_cmd_blk[1] = STLINK_DEBUG_CLEARFP;
 586     sg->cdb_cmd_blk[2] = fp_nr;
 587
 588     sl->q_len = 2;
 589     stlink_q(sl);
 590     stlink_stat(sl, "clear flash breakpoint");
 591 }
 592
 593 // Read a "len" bytes to the sl->q_buf from the memory, max 6kB (6144 bytes)
 594
 595 void _stlink_sg_read_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
 596     struct stlink_libsg *sg = sl->backend_data;
 597     clear_cdb(sg);
 598     sg->cdb_cmd_blk[1] = STLINK_DEBUG_READMEM_32BIT;
 599     // 2-5: addr
 600     // 6-7: len
 601     write_uint32(sg->cdb_cmd_blk + 2, addr);
 602     write_uint16(sg->cdb_cmd_blk + 6, len);
 603
 604     // data_in 0-0x40-len
 605     // !!! len _and_ q_len must be max 6k,
 606     //     i.e. >1024 * 6 = 6144 -> aboard)
 607     // !!! if len < q_len: 64*k, 1024*n, n=1..5  -> aboard
 608     //     (broken residue issue)
 609     sl->q_len = len;
 610     sg->q_addr = addr;
 611     stlink_q(sl);
 612     stlink_print_data(sl);
 613 }
 614
 615 // Write a "len" bytes from the sl->q_buf to the memory, max 64 Bytes.
 616
 617 void _stlink_sg_write_mem8(stlink_t *sl, uint32_t addr, uint16_t len) {
 618     struct stlink_libsg *sg = sl->backend_data;
 619     clear_cdb(sg);
 620     sg->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEMEM_8BIT;
 621     // 2-5: addr
 622     // 6-7: len (>0x40 (64) -> aboard)
 623     write_uint32(sg->cdb_cmd_blk + 2, addr);
 624     write_uint16(sg->cdb_cmd_blk + 6, len);
 625
 626     // data_out 0-len
 627     sl->q_len = len;
 628     sg->q_addr = addr;
 629     sg->q_data_dir = Q_DATA_OUT;
 630     stlink_q(sl);
 631     stlink_print_data(sl);
 632 }
 633
 634 // Write a "len" bytes from the sl->q_buf to the memory, max Q_BUF_LEN bytes.
 635
 636 void _stlink_sg_write_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
 637     struct stlink_libsg *sg = sl->backend_data;
 638     clear_cdb(sg);
 639     sg->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEMEM_32BIT;
 640     // 2-5: addr
 641     // 6-7: len "unlimited"
 642     write_uint32(sg->cdb_cmd_blk + 2, addr);
 643     write_uint16(sg->cdb_cmd_blk + 6, len);
 644
 645     // data_out 0-0x40-...-len
 646     sl->q_len = len;
 647     sg->q_addr = addr;
 648     sg->q_data_dir = Q_DATA_OUT;
 649     stlink_q(sl);
 650     stlink_print_data(sl);
 651 }
 652
 653 #if 0 /* not working */
 654
 655 static int write_flash_mem16
 656 (struct stlink* sl, uint32_t addr, uint16_t val) {
 657     /* half word writes */
 658     if (addr % 2) return -1;
 659
 660     /* unlock if locked */
 661     unlock_flash_if(sl);
 662
 663     /* set flash programming chosen bit */
 664     set_flash_cr_pg(sl);
 665
 666     write_uint16(sl->q_buf, val);
 667     stlink_write_mem16(sl, addr, 2);
 668
 669     /* wait for non business */
 670     wait_flash_busy(sl);
 671
 672     lock_flash(sl);
 673
 674     /* check the programmed value back */
 675     stlink_read_mem16(sl, addr, 2);
 676     if (*(const uint16_t*) sl->q_buf != val) {
 677         /* values differ at i * sizeof(uint16_t) */
 678         return -1;
 679     }
 680
 681     /* success */
 682     return 0;
 683 }
 684 #endif /* not working */
 685
 686 // Exit the jtag or swd mode and enter the mass mode.
 687
 688 void _stlink_sg_exit_debug_mode(stlink_t *stl) {
 689
 690     if (stl) {
 691         struct stlink_libsg* sl = stl->backend_data;
 692         clear_cdb(sl);
 693         sl->cdb_cmd_blk[1] = STLINK_DEBUG_EXIT;
 694         stl->q_len = 0; // >0 -> aboard
 695         stlink_q(stl);
 696     }
 697 }
 698
 699
 700 // 1) open a sg device, switch the stlink from dfu to mass mode
 701 // 2) wait 5s until the kernel driver stops reseting the broken device
 702 // 3) reopen the device
 703 // 4) the device driver is now ready for a switch to jtag/swd mode
 704 // TODO thinking, better error handling, wait until the kernel driver stops reseting the plugged-in device
 705
 706 stlink_backend_t _stlink_sg_backend = {
 707     _stlink_sg_close,
 708     _stlink_sg_exit_debug_mode,
 709     _stlink_sg_enter_swd_mode,
 710     _stlink_sg_enter_jtag_mode,
 711     _stlink_sg_exit_dfu_mode,
 712     _stlink_sg_core_id,
 713     _stlink_sg_reset,
 714     _stlink_sg_run,
 715     _stlink_sg_status,
 716     _stlink_sg_version,
 717     _stlink_sg_read_mem32,
 718     _stlink_sg_write_mem32,
 719     _stlink_sg_write_mem8,
 720     _stlink_sg_read_all_regs,
 721     _stlink_sg_read_reg,
 722     _stlink_sg_write_reg,
 723     _stlink_sg_step,
 724     _stlink_sg_current_mode,
 725     _stlink_sg_force_debug
 726 };
 727
 728 stlink_t* stlink_open(const char *dev_name, const int verbose) {
 729     fprintf(stderr, "\n*** stlink_open [%s] ***\n", dev_name);
 730     int sg_fd = scsi_pt_open_device(dev_name, RDWR, verbose);
 731     if (sg_fd < 0) {
 732         fprintf(stderr, "error opening device: %s: %s\n", dev_name,
 733                 safe_strerror(-sg_fd));
 734         return NULL;
 735     }
 736
 737     stlink_t *sl = malloc(sizeof (stlink_t));
 738     struct stlink_libsg *slsg = malloc(sizeof (struct stlink_libsg));
 739     if (sl == NULL || slsg == NULL) {
 740         fprintf(stderr, "Couldn't malloc stlink and stlink_sg structures out of memory!\n");
 741         return NULL;
 742     }
 743     sl->verbose = verbose;
 744     sl->backend_data = slsg;
 745     sl->backend = &_stlink_sg_backend;
 746
 747     slsg->sg_fd = sg_fd;
 748     sl->core_stat = STLINK_CORE_STAT_UNKNOWN;
 749     slsg->core_id = 0;
 750     slsg->q_addr = 0;
 751     clear_buf(sl);
 752
 753     /* flash memory settings */
 754     sl->flash_base = STM32_FLASH_BASE;
 755     sl->flash_size = STM32_FLASH_SIZE;
 756     sl->flash_pgsz = STM32_FLASH_PGSZ;
 757
 758     /* system memory */
 759     sl->sys_base = STM32_SYSTEM_BASE;
 760     sl->sys_size = STM32_SYSTEM_SIZE;
 761
 762     /* sram memory settings */
 763     sl->sram_base = STM32_SRAM_BASE;
 764     sl->sram_size = STM32_SRAM_SIZE;
 765
 766     return sl;
 767 }
 768
 769
 770
 771 stlink_t* stlink_quirk_open(const char *dev_name, const int verbose) {
 772
 773     stlink_t *sl = stlink_open(dev_name, verbose);
 774     if (sl == NULL) {
 775         fputs("Error: could not open stlink device\n", stderr);
 776         return NULL;
 777     }
 778
 779     stlink_version(sl);
 780     struct stlink_libsg *sg = sl->backend_data;
 781
 782     if (sg->st_vid != USB_ST_VID || sg->stlink_pid != USB_STLINK_PID) {
 783         fprintf(stderr, "Error: the device %s is not a stlink\n",
 784                 dev_name);
 785         fprintf(stderr, "       VID: got %04x expect %04x \n",
 786                 sg->st_vid, USB_ST_VID);
 787         fprintf(stderr, "       PID: got %04x expect %04x \n",
 788                 sg->stlink_pid, USB_STLINK_PID);
 789         return NULL;
 790     }
 791
 792     D(sl, "\n*** stlink_force_open ***\n");
 793     switch (stlink_current_mode(sl)) {
 794         case STLINK_DEV_MASS_MODE:
 795             return sl;
 796         case STLINK_DEV_DEBUG_MODE:
 797             // TODO go to mass?
 798             return sl;
 799     }
 800     DD(sl, "\n*** switch the stlink to mass mode ***\n");
 801     _stlink_sg_exit_dfu_mode(sl);
 802     // exit the dfu mode -> the device is gone
 803     DD(sl, "\n*** reopen the stlink device ***\n");
 804     delay(1000);
 805     stlink_close(sl);
 806     delay(5000);
 807
 808     sl = stlink_open(dev_name, verbose);
 809     if (sl == NULL) {
 810         fputs("Error: could not open stlink device\n", stderr);
 811         return NULL;
 812     }
 813     // re-query device info
 814     stlink_version(sl);
 815     return sl;
 816 }
 817
 818 static void __attribute__((unused)) mark_buf(stlink_t *sl) {
 819     clear_buf(sl);
 820     sl->q_buf[0] = 0x12;
 821     sl->q_buf[1] = 0x34;
 822     sl->q_buf[2] = 0x56;
 823     sl->q_buf[3] = 0x78;
 824     sl->q_buf[4] = 0x90;
 825     sl->q_buf[15] = 0x42;
 826     sl->q_buf[16] = 0x43;
 827     sl->q_buf[63] = 0x42;
 828     sl->q_buf[64] = 0x43;
 829     sl->q_buf[1024 * 6 - 1] = 0x42; //6kB
 830     sl->q_buf[1024 * 8 - 1] = 0x42; //8kB
 831 }