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