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     if (sl->verbose < 2)
 425         return;
 426     stlink_print_data(sl);
 427 }
 428
 429 // Arm-core reset -> halted state.
 430
 431 void _stlink_sg_reset(stlink_t *sl) {
 432     struct stlink_libsg *sg = sl->backend_data;
 433     clear_cdb(sg);
 434     sg->cdb_cmd_blk[1] = STLINK_DEBUG_RESETSYS;
 435     sl->q_len = 2;
 436     sg->q_addr = 0;
 437     stlink_q(sl);
 438     stlink_stat(sl, "core reset");
 439 }
 440
 441 // Arm-core status: halted or running.
 442
 443 void _stlink_sg_status(stlink_t *sl) {
 444     struct stlink_libsg *sg = sl->backend_data;
 445     D(sl, "\n*** stlink_status ***\n");
 446     clear_cdb(sg);
 447     sg->cdb_cmd_blk[1] = STLINK_DEBUG_GETSTATUS;
 448     sl->q_len = 2;
 449     sg->q_addr = 0;
 450     stlink_q(sl);
 451 }
 452
 453 // Force the core into the debug mode -> halted state.
 454
 455 void stlink_force_debug(stlink_t *sl) {
 456     struct stlink_libsg *sg = sl->backend_data;
 457     D(sl, "\n*** stlink_force_debug ***\n");
 458     clear_cdb(sg);
 459     sg->cdb_cmd_blk[1] = STLINK_DEBUG_FORCEDEBUG;
 460     sl->q_len = 2;
 461     sg->q_addr = 0;
 462     stlink_q(sl);
 463     stlink_stat(sl, "force debug");
 464 }
 465
 466 // Read all arm-core registers.
 467
 468 void _stlink_sg_read_all_regs(stlink_t *sl) {
 469     struct stlink_libsg *sg = sl->backend_data;
 470     D(sl, "\n*** stlink_read_all_regs ***\n");
 471     clear_cdb(sg);
 472     sg->cdb_cmd_blk[1] = STLINK_DEBUG_READALLREGS;
 473     sl->q_len = 84;
 474     sg->q_addr = 0;
 475     stlink_q(sl);
 476     stlink_print_data(sl);
 477
 478     // TODO - most of this should be re-extracted up....
 479
 480     // 0-3 | 4-7 | ... | 60-63 | 64-67 | 68-71   | 72-75      | 76-79 | 80-83
 481     // r0  | r1  | ... | r15   | xpsr  | main_sp | process_sp | rw    | rw2
 482     for (int i = 0; i < 16; i++) {
 483         sg->reg.r[i] = read_uint32(sl->q_buf, 4 * i);
 484         if (sl->verbose > 1)
 485             DD(sl, "r%2d = 0x%08x\n", i, sg->reg.r[i]);
 486     }
 487     sg->reg.xpsr = read_uint32(sl->q_buf, 64);
 488     sg->reg.main_sp = read_uint32(sl->q_buf, 68);
 489     sg->reg.process_sp = read_uint32(sl->q_buf, 72);
 490     sg->reg.rw = read_uint32(sl->q_buf, 76);
 491     sg->reg.rw2 = read_uint32(sl->q_buf, 80);
 492     if (sl->verbose < 2)
 493         return;
 494
 495     DD(sl, "xpsr       = 0x%08x\n", sg->reg.xpsr);
 496     DD(sl, "main_sp    = 0x%08x\n", sg->reg.main_sp);
 497     DD(sl, "process_sp = 0x%08x\n", sg->reg.process_sp);
 498     DD(sl, "rw         = 0x%08x\n", sg->reg.rw);
 499     DD(sl, "rw2        = 0x%08x\n", sg->reg.rw2);
 500 }
 501
 502 // Read an arm-core register, the index must be in the range 0..20.
 503 //  0  |  1  | ... |  15   |  16   |   17    |   18       |  19   |  20
 504 // r0  | r1  | ... | r15   | xpsr  | main_sp | process_sp | rw    | rw2
 505
 506 void _stlink_sg_read_reg(stlink_t *sl, int r_idx, reg *regp) {
 507     struct stlink_libsg *sg = sl->backend_data;
 508     clear_cdb(sg);
 509     sg->cdb_cmd_blk[1] = STLINK_DEBUG_READREG;
 510     sg->cdb_cmd_blk[2] = r_idx;
 511     sl->q_len = 4;
 512     sg->q_addr = 0;
 513     stlink_q(sl);
 514     //  0  |  1  | ... |  15   |  16   |   17    |   18       |  19   |  20
 515     // 0-3 | 4-7 | ... | 60-63 | 64-67 | 68-71   | 72-75      | 76-79 | 80-83
 516     // r0  | r1  | ... | r15   | xpsr  | main_sp | process_sp | rw    | rw2
 517     stlink_print_data(sl);
 518
 519     uint32_t r = read_uint32(sl->q_buf, 0);
 520     DD(sl, "r_idx (%2d) = 0x%08x\n", r_idx, r);
 521
 522     switch (r_idx) {
 523         case 16:
 524             regp->xpsr = r;
 525             break;
 526         case 17:
 527             regp->main_sp = r;
 528             break;
 529         case 18:
 530             regp->process_sp = r;
 531             break;
 532         case 19:
 533             regp->rw = r; //XXX ?(primask, basemask etc.)
 534             break;
 535         case 20:
 536             regp->rw2 = r; //XXX ?(primask, basemask etc.)
 537             break;
 538         default:
 539             regp->r[r_idx] = r;
 540     }
 541 }
 542
 543 // Write an arm-core register. Index:
 544 //  0  |  1  | ... |  15   |  16   |   17    |   18       |  19   |  20
 545 // r0  | r1  | ... | r15   | xpsr  | main_sp | process_sp | rw    | rw2
 546
 547 void _stlink_sg_write_reg(stlink_t *sl, uint32_t reg, int idx) {
 548     struct stlink_libsg *sg = sl->backend_data;
 549     clear_cdb(sg);
 550     sg->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEREG;
 551     //   2: reg index
 552     // 3-6: reg content
 553     sg->cdb_cmd_blk[2] = idx;
 554     write_uint32(sg->cdb_cmd_blk + 3, reg);
 555     sl->q_len = 2;
 556     sg->q_addr = 0;
 557     stlink_q(sl);
 558     stlink_stat(sl, "write reg");
 559 }
 560
 561 // Write a register of the debug module of the core.
 562 // XXX ?(atomic writes)
 563 // TODO test
 564
 565 void stlink_write_dreg(stlink_t *sl, uint32_t reg, uint32_t addr) {
 566     struct stlink_libsg *sg = sl->backend_data;
 567     D(sl, "\n*** stlink_write_dreg ***\n");
 568     clear_cdb(sg);
 569     sg->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEDEBUGREG;
 570     // 2-5: address of reg of the debug module
 571     // 6-9: reg content
 572     write_uint32(sg->cdb_cmd_blk + 2, addr);
 573     write_uint32(sg->cdb_cmd_blk + 6, reg);
 574     sl->q_len = 2;
 575     sg->q_addr = addr;
 576     stlink_q(sl);
 577     stlink_stat(sl, "write debug reg");
 578 }
 579
 580 // Force the core exit the debug mode.
 581
 582 void _stlink_sg_run(stlink_t *sl) {
 583     struct stlink_libsg *sg = sl->backend_data;
 584     D(sl, "\n*** stlink_run ***\n");
 585     clear_cdb(sg);
 586     sg->cdb_cmd_blk[1] = STLINK_DEBUG_RUNCORE;
 587     sl->q_len = 2;
 588     sg->q_addr = 0;
 589     stlink_q(sl);
 590     stlink_stat(sl, "run core");
 591 }
 592
 593 // Step the arm-core.
 594
 595 void _stlink_sg_step(stlink_t *sl) {
 596     struct stlink_libsg *sg = sl->backend_data;
 597     clear_cdb(sg);
 598     sg->cdb_cmd_blk[1] = STLINK_DEBUG_STEPCORE;
 599     sl->q_len = 2;
 600     sg->q_addr = 0;
 601     stlink_q(sl);
 602     stlink_stat(sl, "step core");
 603 }
 604
 605 // TODO test
 606 // see Cortex-M3 Technical Reference Manual
 607 // TODO make delegate!
 608 void stlink_set_hw_bp(stlink_t *sl, int fp_nr, uint32_t addr, int fp) {
 609     D(sl, "\n*** stlink_set_hw_bp ***\n");
 610     struct stlink_libsg *sg = sl->backend_data;
 611     clear_cdb(sg);
 612     sg->cdb_cmd_blk[1] = STLINK_DEBUG_SETFP;
 613     // 2:The number of the flash patch used to set the breakpoint
 614     // 3-6: Address of the breakpoint (LSB)
 615     // 7: FP_ALL (0x02) / FP_UPPER (0x01) / FP_LOWER (0x00)
 616     sl->q_buf[2] = fp_nr;
 617     write_uint32(sl->q_buf, addr);
 618     sl->q_buf[7] = fp;
 619
 620     sl->q_len = 2;
 621     stlink_q(sl);
 622     stlink_stat(sl, "set flash breakpoint");
 623 }
 624
 625 // TODO test
 626
 627 // TODO make delegate!
 628 void stlink_clr_hw_bp(stlink_t *sl, int fp_nr) {
 629     struct stlink_libsg *sg = sl->backend_data;
 630     D(sl, "\n*** stlink_clr_hw_bp ***\n");
 631     clear_cdb(sg);
 632     sg->cdb_cmd_blk[1] = STLINK_DEBUG_CLEARFP;
 633     sg->cdb_cmd_blk[2] = fp_nr;
 634
 635     sl->q_len = 2;
 636     stlink_q(sl);
 637     stlink_stat(sl, "clear flash breakpoint");
 638 }
 639
 640 // Read a "len" bytes to the sl->q_buf from the memory, max 6kB (6144 bytes)
 641
 642 void _stlink_sg_read_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
 643     struct stlink_libsg *sg = sl->backend_data;
 644     clear_cdb(sg);
 645     sg->cdb_cmd_blk[1] = STLINK_DEBUG_READMEM_32BIT;
 646     // 2-5: addr
 647     // 6-7: len
 648     write_uint32(sg->cdb_cmd_blk + 2, addr);
 649     write_uint16(sg->cdb_cmd_blk + 6, len);
 650
 651     // data_in 0-0x40-len
 652     // !!! len _and_ q_len must be max 6k,
 653     //     i.e. >1024 * 6 = 6144 -> aboard)
 654     // !!! if len < q_len: 64*k, 1024*n, n=1..5  -> aboard
 655     //     (broken residue issue)
 656     sl->q_len = len;
 657     sg->q_addr = addr;
 658     stlink_q(sl);
 659     stlink_print_data(sl);
 660 }
 661
 662 // Write a "len" bytes from the sl->q_buf to the memory, max 64 Bytes.
 663
 664 void _stlink_sg_write_mem8(stlink_t *sl, uint32_t addr, uint16_t len) {
 665     struct stlink_libsg *sg = sl->backend_data;
 666     clear_cdb(sg);
 667     sg->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEMEM_8BIT;
 668     // 2-5: addr
 669     // 6-7: len (>0x40 (64) -> aboard)
 670     write_uint32(sg->cdb_cmd_blk + 2, addr);
 671     write_uint16(sg->cdb_cmd_blk + 6, len);
 672
 673     // data_out 0-len
 674     sl->q_len = len;
 675     sg->q_addr = addr;
 676     sg->q_data_dir = Q_DATA_OUT;
 677     stlink_q(sl);
 678     stlink_print_data(sl);
 679 }
 680
 681 // Write a "len" bytes from the sl->q_buf to the memory, max Q_BUF_LEN bytes.
 682
 683 void _stlink_sg_write_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
 684     struct stlink_libsg *sg = sl->backend_data;
 685     clear_cdb(sg);
 686     sg->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEMEM_32BIT;
 687     // 2-5: addr
 688     // 6-7: len "unlimited"
 689     write_uint32(sg->cdb_cmd_blk + 2, addr);
 690     write_uint16(sg->cdb_cmd_blk + 6, len);
 691
 692     // data_out 0-0x40-...-len
 693     sl->q_len = len;
 694     sg->q_addr = addr;
 695     sg->q_data_dir = Q_DATA_OUT;
 696     stlink_q(sl);
 697     stlink_print_data(sl);
 698 }
 699
 700 #if 0 /* not working */
 701
 702 static int write_flash_mem16
 703 (struct stlink* sl, uint32_t addr, uint16_t val) {
 704     /* half word writes */
 705     if (addr % 2) return -1;
 706
 707     /* unlock if locked */
 708     unlock_flash_if(sl);
 709
 710     /* set flash programming chosen bit */
 711     set_flash_cr_pg(sl);
 712
 713     write_uint16(sl->q_buf, val);
 714     stlink_write_mem16(sl, addr, 2);
 715
 716     /* wait for non business */
 717     wait_flash_busy(sl);
 718
 719     lock_flash(sl);
 720
 721     /* check the programmed value back */
 722     stlink_read_mem16(sl, addr, 2);
 723     if (*(const uint16_t*) sl->q_buf != val) {
 724         /* values differ at i * sizeof(uint16_t) */
 725         return -1;
 726     }
 727
 728     /* success */
 729     return 0;
 730 }
 731 #endif /* not working */
 732
 733 // Exit the jtag or swd mode and enter the mass mode.
 734
 735 void _stlink_sg_exit_debug_mode(stlink_t *stl) {
 736
 737     if (stl) {
 738         struct stlink_libsg* sl = stl->backend_data;
 739         clear_cdb(sl);
 740         sl->cdb_cmd_blk[1] = STLINK_DEBUG_EXIT;
 741         stl->q_len = 0; // >0 -> aboard
 742         stlink_q(stl);
 743     }
 744 }
 745
 746
 747 // 1) open a sg device, switch the stlink from dfu to mass mode
 748 // 2) wait 5s until the kernel driver stops reseting the broken device
 749 // 3) reopen the device
 750 // 4) the device driver is now ready for a switch to jtag/swd mode
 751 // TODO thinking, better error handling, wait until the kernel driver stops reseting the plugged-in device
 752
 753 stlink_backend_t _stlink_sg_backend = {
 754     _stlink_sg_close,
 755     _stlink_sg_exit_debug_mode,
 756     _stlink_sg_enter_swd_mode,
 757     _stlink_sg_enter_jtag_mode,
 758     _stlink_sg_exit_dfu_mode,
 759     _stlink_sg_core_id,
 760     _stlink_sg_reset,
 761     _stlink_sg_run,
 762     _stlink_sg_status,
 763     _stlink_sg_version,
 764     _stlink_sg_read_mem32,
 765     _stlink_sg_write_mem32,
 766     _stlink_sg_write_mem8,
 767     _stlink_sg_read_all_regs,
 768     _stlink_sg_read_reg,
 769     _stlink_sg_write_reg,
 770     _stlink_sg_step,
 771     _stlink_sg_current_mode
 772 };
 773
 774 stlink_t* stlink_open(const char *dev_name, const int verbose) {
 775     fprintf(stderr, "\n*** stlink_open [%s] ***\n", dev_name);
 776     int sg_fd = scsi_pt_open_device(dev_name, RDWR, verbose);
 777     if (sg_fd < 0) {
 778         fprintf(stderr, "error opening device: %s: %s\n", dev_name,
 779                 safe_strerror(-sg_fd));
 780         return NULL;
 781     }
 782
 783     stlink_t *sl = malloc(sizeof (stlink_t));
 784     struct stlink_libsg *slsg = malloc(sizeof (struct stlink_libsg));
 785     if (sl == NULL || slsg == NULL) {
 786         fprintf(stderr, "Couldn't malloc stlink and stlink_sg structures out of memory!\n");
 787         return NULL;
 788     }
 789     sl->verbose = verbose;
 790     sl->backend_data = slsg;
 791     sl->backend = &_stlink_sg_backend;
 792
 793     slsg->sg_fd = sg_fd;
 794     sl->core_stat = STLINK_CORE_STAT_UNKNOWN;
 795     slsg->core_id = 0;
 796     slsg->q_addr = 0;
 797     clear_buf(sl);
 798
 799     /* flash memory settings */
 800     sl->flash_base = STM32_FLASH_BASE;
 801     sl->flash_size = STM32_FLASH_SIZE;
 802     sl->flash_pgsz = STM32_FLASH_PGSZ;
 803
 804     /* system memory */
 805     sl->sys_base = STM32_SYSTEM_BASE;
 806     sl->sys_size = STM32_SYSTEM_SIZE;
 807
 808     /* sram memory settings */
 809     sl->sram_base = STM32_SRAM_BASE;
 810     sl->sram_size = STM32_SRAM_SIZE;
 811
 812     return sl;
 813 }
 814
 815
 816
 817 stlink_t* stlink_quirk_open(const char *dev_name, const int verbose) {
 818
 819     stlink_t *sl = stlink_open(dev_name, verbose);
 820     if (sl == NULL) {
 821         fputs("Error: could not open stlink device\n", stderr);
 822         return NULL;
 823     }
 824
 825     stlink_version(sl);
 826     struct stlink_libsg *sg = sl->backend_data;
 827
 828     if (sg->st_vid != USB_ST_VID || sg->stlink_pid != USB_STLINK_PID) {
 829         fprintf(stderr, "Error: the device %s is not a stlink\n",
 830                 dev_name);
 831         fprintf(stderr, "       VID: got %04x expect %04x \n",
 832                 sg->st_vid, USB_ST_VID);
 833         fprintf(stderr, "       PID: got %04x expect %04x \n",
 834                 sg->stlink_pid, USB_STLINK_PID);
 835         return NULL;
 836     }
 837
 838     D(sl, "\n*** stlink_force_open ***\n");
 839     switch (stlink_current_mode(sl)) {
 840         case STLINK_DEV_MASS_MODE:
 841             return sl;
 842         case STLINK_DEV_DEBUG_MODE:
 843             // TODO go to mass?
 844             return sl;
 845     }
 846     DD(sl, "\n*** switch the stlink to mass mode ***\n");
 847     _stlink_sg_exit_dfu_mode(sl);
 848     // exit the dfu mode -> the device is gone
 849     DD(sl, "\n*** reopen the stlink device ***\n");
 850     delay(1000);
 851     stlink_close(sl);
 852     delay(5000);
 853
 854     sl = stlink_open(dev_name, verbose);
 855     if (sl == NULL) {
 856         fputs("Error: could not open stlink device\n", stderr);
 857         return NULL;
 858     }
 859     // re-query device info
 860     stlink_version(sl);
 861     return sl;
 862 }
 863
 864 static void __attribute__((unused)) mark_buf(stlink_t *sl) {
 865     clear_buf(sl);
 866     sl->q_buf[0] = 0x12;
 867     sl->q_buf[1] = 0x34;
 868     sl->q_buf[2] = 0x56;
 869     sl->q_buf[3] = 0x78;
 870     sl->q_buf[4] = 0x90;
 871     sl->q_buf[15] = 0x42;
 872     sl->q_buf[16] = 0x43;
 873     sl->q_buf[63] = 0x42;
 874     sl->q_buf[64] = 0x43;
 875     sl->q_buf[1024 * 6 - 1] = 0x42; //6kB
 876     sl->q_buf[1024 * 8 - 1] = 0x42; //8kB
 877 }